joint-torque-controller.hh

Go to the documentation of this file.

/*
 * Copyright 2015, Andrea Del Prete, LAAS-CNRS
 *
 */
 
#ifndef __sot_torque_control_JointTorqueController_H__
#define __sot_torque_control_JointTorqueController_H__
/* --------------------------------------------------------------------- */
/* --- API ------------------------------------------------------------- */
/* --------------------------------------------------------------------- */
 
#if defined(WIN32)
#if defined(joint_torque_controller_EXPORTS)
#define SOTJOINTTORQUECONTROLLER_EXPORT __declspec(dllexport)
#else
#define SOTJOINTTORQUECONTROLLER_EXPORT __declspec(dllimport)
#endif
#else
#define SOTJOINTTORQUECONTROLLER_EXPORT
#endif
 
// #define VP_DEBUG 1        /// enable debug output
// #define VP_DEBUG_MODE 20
 
/* --------------------------------------------------------------------- */
/* --- INCLUDE --------------------------------------------------------- */
/* --------------------------------------------------------------------- */
 
#include <pinocchio/fwd.hpp>
#include <tsid/utils/stop-watch.hpp>
/* HELPER */
#include <dynamic-graph/signal-helper.h>
 
#include <sot/core/matrix-geometry.hh>
#include <sot/core/robot-utils.hh>
#include <sot/torque_control/utils/vector-conversions.hh>
 
/*Motor model*/
#include <sot/torque_control/motor-model.hh>
 
namespace dynamicgraph {
namespace sot {
namespace torque_control {
 
class SOTJOINTTORQUECONTROLLER_EXPORT JointTorqueController
    : public ::dynamicgraph::Entity {
  DYNAMIC_GRAPH_ENTITY_DECL();
 
 public: /* --- SIGNALS --- */
  DECLARE_SIGNAL_IN(jointsPositions, dynamicgraph::Vector);      
  DECLARE_SIGNAL_IN(jointsVelocities, dynamicgraph::Vector);     
  DECLARE_SIGNAL_IN(jointsAccelerations, dynamicgraph::Vector);  
  DECLARE_SIGNAL_IN(jointsTorques,
                    dynamicgraph::Vector);  
  DECLARE_SIGNAL_IN(
      jointsTorquesDerivative,
      dynamicgraph::Vector);  
  DECLARE_SIGNAL_IN(jointsTorquesDesired,
                    dynamicgraph::Vector);  
  //        DECLARE_SIGNAL_IN(jointsTorquesDesiredDerivative,
  //        dynamicgraph::Vector);/// desired joints torques derivative dtauDes
  DECLARE_SIGNAL_IN(dq_des, dynamicgraph::Vector);  
  DECLARE_SIGNAL_IN(KpTorque,
                    dynamicgraph::Vector);  
  DECLARE_SIGNAL_IN(
      KiTorque,
      dynamicgraph::Vector);  
  DECLARE_SIGNAL_IN(
      KdTorque,
      dynamicgraph::Vector);  
  DECLARE_SIGNAL_IN(
      KdVel, dynamicgraph::Vector);  
  DECLARE_SIGNAL_IN(
      KiVel, dynamicgraph::Vector);  
  DECLARE_SIGNAL_IN(torque_integral_saturation,
                    dynamicgraph::Vector);  
 
  //        DECLARE_SIGNAL_IN(dq_threshold,           dynamicgraph::Vector); ///
  //        velocity sign threshold DECLARE_SIGNAL_IN(ddq_threshold,
  //        dynamicgraph::Vector);      /// acceleration sign threshold
 
  DECLARE_SIGNAL_IN(coulomb_friction_compensation_percentage,
                    dynamicgraph::Vector);
  DECLARE_SIGNAL_IN(motorParameterKt_p, dynamicgraph::Vector);
  DECLARE_SIGNAL_IN(motorParameterKt_n, dynamicgraph::Vector);
  DECLARE_SIGNAL_IN(motorParameterKf_p, dynamicgraph::Vector);
  DECLARE_SIGNAL_IN(motorParameterKf_n, dynamicgraph::Vector);
  DECLARE_SIGNAL_IN(motorParameterKv_p, dynamicgraph::Vector);
  DECLARE_SIGNAL_IN(motorParameterKv_n, dynamicgraph::Vector);
  DECLARE_SIGNAL_IN(motorParameterKa_p, dynamicgraph::Vector);
  DECLARE_SIGNAL_IN(motorParameterKa_n, dynamicgraph::Vector);
  DECLARE_SIGNAL_IN(polySignDq, dynamicgraph::Vector);
 
  DECLARE_SIGNAL_OUT(u, dynamicgraph::Vector);  
  DECLARE_SIGNAL_OUT(
      smoothSignDq, dynamicgraph::Vector);  
  DECLARE_SIGNAL_OUT(
      torque_error_integral,
      dynamicgraph::Vector);  
 
 protected:
  MotorModel motorModel;
  double m_dt;  
  Eigen::VectorXd m_tau_star;
  Eigen::VectorXd m_current_des;
  Eigen::VectorXd m_tauErrIntegral;      
  Eigen::VectorXd m_currentErrIntegral;  
  //        Eigen::VectorXd m_dqDesIntegral; /// integral of the desired
  //        velocity
  Eigen::VectorXd m_dqErrIntegral;  
 
  RobotUtilShrPtr m_robot_util;
 
  void sendMsg(const std::string& msg, MsgType t = MSG_TYPE_INFO,
               const char* = "", int = 0) {
    logger_.stream(t) << ("[" + name + "] " + msg) << '\n';
  }
 
 public:
  EIGEN_MAKE_ALIGNED_OPERATOR_NEW
 
  JointTorqueController(const std::string& name);
 
  void init(const double& timestep, const std::string& robotRef);
 
  void reset_integral();
 
 public: /* --- ENTITY INHERITANCE --- */
  virtual void display(std::ostream& os) const;
 
};  // class JointTorqueController
 
}  // namespace torque_control
}  // namespace sot
}  // namespace dynamicgraph
 
#endif  // #ifndef __sot_torque_control_JointTorqueController_H__