model.hpp

//
// Copyright (c) 2015-2020 CNRS INRIA
// Copyright (c) 2015 Wandercraft, 86 rue de Paris 91400 Orsay, France.
//

#ifndef __pinocchio_multibody_model_hpp__
#define __pinocchio_multibody_model_hpp__

#include "pinocchio/spatial/fwd.hpp"
#include "pinocchio/spatial/se3.hpp"
#include "pinocchio/spatial/force.hpp"
#include "pinocchio/spatial/motion.hpp"
#include "pinocchio/spatial/inertia.hpp"

#include "pinocchio/multibody/fwd.hpp"
#include "pinocchio/multibody/frame.hpp"
#include "pinocchio/multibody/joint/joint-generic.hpp"

#include "pinocchio/container/aligned-vector.hpp"

#include "pinocchio/serialization/serializable.hpp"

#include <map>
#include <iterator>

namespace pinocchio
{
 template<
 typename NewScalar,
 typename Scalar,
 int Options,
 template<typename, int> class JointCollectionTpl>
 struct CastType<NewScalar, ModelTpl<Scalar, Options, JointCollectionTpl>>
 {
 typedef ModelTpl<NewScalar, Options, JointCollectionTpl> type;
 };

 template<typename _Scalar, int _Options, template<typename, int> class JointCollectionTpl>
 struct traits<ModelTpl<_Scalar, _Options, JointCollectionTpl>>
 {
 typedef _Scalar Scalar;
 };

