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
62 typedef Eigen::Matrix<Scalar,Eigen::Dynamic,1,Options>
VectorXs;
63 typedef Eigen::Matrix<Scalar,3,1,Options>
Vector3;
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";
195 template<
typename NewScalar>
200 res.nq =
nq; res.nv =
nv;
208 res.gravity = gravity.template cast<NewScalar>();
217 res.rotorInertia = rotorInertia.template cast<NewScalar>();
218 res.rotorGearRatio = rotorGearRatio.template cast<NewScalar>();
219 res.friction = friction.template cast<NewScalar>();
220 res.damping = damping.template cast<NewScalar>();
221 res.effortLimit = effortLimit.template cast<NewScalar>();
222 res.velocityLimit = velocityLimit.template cast<NewScalar>();
223 res.lowerPositionLimit = lowerPositionLimit.template cast<NewScalar>();
224 res.upperPositionLimit = upperPositionLimit.template cast<NewScalar>();
226 typename ConfigVectorMap::const_iterator it;
227 for (it = referenceConfigurations.begin();
228 it != referenceConfigurations.end(); it++ )
230 res.referenceConfigurations.insert(std::make_pair(it->first, it->second.template cast<NewScalar>()));
234 res.inertias.resize(inertias.size());
235 res.jointPlacements.resize(jointPlacements.size());
236 res.joints.resize(joints.size());
237 res.frames.resize(frames.size());
240 for(
size_t k = 0; k < joints.size(); ++k)
242 res.inertias[k] = inertias[k].template cast<NewScalar>();
243 res.jointPlacements[k] = jointPlacements[k].template cast<NewScalar>();
244 res.joints[k] = joints[k].template cast<NewScalar>();
247 for(
size_t k = 0; k < frames.size(); ++k)
249 res.frames[k] = frames[k].template cast<NewScalar>();
265 && other.njoints == njoints
266 && other.nbodies == nbodies
267 && other.nframes == nframes
268 && other.parents == parents
269 && other.names == names
270 && other.subtrees == subtrees
271 && other.gravity == gravity
272 && other.name ==
name;
275 other.idx_qs == idx_qs
277 && other.idx_vs == idx_vs
280 if(other.referenceConfigurations.size() != referenceConfigurations.size())
283 typename ConfigVectorMap::const_iterator it = referenceConfigurations.begin();
284 typename ConfigVectorMap::const_iterator it_other = other.referenceConfigurations.begin();
285 for(
long k = 0; k < (long)referenceConfigurations.size(); ++k)
287 std::advance(it,k); std::advance(it_other,k);
289 if(it->second.size() != it_other->second.size())
291 if(it->second != it_other->second)
295 if(other.rotorInertia.size() != rotorInertia.size())
300 if(other.friction.size() != friction.size())
305 if(other.damping.size() != damping.size())
307 res &= other.damping ==
damping;
310 if(other.rotorGearRatio.size() != rotorGearRatio.size())
315 if(other.effortLimit.size() != effortLimit.size())
320 if(other.velocityLimit.size() != velocityLimit.size())
325 if(other.lowerPositionLimit.size() != lowerPositionLimit.size())
330 if(other.upperPositionLimit.size() != upperPositionLimit.size())
335 for(
size_t k = 1; k < inertias.size(); ++k)
337 res &= other.inertias[k] == inertias[k];
341 for(
size_t k = 1; k < other.jointPlacements.size(); ++k)
343 res &= other.jointPlacements[k] == jointPlacements[k];
348 other.joints == joints
349 && other.frames ==
frames;
358 {
return !(*
this == other); }
379 const JointModel & joint_model,
391 JointIndex
addJoint(
const JointIndex parent,
392 const JointModel & joint_model,
393 const SE3 & joint_placement,
394 const std::string & joint_name,
395 const VectorXs & max_effort,
396 const VectorXs & max_velocity,
397 const VectorXs & min_config,
398 const VectorXs & max_config);
406 JointIndex
addJoint(
const JointIndex parent,
407 const JointModel & joint_model,
408 const SE3 & joint_placement,
409 const std::string & joint_name,
410 const VectorXs & max_effort,
411 const VectorXs & max_velocity,
412 const VectorXs & min_config,
413 const VectorXs & max_config,
414 const VectorXs & friction,
415 const VectorXs & damping);
427 int previous_frame_index = -1);
453 const JointIndex & parentJoint,
455 int previousFrame = -1);
468 FrameIndex
getBodyId(
const std::string & name)
const;
490 JointIndex
getJointId(
const std::string & name)
const;
514 FrameIndex
getFrameId(
const std::string & name,
537 FrameIndex
addFrame(
const Frame & frame,
538 const bool append_inertia =
true);
552 {
return checker.checkModel(*
this); }
569 inline bool check()
const;
578 inline bool check(
const Data &
data)
const;
595 #include "pinocchio/multibody/model.hxx" 597 #endif // ifndef __pinocchio_multibody_model_hpp__
bool existBodyName(const std::string &name) const
Check if a body given by its name exists.
JointModelVector joints
Model of joint i, encapsulated in a JointModelAccessor.
FrameIndex addJointFrame(const JointIndex &joint_index, int previous_frame_index=-1)
Add a joint to the frame tree.
joint frame: attached to the child body of a joint (a.k.a. child frame)
DataTpl< Scalar, Options, JointCollectionTpl > Data
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.
ModelTpl< NewScalar, Options, JointCollectionTpl > cast() const
A Plucker coordinate frame attached to a parent joint inside a kinematic tree.
int njoints
Number of joints.
std::vector< std::string > names
Name of joint i
ConfigVectorType lowerPositionLimit
Lower joint configuration limit.
FrameVector frames
Vector of operational frames registered on the model.
std::vector< bool > hasConfigurationLimit()
Check if joints have configuration limits.
std::vector< int > nvs
Dimension of the joint i tangent subspace.
TangentVectorType rotorInertia
Vector of rotor inertia parameters.
bool existJointName(const std::string &name) const
Check if a joint given by its name exists.
bool operator==(const ModelTpl &other) const
Equality comparison operator.
typedef PINOCCHIO_ALIGNED_STD_VECTOR(JointModel) JointModelVector
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).
pinocchio::GeomIndex GeomIndex
fixed joint frame: joint frame but for a fixed joint
JointCollectionTpl< Scalar, Options > JointCollection
std::vector< IndexVector > subtrees
Vector of joint subtrees. subtree[j] corresponds to the subtree supported by the joint j...
ConfigVectorMap referenceConfigurations
Map of reference configurations, indexed by user given names.
operational frame: user-defined frames that are defined at runtime
int nframes
Number of operational frames.
FrameType
Enum on the possible types of frames.
void addJointIndexToParentSubtrees(const JointIndex joint_id)
Add the joint_id to its parent subtrees.
std::string name
Model name;.
TangentVectorType rotorGearRatio
Vector of rotor gear ratio parameters.
pinocchio::FrameIndex FrameIndex
body frame: attached to the collision, inertial or visual properties of a link
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.
std::vector< int > nqs
Dimension of the joint i configuration subspace.
TangentVectorType friction
Vector of joint friction parameters.
std::vector< int > idx_qs
Starting index of the joint i in the configuration space.
std::vector< Index > IndexVector
int nbodies
Number of bodies.
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...
pinocchio::JointIndex JointIndex
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.
Motion gravity
Spatial gravity of the model.
std::map< std::string, ConfigVectorType > ConfigVectorMap
Map between a string (key) and a configuration vector.
EIGEN_MAKE_ALIGNED_OPERATOR_NEW typedef _Scalar Scalar
JointModelTpl< Scalar, Options, JointCollectionTpl > JointModel
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...
CRTP class describing the API of the checkers.
ForceTpl< Scalar, Options > Force
ModelTpl()
Default constructor. Builds an empty model with no joints.
Main pinocchio namespace.
std::vector< int > idx_vs
Starting index of the joint i in the tangent configuration space.
std::vector< bool > hasConfigurationLimitInTangent()
Check if joints have configuration limits.
TangentVectorType effortLimit
Vector of maximal joint torques.
Eigen::Matrix< Scalar, Eigen::Dynamic, 1, Options > VectorXs
JointIndex getJointId(const std::string &name) const
Return the index of a joint given by its name.
TangentVectorType damping
Vector of joint damping parameters.
std::vector< JointIndex > parents
Joint parent of joint i, denoted li (li==parents[i]).
JointDataTpl< Scalar, Options, JointCollectionTpl > JointData
int nv
Dimension of the velocity vector space.
TangentVectorType velocityLimit
Vector of maximal joint velocities.
FrameTpl< Scalar, Options > Frame
Eigen::Matrix< Scalar, 3, 1, Options > Vector3
FrameIndex getBodyId(const std::string &name) const
Return the index of a body given by its name.
VectorXs ConfigVectorType
Dense vectorized version of a joint configuration vector.
ConfigVectorType upperPositionLimit
Upper joint configuration limit.
sensor frame: defined in a sensor element
static const Vector3 gravity981
Default 3D gravity vector (=(0,0,-9.81)).
std::vector< IndexVector > supports
Vector of joint supports. supports[j] corresponds to the collection of all joints located on the path...
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.
int nq
Dimension of the configuration vector representation.
InertiaTpl< Scalar, Options > Inertia
bool check() const
Run check(fusion::list) with DEFAULT_CHECKERS as argument.
SE3Tpl< Scalar, Options > SE3
bool operator!=(const ModelTpl &other) const
MotionTpl< Scalar, Options > Motion
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.