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 <iostream>
#include <map>
#include <iterator>

namespace pinocchio
{
  
  template<typename _Scalar, int _Options, template<typename,int> class JointCollectionTpl>
  struct ModelTpl
  : serialization::Serializable< 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 VectorXs ConfigVectorType;

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

    int nq;
    
    int nv;
    
    int njoints;

    int nbodies;
    
    int nframes;

    PINOCCHIO_ALIGNED_STD_VECTOR(Inertia) inertias;
    
    PINOCCHIO_ALIGNED_STD_VECTOR(SE3) 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<std::string> names;

    ConfigVectorMap referenceConfigurations;

    VectorXs rotorInertia;
    
    VectorXs rotorGearRatio;
    
    TangentVectorType effortLimit;
    
    TangentVectorType velocityLimit;

    VectorXs lowerPositionLimit;
    
    VectorXs 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)
    , 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));
    }
    ~ModelTpl() {} // std::cout << "Destroy model" << std::endl; }
    
    template<typename NewScalar>
    ModelTpl<NewScalar,Options,JointCollectionTpl> cast() const
    {
      typedef ModelTpl<NewScalar,Options,JointCollectionTpl> ReturnType;
      ReturnType res;
      res.nq = nq; res.nv = nv;
      res.njoints = njoints;
      res.nbodies = nbodies;
      res.nframes = nframes;
      res.parents = parents;
      res.names = names;
      res.subtrees = subtrees;
      res.gravity = gravity.template cast<NewScalar>();
      res.name = name;
      
      res.idx_qs = idx_qs;
      res.nqs = nqs;
      res.idx_vs = idx_vs;
      res.nvs = nvs;
      
      // Eigen Vectors
      res.rotorInertia = rotorInertia.template cast<NewScalar>();
      res.rotorGearRatio = rotorGearRatio.template cast<NewScalar>();
      res.effortLimit = effortLimit.template cast<NewScalar>();
      res.velocityLimit = velocityLimit.template cast<NewScalar>();
      res.lowerPositionLimit = lowerPositionLimit.template cast<NewScalar>();
      res.upperPositionLimit = upperPositionLimit.template cast<NewScalar>();

      typename ConfigVectorMap::const_iterator it;
      for (it = referenceConfigurations.begin();
           it != referenceConfigurations.end(); it++ )
      {
        res.referenceConfigurations.insert(std::make_pair(it->first, it->second.template cast<NewScalar>()));
      }
        
      // reserve vectors
      res.inertias.resize(inertias.size());
      res.jointPlacements.resize(jointPlacements.size());
      res.joints.resize(joints.size());
      res.frames.resize(frames.size());

      for(size_t k = 0; k < joints.size(); ++k)
      {
        res.inertias[k] = inertias[k].template cast<NewScalar>();
        res.jointPlacements[k] = jointPlacements[k].template cast<NewScalar>();
        res.joints[k] = joints[k].template cast<NewScalar>();
      }
      
      for(size_t k = 0; k < frames.size(); ++k)
      {
        res.frames[k] = frames[k].template cast<NewScalar>();
      }
      
      return res;
    }
    
    bool operator==(const ModelTpl & other) const
    {
      bool res =
         other.nq == nq
      && other.nv == nv
      && other.njoints == njoints
      && other.nbodies == nbodies
      && other.nframes == nframes
      && other.parents == parents
      && other.names == names
      && other.subtrees == subtrees
      && other.gravity == gravity
      && other.name == name;
      
      res &=
         other.idx_qs == idx_qs
      && other.nqs == nqs
      && other.idx_vs == idx_vs
      && other.nvs == nvs;

      if(other.referenceConfigurations.size() != referenceConfigurations.size())
        return false;
      
      typename ConfigVectorMap::const_iterator it = referenceConfigurations.begin();
      typename ConfigVectorMap::const_iterator it_other = other.referenceConfigurations.begin();
      for(long k = 0; k < (long)referenceConfigurations.size(); ++k)
      {
        std::advance(it,k); std::advance(it_other,k);
        
        if(it->second.size() != it_other->second.size())
          return false;
        if(it->second != it_other->second)
          return false;
      }

      if(other.rotorInertia.size() != rotorInertia.size())
        return false;
      res &= other.rotorInertia == rotorInertia;
      if(!res) return res;

      if(other.rotorGearRatio.size() != rotorGearRatio.size())
        return false;
      res &= other.rotorGearRatio == rotorGearRatio;
      if(!res) return res;

      if(other.effortLimit.size() != effortLimit.size())
        return false;
      res &= other.effortLimit == effortLimit;
      if(!res) return res;

      if(other.velocityLimit.size() != velocityLimit.size())
        return false;
      res &= other.velocityLimit == velocityLimit;
      if(!res) return res;

      if(other.lowerPositionLimit.size() != lowerPositionLimit.size())
        return false;
      res &= other.lowerPositionLimit == lowerPositionLimit;
      if(!res) return res;

      if(other.upperPositionLimit.size() != upperPositionLimit.size())
        return false;
      res &= other.upperPositionLimit == upperPositionLimit;
      if(!res) return res;

      for(size_t k = 1; k < inertias.size(); ++k)
      {
        res &= other.inertias[k] == inertias[k];
        if(!res) return res;
      }

      for(size_t k = 1; k < other.jointPlacements.size(); ++k)
      {
        res &= other.jointPlacements[k] == jointPlacements[k];
        if(!res) return res;
      }

      res &=
         other.joints == joints
      && other.frames == frames;
      
      return res;
    }
    
    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,
                        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);

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

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

    inline bool check() const;
    
    inline bool check(const Data & data) const;

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

} // namespace pinocchio

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

#endif // ifndef __pinocchio_multibody_model_hpp__