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