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;
68 typedef std::map<std::string, ConfigVectorType> ConfigVectorMap;
90 PINOCCHIO_ALIGNED_STD_VECTOR(Inertia) inertias;
93 PINOCCHIO_ALIGNED_STD_VECTOR(SE3) jointPlacements;
166 , inertias(1, Inertia::Zero())
167 , jointPlacements(1, SE3::Identity())
175 , supports(1,IndexVector(1,0))
177 , gravity(gravity981,Vector3::Zero())
179 names[0] =
"universe";
183 addFrame(Frame(
"universe", 0, 0, SE3::Identity(), FIXED_JOINT));
188 template<
typename NewScalar>
200 res.gravity = gravity.template cast<NewScalar>();
209 res.rotorInertia = rotorInertia.template cast<NewScalar>();
210 res.rotorGearRatio = rotorGearRatio.template cast<NewScalar>();
211 res.effortLimit = effortLimit.template cast<NewScalar>();
212 res.velocityLimit = velocityLimit.template cast<NewScalar>();
213 res.lowerPositionLimit = lowerPositionLimit.template cast<NewScalar>();
214 res.upperPositionLimit = upperPositionLimit.template cast<NewScalar>();
216 typename ReturnType::ConfigVectorMap::iterator rit = res.referenceConfigurations.begin();
217 typename ConfigVectorMap::const_iterator it;
218 for (it = referenceConfigurations.begin();
219 it != referenceConfigurations.end(); it++ )
221 rit->second = it->second.template cast<NewScalar>();
226 res.inertias.resize(inertias.size());
227 res.jointPlacements.resize(jointPlacements.size());
228 res.joints.resize(joints.size());
229 res.frames.resize(frames.size());
232 for(
size_t k = 0; k < joints.size(); ++k)
234 res.inertias[k] = inertias[k].template cast<NewScalar>();
235 res.jointPlacements[k] = jointPlacements[k].template cast<NewScalar>();
236 res.joints[k] = joints[k].template cast<NewScalar>();
239 for(
size_t k = 0; k < frames.size(); ++k)
241 res.frames[k] = frames[k].template cast<NewScalar>();
257 && other.njoints == njoints
258 && other.nbodies == nbodies
259 && other.nframes == nframes
260 && other.parents == parents
261 && other.names == names
262 && other.subtrees == subtrees
263 && other.gravity == gravity
264 && other.name ==
name;
267 other.idx_qs == idx_qs
269 && other.idx_vs == idx_vs
272 if(other.referenceConfigurations.size() != referenceConfigurations.size())
275 typename ConfigVectorMap::const_iterator it = referenceConfigurations.begin();
276 typename ConfigVectorMap::const_iterator it_other = other.referenceConfigurations.begin();
277 for(
long k = 0; k < (long)referenceConfigurations.size(); ++k)
279 std::advance(it,k); std::advance(it_other,k);
281 if(it->second.size() != it_other->second.size())
283 if(it->second != it_other->second)
287 if(other.rotorInertia.size() != rotorInertia.size())
292 if(other.rotorGearRatio.size() != rotorGearRatio.size())
297 if(other.effortLimit.size() != effortLimit.size())
302 if(other.velocityLimit.size() != velocityLimit.size())
307 if(other.lowerPositionLimit.size() != lowerPositionLimit.size())
312 if(other.upperPositionLimit.size() != upperPositionLimit.size())
317 for(
size_t k = 1; k < inertias.size(); ++k)
319 res &= other.inertias[k] == inertias[k];
323 for(
size_t k = 1; k < other.jointPlacements.size(); ++k)
325 res &= other.jointPlacements[k] == jointPlacements[k];
330 other.joints == joints
331 && other.frames ==
frames;
340 {
return !(*
this == other); }
364 JointIndex
addJoint(
const JointIndex parent,
365 const JointModel & joint_model,
366 const SE3 & joint_placement,
367 const std::string & joint_name,
368 const VectorXs & max_effort,
369 const VectorXs & max_velocity,
370 const VectorXs & min_config,
371 const VectorXs & max_config
391 JointIndex
addJoint(
const JointIndex parent,
392 const JointModel & joint_model,
393 const SE3 & joint_placement,
394 const std::string & joint_name);
406 int previous_frame_index = -1);
418 const SE3 & body_placement = SE3::Identity());
432 const JointIndex & parentJoint,
433 const SE3 & body_placement = SE3::Identity(),
434 int previousFrame = -1);
447 JointIndex
getBodyId(
const std::string & name)
const;
469 JointIndex
getJointId(
const std::string & name)
const;
493 FrameIndex
getFrameId(
const std::string & name,
494 const FrameType & type = (
FrameType) (JOINT | FIXED_JOINT | BODY | OP_FRAME | SENSOR ))
const;
505 const FrameType& type = (
FrameType) (JOINT | FIXED_JOINT | BODY | OP_FRAME | SENSOR ))
const;
529 {
return checker.checkModel(*
this); }
532 inline bool check()
const;
541 inline bool check(
const Data & data)
const;
558 #include "pinocchio/multibody/model.hxx" 560 #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.
int addJointFrame(const JointIndex &joint_index, int previous_frame_index=-1)
Add a joint to the frame tree.
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.
ConfigVectorMap referenceConfigurations
Map of reference configurations, indexed by user given names.
int nv
Dimension of the velocity vector space.
int 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.
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.
VectorXs upperPositionLimit
Upper joint configuration limit.
FrameVector frames
Vector of operational frames registered on the model.
VectorXs rotorInertia
Vector of rotor inertia parameters.
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.
VectorXs lowerPositionLimit
Lower joint configuration limit.
bool operator==(const ModelTpl &other) const
Equality comparison operator.
int nframes
Number of operational frames.
VectorXs rotorGearRatio
Vector of rotor gear ratio parameters.
ModelTpl< NewScalar, Options, JointCollectionTpl > cast() const
bool operator!=(const ModelTpl &other) const
FrameType
Enum on the possible types of frame.
JointIndex getBodyId(const std::string &name) const
Return the index of a body given by its name.
CRTP class describing the API of the checkers.
Main pinocchio namespace.
std::string name
Model name;.
int addFrame(const Frame &frame)
Adds a frame to the kinematic tree.
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::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.
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 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.
JointIndex addJoint(const JointIndex parent, const JointModel &joint_model, const SE3 &joint_placement, const std::string &joint_name, const VectorXs &max_effort, const VectorXs &max_velocity, const VectorXs &min_config, const VectorXs &max_config)
Add a joint to the kinematic tree with given bounds.