sot-torque-control  1.6.5
Collection of dynamic-graph entities aimed at implementing torque control on different robots.
 
Loading...
Searching...
No Matches
admittance-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 <sot/core/debug.hh>
11#include <tsid/utils/stop-watch.hpp>
12
13namespace dynamicgraph {
14namespace sot {
15namespace torque_control {
16namespace dg = ::dynamicgraph;
17using namespace dg;
18using namespace dg::command;
19using namespace std;
20using namespace Eigen;
21using namespace tsid;
22using namespace tsid::math;
23using namespace tsid::tasks;
24using namespace dg::sot;
25
26#define PROFILE_DQ_DES_COMPUTATION "Admittance control computation"
27
28#define REF_FORCE_SIGNALS m_fRightFootRefSIN << m_fLeftFootRefSIN
29// m_fRightHandRefSIN << m_fLeftHandRefSIN
30#define FORCE_SIGNALS \
31 m_fRightFootSIN << m_fLeftFootSIN << m_fRightFootFilteredSIN \
32 << m_fLeftFootFilteredSIN
33// m_fRightHandSIN << m_fLeftHandSIN
34#define GAIN_SIGNALS \
35 m_kp_forceSIN << m_ki_forceSIN << m_kp_velSIN << m_ki_velSIN \
36 << m_force_integral_saturationSIN \
37 << m_force_integral_deadzoneSIN
38#define STATE_SIGNALS m_encodersSIN << m_jointsVelocitiesSIN
39
40#define INPUT_SIGNALS \
41 STATE_SIGNALS << REF_FORCE_SIGNALS << FORCE_SIGNALS << GAIN_SIGNALS \
42 << m_controlledJointsSIN << m_dampingSIN
43
44#define DES_VEL_SIGNALS \
45 m_vDesRightFootSOUT << m_vDesLeftFootSOUT //<< m_fRightHandErrorSOUT <<
46 // m_fLeftHandErrorSOUT
47#define OUTPUT_SIGNALS m_uSOUT << m_dqDesSOUT << DES_VEL_SIGNALS
48
52
53typedef Eigen::Matrix<double, 3, 1> Vector3;
54typedef Eigen::Matrix<double, 6, 1> Vector6;
55/* --- DG FACTORY ---------------------------------------------------- */
57 "AdmittanceController");
58
59/* ------------------------------------------------------------------- */
60/* --- CONSTRUCTION -------------------------------------------------- */
61/* ------------------------------------------------------------------- */
63 : Entity(name),
64 CONSTRUCT_SIGNAL_IN(encoders, dynamicgraph::Vector),
65 CONSTRUCT_SIGNAL_IN(jointsVelocities, dynamicgraph::Vector),
66 CONSTRUCT_SIGNAL_IN(kp_force, dynamicgraph::Vector),
67 CONSTRUCT_SIGNAL_IN(ki_force, dynamicgraph::Vector),
68 CONSTRUCT_SIGNAL_IN(kp_vel, dynamicgraph::Vector),
69 CONSTRUCT_SIGNAL_IN(ki_vel, dynamicgraph::Vector),
70 CONSTRUCT_SIGNAL_IN(force_integral_saturation, dynamicgraph::Vector),
71 CONSTRUCT_SIGNAL_IN(force_integral_deadzone, dynamicgraph::Vector),
72 CONSTRUCT_SIGNAL_IN(fRightFootRef, dynamicgraph::Vector),
73 CONSTRUCT_SIGNAL_IN(fLeftFootRef, dynamicgraph::Vector),
74 CONSTRUCT_SIGNAL_IN(fRightFoot, dynamicgraph::Vector),
75 CONSTRUCT_SIGNAL_IN(fLeftFoot, dynamicgraph::Vector),
76 CONSTRUCT_SIGNAL_IN(fRightFootFiltered, dynamicgraph::Vector),
77 CONSTRUCT_SIGNAL_IN(fLeftFootFiltered, dynamicgraph::Vector),
78 CONSTRUCT_SIGNAL_IN(controlledJoints, dynamicgraph::Vector),
79 CONSTRUCT_SIGNAL_IN(damping, dynamicgraph::Vector)
80 // ,CONSTRUCT_SIGNAL_IN(fRightHandRef, dynamicgraph::Vector)
81 // ,CONSTRUCT_SIGNAL_IN(fLeftHandRef, dynamicgraph::Vector)
82 // ,CONSTRUCT_SIGNAL_IN(fRightHand, dynamicgraph::Vector)
83 // ,CONSTRUCT_SIGNAL_IN(fLeftHand, dynamicgraph::Vector)
84 ,
85 CONSTRUCT_SIGNAL_OUT(u, dynamicgraph::Vector,
86 STATE_SIGNALS << m_controlledJointsSIN),
87 CONSTRUCT_SIGNAL_OUT(dqDes, dynamicgraph::Vector,
88 STATE_SIGNALS << DES_VEL_SIGNALS << m_dampingSIN),
89 CONSTRUCT_SIGNAL_OUT(vDesRightFoot, dynamicgraph::Vector,
90 m_fRightFootSIN << m_fRightFootFilteredSIN
91 << m_fRightFootRefSIN
92 << GAIN_SIGNALS),
93 CONSTRUCT_SIGNAL_OUT(vDesLeftFoot, dynamicgraph::Vector,
94 m_fLeftFootSIN << m_fLeftFootFilteredSIN
95 << m_fLeftFootRefSIN << GAIN_SIGNALS)
96 // ,CONSTRUCT_SIGNAL_OUT(fRightHandError, dynamicgraph::Vector,
97 // m_fRightHandSIN <<
98 // m_fRightHandRefSIN)
99 // ,CONSTRUCT_SIGNAL_OUT(fLeftHandError, dynamicgraph::Vector,
100 // m_fLeftHandSIN <<
101 // m_fLeftHandRefSIN)
102 ,
103 m_firstIter(true),
104 m_initSucceeded(false),
105 m_useJacobianTranspose(true) {
106 Entity::signalRegistration(INPUT_SIGNALS << OUTPUT_SIGNALS);
107
108 m_v_RF_int.setZero();
109 m_v_LF_int.setZero();
110
111 /* Commands. */
112 addCommand(
113 "getUseJacobianTranspose",
114 makeDirectGetter(*this, &m_useJacobianTranspose,
115 docDirectGetter("If true it uses the Jacobian "
116 "transpose, otherwise the pseudoinverse",
117 "bool")));
118 addCommand(
119 "setUseJacobianTranspose",
120 makeDirectSetter(*this, &m_useJacobianTranspose,
121 docDirectSetter("If true it uses the Jacobian "
122 "transpose, otherwise the pseudoinverse",
123 "bool")));
124 addCommand("init",
125 makeCommandVoid2(*this, &AdmittanceController::init,
126 docCommandVoid2("Initialize the entity.",
127 "Time period in seconds (double)",
128 "Robot name (string)")));
129}
130
131void AdmittanceController::init(const double& dt, const std::string& robotRef) {
132 if (dt <= 0.0) return SEND_MSG("Timestep must be positive", MSG_TYPE_ERROR);
133 if (!m_encodersSIN.isPlugged())
134 return SEND_MSG("Init failed: signal encoders is not plugged",
135 MSG_TYPE_ERROR);
136 if (!m_jointsVelocitiesSIN.isPlugged())
137 return SEND_MSG("Init failed: signal jointsVelocities is not plugged",
138 MSG_TYPE_ERROR);
139 if (!m_controlledJointsSIN.isPlugged())
140 return SEND_MSG("Init failed: signal controlledJoints is not plugged",
141 MSG_TYPE_ERROR);
142
143 m_dt = dt;
144 m_initSucceeded = true;
145
146 /* Retrieve m_robot_util informations */
147 std::string localName(robotRef);
148 if (isNameInRobotUtil(localName))
149 m_robot_util = getRobotUtil(localName);
150 else
151 return SEND_MSG(
152 "You should have an entity controller manager initialized before",
153 MSG_TYPE_ERROR);
154
155 try {
156 vector<string> package_dirs;
157 m_robot =
158 new robots::RobotWrapper(m_robot_util->m_urdf_filename, package_dirs,
159 pinocchio::JointModelFreeFlyer());
160 m_data = new pinocchio::Data(m_robot->model());
161
162 assert(m_robot->nv() >= 6);
163 m_robot_util->m_nbJoints = m_robot->nv() - 6;
164 m_nj = m_robot->nv() - 6;
165 m_u.setZero(m_nj);
166 m_dq_des_urdf.setZero(m_nj);
167 m_dqErrIntegral.setZero(m_nj);
168 // m_dqDesIntegral.setZero(m_nj);
169
170 m_q_urdf.setZero(m_robot->nq());
171 m_q_urdf(6) = 1.0;
172 m_v_urdf.setZero(m_robot->nv());
173 m_J_RF.setZero(6, m_robot->nv());
174 m_J_LF.setZero(6, m_robot->nv());
175
176 m_frame_id_rf = (int)m_robot->model().getFrameId(
177 m_robot_util->m_foot_util.m_Right_Foot_Frame_Name);
178 m_frame_id_lf = (int)m_robot->model().getFrameId(
179 m_robot_util->m_foot_util.m_Left_Foot_Frame_Name);
180
181 } catch (const std::exception& e) {
182 std::cout << e.what();
183 return SEND_MSG(
184 "Init failed: Could load URDF :" + m_robot_util->m_urdf_filename,
185 MSG_TYPE_ERROR);
186 }
187}
188
189/* ------------------------------------------------------------------- */
190/* --- SIGNALS ------------------------------------------------------- */
191/* ------------------------------------------------------------------- */
192
193DEFINE_SIGNAL_OUT_FUNCTION(u, dynamicgraph::Vector) {
194 if (!m_initSucceeded) {
195 SEND_WARNING_STREAM_MSG("Cannot compute signal u before initialization!");
196 return s;
197 }
198
199 const Vector& dqDes = m_dqDesSOUT(iter); // n
200 const Vector& q = m_encodersSIN(iter); // n
201 const Vector& dq = m_jointsVelocitiesSIN(iter); // n
202 const Vector& kp = m_kp_velSIN(iter);
203 const Vector& ki = m_ki_velSIN(iter);
204
205 if (m_firstIter) {
206 m_qPrev = q;
207 m_firstIter = false;
208 }
209
210 // estimate joint velocities using finite differences
211 m_dq_fd = (q - m_qPrev) / m_dt;
212 m_qPrev = q;
213
214 m_dqErrIntegral += m_dt * ki.cwiseProduct(dqDes - m_dq_fd);
215 s = kp.cwiseProduct(dqDes - dq) + m_dqErrIntegral;
216
217 return s;
218}
219
220DEFINE_SIGNAL_OUT_FUNCTION(dqDes, dynamicgraph::Vector) {
221 if (!m_initSucceeded) {
222 SEND_WARNING_STREAM_MSG(
223 "Cannot compute signal dqDes before initialization!");
224 return s;
225 }
226
227 getProfiler().start(PROFILE_DQ_DES_COMPUTATION);
228 {
229 const dg::sot::Vector6d v_des_LF = m_vDesLeftFootSOUT(iter);
230 const dg::sot::Vector6d v_des_RF = m_vDesRightFootSOUT(iter);
231 const Vector& q_sot = m_encodersSIN(iter); // n
232 // const Vector& dq_sot = m_jointsVelocitiesSIN(iter); //
233 // n
234 // const Vector& qMask = m_controlledJointsSIN(iter); // n
235 // const Eigen::Vector4d& damping = m_dampingSIN(iter); // 4
236
237 assert(q_sot.size() == m_nj && "Unexpected size of signal encoder");
238
240 m_robot_util->joints_sot_to_urdf(q_sot, m_q_urdf.tail(m_nj));
241 m_robot->computeAllTerms(*m_data, m_q_urdf, m_v_urdf);
242 m_robot->frameJacobianLocal(*m_data, m_frame_id_rf, m_J_RF);
243 m_robot->frameJacobianLocal(*m_data, m_frame_id_lf, m_J_LF);
244
245 // SEND_INFO_STREAM_MSG("RFoot Jacobian :" +
246 // toString(m_J_RF.rightCols(m_nj))); set to zero columns of Jacobian
247 // corresponding to unactive joints
248 // for(int i=0; i<m_nj; i++)
249 // if(qMask(i)==0)
250 // m_J_all.col(6+i).setZero();
251
253 if (m_useJacobianTranspose) {
254 m_dq_des_urdf = m_J_RF.rightCols(m_nj).transpose() * v_des_RF;
255 m_dq_des_urdf += m_J_LF.rightCols(m_nj).transpose() * v_des_LF;
256 } else {
257 m_J_RF_QR.compute(m_J_RF.rightCols(m_nj));
258 m_J_LF_QR.compute(m_J_LF.rightCols(m_nj));
259
260 m_dq_des_urdf = m_J_RF_QR.solve(v_des_RF);
261 m_dq_des_urdf += m_J_LF_QR.solve(v_des_LF);
262 }
263
264 if (s.size() != m_nj) s.resize(m_nj);
265
266 m_robot_util->joints_urdf_to_sot(m_dq_des_urdf, s);
267 }
268 getProfiler().stop(PROFILE_DQ_DES_COMPUTATION);
269
270 return s;
271}
272
273DEFINE_SIGNAL_OUT_FUNCTION(vDesRightFoot, dynamicgraph::Vector) {
274 if (!m_initSucceeded) {
275 SEND_MSG("Cannot compute signal vDesRightFoot before initialization!",
276 MSG_TYPE_WARNING_STREAM);
277 return s;
278 }
279 const Vector6& f = m_fRightFootSIN(iter);
280 const Vector6& f_filt = m_fRightFootFilteredSIN(iter);
281 const Vector6& fRef = m_fRightFootRefSIN(iter);
282 const Vector6d& kp = m_kp_forceSIN(iter);
283 const Vector6d& ki = m_ki_forceSIN(iter);
284 const Vector6d& f_sat = m_force_integral_saturationSIN(iter);
285 const Vector6d& dz = m_force_integral_deadzoneSIN(iter);
286
287 dg::sot::Vector6d err = fRef - f;
288 dg::sot::Vector6d err_filt = fRef - f_filt;
289 dg::sot::Vector6d v_des = -kp.cwiseProduct(err_filt);
290
291 for (int i = 0; i < 6; i++) {
292 if (err(i) > dz(i))
293 m_v_RF_int(i) -= m_dt * ki(i) * (err(i) - dz(i));
294 else if (err(i) < -dz(i))
295 m_v_RF_int(i) -= m_dt * ki(i) * (err(i) + dz(i));
296 }
297
298 // saturate
299 bool saturating = false;
300 for (int i = 0; i < 6; i++) {
301 if (m_v_RF_int(i) > f_sat(i)) {
302 saturating = true;
303 m_v_RF_int(i) = f_sat(i);
304 } else if (m_v_RF_int(i) < -f_sat(i)) {
305 saturating = true;
306 m_v_RF_int(i) = -f_sat(i);
307 }
308 }
309 if (saturating)
310 SEND_INFO_STREAM_MSG("Saturate m_v_RF_int integral: " +
311 toString(m_v_RF_int.transpose()));
312 s = v_des + m_v_RF_int;
313 return s;
314}
315
316DEFINE_SIGNAL_OUT_FUNCTION(vDesLeftFoot, dynamicgraph::Vector) {
317 if (!m_initSucceeded) {
318 SEND_MSG("Cannot compute signal vDesLeftFoot before initialization!",
319 MSG_TYPE_WARNING_STREAM);
320 return s;
321 }
322 const Vector6& f = m_fLeftFootSIN(iter);
323 const Vector6& f_filt = m_fLeftFootFilteredSIN(iter);
324 const Vector6& fRef = m_fLeftFootRefSIN(iter);
325 const Vector6d& kp = m_kp_forceSIN(iter);
326 const Vector6d& ki = m_ki_forceSIN(iter);
327 const Vector6d& f_sat = m_force_integral_saturationSIN(iter);
328 const Vector6d& dz = m_force_integral_deadzoneSIN(iter);
329
330 dg::sot::Vector6d err = fRef - f;
331 dg::sot::Vector6d err_filt = fRef - f_filt;
332 dg::sot::Vector6d v_des = -kp.cwiseProduct(err_filt);
333
334 for (int i = 0; i < 6; i++) {
335 if (err(i) > dz(i))
336 m_v_LF_int(i) -= m_dt * ki(i) * (err(i) - dz(i));
337 else if (err(i) < -dz(i))
338 m_v_LF_int(i) -= m_dt * ki(i) * (err(i) + dz(i));
339 }
340
341 // saturate
342 bool saturating = false;
343 for (int i = 0; i < 6; i++) {
344 if (m_v_LF_int(i) > f_sat(i)) {
345 saturating = true;
346 m_v_LF_int(i) = f_sat(i);
347 } else if (m_v_LF_int(i) < -f_sat(i)) {
348 saturating = true;
349 m_v_LF_int(i) = -f_sat(i);
350 }
351 }
352 if (saturating)
353 SEND_INFO_STREAM_MSG("Saturate m_v_LF_int integral: " +
354 toString(m_v_LF_int.transpose()));
355 s = v_des + m_v_LF_int;
356 return s;
357}
358
359// DEFINE_SIGNAL_OUT_FUNCTION(fRightHandError,dynamicgraph::Vector)
360// {
361// if(!m_initSucceeded)
362// {
363// SEND_MSG("Cannot compute signal fRightHandError before
364// initialization!", MSG_TYPE_WARNING_STREAM); return s;
365// }
366
367// const Eigen::Matrix<double,24,1>& Kf = m_KfSIN(iter); // 6*4
368// const Vector6& f = m_fRightHandSIN(iter); // 6
369// const Vector6& fRef = m_fRightHandRefSIN(iter); // 6
370// assert(f.size()==6 && "Unexpected size of signal fRightHand");
371// assert(fRef.size()==6 && "Unexpected size of signal fRightHandRef");
372
373// if(s.size()!=6)
374// s.resize(6);
375// s.tail<3>() = Kf.segment<3>(12).cwiseProduct(fRef.head<3>() -
376// f.head<3>() ); s.head<3>() =
377// Kf.segment<3>(15).cwiseProduct(fRef.tail<3>() - f.tail<3>()); return
378// s;
379// }
380
381// DEFINE_SIGNAL_OUT_FUNCTION(fLeftHandError,dynamicgraph::Vector)
382// {
383// if(!m_initSucceeded)
384// {
385// SEND_MSG("Cannot compute signal fLeftHandError before
386// initialization!", MSG_TYPE_WARNING_STREAM); return s;
387// }
388
389// const Eigen::Matrix<double,24,1>& Kf = m_KfSIN(iter); // 6*4
390// const Vector6& f = m_fLeftHandSIN(iter); // 6
391// const Vector6& fRef = m_fLeftHandRefSIN(iter); // 6
392// assert(f.size()==6 && "Unexpected size of signal fLeftHand");
393// assert(fRef.size()==6 && "Unexpected size of signal fLeftHandRef");
394
395// if(s.size()!=6)
396// s.resize(6);
397// s.tail<3>() = Kf.segment<3>(18).cwiseProduct(fRef.head<3>() -
398// f.head<3>() ); s.head<3>() =
399// Kf.segment<3>(21).cwiseProduct(fRef.tail<3>() - f.tail<3>());
400
401// return s;
402// }
403
404/* --- COMMANDS ---------------------------------------------------------- */
405
406/* ------------------------------------------------------------------- */
407/* --- ENTITY -------------------------------------------------------- */
408/* ------------------------------------------------------------------- */
409
410void AdmittanceController::display(std::ostream& os) const {
411 os << "AdmittanceController " << getName();
412 try {
413 getProfiler().report_all(3, os);
414 } catch (ExceptionSignal e) {
415 }
416}
417
418//**************************************************************************************************
419VectorXd svdSolveWithDamping(const JacobiSVD<MatrixXd>& A, const VectorXd& b,
420 double damping) {
421 eigen_assert(A.computeU() && A.computeV() &&
422 "svdSolveWithDamping() requires both unitaries U and V to be "
423 "computed (thin unitaries suffice).");
424 assert(A.rows() == b.size());
425
426 // cout<<"b = "<<toString(b,1)<<endl;
427 VectorXd tmp(A.cols());
428 int nzsv = static_cast<int>(A.nonzeroSingularValues());
429 tmp.noalias() = A.matrixU().leftCols(nzsv).adjoint() * b;
430 // cout<<"U^T*b = "<<toString(tmp,1)<<endl;
431 double sv, d2 = damping * damping;
432 for (int i = 0; i < nzsv; i++) {
433 sv = A.singularValues()(i);
434 tmp(i) *= sv / (sv * sv + d2);
435 }
436 // cout<<"S^+ U^T b = "<<toString(tmp,1)<<endl;
437 VectorXd res = A.matrixV().leftCols(nzsv) * tmp;
438
439 // getLogger().sendMsg("sing val = "+toString(A.singularValues(),3),
440 // MSG_STREAM_INFO); getLogger().sendMsg("solution with damp
441 // "+toString(damping)+" = "+toString(res.norm()), MSG_STREAM_INFO);
442 // getLogger().sendMsg("solution without damping
443 // ="+toString(A.solve(b).norm()), MSG_STREAM_INFO);
444
445 return res;
446}
447
448} // namespace torque_control
449} // namespace sot
450} // namespace dynamicgraph
#define STATE_SIGNALS
#define DES_VEL_SIGNALS
#define PROFILE_DQ_DES_COMPUTATION
#define INPUT_SIGNALS
#define OUTPUT_SIGNALS
#define GAIN_SIGNALS
bool m_useJacobianTranspose
true if the entity has been successfully initialized
EIGEN_MAKE_ALIGNED_OPERATOR_NEW AdmittanceController(const std::string &name)
tsid::robots::RobotWrapper * m_robot
frame id of left foot
tsid::math::Vector m_q_urdf
desired 6d wrench left foot
void init(const double &dt, const std::string &robotRef)
DEFINE_SIGNAL_OUT_FUNCTION(u, dynamicgraph::Vector)
Eigen::Matrix< double, 3, 1 > Vector3
DYNAMICGRAPH_FACTORY_ENTITY_PLUGIN(AdmittanceController, "AdmittanceController")
Eigen::Matrix< double, 6, 1 > Vector6
VectorXd svdSolveWithDamping(const JacobiSVD< MatrixXd > &A, const VectorXd &b, double damping)
to read text file
Definition treeview.dox:22