Commit 4477e5ed authored by Justin Carpentier's avatar Justin Carpentier
Browse files

Merge pull request #24 from fvalenza/topic/joint-limits

Topic/joint limits
parents c4b98ace 35994494
......@@ -114,6 +114,7 @@ SET(${PROJECT_NAME}_ALGORITHM_HEADERS
algorithm/kinematics.hpp
algorithm/center-of-mass.hpp
algorithm/non-linear-effects.hpp
algorithm/joint-limits.hpp
)
SET(${PROJECT_NAME}_SIMULATION_HEADERS
......
//
// Copyright (c) 2015 CNRS
//
// This file is part of Pinocchio
// Pinocchio is free software: you can redistribute it
// and/or modify it under the terms of the GNU Lesser General Public
// License as published by the Free Software Foundation, either version
// 3 of the License, or (at your option) any later version.
//
// Pinocchio is distributed in the hope that it will be
// useful, but WITHOUT ANY WARRANTY; without even the implied warranty
// of MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU
// General Lesser Public License for more details. You should have
// received a copy of the GNU Lesser General Public License along with
// Pinocchio If not, see
// <http://www.gnu.org/licenses/>.
#ifndef __se3_joint_limits_hpp__
#define __se3_joint_limits_hpp__
#include "pinocchio/multibody/visitor.hpp"
#include "pinocchio/multibody/model.hpp"
namespace se3
{
inline void jointLimits(const Model & model,
Data & data,
const Eigen::VectorXd & q);
} // namespace se3
/* --- Details -------------------------------------------------------------------- */
namespace se3
{
struct JointLimitsStep : public fusion::JointVisitor<JointLimitsStep>
{
typedef boost::fusion::vector<const se3::Model &,
se3::Data &,
const Model::Index,
const Eigen::VectorXd &
> ArgsType;
JOINT_VISITOR_INIT(JointLimitsStep);
template<typename JointModel>
static void algo(const se3::JointModelBase<JointModel> & jmodel,
se3::JointDataBase<typename JointModel::JointData> & jdata,
const se3::Model & model,
se3::Data & data,
const Model::Index i,
const Eigen::VectorXd & q)
{
using namespace Eigen;
using namespace se3;
// TODO: as limits are static, no need of q, nor computations
jmodel.jointLimit(data.effortLimit) = jmodel.maxEffortLimit();
jmodel.jointTangentLimit(data.velocityLimit) = jmodel.maxVelocityLimit();
jmodel.jointLimit(data.lowerPositionLimit) = jmodel.lowerPosLimit(); // if computation needed, do it here, or may be in lowerPosLimit
jmodel.jointLimit(data.upperPositionLimit) = jmodel.upperPosLimit();
}
};
inline void
jointLimits(const Model & model,
Data& data,
const Eigen::VectorXd & q)
{
for( Model::Index i=1; i<(Model::Index) model.nbody; ++i )
{
JointLimitsStep::run(model.joints[i],
data.joints[i],
JointLimitsStep::ArgsType (model,data,i,q)
);
}
}
} // namespace se3
#endif // ifndef __se3_joint_limits_hpp__
......@@ -24,6 +24,7 @@
#include <Eigen/Geometry>
#include <Eigen/StdVector>
#include <boost/variant.hpp>
#include <limits>
namespace se3
{
......@@ -146,6 +147,12 @@ namespace se3
int i_q; // Index of the joint configuration in the joint configuration vector.
int i_v; // Index of the joint velocity in the joint velocity vector.
Eigen::Matrix<double,NQ,1> position_lower;
Eigen::Matrix<double,NQ,1> position_upper;
Eigen::Matrix<double,NQ,1> effortMax;
Eigen::Matrix<double,NV,1> velocityMax;
public:
int nv() const { return NV; }
int nq() const { return NQ; }
......@@ -153,8 +160,48 @@ namespace se3
const int & idx_v() const { return i_v; }
const Index & id() const { return i_id; }
const Eigen::Matrix<double,NQ,1> & lowerPosLimit() const { return position_lower;}
const Eigen::Matrix<double,NQ,1> & upperPosLimit() const { return position_upper;}
const Eigen::Matrix<double,NQ,1> & maxEffortLimit() const { return effortMax;}
const Eigen::Matrix<double,NV,1> & maxVelocityLimit() const { return velocityMax;}
void setIndexes(Index id,int q,int v) { i_id = id, i_q = q; i_v = v; }
void setLowerPositionLimit(const Eigen::VectorXd & lowerPos)
{
if (lowerPos.rows() == NQ)
position_lower = lowerPos;
else
position_lower.fill(-std::numeric_limits<double>::infinity());
}
void setUpperPositionLimit(const Eigen::VectorXd & upperPos)
{
if (upperPos.rows() == NQ)
position_upper = upperPos;
else
position_upper.fill(std::numeric_limits<double>::infinity());
}
void setMaxEffortLimit(const Eigen::VectorXd & effort)
{
if (effort.rows() == NQ)
effortMax = effort;
else
effortMax.fill(std::numeric_limits<double>::infinity());
}
void setMaxVelocityLimit(const Eigen::VectorXd & v)
{
if (v.rows() == NV)
velocityMax = v;
else
velocityMax.fill(std::numeric_limits<double>::infinity());
}
template<typename D>
typename D::template ConstFixedSegmentReturnType<NV>::Type jointMotion(const Eigen::MatrixBase<D>& a) const { return a.template segment<NV>(i_v); }
template<typename D>
......@@ -166,6 +213,21 @@ namespace se3
template<typename D>
typename D::template FixedSegmentReturnType<NV>::Type jointForce(Eigen::MatrixBase<D>& tau) const
{ return tau.template segment<NV>(i_v); }
template<typename D>
typename D::template ConstFixedSegmentReturnType<NQ>::Type jointLimit(const Eigen::MatrixBase<D>& limit) const
{ return limit.template segment<NQ>(i_q); }
template<typename D>
typename D::template FixedSegmentReturnType<NQ>::Type jointLimit(Eigen::MatrixBase<D>& limit) const
{ return limit.template segment<NQ>(i_q); }
template<typename D>
typename D::template ConstFixedSegmentReturnType<NV>::Type jointTangentLimit(const Eigen::MatrixBase<D>& limit) const
{ return limit.template segment<NV>(i_v); }
template<typename D>
typename D::template FixedSegmentReturnType<NV>::Type jointTangentLimit(Eigen::MatrixBase<D>& limit) const
{ return limit.template segment<NV>(i_v); }
};
} // namespace se3
......
......@@ -94,6 +94,54 @@ namespace se3
};
inline int idx_v(const JointModelVariant & jmodel) { return Joint_idx_v::run(jmodel); }
class Joint_lowerPosLimit: public boost::static_visitor<Eigen::MatrixXd>
{
public:
template<typename D>
Eigen::MatrixXd operator()(const JointModelBase<D> & jmodel) const
{ return jmodel.lowerPosLimit(); }
static Eigen::MatrixXd run( const JointModelVariant & jmodel)
{ return boost::apply_visitor( Joint_lowerPosLimit(), jmodel ); }
};
inline Eigen::MatrixXd lowerPosLimit(const JointModelVariant & jmodel) { return Joint_lowerPosLimit::run(jmodel); }
class Joint_upperPosLimit: public boost::static_visitor<Eigen::MatrixXd>
{
public:
template<typename D>
Eigen::MatrixXd operator()(const JointModelBase<D> & jmodel) const
{ return jmodel.upperPosLimit(); }
static Eigen::MatrixXd run( const JointModelVariant & jmodel)
{ return boost::apply_visitor( Joint_upperPosLimit(), jmodel ); }
};
inline Eigen::MatrixXd upperPosLimit(const JointModelVariant & jmodel) { return Joint_upperPosLimit::run(jmodel); }
class Joint_maxEffortLimit: public boost::static_visitor<Eigen::MatrixXd>
{
public:
template<typename D>
Eigen::MatrixXd operator()(const JointModelBase<D> & jmodel) const
{ return jmodel.maxEffortLimit(); }
static Eigen::MatrixXd run( const JointModelVariant & jmodel)
{ return boost::apply_visitor( Joint_maxEffortLimit(), jmodel ); }
};
inline Eigen::MatrixXd maxEffortLimit(const JointModelVariant & jmodel) { return Joint_maxEffortLimit::run(jmodel); }
class Joint_maxVelocityLimit: public boost::static_visitor<Eigen::MatrixXd>
{
public:
template<typename D>
Eigen::MatrixXd operator()(const JointModelBase<D> & jmodel) const
{ return jmodel.maxVelocityLimit(); }
static Eigen::MatrixXd run( const JointModelVariant & jmodel)
{ return boost::apply_visitor( Joint_maxVelocityLimit(), jmodel ); }
};
inline Eigen::MatrixXd maxVelocityLimit(const JointModelVariant & jmodel) { return Joint_maxVelocityLimit::run(jmodel); }
} // namespace se3
......
......@@ -43,25 +43,25 @@ namespace se3
public:
typedef std::size_t Index;
int nq; // Dimension of the configuration representation
int nv; // Dimension of the velocity vector space
int nbody; // Number of bodies (= number of joints + 1)
int nFixBody; // Number of fixed-bodies (= number of fixed-joints)
std::vector<Inertia> inertias; // Spatial inertias of the body <i> in the supporting joint frame <i>
std::vector<SE3> jointPlacements; // Placement (SE3) of the input of joint <i> in parent joint output <li>
JointModelVector joints; // Model of joint <i>
std::vector<Index> parents; // Joint parent of joint <i>, denoted <li> (li==parents[i])
std::vector<std::string> names; // Name of joint <i>
std::vector<std::string> bodyNames;// Name of the body attached to the output of joint <i>
std::vector<bool> hasVisual; // True iff body <i> has a visual mesh.
std::vector<SE3> fix_lmpMi; // Fixed-body relative placement (wrt last moving parent)
int nq; // Dimension of the configuration representation
int nv; // Dimension of the velocity vector space
int nbody; // Number of bodies (= number of joints + 1)
int nFixBody; // Number of fixed-bodies (= number of fixed-joints)
std::vector<Inertia> inertias; // Spatial inertias of the body <i> in the supporting joint frame <i>
std::vector<SE3> jointPlacements; // Placement (SE3) of the input of joint <i> in parent joint output <li>
JointModelVector joints; // Model of joint <i>
std::vector<Index> parents; // Joint parent of joint <i>, denoted <li> (li==parents[i])
std::vector<std::string> names; // Name of joint <i>
std::vector<std::string> bodyNames; // Name of the body attached to the output of joint <i>
std::vector<bool> hasVisual; // True iff body <i> has a visual mesh.
std::vector<SE3> fix_lmpMi; // Fixed-body relative placement (wrt last moving parent)
std::vector<Model::Index> fix_lastMovingParent; // Fixed-body index of the last moving parent
std::vector<bool> fix_hasVisual; // True iff fixed-body <i> has a visual mesh.
std::vector<bool> fix_hasVisual; // True iff fixed-body <i> has a visual mesh.
std::vector<std::string> fix_bodyNames;// Name of fixed-joint <i>
Motion gravity; // Spatial gravity
Motion gravity; // Spatial gravity
static const Eigen::Vector3d gravity981; // Default 3D gravity (=(0,0,9.81))
Model()
......@@ -83,9 +83,17 @@ namespace se3
}
~Model() {} // std::cout << "Destroy model" << std::endl; }
template<typename D>
Index addBody( Index parent,const JointModelBase<D> & j,const SE3 & placement,
const Inertia & Y, const std::string & jointName = "",
const std::string & bodyName = "", bool visual = false );
Index addBody( Index parent,const JointModelBase<D> & j,const SE3 & placement,
const Inertia & Y,
const std::string & jointName = "", const std::string & bodyName = "",
bool visual = false);
template<typename D>
Index addBody( Index parent,const JointModelBase<D> & j,const SE3 & placement,
const Inertia & Y,
const Eigen::VectorXd & effort, const Eigen::VectorXd & velocity,
const Eigen::VectorXd & lowPos, const Eigen::VectorXd & upPos,
const std::string & jointName = "", const std::string & bodyName = "",
bool visual = false);
Index addFixedBody( Index fix_lastMovingParent,
const SE3 & placementFromLastMoving,
const std::string &jointName = "",
......@@ -133,6 +141,13 @@ namespace se3
std::vector<Eigen::Vector3d> com; // Subtree com position.
std::vector<double> mass; // Subtree total mass.
Eigen::Matrix<double,3,Eigen::Dynamic> Jcom; // Jacobian of center of mass.
Eigen::VectorXd effortLimit; // Joint max effort
Eigen::VectorXd velocityLimit; // Joint max velocity
Eigen::VectorXd lowerPositionLimit; // limit for joint lower position
Eigen::VectorXd upperPositionLimit; // limit for joint upper position
Data( const Model& ref );
private:
......@@ -179,8 +194,36 @@ namespace se3
template<typename D>
Model::Index Model::addBody( Index parent,const JointModelBase<D> & j,const SE3 & placement,
const Inertia & Y,const std::string & jointName,
const std::string & bodyName, bool visual )
const Inertia & Y,const std::string & jointName,
const std::string & bodyName, bool visual)
{
assert( (nbody==(int)joints.size())&&(nbody==(int)inertias.size())
&&(nbody==(int)parents.size())&&(nbody==(int)jointPlacements.size()) );
assert( (j.nq()>=0)&&(j.nv()>=0) );
Model::Index idx = (Model::Index) (nbody ++);
joints .push_back(j.derived());
boost::get<D&>(joints.back()).setIndexes((int)idx,nq,nv);
inertias .push_back(Y);
parents .push_back(parent);
jointPlacements.push_back(placement);
names .push_back( (jointName!="")?jointName:random(8) );
hasVisual .push_back(visual);
bodyNames .push_back( (bodyName!="")?bodyName:random(8));
nq += j.nq();
nv += j.nv();
return idx;
}
template<typename D>
Model::Index Model::addBody( Index parent,const JointModelBase<D> & j,const SE3 & placement,
const Inertia & Y,
const Eigen::VectorXd & effort, const Eigen::VectorXd & velocity,
const Eigen::VectorXd & lowPos, const Eigen::VectorXd & upPos,
const std::string & jointName,
const std::string & bodyName, bool visual)
{
assert( (nbody==(int)joints.size())&&(nbody==(int)inertias.size())
&&(nbody==(int)parents.size())&&(nbody==(int)jointPlacements.size()) );
......@@ -191,6 +234,12 @@ namespace se3
joints .push_back(j.derived());
boost::get<D&>(joints.back()).setIndexes((int)idx,nq,nv);
boost::get<D&>(joints.back()).setMaxEffortLimit(effort);
boost::get<D&>(joints.back()).setMaxVelocityLimit(velocity);
boost::get<D&>(joints.back()).setLowerPositionLimit(lowPos);
boost::get<D&>(joints.back()).setUpperPositionLimit(upPos);
inertias .push_back(Y);
parents .push_back(parent);
jointPlacements.push_back(placement);
......@@ -268,6 +317,10 @@ namespace se3
,com((std::size_t)ref.nbody)
,mass((std::size_t)ref.nbody)
,Jcom(3,ref.nv)
,effortLimit(ref.nq)
,velocityLimit(ref.nv)
,lowerPositionLimit(ref.nq)
,upperPositionLimit(ref.nq)
{
for(Model::Index i=0;i<(Model::Index)(model.nbody);++i)
joints.push_back(CreateJointData::run(model.joints[i]));
......
......@@ -113,24 +113,55 @@ namespace se3
case ::urdf::Joint::REVOLUTE:
case ::urdf::Joint::CONTINUOUS: // Revolute with no joint limits
{
// const double maxEffort = (joint->limits) ?
// (joint->limits->effort)
// : std::numeric_limits<double>::infinity();
// const double lowerPosition = (joint->limits) ?
// (joint->limits->lower != 0. ?
// joint->limits->lower
// : -std::numeric_limits<double>::infinity()
// )
// : -std::numeric_limits<double>::infinity();
Eigen::VectorXd maxEffort;
Eigen::VectorXd velocity;
Eigen::VectorXd lowerPosition;
Eigen::VectorXd upperPosition;
if (joint->limits)
{
maxEffort.resize(1); maxEffort << joint->limits->effort;
velocity.resize(1); velocity << joint->limits->velocity;
lowerPosition.resize(1); lowerPosition << joint->limits->lower;
upperPosition.resize(1); upperPosition << joint->limits->upper;
}
Eigen::Vector3d jointAxis(Eigen::Vector3d::Zero());
AxisCartesian axis = extractCartesianAxis(joint->axis);
switch(axis)
{
case AXIS_X:
model.addBody( parent, JointModelRX(), jointPlacement, Y, joint->name,link->name, visual );
model.addBody( parent, JointModelRX(), jointPlacement, Y,
maxEffort, velocity, lowerPosition, upperPosition,
joint->name,link->name, visual );
break;
case AXIS_Y:
model.addBody( parent, JointModelRY(), jointPlacement, Y, joint->name,link->name, visual );
model.addBody( parent, JointModelRY(), jointPlacement, Y,
maxEffort, velocity, lowerPosition, upperPosition,
joint->name,link->name, visual );
break;
case AXIS_Z:
model.addBody( parent, JointModelRZ(), jointPlacement, Y, joint->name,link->name, visual );
model.addBody( parent, JointModelRZ(), jointPlacement, Y,
maxEffort, velocity, lowerPosition, upperPosition,
joint->name,link->name, visual );
break;
case AXIS_UNALIGNED:
jointAxis= Eigen::Vector3d( joint->axis.x,joint->axis.y,joint->axis.z );
jointAxis.normalize();
model.addBody( parent, JointModelRevoluteUnaligned(jointAxis),
jointPlacement, Y, joint->name,link->name, visual );
model.addBody( parent, JointModelRevoluteUnaligned(jointAxis),
jointPlacement, Y,
maxEffort, velocity, lowerPosition, upperPosition,
joint->name,link->name, visual );
break;
default:
assert( false && "Fatal Error while extracting revolute joint axis");
......@@ -140,17 +171,35 @@ namespace se3
}
case ::urdf::Joint::PRISMATIC:
{
Eigen::VectorXd maxEffort = Eigen::VectorXd(0.);
Eigen::VectorXd velocity = Eigen::VectorXd(0.);
Eigen::VectorXd lowerPosition = Eigen::VectorXd(0.);
Eigen::VectorXd upperPosition = Eigen::VectorXd(0.);
if (joint->limits)
{
maxEffort.resize(1); maxEffort << joint->limits->effort;
velocity.resize(1); velocity << joint->limits->velocity;
lowerPosition.resize(1); lowerPosition << joint->limits->lower;
upperPosition.resize(1); upperPosition << joint->limits->upper;
}
AxisCartesian axis = extractCartesianAxis(joint->axis);
switch(axis)
{
case AXIS_X:
model.addBody( parent, JointModelPX(), jointPlacement, Y, joint->name,link->name, visual );
model.addBody( parent, JointModelPX(), jointPlacement, Y,
maxEffort, velocity, lowerPosition, upperPosition,
joint->name,link->name, visual );
break;
case AXIS_Y:
model.addBody( parent, JointModelPY(), jointPlacement, Y, joint->name,link->name, visual );
model.addBody( parent, JointModelPY(), jointPlacement, Y,
maxEffort, velocity, lowerPosition, upperPosition,
joint->name,link->name, visual );
break;
case AXIS_Z:
model.addBody( parent, JointModelPZ(), jointPlacement, Y, joint->name,link->name, visual );
model.addBody( parent, JointModelPZ(), jointPlacement, Y,
maxEffort, velocity, lowerPosition, upperPosition,
joint->name,link->name, visual );
break;
case AXIS_UNALIGNED:
std::cerr << "Bad axis = (" <<joint->axis.x<<","<<joint->axis.y
......
......@@ -30,6 +30,7 @@
#include "pinocchio/algorithm/kinematics.hpp"
#include "pinocchio/algorithm/jacobian.hpp"
#include "pinocchio/algorithm/center-of-mass.hpp"
#include "pinocchio/algorithm/joint-limits.hpp"
#include "pinocchio/simulation/compute-all-terms.hpp"
......@@ -42,10 +43,10 @@ namespace se3
typedef eigenpy::UnalignedEquivalent<Eigen::VectorXd>::type VectorXd_fx;
static Eigen::VectorXd rnea_proxy( const ModelHandler& model,
DataHandler & data,
const VectorXd_fx & q,
const VectorXd_fx & qd,
const VectorXd_fx & qdd )
DataHandler & data,
const VectorXd_fx & q,
const VectorXd_fx & qd,
const VectorXd_fx & qdd )
{ return rnea(*model,*data,q,qd,qdd); }
static Eigen::VectorXd nle_proxy( const ModelHandler& model,
......@@ -55,45 +56,45 @@ namespace se3
{ return nonLinearEffects(*model,*data,q,qd); }
static Eigen::MatrixXd crba_proxy( const ModelHandler& model,
DataHandler & data,
const VectorXd_fx & q )
DataHandler & data,
const VectorXd_fx & q )
{
data->M.fill(0);
crba(*model,*data,q);
data->M.triangularView<Eigen::StrictlyLower>()
= data->M.transpose().triangularView<Eigen::StrictlyLower>();
return data->M;
data->M.fill(0);
crba(*model,*data,q);
data->M.triangularView<Eigen::StrictlyLower>()
= data->M.transpose().triangularView<Eigen::StrictlyLower>();
return data->M;
}
static Eigen::VectorXd com_proxy( const ModelHandler& model,
DataHandler & data,
const VectorXd_fx & q )
DataHandler & data,
const VectorXd_fx & q )
{ return centerOfMass(*model,*data,q); }
static Eigen::MatrixXd Jcom_proxy( const ModelHandler& model,
DataHandler & data,
const VectorXd_fx & q )
DataHandler & data,
const VectorXd_fx & q )
{ return jacobianCenterOfMass(*model,*data,q); }
static Eigen::MatrixXd jacobian_proxy( const ModelHandler& model,
DataHandler & data,
Model::Index jointId,
const VectorXd_fx & q,
bool local )
DataHandler & data,
Model::Index jointId,
const VectorXd_fx & q,
bool local )
{
Eigen::MatrixXd J( 6,model->nv ); J.setZero();
computeJacobians( *model,*data,q );
if(local) getJacobian<true> (*model, *data, jointId, J);
else getJacobian<false> (*model, *data, jointId, J);
return J;
Eigen::MatrixXd J( 6,model->nv ); J.setZero();
computeJacobians( *model,*data,q );
if(local) getJacobian<true> (*model, *data, jointId, J);
else getJacobian<false> (*model, *data, jointId, J);
return J;
}
static void kinematics_proxy( const ModelHandler& model,
DataHandler & data,
const VectorXd_fx & q,
const VectorXd_fx & qdot )
DataHandler & data,
const VectorXd_fx & q,
const VectorXd_fx & qdot )
{
kinematics( *model,*data,q,qdot );
kinematics( *model,*data,q,qdot );
}
static void geometry_proxy(const ModelHandler & model,
......@@ -111,17 +112,24 @@ namespace se3
computeAllTerms(*model,*data,q,v);
}
static void jointLimits_proxy(const ModelHandler & model,
DataHandler & data,
const VectorXd_fx & q)
{
jointLimits(*model,*data,q);
}
/* --- Expose --------------------------------------------------------- */
static void expose()
{
bp::def("rnea",rnea_proxy,
bp::args("Model","Data",
"Configuration q (size Model::nq)",
"Velocity qdot (size Model::nv)",
"Acceleration qddot (size Model::nv)"),
"Compute the RNEA, put the result in Data and return it.");