model.hpp

//
// Copyright (c) 2015-2018 CNRS
// Copyright (c) 2015 Wandercraft, 86 rue de Paris 91400 Orsay, France.
//
// This file is part of Pinocchio
// Pinocchio is free software: you can redistribute it
// and/or modify it under the terms of the GNU Lesser General Public
// License as published by the Free Software Foundation, either version
// 3 of the License, or (at your option) any later version.
//
// Pinocchio is distributed in the hope that it will be
// useful, but WITHOUT ANY WARRANTY; without even the implied warranty
// of MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU
// General Lesser Public License for more details. You should have
// received a copy of the GNU Lesser General Public License along with
// Pinocchio If not, see
// <http://www.gnu.org/licenses/>.

#ifndef __se3_model_hpp__
#define __se3_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.hpp"
#include "pinocchio/deprecated.hh"
#include "pinocchio/utils/string-generator.hpp"
#include "pinocchio/container/aligned-vector.hpp"

#include <iostream>
#include <Eigen/Cholesky>

namespace se3
{
  struct Model
  {
    EIGEN_MAKE_ALIGNED_OPERATOR_NEW
    typedef se3::Index Index;
    typedef se3::JointIndex JointIndex;
    typedef se3::GeomIndex GeomIndex;
    typedef se3::FrameIndex FrameIndex;
    typedef std::vector<Index> IndexVector;

    int nq;
    
    int nv;
    
    int njoints;

    int nbodies;
    
    int nframes;

    container::aligned_vector<Inertia> inertias;
    
    container::aligned_vector<SE3> jointPlacements;

    JointModelVector joints;
    
    std::vector<JointIndex> parents;

    std::vector<std::string> names;
    
    Eigen::VectorXd neutralConfiguration;

    Eigen::VectorXd rotorInertia;
    
    Eigen::VectorXd rotorGearRatio;
    
    Eigen::VectorXd effortLimit;
    Eigen::VectorXd velocityLimit;

    Eigen::VectorXd lowerPositionLimit;
    Eigen::VectorXd upperPositionLimit;

    container::aligned_vector<Frame> frames;
    
    std::vector<IndexVector> subtrees;

    Motion gravity;
    
    static const Eigen::Vector3d gravity981;

    std::string name;

    Model()
      : nq(0)
      , nv(0)
      , njoints(1)
      , nbodies(1)
      , nframes(0)
      , inertias(1)
      , jointPlacements(1, SE3::Identity())
      , joints(1)
      , parents(1, 0)
      , names(1)
      , subtrees(1)
      , gravity( gravity981,Eigen::Vector3d::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));
      // Inertia of universe has no sense.
      inertias[0].mass() = std::numeric_limits<double>::quiet_NaN();
      inertias[0].lever().fill (std::numeric_limits<double>::quiet_NaN());
      inertias[0].inertia().fill (std::numeric_limits<double>::quiet_NaN());
    }
    ~Model() {} // std::cout << "Destroy model" << std::endl; }
    
    template<typename JointModelDerived>
    JointIndex addJoint(const JointIndex parent, const JointModelBase<JointModelDerived> & joint_model, const SE3 & joint_placement,
                        const std::string & joint_name,
                        const Eigen::VectorXd & max_effort,
                        const Eigen::VectorXd & max_velocity,
                        const Eigen::VectorXd & min_config,
                        const Eigen::VectorXd & max_config
                        );

    template<typename JointModelDerived>
    JointIndex addJoint(const JointIndex parent, const JointModelBase<JointModelDerived> & joint_model, const SE3 & joint_placement,
                        const std::string & joint_name
                        );

    int addJointFrame (const JointIndex& jointIndex,
                             int         frameIndex = -1);

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

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

    JointIndex 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;
    
    PINOCCHIO_DEPRECATED const std::string & getJointName(const JointIndex index) 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;
    
    
    int 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 se3

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

#endif // ifndef __se3_model_hpp__