sot-torque-control  1.5.2
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 <tsid/utils/stop-watch.hpp>
30 /* HELPER */
31 #include <dynamic-graph/signal-helper.h>
32 #include <sot/core/matrix-geometry.hh>
33 #include <sot/core/robot-utils.hh>
35 
36 /*Motor model*/
38 
39 namespace dynamicgraph {
40 namespace sot {
41 namespace torque_control {
42 
65 class SOTJOINTTORQUECONTROLLER_EXPORT JointTorqueController : public ::dynamicgraph::Entity {
66  DYNAMIC_GRAPH_ENTITY_DECL();
67 
68  public: /* --- SIGNALS --- */
69  DECLARE_SIGNAL_IN(jointsPositions, dynamicgraph::Vector);
70  DECLARE_SIGNAL_IN(jointsVelocities, dynamicgraph::Vector);
71  DECLARE_SIGNAL_IN(jointsAccelerations, dynamicgraph::Vector);
72  DECLARE_SIGNAL_IN(jointsTorques, dynamicgraph::Vector);
73  DECLARE_SIGNAL_IN(jointsTorquesDerivative, dynamicgraph::Vector);
74  DECLARE_SIGNAL_IN(jointsTorquesDesired, dynamicgraph::Vector);
75  // DECLARE_SIGNAL_IN(jointsTorquesDesiredDerivative, dynamicgraph::Vector);/// desired joints torques
76  // derivative dtauDes
77  DECLARE_SIGNAL_IN(dq_des, dynamicgraph::Vector);
78  DECLARE_SIGNAL_IN(KpTorque, dynamicgraph::Vector);
79  DECLARE_SIGNAL_IN(KiTorque, dynamicgraph::Vector);
80  DECLARE_SIGNAL_IN(KdTorque, dynamicgraph::Vector);
81  DECLARE_SIGNAL_IN(KdVel, dynamicgraph::Vector);
82  DECLARE_SIGNAL_IN(KiVel, dynamicgraph::Vector);
83  DECLARE_SIGNAL_IN(torque_integral_saturation, dynamicgraph::Vector);
84 
85  // DECLARE_SIGNAL_IN(dq_threshold, dynamicgraph::Vector); /// velocity sign threshold
86  // DECLARE_SIGNAL_IN(ddq_threshold, dynamicgraph::Vector); /// acceleration sign threshold
87 
89  DECLARE_SIGNAL_IN(coulomb_friction_compensation_percentage, dynamicgraph::Vector);
90  DECLARE_SIGNAL_IN(motorParameterKt_p, dynamicgraph::Vector);
91  DECLARE_SIGNAL_IN(motorParameterKt_n, dynamicgraph::Vector);
92  DECLARE_SIGNAL_IN(motorParameterKf_p, dynamicgraph::Vector);
93  DECLARE_SIGNAL_IN(motorParameterKf_n, dynamicgraph::Vector);
94  DECLARE_SIGNAL_IN(motorParameterKv_p, dynamicgraph::Vector);
95  DECLARE_SIGNAL_IN(motorParameterKv_n, dynamicgraph::Vector);
96  DECLARE_SIGNAL_IN(motorParameterKa_p, dynamicgraph::Vector);
97  DECLARE_SIGNAL_IN(motorParameterKa_n, dynamicgraph::Vector);
98  DECLARE_SIGNAL_IN(polySignDq, dynamicgraph::Vector);
99 
100  DECLARE_SIGNAL_OUT(u, dynamicgraph::Vector);
101  DECLARE_SIGNAL_OUT(smoothSignDq, dynamicgraph::Vector);
102  DECLARE_SIGNAL_OUT(torque_error_integral, dynamicgraph::Vector);
103 
104  protected:
106  double m_dt;
107  Eigen::VectorXd m_tau_star;
108  Eigen::VectorXd m_current_des;
109  Eigen::VectorXd m_tauErrIntegral;
110  Eigen::VectorXd m_currentErrIntegral;
111  // Eigen::VectorXd m_dqDesIntegral; /// integral of the desired velocity
112  Eigen::VectorXd m_dqErrIntegral;
113 
114  RobotUtilShrPtr m_robot_util;
115 
116  void sendMsg(const std::string& msg, MsgType t = MSG_TYPE_INFO, const char* = "", int = 0) {
117  logger_.stream(t) << ("[" + name + "] " + msg) << '\n';
118  }
119 
120  public:
121  EIGEN_MAKE_ALIGNED_OPERATOR_NEW
122 
124  JointTorqueController(const std::string& name);
125 
129  void init(const double& timestep, const std::string& robotRef);
130 
131  void reset_integral();
132 
133  public: /* --- ENTITY INHERITANCE --- */
134  virtual void display(std::ostream& os) const;
135 
136 }; // class JointTorqueController
137 
138 } // namespace torque_control
139 } // namespace sot
140 } // namespace dynamicgraph
141 
142 #endif // #ifndef __sot_torque_control_JointTorqueController_H__
dynamicgraph::sot::torque_control::JointTorqueController::m_dt
double m_dt
Definition: joint-torque-controller.hh:106
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:116
dynamicgraph::sot::torque_control::JointTorqueController
Definition: joint-torque-controller.hh:65
dynamicgraph::sot::torque_control::JointTorqueController::m_tau_star
Eigen::VectorXd m_tau_star
timestep of the controller
Definition: joint-torque-controller.hh:107
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:114
dynamicgraph::sot::torque_control::JointTorqueController::m_current_des
Eigen::VectorXd m_current_des
Definition: joint-torque-controller.hh:108
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:109
dynamicgraph::sot::torque_control::JointTorqueController::m_currentErrIntegral
Eigen::VectorXd m_currentErrIntegral
integral of the torque error
Definition: joint-torque-controller.hh:110
dynamicgraph::sot::torque_control::JointTorqueController::m_dqErrIntegral
Eigen::VectorXd m_dqErrIntegral
integral of the current error
Definition: joint-torque-controller.hh:112
dynamicgraph::sot::torque_control::JointTorqueController::motorModel
MotorModel motorModel
integral of the torque tracking error
Definition: joint-torque-controller.hh:105