 template<typename _Scalar, int _Options, template<typename, int> class JointCollectionTpl>
 struct ModelTpl
 : serialization::Serializable<ModelTpl<_Scalar, _Options, JointCollectionTpl>>
 , NumericalBase<ModelTpl<_Scalar, _Options, JointCollectionTpl>>
 {
 EIGEN_MAKE_ALIGNED_OPERATOR_NEW

 typedef _Scalar Scalar;
 enum
 {
 Options = _Options
 };

 typedef JointCollectionTpl<Scalar, Options> JointCollection;
 typedef DataTpl<Scalar, Options, JointCollectionTpl> Data;

 typedef SE3Tpl<Scalar, Options> SE3;
 typedef MotionTpl<Scalar, Options> Motion;
 typedef ForceTpl<Scalar, Options> Force;
 typedef InertiaTpl<Scalar, Options> Inertia;
 typedef FrameTpl<Scalar, Options> Frame;

 typedef pinocchio::Index Index;
 typedef pinocchio::JointIndex JointIndex;
 typedef pinocchio::GeomIndex GeomIndex;
 typedef pinocchio::FrameIndex FrameIndex;
 typedef std::vector<Index> IndexVector;

 typedef JointModelTpl<Scalar, Options, JointCollectionTpl> JointModel;
 typedef JointDataTpl<Scalar, Options, JointCollectionTpl> JointData;

 typedef PINOCCHIO_ALIGNED_STD_VECTOR(JointModel) JointModelVector;
 typedef PINOCCHIO_ALIGNED_STD_VECTOR(JointData) JointDataVector;

 typedef PINOCCHIO_ALIGNED_STD_VECTOR(Frame) FrameVector;

 typedef Eigen::Matrix<Scalar, Eigen::Dynamic, 1, Options> VectorXs;
 typedef Eigen::Matrix<Scalar, 3, 1, Options> Vector3;

 typedef PINOCCHIO_ALIGNED_STD_VECTOR(Inertia) InertiaVector;
 typedef PINOCCHIO_ALIGNED_STD_VECTOR(SE3) SE3Vector;

 typedef VectorXs ConfigVectorType;

 typedef std::map<std::string, ConfigVectorType> ConfigVectorMap;

 typedef VectorXs TangentVectorType;

 int nq;

 int nv;

 int nvExtended;

 int njoints;

 int nbodies;

 int nframes;

 InertiaVector inertias;

 SE3Vector jointPlacements;

 JointModelVector joints;

 std::vector<int> idx_qs;

 std::vector<int> nqs;

 std::vector<int> idx_vs;

 std::vector<int> nvs;

 std::vector<int> idx_vExtendeds;

 std::vector<int> nvExtendeds;

 std::vector<JointIndex> parents;

 std::vector<IndexVector> children;

 std::vector<JointIndex> mimicking_joints;

 std::vector<JointIndex> mimicked_joints;

 std::vector<std::string> names;

 ConfigVectorMap referenceConfigurations;

 VectorXs armature;

 TangentVectorType rotorInertia;

 TangentVectorType rotorGearRatio;

 TangentVectorType friction;

 TangentVectorType damping;

 TangentVectorType effortLimit;

 TangentVectorType velocityLimit;

 ConfigVectorType lowerPositionLimit;

 ConfigVectorType upperPositionLimit;

 FrameVector frames;

 std::vector<IndexVector> supports;

 std::vector<IndexVector> mimic_joint_supports;

 std::vector<IndexVector> subtrees;

 Motion gravity;

 static const Vector3 gravity981;

 std::string name;

 ModelTpl()
 : nq(0)
 , nv(0)
 , nvExtended(0)
 , njoints(1)
 , nbodies(1)
 , nframes(0)
 , inertias(1, Inertia::Zero())
 , jointPlacements(1, SE3::Identity())
 , joints(1)
 , idx_qs(1, 0)
 , nqs(1, 0)
 , idx_vs(1, 0)
 , nvs(1, 0)
 , idx_vExtendeds(1, 0)
 , nvExtendeds(1, 0)
 , parents(1, 0)
 , children(1)
 , names(1)
 , supports(1, IndexVector(1, 0))
 , mimic_joint_supports(1, IndexVector(1, 0))
 , subtrees(1)
 , gravity(gravity981, Vector3::Zero())
 {
 names[0] = "universe"; // Should be "universe joint (trivial)"
 // FIXME Should the universe joint be a FIXED_JOINT even if it is
 // in the list of joints ? See comment in definition of
 // Model::addJointFrame and Model::addBodyFrame
 addFrame(Frame("universe", 0, 0, SE3::Identity(), FIXED_JOINT));
 }

 template<typename S2, int O2>
 explicit ModelTpl(const ModelTpl<S2, O2> & other)
 {
 *this = other.template cast<Scalar>();
 }

 template<typename NewScalar>
 typename CastType<NewScalar, ModelTpl>::type cast() const;

 bool operator==(const ModelTpl & other) const;

 bool operator!=(const ModelTpl & other) const
 {
 return !(*this == other);
 }

 JointIndex addJoint(
 const JointIndex parent,
 const JointModel & joint_model,
 const SE3 & joint_placement,
 const std::string & joint_name);

 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);

 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,
 const VectorXs & friction,
 const VectorXs & damping);

 FrameIndex addJointFrame(const JointIndex & joint_index, int previous_frame_index = -1);

 void appendBodyToJoint(
 const JointIndex joint_index,
 const Inertia & Y,
 const SE3 & body_placement = SE3::Identity());

 FrameIndex addBodyFrame(
 const std::string & body_name,
 const JointIndex & parentJoint,
 const SE3 & body_placement = SE3::Identity(),
 int parentFrame = -1);

 FrameIndex getBodyId(const std::string & name) const;

 bool existBodyName(const std::string & name) const;

 JointIndex getJointId(const std::string & name) const;

 bool existJointName(const std::string & name) const;

 FrameIndex getFrameId(
 const std::string & name,
 const FrameType & type = (FrameType)(JOINT | FIXED_JOINT | BODY | OP_FRAME | SENSOR)) const;

 bool existFrame(
 const std::string & name,
 const FrameType & type = (FrameType)(JOINT | FIXED_JOINT | BODY | OP_FRAME | SENSOR)) const;

 FrameIndex addFrame(const Frame & frame, const bool append_inertia = true);

 template<typename D>
 bool check(const AlgorithmCheckerBase<D> & checker = AlgorithmCheckerBase<D>()) const
 {
 return checker.checkModel(*this);
 }

 std::vector<bool> hasConfigurationLimit();

 std::vector<bool> hasConfigurationLimitInTangent();

 bool check() const;

 bool check(const Data & data) const;

 protected:
 void addJointIndexToParentSubtrees(const JointIndex joint_id);
 };

} // namespace pinocchio

/* --- Details -------------------------------------------------------------- */
/* --- Details -------------------------------------------------------------- */
/* --- Details -------------------------------------------------------------- */
#include "pinocchio/multibody/model.hxx"

#if PINOCCHIO_ENABLE_TEMPLATE_INSTANTIATION
 #include "pinocchio/multibody/model.txx"
#endif // PINOCCHIO_ENABLE_TEMPLATE_INSTANTIATION

#endif // ifndef __pinocchio_multibody_model_hpp__