Program Listing for File model.hpp

Return to documentation for file (include/pinocchio/multibody/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__