6 #ifndef __pinocchio_model_hpp__ 7 #define __pinocchio_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" 29 template<
typename _Scalar,
int _Options,
template<
typename,
int>
class JointCollectionTpl>
31 : serialization::Serializable< ModelTpl<_Scalar,_Options,JointCollectionTpl> >
33 EIGEN_MAKE_ALIGNED_OPERATOR_NEW
35 typedef _Scalar Scalar;
36 enum { Options = _Options };
38 typedef JointCollectionTpl<Scalar,Options> JointCollection;
39 typedef DataTpl<Scalar,Options,JointCollectionTpl> Data;
41 typedef SE3Tpl<Scalar,Options> SE3;
42 typedef MotionTpl<Scalar,Options> Motion;
43 typedef ForceTpl<Scalar,Options> Force;
44 typedef InertiaTpl<Scalar,Options> Inertia;
45 typedef FrameTpl<Scalar,Options> Frame;
47 typedef pinocchio::Index Index;
48 typedef pinocchio::JointIndex JointIndex;
49 typedef pinocchio::GeomIndex GeomIndex;
50 typedef pinocchio::FrameIndex FrameIndex;
51 typedef std::vector<Index> IndexVector;
53 typedef JointModelTpl<Scalar,Options,JointCollectionTpl> JointModel;
54 typedef JointDataTpl<Scalar,Options,JointCollectionTpl> JointData;
56 typedef container::aligned_vector<JointModel> JointModelVector;
57 typedef container::aligned_vector<JointData> JointDataVector;
59 typedef container::aligned_vector<Frame> FrameVector;
61 typedef Eigen::Matrix<Scalar,Eigen::Dynamic,1,Options> VectorXs;
62 typedef Eigen::Matrix<Scalar,3,1,Options> Vector3;
67 typedef std::map<std::string, ConfigVectorType> ConfigVectorMap;
165 , inertias(1, Inertia::Zero())
166 , jointPlacements(1, SE3::Identity())
174 , supports(1,IndexVector(1,0))
176 , gravity(gravity981,Vector3::Zero())
178 names[0] =
"universe";
182 addFrame(Frame(
"universe", 0, 0, SE3::Identity(), FIXED_JOINT));
187 template<
typename NewScalar>
199 res.gravity = gravity.template cast<NewScalar>();
208 res.rotorInertia = rotorInertia.template cast<NewScalar>();
209 res.rotorGearRatio = rotorGearRatio.template cast<NewScalar>();
210 res.effortLimit = effortLimit.template cast<NewScalar>();
211 res.velocityLimit = velocityLimit.template cast<NewScalar>();
212 res.lowerPositionLimit = lowerPositionLimit.template cast<NewScalar>();
213 res.upperPositionLimit = upperPositionLimit.template cast<NewScalar>();
215 typename ReturnType::ConfigVectorMap::iterator rit = res.referenceConfigurations.begin();
216 typename ConfigVectorMap::const_iterator it;
217 for (it = referenceConfigurations.begin();
218 it != referenceConfigurations.end(); it++ )
220 rit->second = it->second.template cast<NewScalar>();
225 res.inertias.resize(inertias.size());
226 res.jointPlacements.resize(jointPlacements.size());
227 res.joints.resize(joints.size());
228 res.frames.resize(frames.size());
231 for(
size_t k = 0; k < joints.size(); ++k)
233 res.inertias[k] = inertias[k].template cast<NewScalar>();
234 res.jointPlacements[k] = jointPlacements[k].template cast<NewScalar>();
235 res.joints[k] = joints[k].template cast<NewScalar>();
238 for(
size_t k = 0; k < frames.size(); ++k)
240 res.frames[k] = frames[k].template cast<NewScalar>();
256 && other.njoints == njoints
257 && other.nbodies == nbodies
258 && other.nframes == nframes
259 && other.parents == parents
260 && other.names == names
261 && other.subtrees == subtrees
262 && other.gravity == gravity
263 && other.name ==
name;
266 other.idx_qs == idx_qs
268 && other.idx_vs == idx_vs
271 if(other.referenceConfigurations.size() != referenceConfigurations.size())
276 if(other.rotorInertia.size() != rotorInertia.size())
281 if(other.rotorGearRatio.size() != rotorGearRatio.size())
286 if(other.effortLimit.size() != effortLimit.size())
291 if(other.velocityLimit.size() != velocityLimit.size())
296 if(other.lowerPositionLimit.size() != lowerPositionLimit.size())
301 if(other.upperPositionLimit.size() != upperPositionLimit.size())
306 for(
size_t k = 1; k < inertias.size(); ++k)
308 res &= other.inertias[k] == inertias[k];
312 for(
size_t k = 1; k < other.jointPlacements.size(); ++k)
314 res &= other.jointPlacements[k] == jointPlacements[k];
319 other.joints == joints
320 && other.frames ==
frames;
329 {
return !(*
this == other); }
353 JointIndex
addJoint(
const JointIndex parent,
354 const JointModel & joint_model,
355 const SE3 & joint_placement,
356 const std::string & joint_name,
357 const VectorXs & max_effort,
358 const VectorXs & max_velocity,
359 const VectorXs & min_config,
360 const VectorXs & max_config
380 JointIndex
addJoint(
const JointIndex parent,
381 const JointModel & joint_model,
382 const SE3 & joint_placement,
383 const std::string & joint_name);
395 int previous_frame_index = -1);
407 const SE3 & body_placement = SE3::Identity());
421 const JointIndex & parentJoint,
422 const SE3 & body_placement = SE3::Identity(),
423 int previousFrame = -1);
436 JointIndex
getBodyId(
const std::string & name)
const;
458 JointIndex
getJointId(
const std::string & name)
const;
482 FrameIndex
getFrameId(
const std::string & name,
483 const FrameType & type = (
FrameType) (JOINT | FIXED_JOINT | BODY | OP_FRAME | SENSOR ))
const;
494 const FrameType& type = (
FrameType) (JOINT | FIXED_JOINT | BODY | OP_FRAME | SENSOR ))
const;
518 {
return checker.checkModel(*
this); }
521 inline bool check()
const;
530 inline bool check(
const Data & data)
const;
547 #include "pinocchio/multibody/model.hxx" 549 #endif // ifndef __pinocchio_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.
container::aligned_vector< SE3 > jointPlacements
Placement (SE3) of the input of joint i regarding to the parent joint output li.
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 supports. supports[j] corresponds to the collection of all joints located on the path betwe...
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 subtrees. subtree[j] corresponds to the subtree supported by the joint j. The first element of subtree[j] is the index of the joint j itself.
TangentVectorType effortLimit
Vector of maximal joint torques.
Specialization of an std::vector with an aligned allocator. This specialization might be used when th...
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]).
container::aligned_vector< Inertia > inertias
Spatial inertias of the body i expressed in the supporting joint frame 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.