ModelInterface.h

Go to the documentation of this file.

/*
 * Copyright (C) 2016 IIT-ADVR
 * Author: Arturo Laurenzi, Luca Muratore
 * email:  arturo.laurenzi@iit.it, luca.muratore@iit.it
 *
 * This program 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.
 *
 * This program 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 Lesser General Public License for more details.
 *
 * You should have received a copy of the GNU Lesser General Public License
 * along with this program. If not, see <http://www.gnu.org/licenses/>
 *
 * NOTE: implementation of the jacobian derivative was adapted from KDL::ChainJntToJacDotSolver
 */
 
#ifndef __MODEL_INTERFACE_H__
#define __MODEL_INTERFACE_H__
 
 
#include <vector>
#include <map>
 
#include <Eigen/Geometry>
#include <kdl/frames.hpp>
#include <kdl/jacobian.hpp>
#include <eigen_conversions/eigen_kdl.h>
 
#include <XBotInterface/XBotInterface.h>
#include <XBotInterface/ModelChain.h>
 
namespace XBot {
 
class RobotInterface;
 
class ModelInterface : public XBotInterface {
 
public:
 
 
    friend XBot::RobotInterface;
 
    typedef std::shared_ptr<ModelInterface> Ptr;
 
    typedef std::shared_ptr<const ModelInterface> ConstPtr;
 
    // ModelInterface must be default constructible and non-copyable
    ModelInterface() = default;
    ModelInterface& operator=(const ModelInterface& other) = delete;
    ModelInterface(const ModelInterface& other) = delete;
 
    
    static ModelInterface::Ptr getModel(const ConfigOptions& config);
 
    static ModelInterface::Ptr getModel(const std::string& path_to_cfg, AnyMapConstPtr any_map = AnyMapConstPtr());
 
 
    ModelChain& chain(const std::string& chain_name);
    const ModelChain& chain(const std::string& chain_name) const;
 
    ModelChain& operator()(const std::string& chain_name);
    const ModelChain& operator()(const std::string& chain_name) const;
 
    ModelChain& arm(int arm_id);
    const ModelChain& arm(int arm_id) const;
 
    ModelChain& leg(int leg_id);
    const ModelChain& leg(int leg_id) const;
 
    template <typename... SyncFlags>
    bool syncFrom(const XBotInterface& other, SyncFlags... flags);
 
 
    template <typename... SyncFlags>
    bool syncFrom_no_update(const XBotInterface& other, SyncFlags... flags);
 
    bool update( bool update_position = true,
                         bool update_velocity = true,
                         bool update_desired_acceleration = true );
 
    virtual void getModelOrderedJoints( std::vector<std::string>& joint_name ) const = 0;
 
    int getActuatedJointNum() const;
 
    bool isFloatingBase() const;
 
    virtual bool setFloatingBasePose( const KDL::Frame& floating_base_pose ) = 0;
 
    bool setFloatingBaseOrientation( const KDL::Rotation& world_R_floating_base );
 
    bool getFloatingBasePose( KDL::Frame& floating_base_pose ) const;
 
    virtual bool setFloatingBaseTwist( const KDL::Twist& floating_base_twist ) = 0;
 
    bool setFloatingBaseAngularVelocity( const KDL::Vector& floating_base_omega );
 
    bool setFloatingBaseState( const KDL::Frame& pose, const KDL::Twist& twist );
 
    bool setFloatingBaseState( XBot::ImuSensor::ConstPtr imu, const Eigen::Matrix3d& offset = Eigen::Matrix3d::Identity());
 
    bool getFloatingBaseTwist( KDL::Twist& floating_base_twist ) const;
 
    virtual bool getFloatingBaseLink( std::string& floating_base_link) const = 0;
 
    virtual bool getPose( const std::string& source_frame,
                          KDL::Frame& pose ) const = 0;
 
    bool getPose( const std::string& source_frame,
                  const std::string& target_frame,
                  KDL::Frame& pose ) const;
 
    bool getOrientation(const std::string& source_frame,
                        KDL::Rotation& orientation) const;
 
    bool getOrientation(const std::string& source_frame,
                        const std::string& target_frame,
                        KDL::Rotation& orientation) const;
 
 
    bool getPointPosition(const std::string& source_frame,
                          const KDL::Vector& source_point,
                          KDL::Vector& world_point) const;
 
    bool getPointPosition(const std::string& source_frame,
                          const std::string& target_frame,
                          const KDL::Vector& source_point,
                          KDL::Vector& target_point) const;
    virtual bool getJacobian( const std::string& link_name,
                              const KDL::Vector& reference_point,
                              KDL::Jacobian& J) const = 0;
 
