Commit 128c72f7 authored by Florian Valenza's avatar Florian Valenza
Browse files

[Major][C++] Added Joint limits to joint model and to Pinocchio Data. Also...

[Major][C++] Added Joint limits to joint model and to Pinocchio Data. Also added an algorithm to compute joint limits. Based on the model of the limits, it computes and store the actual limits in Data. We then have access to the values in Data in python binding. The algorithm is here in future cases where the limits will not only be static but dependent of the configuration
parent c4b98ace
......@@ -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]));
......
......@@ -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.");
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.");
bp::def("nle",nle_proxy,
bp::args("Model","Data",
......@@ -129,27 +137,27 @@ namespace se3
"Velocity qdot (size Model::nv)"),
"Compute the Non Linear Effects (coriolis, centrifugal and gravitational effects), put the result in Data and return it.");
bp::def("crba",crba_proxy,
bp::args("Model","Data",
"Configuration q (size Model::nq)"),
"Compute CRBA, put the result in Data and return it.");
bp::def("crba",crba_proxy,
bp::args("Model","Data",
"Configuration q (size Model::nq)"),
"Compute CRBA, put the result in Data and return it.");
bp::def("centerOfMass",com_proxy,
bp::args("Model","Data",
"Configuration q (size Model::nq)"),
"Compute the center of mass, putting the result in Data and return it.");
bp::def("centerOfMass",com_proxy,
bp::args("Model","Data",
"Configuration q (size Model::nq)"),
"Compute the center of mass, putting the result in Data and return it.");
bp::def("jacobianCenterOfMass",Jcom_proxy,
bp::args("Model","Data",
"Configuration q (size Model::nq)"),
"Compute the jacobian of the center of mass, put the result in Data and return it.");
bp::def("jacobianCenterOfMass",Jcom_proxy,
bp::args("Model","Data",
"Configuration q (size Model::nq)"),
"Compute the jacobian of the center of mass, put the result in Data and return it.");
bp::def("kinematics",kinematics_proxy,
bp::args("Model","Data",
"Configuration q (size Model::nq)",
"Velocity qdot (size Model::nv)"),
"Compute the placements and spatial velocities of all the frames of the kinematic "
"tree and put the results in data.");
bp::def("kinematics",kinematics_proxy,
bp::args("Model","Data",
"Configuration q (size Model::nq)",
"Velocity qdot (size Model::nv)"),
"Compute the placements and spatial velocities of all the frames of the kinematic "
"tree and put the results in data.");
bp::def("geometry",geometry_proxy,
bp::args("Model","Data",
......@@ -157,23 +165,28 @@ namespace se3
"Compute the placements of all the frames of the kinematic "
"tree and put the results in data.");
bp::def("geometry",computeAllTerms_proxy,
bp::def("computeAllTerms",computeAllTerms_proxy,
bp::args("Model","Data",
"Configuration q (size Model::nq)",
"Velocity v (size Model::nv)"),
"Compute all the terms M, non linear effects and Jacobians in"
"in the same loop and put the results in data.");
bp::def("jacobian",jacobian_proxy,
bp::args("Model","Data",
"Configuration q (size Model::nq)",
"Joint ID (int)",
"frame (true = local, false = world)"),
"Calling computeJacobians then getJacobian, return the result. Attention: the "
"function computes indeed all the jacobians of the model, even if just outputing "
"the demanded one. It is therefore outrageously costly wrt a dedicated "
"call. Function to be used only for prototyping.");
bp::def("jacobian",jacobian_proxy,
bp::args("Model","Data",
"Configuration q (size Model::nq)",
"Joint ID (int)",
"frame (true = local, false = world)"),
"Calling computeJacobians then getJacobian, return the result. Attention: the "
"function computes indeed all the jacobians of the model, even if just outputing "
"the demanded one. It is therefore outrageously costly wrt a dedicated "
"call. Function to be used only for prototyping.");
bp::def("jointLimits",jointLimits_proxy,
bp::args("Model","Data",
"Configuration q (size Model::nq)"),
"Compute the maximum limits of all the joints of the model "
"and put the results in data.");
}
};
......
......@@ -88,6 +88,12 @@ namespace se3
.ADD_DATA_PROPERTY_CONST(std::vector<Eigen::Vector3d>,com,"Subtree com position.")
.ADD_DATA_PROPERTY(std::vector<double>,mass,"Subtree total mass.")
.ADD_DATA_PROPERTY_CONST(Matrix3x,Jcom,"Jacobian of center of mass.")
.ADD_DATA_PROPERTY_CONST(Eigen::VectorXd,effortLimit,"Joint max effort")
.ADD_DATA_PROPERTY_CONST(Eigen::VectorXd,velocityLimit,"Joint max velocity")
.ADD_DATA_PROPERTY_CONST(Eigen::VectorXd,lowerPositionLimit,"Limit for joint lower position")
.ADD_DATA_PROPERTY_CONST(Eigen::VectorXd,upperPositionLimit,"Limit for joint upper position")