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;
174 , jointPlacements(1,
SE3::Identity())
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>();
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>();
225 typename ConfigVectorMap::const_iterator 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>();
268 && other.names ==
names
271 && other.name ==
name;
283 typename ConfigVectorMap::const_iterator it_other = other.referenceConfigurations.begin();
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)
299 if(other.friction.size() !=
friction.size())
304 if(other.damping.size() !=
damping.size())
306 res &= other.damping ==
damping;
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];
348 && other.frames ==
frames;
357 {
return !(*
this == other); }
376 JointIndex
addJoint(
const JointIndex parent,
378 const SE3 & joint_placement,
379 const std::string & joint_name);
389 JointIndex
addJoint(
const JointIndex parent,
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,
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,
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);
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;
547 {
return checker.checkModel(*
this); }
550 inline bool check()
const;
559 inline bool check(
const Data & data)
const;
576 #include "pinocchio/multibody/model.hxx"
578 #endif // ifndef __pinocchio_multibody_model_hpp__