    bool getJacobian( const std::string& link_name,
                      KDL::Jacobian& J) const;
 
    bool getJacobian( const std::string& link_name,
                      const std::string& target_frame,
                      KDL::Jacobian& J) const;
 
    void getPosturalJacobian(Eigen::MatrixXd& J) const;
 
    virtual bool getRelativeJacobian( const std::string& target_link_name,
                              const std::string& base_link_name,
                              KDL::Jacobian& J) const;
 
    virtual bool getVelocityTwist( const std::string& link_name,
                                     KDL::Twist& velocity) const = 0;
                                     
    bool getRelativeVelocityTwist( const std::string& link_name, const std::string& base_link_name,
                             KDL::Twist& velocity) const;
 
    virtual bool getVelocityTwist( const std::string& source_frame,
                                   const std::string& target_frame,
                                   KDL::Twist& velocity) const;
 
    virtual bool getAccelerationTwist( const std::string& link_name,
                                         KDL::Twist& acceleration) const = 0;
 
    virtual bool getRelativeAccelerationTwist(const std::string& link_name, const std::string& base_link_name,
                                              KDL::Twist& acceleration) const = 0;
 
 
    virtual bool getPointAcceleration(const std::string& link_name,
                                      const KDL::Vector& point,
                                      KDL::Vector& acceleration) const = 0;
 
    virtual bool computeJdotQdot(const std::string& link_name,
                                 const KDL::Vector& point,
                                 KDL::Twist& jdotqdot) const;
 
    virtual bool computeRelativeJdotQdot(const std::string& target_link_name,
                                         const std::string& base_link_name,
                                         KDL::Twist& jdotqdot) const = 0;
 
    virtual void getCOM( KDL::Vector& com_position ) const = 0;
 
    bool getCOM( const std::string& reference_frame,
                         KDL::Vector& com_position ) const;
 
 
    virtual void getCOMJacobian( KDL::Jacobian& J, KDL::Vector& dJcomQdot) const;
 
 
    virtual void getCOMJacobian( KDL::Jacobian& J) const;
 
    virtual void getCOMVelocity( KDL::Vector& velocity) const = 0;
 
    virtual void getCOMAcceleration( KDL::Vector& acceleration) const = 0;
 
 
    virtual void getCentroidalMomentum(Eigen::Vector6d& centroidal_momentum) const = 0;
 
    virtual void getCentroidalMomentumMatrix(Eigen::MatrixXd& centroidal_momentum_matrix,
                                             Eigen::Vector6d& CMMdotQdot) const;
 
    virtual void getCentroidalMomentumMatrix(Eigen::MatrixXd& centroidal_momentum_matrix) const;
 
    virtual double getMass() const = 0;
 
 
    virtual void getGravity( KDL::Vector& gravity ) const = 0;
 
    bool getGravity( const std::string& reference_frame,
                     KDL::Vector& gravity ) const;
    virtual void setGravity( const KDL::Vector& gravity ) = 0;
 
    bool setGravity( const std::string& reference_frame,
                     const KDL::Vector& gravity );
 
    virtual void getInertiaMatrix(Eigen::MatrixXd& M) const = 0;
 
    virtual void getInertiaInverseTimesVector(const Eigen::VectorXd& vec, Eigen::VectorXd& minv_vec) const;
 
    virtual void getInertiaInverseTimesMatrix(const Eigen::MatrixXd& Mat, Eigen::MatrixXd& Minv_Mat) const;
 
    virtual void getInertiaInverse(Eigen::MatrixXd& Minv) const;
 
 
    virtual void computeGravityCompensation( Eigen::VectorXd& g ) const = 0;
 
    virtual void computeNonlinearTerm( Eigen::VectorXd& n ) const = 0;
 
    virtual void computeInverseDynamics( Eigen::VectorXd& tau) const = 0;
 
    bool computeConstrainedInverseDynamics( const Eigen::MatrixXd& J, const Eigen::VectorXd& weights, Eigen::VectorXd& tau) const;
 
    bool getChainSelectionMatrix( const std::string& chain_name, Eigen::MatrixXd& S ) const;
 
    bool getJointSelectionMatrix( const std::string& joint_name, Eigen::RowVectorXd& S ) const;
 
    bool getJointSelectionMatrix( int joint_id, Eigen::RowVectorXd& S ) const;
 
    bool maskJacobian( const std::string& chain_name, KDL::Jacobian& J) const;
 
 
    /*************************/
    /**** EIGEN OVERLOADS ****/
    /*************************/
 
    bool setFloatingBasePose( const Eigen::Affine3d& floating_base_pose );
 
    bool setFloatingBaseState( const Eigen::Affine3d& pose, const Eigen::Vector6d& twist );
 
