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__ std::vector< int > idx_vs
Starting index of the joint i in the tangent configuration space.
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.
bool check() const
Run check(fusion::list) with DEFAULT_CHECKERS as argument.
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.
VectorXs ConfigVectorType
Dense vectorized version of a joint configuration vector.
JointIndex getBodyId(const std::string &name) const
Return the index of a body given by its name.
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.
ModelTpl< NewScalar, Options, JointCollectionTpl > cast() const
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.
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.
JointIndex getJointId(const std::string &name) const
Return the index of a joint given by its name.
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.
int nframes
Number of operational frames.
bool existJointName(const std::string &name) const
Check if a joint given by its name exists.
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...
VectorXs rotorGearRatio
Vector of rotor gear ratio parameters.
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.
FrameType
Enum on the possible types of frame.
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.
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.
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.
int nbodies
Number of bodies.
bool operator!=(const ModelTpl &other) const
int nq
Dimension of the configuration vector representation.
std::vector< int > idx_qs
Starting index of the joint i in the configuration space.
bool operator==(const ModelTpl &other) const
Equality comparison operator.
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.