6 #ifndef __pinocchio_multibody_model_hpp__ 7 #define __pinocchio_multibody_model_hpp__ 9 #include "pinocchio/spatial/fwd.hpp" 10 #include "pinocchio/spatial/se3.hpp" 11 #include "pinocchio/spatial/force.hpp" 12 #include "pinocchio/spatial/motion.hpp" 13 #include "pinocchio/spatial/inertia.hpp" 15 #include "pinocchio/multibody/fwd.hpp" 16 #include "pinocchio/multibody/frame.hpp" 17 #include "pinocchio/multibody/joint/joint-generic.hpp" 19 #include "pinocchio/container/aligned-vector.hpp" 21 #include "pinocchio/serialization/serializable.hpp" 30 template<
typename _Scalar,
int _Options,
template<
typename,
int>
class JointCollectionTpl>
32 : serialization::Serializable< ModelTpl<_Scalar,_Options,JointCollectionTpl> >
34 EIGEN_MAKE_ALIGNED_OPERATOR_NEW
36 typedef _Scalar Scalar;
37 enum { Options = _Options };
39 typedef JointCollectionTpl<Scalar,Options> JointCollection;
40 typedef DataTpl<Scalar,Options,JointCollectionTpl> Data;
42 typedef SE3Tpl<Scalar,Options> SE3;
43 typedef MotionTpl<Scalar,Options> Motion;
44 typedef ForceTpl<Scalar,Options> Force;
45 typedef InertiaTpl<Scalar,Options> Inertia;
46 typedef FrameTpl<Scalar,Options> Frame;
48 typedef pinocchio::Index Index;
49 typedef pinocchio::JointIndex JointIndex;
50 typedef pinocchio::GeomIndex GeomIndex;
51 typedef pinocchio::FrameIndex FrameIndex;
52 typedef std::vector<Index> IndexVector;
54 typedef JointModelTpl<Scalar,Options,JointCollectionTpl> JointModel;
55 typedef JointDataTpl<Scalar,Options,JointCollectionTpl> JointData;
57 typedef PINOCCHIO_ALIGNED_STD_VECTOR(JointModel) JointModelVector;
58 typedef PINOCCHIO_ALIGNED_STD_VECTOR(JointData) JointDataVector;
60 typedef PINOCCHIO_ALIGNED_STD_VECTOR(Frame) FrameVector;
62 typedef Eigen::Matrix<Scalar,Eigen::Dynamic,1,Options> VectorXs;
63 typedef Eigen::Matrix<Scalar,3,1,Options> Vector3;
91 PINOCCHIO_ALIGNED_STD_VECTOR(Inertia) inertias;
94 PINOCCHIO_ALIGNED_STD_VECTOR(SE3) jointPlacements;
173 , inertias(1, Inertia::Zero())
174 , jointPlacements(1, SE3::Identity())
182 , supports(1,IndexVector(1,0))
184 , gravity(gravity981,Vector3::Zero())
186 names[0] =
"universe";
190 addFrame(Frame(
"universe", 0, 0, SE3::Identity(), FIXED_JOINT));
195 template<
typename NewScalar>
207 res.gravity = gravity.template cast<NewScalar>();
216 res.rotorInertia = rotorInertia.template cast<NewScalar>();
217 res.rotorGearRatio = rotorGearRatio.template cast<NewScalar>();
218 res.friction = friction.template cast<NewScalar>();
219 res.damping = damping.template cast<NewScalar>();
220 res.effortLimit = effortLimit.template cast<NewScalar>();
221 res.velocityLimit = velocityLimit.template cast<NewScalar>();
222 res.lowerPositionLimit = lowerPositionLimit.template cast<NewScalar>();
223 res.upperPositionLimit = upperPositionLimit.template cast<NewScalar>();
225 typename ConfigVectorMap::const_iterator it;
226 for (it = referenceConfigurations.begin();
227 it != referenceConfigurations.end(); it++ )
229 res.referenceConfigurations.insert(std::make_pair(it->first, it->second.template cast<NewScalar>()));
233 res.inertias.resize(inertias.size());
234 res.jointPlacements.resize(jointPlacements.size());
235 res.joints.resize(joints.size());
236 res.frames.resize(frames.size());
239 for(
size_t k = 0; k < joints.size(); ++k)
241 res.inertias[k] = inertias[k].template cast<NewScalar>();
242 res.jointPlacements[k] = jointPlacements[k].template cast<NewScalar>();
243 res.joints[k] = joints[k].template cast<NewScalar>();
246 for(
size_t k = 0; k < frames.size(); ++k)
248 res.frames[k] = frames[k].template cast<NewScalar>();
264 && other.njoints == njoints
265 && other.nbodies == nbodies
266 && other.nframes == nframes
267 && other.parents == parents
268 && other.names == names
269 && other.subtrees == subtrees
270 && other.gravity == gravity
271 && other.name ==
name;
274 other.idx_qs == idx_qs
276 && other.idx_vs == idx_vs
279 if(other.referenceConfigurations.size() != referenceConfigurations.size())
282 typename ConfigVectorMap::const_iterator it = referenceConfigurations.begin();
283 typename ConfigVectorMap::const_iterator it_other = other.referenceConfigurations.begin();
284 for(
long k = 0; k < (long)referenceConfigurations.size(); ++k)
286 std::advance(it,k); std::advance(it_other,k);
288 if(it->second.size() != it_other->second.size())
290 if(it->second != it_other->second)
294 if(other.rotorInertia.size() != rotorInertia.size())
299 if(other.friction.size() != friction.size())
304 if(other.damping.size() != damping.size())
306 res &= other.damping ==
damping;
309 if(other.rotorGearRatio.size() != rotorGearRatio.size())
314 if(other.effortLimit.size() != effortLimit.size())
319 if(other.velocityLimit.size() != velocityLimit.size())
324 if(other.lowerPositionLimit.size() != lowerPositionLimit.size())
329 if(other.upperPositionLimit.size() != upperPositionLimit.size())
334 for(
size_t k = 1; k < inertias.size(); ++k)
336 res &= other.inertias[k] == inertias[k];
340 for(
size_t k = 1; k < other.jointPlacements.size(); ++k)
342 res &= other.jointPlacements[k] == jointPlacements[k];
347 other.joints == joints
348 && other.frames ==
frames;
357 {
return !(*
this == other); }
376 JointIndex
addJoint(
const JointIndex parent,
377 const JointModel & joint_model,
378 const SE3 & joint_placement,
379 const std::string & joint_name);
389 JointIndex
addJoint(
const JointIndex parent,
390 const JointModel & joint_model,
391 const SE3 & joint_placement,
392 const std::string & joint_name,
393 const VectorXs & max_effort,
394 const VectorXs & max_velocity,
395 const VectorXs & min_config,
396 const VectorXs & max_config);
404 JointIndex
addJoint(
const JointIndex parent,
405 const JointModel & joint_model,
406 const SE3 & joint_placement,
407 const std::string & joint_name,
408 const VectorXs & max_effort,
409 const VectorXs & max_velocity,
410 const VectorXs & min_config,
411 const VectorXs & max_config,
412 const VectorXs & friction,
413 const VectorXs & damping);
425 int previous_frame_index = -1);
437 const SE3 & body_placement = SE3::Identity());
451 const JointIndex & parentJoint,
452 const SE3 & body_placement = SE3::Identity(),
453 int previousFrame = -1);
466 FrameIndex
getBodyId(
const std::string & name)
const;
488 JointIndex
getJointId(
const std::string & name)
const;
512 FrameIndex
getFrameId(
const std::string & name,
513 const FrameType & type = (
FrameType) (JOINT | FIXED_JOINT | BODY | OP_FRAME | SENSOR ))
const;
524 const FrameType& type = (
FrameType) (JOINT | FIXED_JOINT | BODY | OP_FRAME | SENSOR ))
const;
535 FrameIndex
addFrame(
const Frame & frame,
536 const bool append_inertia =
true);
550 {
return checker.checkModel(*
this); }
553 inline bool check()
const;
562 inline bool check(
const Data & data)
const;
579 #include "pinocchio/multibody/model.hxx" 581 #endif // ifndef __pinocchio_multibody_model_hpp__ bool existJointName(const std::string &name) const
Check if a joint given by its name exists.
std::vector< int > idx_vs
Starting index of the joint i in the tangent configuration space.
FrameIndex getFrameId(const std::string &name, const FrameType &type=(FrameType)(JOINT|FIXED_JOINT|BODY|OP_FRAME|SENSOR)) const
Returns the index of a frame given by its name.
JointModelVector joints
Model of joint i, encapsulated in a JointModelAccessor.
void appendBodyToJoint(const JointIndex joint_index, const Inertia &Y, const SE3 &body_placement=SE3::Identity())
Append a body to a given joint of the kinematic tree.
ModelTpl()
Default constructor. Builds an empty model with no joints.
JointIndex getJointId(const std::string &name) const
Return the index of a joint given by its name.
VectorXs ConfigVectorType
Dense vectorized version of a joint configuration vector.
int njoints
Number of joints.
FrameIndex getBodyId(const std::string &name) const
Return the index of a body given by its name.
ConfigVectorMap referenceConfigurations
Map of reference configurations, indexed by user given names.
int nv
Dimension of the velocity vector space.
bool check() const
Run check(fusion::list) with DEFAULT_CHECKERS as argument.
std::vector< IndexVector > supports
Vector of joint supports. supports[j] corresponds to the collection of all joints located on the path...
bool existBodyName(const std::string &name) const
Check if a body given by its name exists.
Motion gravity
Spatial gravity of the model.
std::vector< std::string > names
Name of joint i
void addJointIndexToParentSubtrees(const JointIndex joint_id)
Add the joint_id to its parent subtrees.
FrameIndex addBodyFrame(const std::string &body_name, const JointIndex &parentJoint, const SE3 &body_placement=SE3::Identity(), int previousFrame=-1)
Add a body to the frame tree.
FrameVector frames
Vector of operational frames registered on the model.
std::vector< IndexVector > subtrees
Vector of joint subtrees. subtree[j] corresponds to the subtree supported by the joint j...
TangentVectorType effortLimit
Vector of maximal joint torques.
std::vector< int > nqs
Dimension of the joint i configuration subspace.
bool operator==(const ModelTpl &other) const
Equality comparison operator.
int nframes
Number of operational frames.
ModelTpl< NewScalar, Options, JointCollectionTpl > cast() const
ConfigVectorType upperPositionLimit
Upper joint configuration limit.
TangentVectorType friction
Vector of joint friction parameters.
bool operator!=(const ModelTpl &other) const
FrameIndex addJointFrame(const JointIndex &joint_index, int previous_frame_index=-1)
Add a joint to the frame tree.
FrameType
Enum on the possible types of frame.
ConfigVectorType lowerPositionLimit
Lower joint configuration limit.
CRTP class describing the API of the checkers.
FrameIndex addFrame(const Frame &frame, const bool append_inertia=true)
Adds a frame to the kinematic tree. The inertia stored within the frame will be happened to the inert...
Main pinocchio namespace.
std::string name
Model name;.
bool existFrame(const std::string &name, const FrameType &type=(FrameType)(JOINT|FIXED_JOINT|BODY|OP_FRAME|SENSOR)) const
Checks if a frame given by its name exists.
std::map< std::string, ConfigVectorType > ConfigVectorMap
Map between a string (key) and a configuration vector.
std::vector< JointIndex > parents
Joint parent of joint i, denoted li (li==parents[i]).
static const Vector3 gravity981
Default 3D gravity vector (=(0,0,-9.81)).
std::vector< int > nvs
Dimension of the joint i tangent subspace.
TangentVectorType rotorInertia
Vector of rotor inertia parameters.
JointIndex addJoint(const JointIndex parent, const JointModel &joint_model, const SE3 &joint_placement, const std::string &joint_name)
Add a joint to the kinematic tree with infinite bounds.
VectorXs TangentVectorType
Dense vectorized version of a joint tangent vector (e.g. velocity, acceleration, etc). It also handles the notion of co-tangent vector (e.g. torque, etc).
TangentVectorType damping
Vector of joint damping parameters.
TangentVectorType velocityLimit
Vector of maximal joint velocities.
bool check(const AlgorithmCheckerBase< D > &checker=AlgorithmCheckerBase< D >()) const
Check the validity of the attributes of Model with respect to the specification of some algorithms...
int nbodies
Number of bodies.
int nq
Dimension of the configuration vector representation.
std::vector< int > idx_qs
Starting index of the joint i in the configuration space.
TangentVectorType rotorGearRatio
Vector of rotor gear ratio parameters.