sot-torque-control  1.6.4
Collection of dynamic-graph entities aimed at implementing torque control on different robots.
 
Loading...
Searching...
No Matches
device-torque-ctrl.cpp
Go to the documentation of this file.
1/*
2 * Copyright 2014, Andrea Del Prete, LAAS-CNRS
3 *
4 */
5
7
8#include <dynamic-graph/all-commands.h>
9#include <dynamic-graph/factory.h>
10
11#include <fstream>
12#include <map>
13#include <pinocchio/algorithm/joint-configuration.hpp> // integrate
14#include <sot/core/debug.hh>
15#include <tsid/math/constraint-base.hpp>
16#include <tsid/math/utils.hpp>
17
18using namespace std;
19using namespace dynamicgraph;
20using namespace sot::torque_control;
21using namespace tsid;
22using namespace tsid::tasks;
23using namespace dynamicgraph::sot;
24
25typedef tsid::math::ConstraintBase ConstraintBase;
26
27const double DeviceTorqueCtrl::TIMESTEP_DEFAULT = 0.001;
29
31
33 : dgsot::Device(RobotName),
34 timestep_(TIMESTEP_DEFAULT),
35 m_initSucceeded(false),
36 accelerometerSOUT_("DeviceTorqueCtrl(" + RobotName +
37 ")::output(vector)::accelerometer"),
38 gyrometerSOUT_("DeviceTorqueCtrl(" + RobotName +
39 ")::output(vector)::gyrometer"),
40 robotStateSOUT_("DeviceTorqueCtrl(" + RobotName +
41 ")::output(vector)::robotState"),
42 jointsVelocitiesSOUT_("DeviceTorqueCtrl(" + RobotName +
43 ")::output(vector)::jointsVelocities"),
44 jointsAccelerationsSOUT_("DeviceTorqueCtrl(" + RobotName +
45 ")::output(vector)::jointsAccelerations"),
46 currentSOUT_("DeviceTorqueCtrl(" + RobotName +
47 ")::output(vector)::currents"),
48 p_gainsSOUT_("DeviceTorqueCtrl(" + RobotName +
49 ")::output(vector)::p_gains"),
50 d_gainsSOUT_("DeviceTorqueCtrl(" + RobotName +
51 ")::output(vector)::d_gains"),
52 CONSTRUCT_SIGNAL_IN(kp_constraints, dynamicgraph::Vector),
53 CONSTRUCT_SIGNAL_IN(kd_constraints, dynamicgraph::Vector),
54 CONSTRUCT_SIGNAL_IN(rotor_inertias, dynamicgraph::Vector),
55 CONSTRUCT_SIGNAL_IN(gear_ratios, dynamicgraph::Vector),
56 accelerometer_(3),
57 gyrometer_(3),
58 m_isTorqueControlled(true),
59 m_numericalDamping(1e-8),
60 normalDistribution_(0.0, FORCE_SENSOR_NOISE_STD_DEV),
61 normalRandomNumberGenerator_(randomNumberGenerator_,
62 normalDistribution_) {
63 forcesSIN_[0] = new SignalPtr<dynamicgraph::Vector, int>(
64 NULL, "DeviceTorqueCtrl::input(vector6)::inputForceRLEG");
65 forcesSIN_[1] = new SignalPtr<dynamicgraph::Vector, int>(
66 NULL, "DeviceTorqueCtrl::input(vector6)::inputForceLLEG");
67 forcesSIN_[2] = new SignalPtr<dynamicgraph::Vector, int>(
68 NULL, "DeviceTorqueCtrl::input(vector6)::inputForceRARM");
69 forcesSIN_[3] = new SignalPtr<dynamicgraph::Vector, int>(
70 NULL, "DeviceTorqueCtrl::input(vector6)::inputForceLARM");
71 signalRegistration(accelerometerSOUT_
74 << *(forcesSIN_[0]) << *(forcesSIN_[1]) << *(forcesSIN_[2])
76 << d_gainsSOUT_ << m_kp_constraintsSIN
77 << m_kd_constraintsSIN << m_rotor_inertiasSIN
78 << m_gear_ratiosSIN);
79
80 // set controlInputType to acceleration so that at the end of the increment
81 // method the velocity is copied in the velocity output signal (and not the
82 // control)
83 controlInputType_ = CONTROL_INPUT_TWO_INTEGRATION;
84
85 // initialize gyrometer and accelerometer
86 dynamicgraph::Vector data(3);
87 data.setZero();
88 gyrometerSOUT_.setConstant(data);
89 data(2) = 9.81;
90 accelerometerSOUT_.setConstant(data);
91
92 // initialize force/torque sensors
93 for (int i = 0; i < 4; ++i) {
94 withForceSignals[i] = true;
95 wrenches_[i].resize(6);
96 wrenches_[i].setZero();
97 if (i == FORCE_SIGNAL_RLEG || i == FORCE_SIGNAL_LLEG)
98 wrenches_[i](2) = 350.0;
99 forcesSOUT[i]->setConstant(wrenches_[i]);
100 }
101
102 temp6_.resize(6);
103 m_nk = 12;
104
105 using namespace dynamicgraph::command;
106 std::string docstring;
107 /* Command increment. */
108 docstring =
109 "\n Integrate dynamics for time step provided as input\n"
110 "\n take one floating point number as input\n\n";
111 addCommand("increment",
112 makeCommandVoid1((Device&)*this, &Device::increment, docstring));
113 addCommand("init",
114 makeCommandVoid2(*this, &DeviceTorqueCtrl::init,
115 docCommandVoid2("Initialize the entity.",
116 "Time period in seconds (double)",
117 "Robot reference (string)")));
118}
119
121
122void DeviceTorqueCtrl::init(const double& dt, const std::string& robotRef) {
123 if (dt <= 0.0)
124 return SEND_MSG("Init failed: Timestep must be positive", MSG_TYPE_ERROR);
125
126 /* Retrieve m_robot_util informations */
127 std::string localName(robotRef);
128 if (isNameInRobotUtil(localName))
129 m_robot_util = getRobotUtil(localName);
130 else {
131 SEND_MSG("You should first initialize the entity control-manager",
132 MSG_TYPE_ERROR);
133 return;
134 }
135
136 m_nj = static_cast<int>(m_robot_util->m_nbJoints);
137
138 const dynamicgraph::sot::Vector6d& kp_contact = m_kp_constraintsSIN(0);
139 const dynamicgraph::sot::Vector6d& kd_contact = m_kd_constraintsSIN(0);
140 const Vector rotor_inertias = m_rotor_inertiasSIN(0);
141 const Vector gear_ratios = m_gear_ratiosSIN(0);
142 const std::string& urdfFile = m_robot_util->m_urdf_filename;
143
144 try {
145 vector<string> package_dirs;
146 m_robot = new robots::RobotWrapper(urdfFile, package_dirs,
147 pinocchio::JointModelFreeFlyer());
148 m_data = new pinocchio::Data(m_robot->model());
149 m_robot->rotor_inertias(rotor_inertias);
150 m_robot->gear_ratios(gear_ratios);
151
152 assert(m_robot->nq() == m_nj + 7);
153 assert(m_robot->nv() == m_nj + 6);
154
155 m_q.setZero(m_robot->nq());
156 m_v.setZero(m_robot->nv());
157 m_q_sot.setZero(m_nj + 6);
158 m_v_sot.setZero(m_nj + 6);
159 m_dv_sot.setZero(m_nj + 6);
160 m_dvBar.setZero(m_nj + 6);
161 m_tau_np6.setZero(m_nj + 6);
162 m_K.setZero(m_robot->nv() + m_nk, m_robot->nv() + m_nk);
163 m_k.setZero(m_robot->nv() + m_nk);
164 m_f.setZero(m_nk);
165 m_Jc.setZero(m_nk, m_robot->nv());
166 m_dJcv.setZero(m_nk);
167
169 new TaskSE3Equality("contact_rfoot", *m_robot,
170 m_robot_util->m_foot_util.m_Right_Foot_Frame_Name);
171 m_contactRF->Kp(kp_contact);
172 m_contactRF->Kd(kd_contact);
173
175 new TaskSE3Equality("contact_lfoot", *m_robot,
176 m_robot_util->m_foot_util.m_Left_Foot_Frame_Name);
177 m_contactLF->Kp(kp_contact);
178 m_contactLF->Kd(kd_contact);
179 } catch (const std::exception& e) {
180 std::cout << e.what();
181 return SEND_MSG("Init failed: Could load URDF :" + urdfFile,
182 MSG_TYPE_ERROR);
183 }
184 timestep_ = dt;
185 setStateSize(m_nj + 6);
186 m_initSucceeded = true;
187}
188
189void DeviceTorqueCtrl::setStateSize(const unsigned int& size) {
190 assert(size == static_cast<unsigned int>(m_nj + 6));
191 Device::setStateSize(size);
192
193 base6d_encoders_.resize(size);
194 base6d_encoders_.fill(0.0);
195 jointsVelocities_.resize(size - 6);
196 jointsVelocities_.fill(0.0);
198
202}
203
204void DeviceTorqueCtrl::setState(const dynamicgraph::Vector& q) {
205 assert(q.size() == m_nj + 6);
206 if (!m_initSucceeded) {
207 SEND_WARNING_STREAM_MSG("Cannot setState before initialization!");
208 return;
209 }
210 Device::setState(q);
211 m_q_sot = q;
212 m_robot_util->config_sot_to_urdf(m_q_sot, m_q);
213
214 m_robot->computeAllTerms(*m_data, m_q, m_v);
215 pinocchio::SE3 H_lf = m_robot->position(
216 *m_data, m_robot->model().getJointId(
217 m_robot_util->m_foot_util.m_Left_Foot_Frame_Name));
218 tsid::trajectories::TrajectorySample s(12, 6);
219 tsid::math::SE3ToVector(H_lf, s.pos);
220 m_contactLF->setReference(s);
221 SEND_MSG("Setting left foot reference to " + toString(H_lf), MSG_TYPE_DEBUG);
222
223 pinocchio::SE3 H_rf = m_robot->position(
224 *m_data, m_robot->model().getJointId(
225 m_robot_util->m_foot_util.m_Right_Foot_Frame_Name));
226 tsid::math::SE3ToVector(H_rf, s.pos);
227 m_contactRF->setReference(s);
228 SEND_MSG("Setting right foot reference to " + toString(H_rf), MSG_TYPE_DEBUG);
230}
231
232void DeviceTorqueCtrl::setVelocity(const dynamicgraph::Vector& v) {
233 assert(v.size() == m_nj + 6);
234 if (!m_initSucceeded) {
235 SEND_WARNING_STREAM_MSG("Cannot setVelocity before initialization!");
236 return;
237 }
238 Device::setVelocity(v);
239 m_v_sot = v;
240 m_robot_util->velocity_sot_to_urdf(m_q, m_v_sot, m_v);
241}
242
243void DeviceTorqueCtrl::setControlInputType(const std::string& cit) {
244 if (cit == "torque") {
246 return;
247 }
248 m_isTorqueControlled = false;
249 return Device::setControlInputType(cit);
250}
251
253 const Vector& tauDes = controlSIN.accessCopy();
254 assert(tauDes.size() == m_nj);
255
256 // compute mass matrix
257 m_robot->computeAllTerms(*m_data, m_q, m_v);
258
259 const ConstraintBase& constrRF = m_contactRF->compute(0.0, m_q, m_v, *m_data);
260 const ConstraintBase& constrLF = m_contactLF->compute(0.0, m_q, m_v, *m_data);
261 assert(constrRF.matrix().rows() == 6 && constrRF.matrix().cols() == m_nj + 6);
262 assert(constrLF.matrix().rows() == 6 && constrLF.matrix().cols() == m_nj + 6);
263 assert(constrRF.vector().size() == 6 && constrLF.vector().size() == 6);
264 assert(m_Jc.rows() == 12 && m_Jc.cols() == m_nj + 6);
265 assert(m_K.rows() == m_nj + 6 + 12 && m_K.cols() == m_nj + 6 + 12);
266 m_Jc.topRows<6>() = constrRF.matrix();
267 m_Jc.bottomRows<6>() = constrLF.matrix();
268 Matrix JcT = m_Jc.transpose();
269 m_dJcv.head<6>() = constrRF.vector();
270 m_dJcv.tail<6>() = constrLF.vector();
271
272 // compute constraint solution: ddqBar = - Jc^+ * dJc * dq
273 m_Jc_svd.compute(m_Jc, Eigen::ComputeThinU | Eigen::ComputeFullV);
274 tsid::math::solveWithDampingFromSvd(m_Jc_svd, m_dJcv, m_dvBar,
276
277 // compute base of null space of constraint Jacobian
278 Eigen::VectorXd::Index r = (m_Jc_svd.singularValues().array() > 1e-8).count();
279 m_Z = m_Jc_svd.matrixV().rightCols(m_nj + 6 - r);
280
281 // compute constrained accelerations ddq_c = (Z^T*M*Z)^{-1}*Z^T*(S^T*tau - h -
282 // M*ddqBar)
283 const Matrix& M = m_robot->mass(*m_data);
284 const Vector& h = m_robot->nonLinearEffects(*m_data);
285 m_ZMZ = m_Z.transpose() * M * m_Z;
286 m_robot_util->joints_sot_to_urdf(tauDes, m_tau_np6.tail(m_nj));
287 m_dv_c = m_Z.transpose() * (m_tau_np6 - h - M * m_dvBar);
288 Vector rhs = m_dv_c;
289 // m_ZMZ_chol.compute(m_ZMZ);
290 // m_ZMZ_chol.solveInPlace(m_dv_c);
291 tsid::math::svdSolveWithDamping(m_ZMZ, rhs, m_dv_c, m_numericalDamping);
292
293 if ((m_ZMZ * m_dv_c - rhs).norm() > 1e-10)
294 SEND_MSG("ZMZ*dv - ZT*(tau-h-Mdv_bar) = " +
295 toString((m_ZMZ * m_dv_c - rhs).norm()),
296 MSG_TYPE_DEBUG);
297
298 // compute joint accelerations
299 m_dv = m_dvBar + m_Z * m_dv_c;
300
301 // compute contact forces
302 Vector b = M * m_dv + h - m_tau_np6;
303 // const double d2 = m_numericalDamping*m_numericalDamping;
304 // const unsigned int nzsv = m_Jc_svd.nonzeroSingularValues();
305 // Eigen::VectorXd tmp(m_Jc_svd.rows());
306 // tmp.noalias() = m_Jc_svd.matrixV().leftCols(nzsv).adjoint() * b;
307 // double sv;
308 // for(int i=0; i<nzsv; i++)
309 // {
310 // sv = m_Jc_svd.singularValues()(i);
311 // tmp(i) *= sv/(sv*sv + d2);
312 // }
313 // m_f = m_Jc_svd.matrixU().leftCols(nzsv) * tmp;
314 tsid::math::svdSolveWithDamping(JcT, b, m_f, m_numericalDamping);
315
316 // SEND_MSG("dv = "+toString(m_dv.norm()), MSG_TYPE_DEBUG);
317 // SEND_MSG("f = "+toString(m_f), MSG_TYPE_DEBUG);
318 if ((JcT * m_f - b).norm() > 1e-10)
319 SEND_MSG("M*dv +h - JT*f - tau = " + toString((JcT * m_f - b).norm()),
320 MSG_TYPE_DEBUG);
321 if ((m_Jc * m_dv - m_dJcv).norm() > 1e-10)
322 SEND_MSG("Jc*dv - dJc*v = " + toString((m_Jc * m_dv - m_dJcv).norm()),
323 MSG_TYPE_DEBUG);
324}
325
326void DeviceTorqueCtrl::integrate(const double& dt) {
327 if (!m_initSucceeded) {
328 SEND_WARNING_STREAM_MSG("Cannot integrate before initialization!");
329 return;
330 }
331
334 // integrate
335 m_q = pinocchio::integrate(m_robot->model(), m_q, dt * m_v);
336 m_v += dt * m_dv;
337
338 m_robot_util->config_urdf_to_sot(m_q, m_q_sot);
339 m_robot_util->velocity_urdf_to_sot(m_q, m_v, m_v_sot);
340 m_robot_util->velocity_urdf_to_sot(m_q, m_dv, m_dv_sot);
341
342 state_ = m_q_sot;
343 velocity_ = m_v_sot;
345 } else {
346 Device::integrate(dt);
347 if (controlInputType_ == CONTROL_INPUT_TWO_INTEGRATION)
348 jointsAccelerations_ = controlSIN.accessCopy().tail(m_nj);
349 else
350 jointsAccelerations_.setZero(m_nj);
351 }
352
353 base6d_encoders_ = state_;
354 jointsVelocities_ = velocity_.tail(m_nj);
355
356 // set the value for the encoders, joints velocities and accelerations output
357 // signal
361
362 int time = robotStateSOUT_.getTime() + 1;
363 for (int i = 0; i < 4; i++) {
364 // read input force (if any)
365 if (forcesSIN_[i]->isPlugged() &&
366 forcesSIN_[i]->operator()(time).size() == 6)
367 wrenches_[i] = forcesSIN_[i]->accessCopy();
368 // add random noise
369 for (int j = 0; j < 6; j++)
371 // set output force
372 forcesSOUT[i]->setConstant(temp6_);
373 }
374
375 robotStateSOUT_.setTime(time);
376 jointsVelocitiesSOUT_.setTime(time);
377 jointsAccelerationsSOUT_.setTime(time);
378 accelerometerSOUT_.setTime(time);
379 gyrometerSOUT_.setTime(time);
380 for (int i = 0; i < 4; ++i) forcesSOUT[i]->setTime(time);
381}
dynamicgraph::Vector jointsVelocities_
base 6d pose + joints' angles
virtual void setControlInputType(const std::string &cit)
dynamicgraph::Signal< dynamicgraph::Vector, int > robotStateSOUT_
base 6d pose + joints' angles measured by encoders
dynamicgraph::Vector wrenches_[4]
joints' accelerations
dynamicgraph::Signal< dynamicgraph::Vector, int > jointsAccelerationsSOUT_
joints' accelerations computed by the integrator
virtual void setVelocity(const dynamicgraph::Vector &vel)
virtual void setStateSize(const unsigned int &size)
dynamicgraph::Signal< dynamicgraph::Vector, int > currentSOUT_
motor currents
tsid::robots::RobotWrapper * m_robot
robot geometric/inertial data
dynamicgraph::Signal< dynamicgraph::Vector, int > gyrometerSOUT_
Rotation velocity measured by gyrometers.
dynamicgraph::Signal< dynamicgraph::Vector, int > p_gainsSOUT_
proportional and derivative position-control gains
virtual void setState(const dynamicgraph::Vector &st)
dynamicgraph::SignalPtr< dynamicgraph::Vector, int > * forcesSIN_[4]
input force sensor values
virtual void init(const double &dt, const std::string &urdfFile)
dynamicgraph::Signal< dynamicgraph::Vector, int > d_gainsSOUT_
Vector m_dv_c
Cholesky decomposition of _ZMZ.
dynamicgraph::Signal< dynamicgraph::Vector, int > accelerometerSOUT_
Accelerations measured by accelerometers.
dynamicgraph::Signal< dynamicgraph::Vector, int > jointsVelocitiesSOUT_
joints' velocities computed by the integrator
Eigen::MatrixXd m_K
cholesky decomposition of the K matrix
dynamicgraph::Vector jointsAccelerations_
joints' velocities
DYNAMICGRAPH_FACTORY_ENTITY_PLUGIN(DeviceTorqueCtrl, "DeviceTorqueCtrl")
tsid::math::ConstraintBase ConstraintBase
to read text file
Definition: treeview.dox:22