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 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<JointIndex> parents;
 
    std::vector<IndexVector> children;
 
    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> subtrees;
 
    Motion gravity;
 
    static const Vector3 gravity981;
 
    std::string name;
 
    ModelTpl()
    : nq(0)
    , nv(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)
    , parents(1, 0)
    , children(1)
    , names(1)
    , 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__