    bool setFloatingBaseOrientation( const Eigen::Matrix3d& world_R_floating_base );
 
    virtual bool setFloatingBaseTwist( const Eigen::Vector6d& floating_base_twist );
 
    bool setFloatingBaseAngularVelocity( const Eigen::Vector3d& floating_base_omega );
 
    bool getFloatingBasePose( Eigen::Affine3d& floating_base_pose ) const;
 
 
    bool getFloatingBaseTwist( Eigen::Vector6d& floating_base_twist ) const;
 
    bool getPose(   const std::string& source_frame,
                    const std::string& target_frame,
                    Eigen::Affine3d& pose ) const;
    bool getPose( const std::string& source_frame,
                  Eigen::Affine3d& pose ) const;
 
    bool getOrientation(    const std::string& source_frame,
                            Eigen::Matrix3d& orientation) const;
    bool getOrientation(    const std::string& source_frame,
                            const std::string& target_frame,
                            Eigen::Matrix3d& orientation) const;
    bool getPointPosition(  const std::string& source_frame,
                            const Eigen::Vector3d& source_point,
                            Eigen::Vector3d& world_point) const;
 
    bool getPointPosition(  const std::string& source_frame,
                            const std::string& target_frame,
                            const Eigen::Vector3d& source_point,
                            Eigen::Vector3d& target_point) const;
 
    bool getJacobian( const std::string& link_name,
                      Eigen::MatrixXd& J) const;
 
    bool getJacobian( const std::string& link_name,
                      const std::string& target_frame,
                      Eigen::MatrixXd& J) const;
 
    bool getJacobian( const std::string& link_name,
                      const Eigen::Vector3d& reference_point,
                      Eigen::MatrixXd& J) const;
 
    bool getRelativeJacobian( const std::string& target_link_name,
                              const std::string& base_link_name,
                              Eigen::MatrixXd& J) const;
 
    void getCOM( Eigen::Vector3d& com_position ) const;
 
    bool getCOM( const std::string& reference_frame,
                 Eigen::Vector3d& com_position ) const;
 
    virtual void getCOMJacobian( Eigen::MatrixXd& J, Eigen::Vector3d& dJcomQdot) const;
 
    void getCOMJacobian( Eigen::MatrixXd& J) const;
 
    void getCOMVelocity( Eigen::Vector3d& velocity) const;
 
    void getCOMAcceleration( Eigen::Vector3d& acceleration) const;
 
    bool getRelativeVelocityTwist( const std::string& link_name, const std::string& base_link_name,
                             Eigen::Vector6d& velocity) const;
 
    bool getRelativeAccelerationTwist(const std::string& link_name, const std::string& base_link_name,
                                              Eigen::Vector6d& acceleration) const;
                             
    bool getVelocityTwist( const std::string& link_name,
                           Eigen::Vector6d& velocity) const;
 
    virtual bool getVelocityTwist( const std::string& source_frame,
                                   const std::string& target_frame,
                                   Eigen::Matrix<double,6,1>& velocity) const;
 
    bool getAccelerationTwist( const std::string& link_name,
                          Eigen::Vector6d& acceleration) const;
 
    bool getPointAcceleration(const std::string& link_name,
                              const Eigen::Vector3d& point,
                              Eigen::Vector3d& acceleration) const;
 
    bool computeJdotQdot(const std::string& link_name,
                         const Eigen::Vector3d& point,
                         Eigen::Vector6d& jdotqdot) const;
 
    bool computeRelativeJdotQdot(const std::string& target_link_name,
                                 const std::string& base_link_name,
                                 Eigen::Vector6d& jdotqdot) const;
 
    void getGravity( Eigen::Vector3d& gravity ) const;
 
    bool getGravity( const std::string& reference_frame,
                     Eigen::Vector3d& gravity ) const;
 
    void setGravity( const Eigen::Vector3d& gravity );
 
    bool setGravity( const std::string& reference_frame,
                     const Eigen::Vector3d& gravity );
 
    bool maskJacobian( const std::string& chain_name, Eigen::MatrixXd& J) const;
 
    virtual int getLinkID(const std::string& link_name) const = 0;
 
 
    // Getters for RX
 
    using XBotInterface::getJointPosition;
    using XBotInterface::getMotorPosition;
    using XBotInterface::getJointVelocity;
    using XBotInterface::getMotorVelocity;
    using XBotInterface::getJointAcceleration;
    using XBotInterface::getJointEffort;
    using XBotInterface::getStiffness;
    using XBotInterface::getDamping;
 
 
    // Setters for RX
 
