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 |
|
|
#include <tsid/utils/stop-watch.hpp> |
31 |
|
|
/* HELPER */ |
32 |
|
|
#include <dynamic-graph/signal-helper.h> |
33 |
|
|
|
34 |
|
|
#include <sot/core/matrix-geometry.hh> |
35 |
|
|
#include <sot/core/robot-utils.hh> |
36 |
|
|
#include <sot/torque_control/utils/vector-conversions.hh> |
37 |
|
|
|
38 |
|
|
/*Motor model*/ |
39 |
|
|
#include <sot/torque_control/motor-model.hh> |
40 |
|
|
|
41 |
|
|
namespace dynamicgraph { |
42 |
|
|
namespace sot { |
43 |
|
|
namespace torque_control { |
44 |
|
|
|
45 |
|
|
/** |
46 |
|
|
* This Entity takes as inputs the estimated joints' positions, |
47 |
|
|
* velocities, accelerations and torques and it computes the desired |
48 |
|
|
* current to send to the motors board in order to track the |
49 |
|
|
* desired joints torques. Most of the input of this entity are |
50 |
|
|
* computed by the entity ForceTorqueEstimator. |
51 |
|
|
* |
52 |
|
|
* QUICK START |
53 |
|
|
* Create the entity, plug all the input signals, call the init method |
54 |
|
|
* specifying the control-loop time step. For instance: |
55 |
|
|
* jtc = JointTorqueController("jtc"); |
56 |
|
|
* plug(estimator.jointsPositions, jtc.jointsPositions); |
57 |
|
|
* plug(estimator.jointsVelocities, jtc.jointsVelocities); |
58 |
|
|
* plug(estimator.jointsAccelerations, jtc.jointsAccelerations); |
59 |
|
|
* plug(estimator.jointsTorques, jtc.jointsTorques); |
60 |
|
|
* jtc.KpTorque.value = N_DOF*(10.0,); |
61 |
|
|
* jtc.KiTorque.value = N_DOF*(0.01,); |
62 |
|
|
* jtc.init(dt); |
63 |
|
|
* |
64 |
|
|
* DETAILS |
65 |
|
|
* To do... |
66 |
|
|
*/ |
67 |
|
|
class SOTJOINTTORQUECONTROLLER_EXPORT JointTorqueController |
68 |
|
|
: public ::dynamicgraph::Entity { |
69 |
|
|
DYNAMIC_GRAPH_ENTITY_DECL(); |
70 |
|
|
|
71 |
|
|
public: /* --- SIGNALS --- */ |
72 |
|
|
DECLARE_SIGNAL_IN(jointsPositions, dynamicgraph::Vector); /// q |
73 |
|
|
DECLARE_SIGNAL_IN(jointsVelocities, dynamicgraph::Vector); /// dq |
74 |
|
|
DECLARE_SIGNAL_IN(jointsAccelerations, dynamicgraph::Vector); /// ddq |
75 |
|
|
DECLARE_SIGNAL_IN(jointsTorques, |
76 |
|
|
dynamicgraph::Vector); /// estimated joints torques tau |
77 |
|
|
DECLARE_SIGNAL_IN( |
78 |
|
|
jointsTorquesDerivative, |
79 |
|
|
dynamicgraph::Vector); /// estimated joints torques derivative dtau |
80 |
|
|
DECLARE_SIGNAL_IN(jointsTorquesDesired, |
81 |
|
|
dynamicgraph::Vector); /// desired joints torques tauDes |
82 |
|
|
// DECLARE_SIGNAL_IN(jointsTorquesDesiredDerivative, |
83 |
|
|
// dynamicgraph::Vector);/// desired joints torques derivative dtauDes |
84 |
|
|
DECLARE_SIGNAL_IN(dq_des, dynamicgraph::Vector); /// desired joint velocities |
85 |
|
|
DECLARE_SIGNAL_IN(KpTorque, |
86 |
|
|
dynamicgraph::Vector); /// proportional gain for torque |
87 |
|
|
/// feedback controller |
88 |
|
|
DECLARE_SIGNAL_IN( |
89 |
|
|
KiTorque, |
90 |
|
|
dynamicgraph::Vector); /// integral gain for torque feedback controller |
91 |
|
|
DECLARE_SIGNAL_IN( |
92 |
|
|
KdTorque, |
93 |
|
|
dynamicgraph::Vector); /// derivative gain for torque feedback controller |
94 |
|
|
DECLARE_SIGNAL_IN( |
95 |
|
|
KdVel, dynamicgraph::Vector); /// derivative gain for velocity feedback |
96 |
|
|
DECLARE_SIGNAL_IN( |
97 |
|
|
KiVel, dynamicgraph::Vector); /// integral gain for velocity feedback |
98 |
|
|
DECLARE_SIGNAL_IN(torque_integral_saturation, |
99 |
|
|
dynamicgraph::Vector); /// integral error saturation |
100 |
|
|
|
101 |
|
|
// DECLARE_SIGNAL_IN(dq_threshold, dynamicgraph::Vector); /// |
102 |
|
|
// velocity sign threshold DECLARE_SIGNAL_IN(ddq_threshold, |
103 |
|
|
// dynamicgraph::Vector); /// acceleration sign threshold |
104 |
|
|
|
105 |
|
|
/// parameters for the linear model |
106 |
|
|
DECLARE_SIGNAL_IN(coulomb_friction_compensation_percentage, |
107 |
|
|
dynamicgraph::Vector); |
108 |
|
|
DECLARE_SIGNAL_IN(motorParameterKt_p, dynamicgraph::Vector); |
109 |
|
|
DECLARE_SIGNAL_IN(motorParameterKt_n, dynamicgraph::Vector); |
110 |
|
|
DECLARE_SIGNAL_IN(motorParameterKf_p, dynamicgraph::Vector); |
111 |
|
|
DECLARE_SIGNAL_IN(motorParameterKf_n, dynamicgraph::Vector); |
112 |
|
|
DECLARE_SIGNAL_IN(motorParameterKv_p, dynamicgraph::Vector); |
113 |
|
|
DECLARE_SIGNAL_IN(motorParameterKv_n, dynamicgraph::Vector); |
114 |
|
|
DECLARE_SIGNAL_IN(motorParameterKa_p, dynamicgraph::Vector); |
115 |
|
|
DECLARE_SIGNAL_IN(motorParameterKa_n, dynamicgraph::Vector); |
116 |
|
|
DECLARE_SIGNAL_IN(polySignDq, dynamicgraph::Vector); |
117 |
|
|
|
118 |
|
|
DECLARE_SIGNAL_OUT(u, dynamicgraph::Vector); /// Desired current |
119 |
|
|
DECLARE_SIGNAL_OUT( |
120 |
|
|
smoothSignDq, dynamicgraph::Vector); /// smooth approximation of sign(dq) |
121 |
|
|
DECLARE_SIGNAL_OUT( |
122 |
|
|
torque_error_integral, |
123 |
|
|
dynamicgraph::Vector); /// integral of the torque tracking error |
124 |
|
|
|
125 |
|
|
protected: |
126 |
|
|
MotorModel motorModel; |
127 |
|
|
double m_dt; /// timestep of the controller |
128 |
|
|
Eigen::VectorXd m_tau_star; |
129 |
|
|
Eigen::VectorXd m_current_des; |
130 |
|
|
Eigen::VectorXd m_tauErrIntegral; /// integral of the torque error |
131 |
|
|
Eigen::VectorXd m_currentErrIntegral; /// integral of the current error |
132 |
|
|
// Eigen::VectorXd m_dqDesIntegral; /// integral of the desired |
133 |
|
|
// velocity |
134 |
|
|
Eigen::VectorXd m_dqErrIntegral; /// integral of the velocity error |
135 |
|
|
|
136 |
|
|
RobotUtilShrPtr m_robot_util; |
137 |
|
|
|
138 |
|
|
void sendMsg(const std::string& msg, MsgType t = MSG_TYPE_INFO, |
139 |
|
|
const char* = "", int = 0) { |
140 |
|
|
logger_.stream(t) << ("[" + name + "] " + msg) << '\n'; |
141 |
|
|
} |
142 |
|
|
|
143 |
|
|
public: |
144 |
|
|
EIGEN_MAKE_ALIGNED_OPERATOR_NEW |
145 |
|
|
|
146 |
|
|
/** --- CONSTRUCTOR ---- */ |
147 |
|
|
JointTorqueController(const std::string& name); |
148 |
|
|
|
149 |
|
|
/** Initialize the JointTorqueController. |
150 |
|
|
* @param timestep Period (in seconds) after which the sensors' data are |
151 |
|
|
* updated. |
152 |
|
|
*/ |
153 |
|
|
void init(const double& timestep, const std::string& robotRef); |
154 |
|
|
|
155 |
|
|
void reset_integral(); |
156 |
|
|
|
157 |
|
|
public: /* --- ENTITY INHERITANCE --- */ |
158 |
|
|
virtual void display(std::ostream& os) const; |
159 |
|
|
|
160 |
|
|
}; // class JointTorqueController |
161 |
|
|
|
162 |
|
|
} // namespace torque_control |
163 |
|
|
} // namespace sot |
164 |
|
|
} // namespace dynamicgraph |
165 |
|
|
|
166 |
|
|
#endif // #ifndef __sot_torque_control_JointTorqueController_H__ |