sot-torque-control  1.6.5
Collection of dynamic-graph entities aimed at implementing torque control on different robots.
 
Loading...
Searching...
No Matches
joint-torque-controller.cpp
Go to the documentation of this file.
1/*
2 * Copyright 2015, Andrea Del Prete, LAAS-CNRS
3 *
4 */
5
6#include <dynamic-graph/factory.h>
7
8#include <Eigen/Dense>
9#include <sot/core/debug.hh>
12
13namespace dynamicgraph {
14namespace sot {
15namespace torque_control {
16
17#define MODEL_INPUT_SIGNALS \
18 m_motorParameterKt_pSIN << m_motorParameterKt_nSIN \
19 << m_motorParameterKf_pSIN \
20 << m_motorParameterKf_nSIN \
21 << m_motorParameterKv_pSIN \
22 << m_motorParameterKv_nSIN \
23 << m_motorParameterKa_pSIN \
24 << m_motorParameterKa_nSIN << m_polySignDqSIN
25
26#define ESTIMATOR_INPUT_SIGNALS \
27 m_jointsPositionsSIN << m_jointsVelocitiesSIN << m_jointsAccelerationsSIN \
28 << m_jointsTorquesSIN << m_jointsTorquesDerivativeSIN
29
30#define TORQUE_INTEGRAL_INPUT_SIGNALS \
31 m_KiTorqueSIN << m_torque_integral_saturationSIN
32#define TORQUE_CONTROL_INPUT_SIGNALS \
33 m_jointsTorquesDesiredSIN << m_KpTorqueSIN << m_KdTorqueSIN \
34 << m_coulomb_friction_compensation_percentageSIN
35#define VEL_CONTROL_INPUT_SIGNALS m_dq_desSIN << m_KdVelSIN << m_KiVelSIN
36
37#define ALL_INPUT_SIGNALS \
38 ESTIMATOR_INPUT_SIGNALS << TORQUE_INTEGRAL_INPUT_SIGNALS \
39 << TORQUE_CONTROL_INPUT_SIGNALS \
40 << VEL_CONTROL_INPUT_SIGNALS << MODEL_INPUT_SIGNALS
41#define ALL_OUTPUT_SIGNALS \
42 m_uSOUT << m_torque_error_integralSOUT << m_smoothSignDqSOUT
43
44namespace dynamicgraph = ::dynamicgraph;
45using namespace dynamicgraph;
46using namespace dynamicgraph::command;
47using namespace std;
48using namespace Eigen;
49
52typedef JointTorqueController EntityClassName;
53
54/* --- DG FACTORY ------------------------------------------------------- */
56 "JointTorqueController");
57
58/* --- CONSTRUCTION ----------------------------------------------------- */
59/* --- CONSTRUCTION ----------------------------------------------------- */
60/* --- CONSTRUCTION ----------------------------------------------------- */
62 : Entity(name),
63 CONSTRUCT_SIGNAL_IN(jointsPositions, dynamicgraph::Vector),
64 CONSTRUCT_SIGNAL_IN(jointsVelocities, dynamicgraph::Vector),
65 CONSTRUCT_SIGNAL_IN(jointsAccelerations, dynamicgraph::Vector),
66 CONSTRUCT_SIGNAL_IN(jointsTorques, dynamicgraph::Vector),
67 CONSTRUCT_SIGNAL_IN(jointsTorquesDerivative, dynamicgraph::Vector),
68 CONSTRUCT_SIGNAL_IN(jointsTorquesDesired, dynamicgraph::Vector),
69 CONSTRUCT_SIGNAL_IN(dq_des, dynamicgraph::Vector),
70 CONSTRUCT_SIGNAL_IN(KpTorque,
71 dynamicgraph::Vector) // proportional gain for torque
72 // feedback controller
73 ,
74 CONSTRUCT_SIGNAL_IN(
75 KiTorque,
76 dynamicgraph::Vector) // integral gain for torque feedback controller
77 ,
78 CONSTRUCT_SIGNAL_IN(KdTorque,
79 dynamicgraph::Vector) // derivative gain for torque
80 // feedback controller
81 ,
82 CONSTRUCT_SIGNAL_IN(
83 KdVel, dynamicgraph::Vector) // derivative gain for velocity feedback
84 ,
85 CONSTRUCT_SIGNAL_IN(
86 KiVel, dynamicgraph::Vector) // integral gain for velocity feedback
87 ,
88 CONSTRUCT_SIGNAL_IN(torque_integral_saturation, dynamicgraph::Vector),
89 CONSTRUCT_SIGNAL_IN(coulomb_friction_compensation_percentage,
90 dynamicgraph::Vector),
91 CONSTRUCT_SIGNAL_IN(motorParameterKt_p, dynamicgraph::Vector),
92 CONSTRUCT_SIGNAL_IN(motorParameterKt_n, dynamicgraph::Vector),
93 CONSTRUCT_SIGNAL_IN(motorParameterKf_p, dynamicgraph::Vector),
94 CONSTRUCT_SIGNAL_IN(motorParameterKf_n, dynamicgraph::Vector),
95 CONSTRUCT_SIGNAL_IN(motorParameterKv_p, dynamicgraph::Vector),
96 CONSTRUCT_SIGNAL_IN(motorParameterKv_n, dynamicgraph::Vector),
97 CONSTRUCT_SIGNAL_IN(motorParameterKa_p, dynamicgraph::Vector),
98 CONSTRUCT_SIGNAL_IN(motorParameterKa_n, dynamicgraph::Vector),
99 CONSTRUCT_SIGNAL_IN(polySignDq, dynamicgraph::Vector),
100 CONSTRUCT_SIGNAL_OUT(
101 u, dynamicgraph::Vector,
104 << MODEL_INPUT_SIGNALS << m_torque_error_integralSOUT),
105 CONSTRUCT_SIGNAL_OUT(smoothSignDq, dynamicgraph::Vector,
106 m_jointsVelocitiesSIN),
107 CONSTRUCT_SIGNAL_OUT(torque_error_integral, dynamicgraph::Vector,
108 m_jointsTorquesSIN
109 << m_jointsTorquesDesiredSIN
111 Entity::signalRegistration(ALL_INPUT_SIGNALS << ALL_OUTPUT_SIGNALS);
112
113 /* Commands. */
114 addCommand("getTimestep",
115 makeDirectGetter(*this, &m_dt,
116 docDirectGetter("Control timestep", "double")));
117
118 addCommand("init",
119 makeCommandVoid2(*this, &JointTorqueController::init,
120 docCommandVoid2("Initialize the controller.",
121 "Control timestep [s].",
122 "Robot reference (string)")));
123 addCommand("reset_integral",
124 makeCommandVoid0(*this, &JointTorqueController::reset_integral,
125 docCommandVoid0("Reset the integral error.")));
126}
127
128/* --- COMMANDS ---------------------------------------------------------- */
129/* --- COMMANDS ---------------------------------------------------------- */
130/* --- COMMANDS ---------------------------------------------------------- */
131void JointTorqueController::init(const double& timestep,
132 const std::string& robot_ref) {
133 assert(timestep > 0.0 && "Timestep should be > 0");
134 if (!m_jointsVelocitiesSIN.isPlugged())
135 return SEND_MSG("Init failed: signal jointsVelocities is not plugged",
136 MSG_TYPE_ERROR);
137 if (!m_jointsTorquesSIN.isPlugged())
138 return SEND_MSG("Init failed: signal m_jointsTorquesSIN is not plugged",
139 MSG_TYPE_ERROR);
140 if (!m_jointsTorquesDesiredSIN.isPlugged())
141 return SEND_MSG(
142 "Init failed: signal m_jointsTorquesDesiredSIN is not plugged",
143 MSG_TYPE_ERROR);
144 if (!m_KpTorqueSIN.isPlugged())
145 return SEND_MSG("Init failed: signal m_KpTorqueSIN is not plugged",
146 MSG_TYPE_ERROR);
147 if (!m_KiTorqueSIN.isPlugged())
148 return SEND_MSG("Init failed: signal m_KiTorqueSIN is not plugged",
149 MSG_TYPE_ERROR);
150
151 /* Retrieve m_robot_util informations */
152 std::string localName(robot_ref);
153 if (isNameInRobotUtil(localName)) {
154 m_robot_util = getRobotUtil(localName);
155 } else {
156 SEND_MSG("You should have an entity controller manager initialized before",
157 MSG_TYPE_ERROR);
158 return;
159 }
160
161 m_dt = timestep;
162 m_tau_star.setZero(m_robot_util->m_nbJoints);
163 m_current_des.setZero(m_robot_util->m_nbJoints);
164 m_tauErrIntegral.setZero(m_robot_util->m_nbJoints);
165 // m_dqDesIntegral.setZero(m_robot_util->m_nbJoints);
166 m_dqErrIntegral.setZero(m_robot_util->m_nbJoints);
167}
168
173
174/* --- SIGNALS ---------------------------------------------------------- */
175/* --- SIGNALS ---------------------------------------------------------- */
176/* --- SIGNALS ---------------------------------------------------------- */
177
178DEFINE_SIGNAL_OUT_FUNCTION(u, dynamicgraph::Vector) {
179 // const Eigen::VectorXd& q = m_jointsPositionsSIN(iter);
180 const Eigen::VectorXd& dq = m_jointsVelocitiesSIN(iter);
181 const Eigen::VectorXd& ddq = m_jointsAccelerationsSIN(iter);
182 const Eigen::VectorXd& tau = m_jointsTorquesSIN(iter);
183 const Eigen::VectorXd& dtau = m_jointsTorquesDerivativeSIN(iter);
184 const Eigen::VectorXd& tau_d = m_jointsTorquesDesiredSIN(iter);
185 // const Eigen::VectorXd& dtau_d =
186 // m_jointsTorquesDesiredDerivativeSIN(iter);
187 const Eigen::VectorXd& dq_des = m_dq_desSIN(iter);
188 const Eigen::VectorXd& kp = m_KpTorqueSIN(iter);
189 const Eigen::VectorXd& kd = m_KdTorqueSIN(iter);
190 const Eigen::VectorXd& kd_vel = m_KdVelSIN(iter);
191 const Eigen::VectorXd& ki_vel = m_KiVelSIN(iter);
192 const Eigen::VectorXd& tauErrInt = m_torque_error_integralSOUT(iter);
193 const Eigen::VectorXd& colFricCompPerc =
194 m_coulomb_friction_compensation_percentageSIN(iter);
195 const Eigen::VectorXd& motorParameterKt_p = m_motorParameterKt_pSIN(iter);
196 const Eigen::VectorXd& motorParameterKt_n = m_motorParameterKt_nSIN(iter);
197 const Eigen::VectorXd& motorParameterKf_p = m_motorParameterKf_pSIN(iter);
198 const Eigen::VectorXd& motorParameterKf_n = m_motorParameterKf_nSIN(iter);
199 const Eigen::VectorXd& motorParameterKv_p = m_motorParameterKv_pSIN(iter);
200 const Eigen::VectorXd& motorParameterKv_n = m_motorParameterKv_nSIN(iter);
201 const Eigen::VectorXd& motorParameterKa_p = m_motorParameterKa_pSIN(iter);
202 const Eigen::VectorXd& motorParameterKa_n = m_motorParameterKa_nSIN(iter);
203 const Eigen::VectorXd& polySignDq = m_polySignDqSIN(iter);
204 // const Eigen::VectorXd& dq_thr = m_dq_thresholdSIN(iter);
205
206 m_tau_star =
207 tau_d + kp.cwiseProduct(tau_d - tau) + tauErrInt - kd.cwiseProduct(dtau);
208
209 int offset = 0;
210 if (dq.size() == (int)(m_robot_util->m_nbJoints + 6)) offset = 6;
211
212 m_dqErrIntegral += m_dt * ki_vel.cwiseProduct(dq_des - dq);
213 const Eigen::VectorXd& err_int_sat = m_torque_integral_saturationSIN(iter);
214 // saturate
215 bool saturating = false;
216 for (int i = 0; i < (int)m_robot_util->m_nbJoints; i++) {
217 if (m_dqErrIntegral(i) > err_int_sat(i)) {
218 saturating = true;
219 m_dqErrIntegral(i) = err_int_sat(i);
220 } else if (m_dqErrIntegral(i) < -err_int_sat(i)) {
221 saturating = true;
222 m_dqErrIntegral(i) = -err_int_sat(i);
223 }
224 }
225 if (saturating)
226 SEND_INFO_STREAM_MSG("Saturate dqErr integral: " +
227 toString(m_dqErrIntegral.head<12>()));
228
229 for (int i = 0; i < (int)m_robot_util->m_nbJoints; i++) {
230 m_current_des(i) = motorModel.getCurrent(
231 m_tau_star(i),
232 dq(i + offset) + kd_vel(i) * (dq_des(i) - dq(i + offset)) +
233 m_dqErrIntegral(i), // ki_vel(i)*(m_dqDesIntegral(i)-q(i)),
234 ddq(i + offset), motorParameterKt_p(i), motorParameterKt_n(i),
235 motorParameterKf_p(i) * colFricCompPerc(i),
236 motorParameterKf_n(i) * colFricCompPerc(i), motorParameterKv_p(i),
237 motorParameterKv_n(i), motorParameterKa_p(i), motorParameterKa_n(i),
238 static_cast<unsigned int>(polySignDq(i)));
239 }
240
241 s = m_current_des;
242 return s;
243}
244
245DEFINE_SIGNAL_OUT_FUNCTION(torque_error_integral, dynamicgraph::Vector) {
246 const Eigen::VectorXd& tau = m_jointsTorquesSIN(iter);
247 const Eigen::VectorXd& tau_d = m_jointsTorquesDesiredSIN(iter);
248 const Eigen::VectorXd& err_int_sat = m_torque_integral_saturationSIN(iter);
249 const Eigen::VectorXd& ki = m_KiTorqueSIN(iter);
250
251 // compute torque error integral and saturate
252 m_tauErrIntegral += m_dt * ki.cwiseProduct(tau_d - tau);
253 for (int i = 0; i < (int)m_robot_util->m_nbJoints; i++) {
254 if (m_tauErrIntegral(i) > err_int_sat(i))
255 m_tauErrIntegral(i) = err_int_sat(i);
256 else if (m_tauErrIntegral(i) < -err_int_sat(i))
257 m_tauErrIntegral(i) = -err_int_sat(i);
258 }
259
260 s = m_tauErrIntegral;
261 return s;
262}
263
264DEFINE_SIGNAL_OUT_FUNCTION(smoothSignDq, dynamicgraph::Vector) {
265 const Eigen::VectorXd& dq = m_jointsVelocitiesSIN(iter);
266 const Eigen::VectorXd& polySignDq = m_polySignDqSIN(iter);
267 if (s.size() != (int)m_robot_util->m_nbJoints)
268 s.resize(m_robot_util->m_nbJoints);
269
270 for (int i = 0; i < (int)m_robot_util->m_nbJoints; i++)
271 s(i) = motorModel.smoothSign(dq[i], 0.1,
272 static_cast<unsigned int>(polySignDq[i]));
273 // TODO Use Eigen binaryexpr
274 return s;
275}
276
277void JointTorqueController::display(std::ostream& os) const {
278 os << "JointTorqueController " << getName() << ":\n";
279 try {
280 getProfiler().report_all(3, os);
281 } catch (ExceptionSignal e) {
282 }
283}
284
285} // namespace torque_control
286} // namespace sot
287} // namespace dynamicgraph
RobotUtilShrPtr m_robot_util
integral of the velocity error
void init(const double &timestep, const std::string &robotRef)
EIGEN_MAKE_ALIGNED_OPERATOR_NEW JointTorqueController(const std::string &name)
Eigen::VectorXd m_dqErrIntegral
integral of the current error
#define ALL_OUTPUT_SIGNALS
#define ALL_INPUT_SIGNALS
#define TORQUE_CONTROL_INPUT_SIGNALS
#define ESTIMATOR_INPUT_SIGNALS
#define MODEL_INPUT_SIGNALS
#define VEL_CONTROL_INPUT_SIGNALS
#define TORQUE_INTEGRAL_INPUT_SIGNALS
DEFINE_SIGNAL_OUT_FUNCTION(u, dynamicgraph::Vector)
DYNAMICGRAPH_FACTORY_ENTITY_PLUGIN(AdmittanceController, "AdmittanceController")
to read text file
Definition treeview.dox:22