sot-torque-control  1.6.1
Collection of dynamic-graph entities aimed at implementing torque control on different robots.
joint-torque-controller.hh
Go to the documentation of this file.
1 /*
2  * Copyright 2015, Andrea Del Prete, LAAS-CNRS
3  *
4  */
5 
6 #ifndef __sot_torque_control_JointTorqueController_H__
7 #define __sot_torque_control_JointTorqueController_H__
8 /* --------------------------------------------------------------------- */
9 /* --- API ------------------------------------------------------------- */
10 /* --------------------------------------------------------------------- */
11 
12 #if defined(WIN32)
13 #if defined(joint_torque_controller_EXPORTS)
14 #define SOTJOINTTORQUECONTROLLER_EXPORT __declspec(dllexport)
15 #else
16 #define SOTJOINTTORQUECONTROLLER_EXPORT __declspec(dllimport)
17 #endif
18 #else
19 #define SOTJOINTTORQUECONTROLLER_EXPORT
20 #endif
21 
22 //#define VP_DEBUG 1 /// enable debug output
23 //#define VP_DEBUG_MODE 20
24 
25 /* --------------------------------------------------------------------- */
26 /* --- INCLUDE --------------------------------------------------------- */
27 /* --------------------------------------------------------------------- */
28 
29 #include <pinocchio/fwd.hpp>
30 
31 #include <tsid/utils/stop-watch.hpp>
32 /* HELPER */
33 #include <dynamic-graph/signal-helper.h>
34 #include <sot/core/matrix-geometry.hh>
35 #include <sot/core/robot-utils.hh>
37 
38 /*Motor model*/
40 
41 namespace dynamicgraph {
42 namespace sot {
43 namespace torque_control {
44 
67 class SOTJOINTTORQUECONTROLLER_EXPORT JointTorqueController : public ::dynamicgraph::Entity {
68  DYNAMIC_GRAPH_ENTITY_DECL();
69 
70  public: /* --- SIGNALS --- */
71  DECLARE_SIGNAL_IN(jointsPositions, dynamicgraph::Vector);
72  DECLARE_SIGNAL_IN(jointsVelocities, dynamicgraph::Vector);
73  DECLARE_SIGNAL_IN(jointsAccelerations, dynamicgraph::Vector);
74  DECLARE_SIGNAL_IN(jointsTorques, dynamicgraph::Vector);
75  DECLARE_SIGNAL_IN(jointsTorquesDerivative, dynamicgraph::Vector);
76  DECLARE_SIGNAL_IN(jointsTorquesDesired, dynamicgraph::Vector);
77  // DECLARE_SIGNAL_IN(jointsTorquesDesiredDerivative, dynamicgraph::Vector);/// desired joints torques
78  // derivative dtauDes
79  DECLARE_SIGNAL_IN(dq_des, dynamicgraph::Vector);
80  DECLARE_SIGNAL_IN(KpTorque, dynamicgraph::Vector);
81  DECLARE_SIGNAL_IN(KiTorque, dynamicgraph::Vector);
82  DECLARE_SIGNAL_IN(KdTorque, dynamicgraph::Vector);
83  DECLARE_SIGNAL_IN(KdVel, dynamicgraph::Vector);
84  DECLARE_SIGNAL_IN(KiVel, dynamicgraph::Vector);
85  DECLARE_SIGNAL_IN(torque_integral_saturation, dynamicgraph::Vector);
86 
87  // DECLARE_SIGNAL_IN(dq_threshold, dynamicgraph::Vector); /// velocity sign threshold
88  // DECLARE_SIGNAL_IN(ddq_threshold, dynamicgraph::Vector); /// acceleration sign threshold
89 
91  DECLARE_SIGNAL_IN(coulomb_friction_compensation_percentage, dynamicgraph::Vector);
92  DECLARE_SIGNAL_IN(motorParameterKt_p, dynamicgraph::Vector);
93  DECLARE_SIGNAL_IN(motorParameterKt_n, dynamicgraph::Vector);
94  DECLARE_SIGNAL_IN(motorParameterKf_p, dynamicgraph::Vector);
95  DECLARE_SIGNAL_IN(motorParameterKf_n, dynamicgraph::Vector);
96  DECLARE_SIGNAL_IN(motorParameterKv_p, dynamicgraph::Vector);
97  DECLARE_SIGNAL_IN(motorParameterKv_n, dynamicgraph::Vector);
98  DECLARE_SIGNAL_IN(motorParameterKa_p, dynamicgraph::Vector);
99  DECLARE_SIGNAL_IN(motorParameterKa_n, dynamicgraph::Vector);
100  DECLARE_SIGNAL_IN(polySignDq, dynamicgraph::Vector);
101 
102  DECLARE_SIGNAL_OUT(u, dynamicgraph::Vector);
103  DECLARE_SIGNAL_OUT(smoothSignDq, dynamicgraph::Vector);
104  DECLARE_SIGNAL_OUT(torque_error_integral, dynamicgraph::Vector);
105 
106  protected:
108  double m_dt;
109  Eigen::VectorXd m_tau_star;
110  Eigen::VectorXd m_current_des;
111  Eigen::VectorXd m_tauErrIntegral;
112  Eigen::VectorXd m_currentErrIntegral;
113  // Eigen::VectorXd m_dqDesIntegral; /// integral of the desired velocity
114  Eigen::VectorXd m_dqErrIntegral;
115 
116  RobotUtilShrPtr m_robot_util;
117 
118  void sendMsg(const std::string& msg, MsgType t = MSG_TYPE_INFO, const char* = "", int = 0) {
119  logger_.stream(t) << ("[" + name + "] " + msg) << '\n';
120  }
121 
122  public:
123  EIGEN_MAKE_ALIGNED_OPERATOR_NEW
124 
126  JointTorqueController(const std::string& name);
127 
131  void init(const double& timestep, const std::string& robotRef);
132 
133  void reset_integral();
134 
135  public: /* --- ENTITY INHERITANCE --- */
136  virtual void display(std::ostream& os) const;
137 
138 }; // class JointTorqueController
139 
140 } // namespace torque_control
141 } // namespace sot
142 } // namespace dynamicgraph
143 
144 #endif // #ifndef __sot_torque_control_JointTorqueController_H__
dynamicgraph::sot::torque_control::JointTorqueController::m_dt
double m_dt
Definition: joint-torque-controller.hh:108
dynamicgraph
to read text file
Definition: treeview.dox:22
vector-conversions.hh
motor-model.hh
dynamicgraph::sot::torque_control::MotorModel
Definition: motor-model.hh:19
dynamicgraph::sot::torque_control::JointTorqueController::sendMsg
void sendMsg(const std::string &msg, MsgType t=MSG_TYPE_INFO, const char *="", int=0)
Definition: joint-torque-controller.hh:118
dynamicgraph::sot::torque_control::JointTorqueController
Definition: joint-torque-controller.hh:67
dynamicgraph::sot::torque_control::JointTorqueController::m_tau_star
Eigen::VectorXd m_tau_star
timestep of the controller
Definition: joint-torque-controller.hh:109
torque_control
Definition: __init__.py:1
dynamicgraph::sot::torque_control::JointTorqueController::m_robot_util
RobotUtilShrPtr m_robot_util
integral of the velocity error
Definition: joint-torque-controller.hh:116
dynamicgraph::sot::torque_control::JointTorqueController::m_current_des
Eigen::VectorXd m_current_des
Definition: joint-torque-controller.hh:110
SOTJOINTTORQUECONTROLLER_EXPORT
#define SOTJOINTTORQUECONTROLLER_EXPORT
Definition: joint-torque-controller.hh:19
dynamicgraph::sot::torque_control::JointTorqueController::m_tauErrIntegral
Eigen::VectorXd m_tauErrIntegral
Definition: joint-torque-controller.hh:111
dynamicgraph::sot::torque_control::JointTorqueController::m_currentErrIntegral
Eigen::VectorXd m_currentErrIntegral
integral of the torque error
Definition: joint-torque-controller.hh:112
dynamicgraph::sot::torque_control::JointTorqueController::m_dqErrIntegral
Eigen::VectorXd m_dqErrIntegral
integral of the current error
Definition: joint-torque-controller.hh:114
dynamicgraph::sot::torque_control::JointTorqueController::motorModel
MotorModel motorModel
integral of the torque tracking error
Definition: joint-torque-controller.hh:107