    using XBotInterface::setJointPosition;
    using XBotInterface::setMotorPosition;
    using XBotInterface::setJointVelocity;
    using XBotInterface::setMotorVelocity;
    using XBotInterface::setJointAcceleration;
    using XBotInterface::setJointEffort;
 
    using XBotInterface::setStiffness;
    using XBotInterface::setDamping;
 
 
protected:
 
    virtual bool init_internal(const ConfigOptions& config);
    virtual bool init_model(const ConfigOptions& config) = 0;
    virtual bool update_internal( bool update_position,
                                  bool update_velocity,
                                  bool update_desired_acceleration) = 0;
 
    inline void rotationEigenToKDL(const Eigen::Matrix3d& eigen_rotation, KDL::Rotation& kdl_rotation) const;
    inline void rotationKDLToEigen(const KDL::Rotation& kdl_rotation, Eigen::Matrix3d& eigen_rotation) const;
 
    bool _is_floating_base;
 
private:
 
 
 
 
    using XBotInterface::_chain_map;
    using XBotInterface::_ordered_joint_vector;
    std::map<std::string, XBot::ModelChain::Ptr> _model_chain_map;
    XBot::ModelChain _dummy_chain;
 
    using XBotInterface::_ordered_chain_names;
    std::map<int, int> _joint_id_to_model_id;
 
    bool fillModelOrderedChain();
 
    mutable KDL::Twist _tmp_kdl_twist;
    mutable KDL::Frame _tmp_kdl_frame, _tmp_kdl_frame_1;
    mutable KDL::Jacobian _tmp_kdl_jacobian, _tmp_kdl_jacobian_1;
    mutable KDL::Vector _tmp_kdl_vector, _tmp_kdl_vector_1;
    mutable KDL::Rotation _tmp_kdl_rotation, _tmp_kdl_rotation_1;
    mutable std::vector<int> _tmp_int_vector;
    mutable std::string _floating_base_link;
    mutable Eigen::MatrixXd _tmp_postural_jacob;
    mutable Eigen::MatrixXd _tmp_inertia;
    mutable Eigen::MatrixXd _tmp_inverse_inertia;
    mutable Eigen::VectorXd _tmp_inv_inertia_vec;
    mutable Eigen::MatrixXd _tmp_jacobian, _tmp_cmm, _tmp_jacobiandot;
    mutable Eigen::MatrixXd _tmp_M;
    mutable Eigen::MatrixXd _tmp_I;
    mutable Eigen::VectorXd _tmp_gcomp, _tmp_h, _tmp_jstate, _M_col;
    mutable Eigen::MatrixXd _tmp_jacobian_relative;
    mutable Eigen::Vector6d _tmp_jdot_qdot;
    mutable bool _compute_inverse_inertia;
 
    using XBotInterface::getTemperature;
    using XBotInterface::setTemperature;
 
    // Getters for TX
 
    using XBotInterface::getPositionReference;
    using XBotInterface::getVelocityReference;
    using XBotInterface::getEffortReference;
 
 
    // Setters for TX
 
    using XBotInterface::setPositionReference;
    using XBotInterface::setVelocityReference;
    using XBotInterface::setEffortReference;
 
 
    using XBotInterface::getChainMap;
 
    using XBotInterface::sync;
 
    void seekAndDestroyFixedControlledJoints();
 
};
};
 
 
// NOTE we should put all the utils function in an hpp file with a proper namespace.
 
 
inline void XBot::ModelInterface::rotationEigenToKDL(const Eigen::Matrix3d& eigen_rotation, KDL::Rotation& kdl_rotation) const
{
    for(int i = 0; i < 3; i++){
        for(int j = 0; j < 3; j++){
            kdl_rotation.data[3*i+j] = eigen_rotation(i,j);
        }
    }
}
 
inline void XBot::ModelInterface::rotationKDLToEigen(const KDL::Rotation& kdl_rotation, Eigen::Matrix3d& eigen_rotation) const
{
    for(int i = 0; i < 3; i++){
        for(int j = 0; j < 3; j++){
            eigen_rotation(i,j) = kdl_rotation.data[3*i+j];
        }
    }
}
 
template <typename... SyncFlags>
bool XBot::ModelInterface::syncFrom(const XBot::XBotInterface& other, SyncFlags... flags)
{
    if( XBot::XBotInterface::sync(other, flags...) ){
        update(true, true, true);
        return true;
    }
    else{
        return false;
    }
}
 
template <typename... SyncFlags>
bool XBot::ModelInterface::syncFrom_no_update(const XBot::XBotInterface& other, SyncFlags... flags)
{
    if( XBot::XBotInterface::sync(other, flags...) ){
        update(true, true, true);
        return true;
    }
    else{
        return false;
    }
}
 
 
#endif // __MODEL_INTERFACE_H__