sot-torque-control  1.6.2
Collection of dynamic-graph entities aimed at implementing torque control on different robots.
simple-inverse-dyn.cpp
Go to the documentation of this file.
1 /*
2  * Copyright 2017, Andrea Del Prete, LAAS-CNRS
3  *
4  * This file is part of sot-torque-control.
5  * sot-torque-control is free software: you can redistribute it and/or
6  * modify it under the terms of the GNU Lesser General Public License
7  * as published by the Free Software Foundation, either version 3 of
8  * the License, or (at your option) any later version.
9  * sot-torque-control is distributed in the hope that it will be
10  * useful, but WITHOUT ANY WARRANTY; without even the implied warranty
11  * of MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
12  * GNU Lesser General Public License for more details. You should
13  * have received a copy of the GNU Lesser General Public License along
14  * with sot-torque-control. If not, see <http://www.gnu.org/licenses/>.
15  */
16 
17 #include <boost/test/unit_test.hpp>
18 
19 #include <tsid/utils/stop-watch.hpp>
20 #include <tsid/utils/statistics.hpp>
21 #include <tsid/solvers/solver-HQP-factory.hxx>
22 #include <tsid/solvers/utils.hpp>
23 #include <tsid/math/utils.hpp>
24 
25 #include <dynamic-graph/factory.h>
26 
27 #include <sot/core/debug.hh>
28 
31 
32 #if DEBUG
33 #define ODEBUG(x) std::cout << x << std::endl
34 #else
35 #define ODEBUG(x)
36 #endif
37 #define ODEBUG3(x) std::cout << x << std::endl
38 
39 #define DBGFILE "/tmp/debug-sot-torque-control.dat"
40 
41 #define RESETDEBUG5() { std::ofstream DebugFile; \
42  DebugFile.open(DBGFILE,std::ofstream::out); \
43  DebugFile.close();}
44 #define ODEBUG5FULL(x) { std::ofstream DebugFile; \
45  DebugFile.open(DBGFILE,std::ofstream::app); \
46  DebugFile << __FILE__ << ":" \
47  << __FUNCTION__ << "(#" \
48  << __LINE__ << "):" << x << std::endl; \
49  DebugFile.close();}
50 #define ODEBUG5(x) { std::ofstream DebugFile; \
51  DebugFile.open(DBGFILE,std::ofstream::app); \
52  DebugFile << x << std::endl; \
53  DebugFile.close();}
54 
55 #define RESETDEBUG4()
56 #define ODEBUG4FULL(x)
57 #define ODEBUG4(x)
58 
59 namespace dynamicgraph {
60 namespace sot {
61 namespace torque_control {
62 namespace dg = ::dynamicgraph;
63 using namespace dg;
64 using namespace dg::command;
65 using namespace std;
66 using namespace tsid;
67 using namespace tsid::trajectories;
68 using namespace tsid::math;
69 using namespace tsid::contacts;
70 using namespace tsid::tasks;
71 using namespace tsid::solvers;
72 using namespace dg::sot;
73 
74 #define REQUIRE_FINITE(A) assert(is_finite(A))
75 
76 //Size to be aligned "-------------------------------------------------------"
77 #define PROFILE_TAU_DES_COMPUTATION "SimpleInverseDyn: desired tau"
78 #define PROFILE_HQP_SOLUTION "SimpleInverseDyn: HQP"
79 #define PROFILE_PREPARE_INV_DYN "SimpleInverseDyn: prepare inv-dyn"
80 #define PROFILE_READ_INPUT_SIGNALS "SimpleInverseDyn: read input signals"
81 
82 #define ZERO_FORCE_THRESHOLD 1e-3
83 
84 #define INPUT_SIGNALS m_posture_ref_posSIN \
85  << m_posture_ref_velSIN \
86  << m_posture_ref_accSIN \
87  << m_kp_postureSIN \
88  << m_kd_postureSIN \
89  << m_w_postureSIN \
90  << m_kp_posSIN \
91  << m_kd_posSIN \
92  << m_com_ref_posSIN \
93  << m_com_ref_velSIN \
94  << m_com_ref_accSIN \
95  << m_kp_comSIN \
96  << m_kd_comSIN \
97  << m_w_comSIN \
98  << m_kp_contactSIN \
99  << m_kd_contactSIN \
100  << m_w_forcesSIN \
101  << m_muSIN \
102  << m_contact_pointsSIN \
103  << m_contact_normalSIN \
104  << m_f_minSIN \
105  << m_f_maxSIN \
106  << m_waist_ref_posSIN \
107  << m_waist_ref_velSIN \
108  << m_waist_ref_accSIN \
109  << m_kp_waistSIN \
110  << m_kd_waistSIN \
111  << m_w_waistSIN \
112  << m_qSIN \
113  << m_vSIN \
114  << m_active_jointsSIN
115 
116 #define OUTPUT_SIGNALS m_tau_desSOUT << m_dv_desSOUT << m_v_desSOUT << m_q_desSOUT << m_uSOUT
117 
118 
121 typedef SimpleInverseDyn EntityClassName;
122 
123 typedef Eigen::Matrix<double, 2, 1> Vector2;
124 typedef Eigen::Matrix<double, Eigen::Dynamic, 1> VectorN;
125 typedef Eigen::Matrix<double, Eigen::Dynamic, 1> VectorN6;
126 /* --- DG FACTORY ---------------------------------------------------- */
128  "SimpleInverseDyn");
129 
130 /* ------------------------------------------------------------------- */
131 /* --- CONSTRUCTION -------------------------------------------------- */
132 /* ------------------------------------------------------------------- */
134 SimpleInverseDyn(const std::string& name)
135  : Entity(name)
136 
137  , CONSTRUCT_SIGNAL_IN(posture_ref_pos, dynamicgraph::Vector)
138  , CONSTRUCT_SIGNAL_IN(posture_ref_vel, dynamicgraph::Vector)
139  , CONSTRUCT_SIGNAL_IN(posture_ref_acc, dynamicgraph::Vector)
140  , CONSTRUCT_SIGNAL_IN(kp_posture, dynamicgraph::Vector)
141  , CONSTRUCT_SIGNAL_IN(kd_posture, dynamicgraph::Vector)
142  , CONSTRUCT_SIGNAL_IN(w_posture, double)
143  , CONSTRUCT_SIGNAL_IN(kp_pos, dynamicgraph::Vector)
144  , CONSTRUCT_SIGNAL_IN(kd_pos, dynamicgraph::Vector)
145  , CONSTRUCT_SIGNAL_IN(com_ref_pos, dynamicgraph::Vector)
146  , CONSTRUCT_SIGNAL_IN(com_ref_vel, dynamicgraph::Vector)
147  , CONSTRUCT_SIGNAL_IN(com_ref_acc, dynamicgraph::Vector)
148  , CONSTRUCT_SIGNAL_IN(kp_com, dynamicgraph::Vector)
149  , CONSTRUCT_SIGNAL_IN(kd_com, dynamicgraph::Vector)
150  , CONSTRUCT_SIGNAL_IN(w_com, double)
151  , CONSTRUCT_SIGNAL_IN(kp_contact, dynamicgraph::Vector)
152  , CONSTRUCT_SIGNAL_IN(kd_contact, dynamicgraph::Vector)
153  , CONSTRUCT_SIGNAL_IN(w_forces, double)
154  , CONSTRUCT_SIGNAL_IN(mu, double)
155  , CONSTRUCT_SIGNAL_IN(contact_points, dynamicgraph::Matrix)
156  , CONSTRUCT_SIGNAL_IN(contact_normal, dynamicgraph::Vector)
157  , CONSTRUCT_SIGNAL_IN(f_min, double)
158  , CONSTRUCT_SIGNAL_IN(f_max, double)
159  , CONSTRUCT_SIGNAL_IN(waist_ref_pos, dynamicgraph::Vector)
160  , CONSTRUCT_SIGNAL_IN(waist_ref_vel, dynamicgraph::Vector)
161  , CONSTRUCT_SIGNAL_IN(waist_ref_acc, dynamicgraph::Vector)
162  , CONSTRUCT_SIGNAL_IN(kp_waist, dynamicgraph::Vector)
163  , CONSTRUCT_SIGNAL_IN(kd_waist, dynamicgraph::Vector)
164  , CONSTRUCT_SIGNAL_IN(w_waist, double)
165  , CONSTRUCT_SIGNAL_IN(q, dynamicgraph::Vector)
166  , CONSTRUCT_SIGNAL_IN(v, dynamicgraph::Vector)
167  , CONSTRUCT_SIGNAL_IN(active_joints, dynamicgraph::Vector)
168  , CONSTRUCT_SIGNAL_INNER(active_joints_checked, dg::Vector, m_active_jointsSIN)
169  , CONSTRUCT_SIGNAL_OUT(tau_des, dynamicgraph::Vector, INPUT_SIGNALS)
170  , CONSTRUCT_SIGNAL_OUT(dv_des, dg::Vector, m_tau_desSOUT)
171  , CONSTRUCT_SIGNAL_OUT(v_des, dg::Vector, m_dv_desSOUT)
172  , CONSTRUCT_SIGNAL_OUT(q_des, dg::Vector, m_v_desSOUT)
173  , CONSTRUCT_SIGNAL_OUT(u, dg::Vector, INPUT_SIGNALS << m_tau_desSOUT << m_v_desSOUT << m_q_desSOUT)
174  , m_t(0.0)
175  , m_initSucceeded(false)
176  , m_enabled(false)
177  , m_firstTime(true)
178  , m_timeLast(0)
179  , m_robot_util(RefVoidRobotUtil())
180  , m_ctrlMode(CONTROL_OUTPUT_VELOCITY){
181  RESETDEBUG5();
182  Entity::signalRegistration( INPUT_SIGNALS << OUTPUT_SIGNALS );
183 
184  m_com_offset.setZero();
185 
186  /* Commands. */
187  addCommand("init",
188  makeCommandVoid2(*this, &SimpleInverseDyn::init,
189  docCommandVoid2("Initialize the entity.",
190  "Time period in seconds (double)",
191  "Robot reference (string)")));
192  /* SET of control output type. */
193  addCommand("setControlOutputType",
194  makeCommandVoid1(*this, &SimpleInverseDyn::setControlOutputType,
195  docCommandVoid1("Set the type of control output.",
196  "Control type: velocity or torque (string)")));
197 
198  addCommand("updateComOffset",
199  makeCommandVoid0(*this, &SimpleInverseDyn::updateComOffset,
200  docCommandVoid0("Update the offset on the CoM based on the CoP measurement.")));
201 
202 }
203 
205  const Vector3 & com = m_robot->com(m_invDyn->data());
206  m_com_offset = com;
207  SEND_MSG("CoM offset updated: " + toString(m_com_offset), MSG_TYPE_INFO);
208 }
209 
210 void SimpleInverseDyn::setControlOutputType(const std::string& type) {
211  for (int i = 0; i < CONTROL_OUTPUT_SIZE; i++)
212  if (type == ControlOutput_s[i]) {
214  sotDEBUG(25) << "Control output type: " << ControlOutput_s[i] << endl;
215  return;
216  }
217  sotDEBUG(25) << "Unrecognized control output type: " << type << endl;
218 }
219 
220 void SimpleInverseDyn::init(const double& dt, const std::string& robotRef) {
221  if (dt <= 0.0)
222  return SEND_MSG("Init failed: Timestep must be positive", MSG_TYPE_ERROR);
223 
224  /* Retrieve m_robot_util informations */
225  std::string localName(robotRef);
226  if (isNameInRobotUtil(localName))
227  m_robot_util = getRobotUtil(localName);
228  else {
229  SEND_MSG("You should have an entity controller manager initialized before", MSG_TYPE_ERROR);
230  return;
231  }
232 
233  const Eigen::Matrix<double, 3, 4>& contactPoints = m_contact_pointsSIN(0);
234  const Eigen::Vector3d& contactNormal = m_contact_normalSIN(0);
235  const dg::sot::Vector6d& kp_contact = m_kp_contactSIN(0);
236  const dg::sot::Vector6d& kd_contact = m_kd_contactSIN(0);
237  const Eigen::Vector3d& kp_com = m_kp_comSIN(0);
238  const Eigen::Vector3d& kd_com = m_kd_comSIN(0);
239  const VectorN& kp_posture = m_kp_postureSIN(0);
240  const VectorN& kd_posture = m_kd_postureSIN(0);
241  const dg::sot::Vector6d& kp_waist = m_kp_waistSIN(0);
242  const dg::sot::Vector6d& kd_waist = m_kd_waistSIN(0);
243 
244  assert(kp_posture.size() == static_cast<Eigen::VectorXd::Index>(m_robot_util->m_nbJoints));
245  assert(kd_posture.size() == static_cast<Eigen::VectorXd::Index>(m_robot_util->m_nbJoints));
246 
247  m_w_com = m_w_comSIN(0);
248  m_w_posture = m_w_postureSIN(0);
249  m_w_waist = m_w_waistSIN(0);
250  const double & w_forces = m_w_forcesSIN(0);
251  const double & mu = m_muSIN(0);
252  const double & fMin = m_f_minSIN(0);
253  const double & fMax = m_f_maxSIN(0);
254 
255  try {
256  vector<string> package_dirs;
257  m_robot = new robots::RobotWrapper(m_robot_util->m_urdf_filename,
258  package_dirs,
259  pinocchio::JointModelFreeFlyer());
260 
261  assert(m_robot->nv() >= 6);
262  m_robot_util->m_nbJoints = m_robot->nv() - 6;
263 
264  m_q_sot.setZero(m_robot->nv());
265  m_v_sot.setZero(m_robot->nv());
266  m_dv_sot.setZero(m_robot->nv());
267  m_tau_sot.setZero(m_robot->nv() - 6);
268  // m_f.setZero(24);
269  m_q_urdf.setZero(m_robot->nq());
270  m_v_urdf.setZero(m_robot->nv());
271  m_dv_urdf.setZero(m_robot->nv());
272 
273  m_invDyn = new InverseDynamicsFormulationAccForce("invdyn", *m_robot);
274 
275  // CONTACT 6D TASKS
276  m_contactRF = new Contact6d("contact_rfoot", *m_robot,
277  m_robot_util->m_foot_util.m_Right_Foot_Frame_Name,
278  contactPoints, contactNormal,
279  mu, fMin, fMax);
280  m_contactRF->Kp(kp_contact);
281  m_contactRF->Kd(kd_contact);
282  m_invDyn->addRigidContact(*m_contactRF, w_forces);
283 
284  m_contactLF = new Contact6d("contact_lfoot", *m_robot,
285  m_robot_util->m_foot_util.m_Left_Foot_Frame_Name,
286  contactPoints, contactNormal,
287  mu, fMin, fMax);
288  m_contactLF->Kp(kp_contact);
289  m_contactLF->Kd(kd_contact);
290  m_invDyn->addRigidContact(*m_contactLF, w_forces);
291 
292  // TASK COM
293  m_taskCom = new TaskComEquality("task-com", *m_robot);
294  m_taskCom->Kp(kp_com);
295  m_taskCom->Kd(kd_com);
296  m_invDyn->addMotionTask(*m_taskCom, m_w_com, 0);
297 
298  // WAIST Task
299  m_taskWaist = new TaskSE3Equality("task-waist", *m_robot, "root_joint");
300  m_taskWaist->Kp(kp_waist);
301  m_taskWaist->Kd(kd_waist);
302  // Add a Mask to the task which will select the vector dimensions on which the task will act.
303  // In this case the waist configuration is a vector 6d (position and orientation -> SE3)
304  // Here we set a mask = [0 0 0 1 1 1] so the task on the waist will act on the orientation of the robot
305  Eigen::VectorXd mask_orientation(6);
306  mask_orientation << 0, 0, 0, 1, 1, 1;
307  m_taskWaist->setMask(mask_orientation);
308  // Add the task to the HQP with weight = 1.0, priority level = 1 (in the cost function) and a transition duration = 0.0
309  m_invDyn->addMotionTask(*m_taskWaist, m_w_waist, 1);
310 
311  // POSTURE TASK
312  m_taskPosture = new TaskJointPosture("task-posture", *m_robot);
313  m_taskPosture->Kp(kp_posture);
314  m_taskPosture->Kd(kd_posture);
315  m_invDyn->addMotionTask(*m_taskPosture, m_w_posture, 1);
316 
317  // TRAJECTORY INIT
318  m_sampleCom = TrajectorySample(3);
319  m_sampleWaist = TrajectorySample(6);
320  m_samplePosture = TrajectorySample(m_robot->nv() - 6);
321 
322  m_hqpSolver = SolverHQPFactory::createNewSolver(SOLVER_HQP_EIQUADPROG_FAST,
323  "eiquadprog-fast");
324  m_hqpSolver->resize(m_invDyn->nVar(), m_invDyn->nEq(), m_invDyn->nIn());
325 
326  } catch (const std::exception& e) {
327  std::cout << e.what();
328  return SEND_MSG("Init failed: Could load URDF :" + m_robot_util->m_urdf_filename, MSG_TYPE_ERROR);
329  }
330  m_dt = dt;
331  m_initSucceeded = true;
332 }
333 
334 /* ------------------------------------------------------------------- */
335 /* --- SIGNALS ------------------------------------------------------- */
336 /* ------------------------------------------------------------------- */
338 DEFINE_SIGNAL_INNER_FUNCTION(active_joints_checked, dynamicgraph::Vector) {
339  if (s.size() != static_cast<Eigen::VectorXd::Index>(m_robot_util->m_nbJoints))
340  s.resize(m_robot_util->m_nbJoints);
341 
342  const Eigen::VectorXd& active_joints_sot = m_active_jointsSIN(iter);
343  if (m_enabled == false) {
344  if (active_joints_sot.any()) {
345  /* from all OFF to some ON */
346  m_enabled = true ;
347  s = active_joints_sot;
348  Eigen::VectorXd active_joints_urdf(m_robot_util->m_nbJoints);
349  m_robot_util->joints_sot_to_urdf(active_joints_sot, active_joints_urdf);
350 
351  m_taskBlockedJoints = new TaskJointPosture("task-posture-blocked", *m_robot);
352  Eigen::VectorXd blocked_joints(m_robot_util->m_nbJoints);
353  for (unsigned int i = 0; i < m_robot_util->m_nbJoints; i++)
354  if (active_joints_urdf(i) == 0.0)
355  blocked_joints(i) = 1.0;
356  else
357  blocked_joints(i) = 0.0;
358  SEND_MSG("Blocked joints: " + toString(blocked_joints.transpose()), MSG_TYPE_INFO);
359  m_taskBlockedJoints->mask(blocked_joints);
360  TrajectorySample ref_zero(static_cast<unsigned int>(m_robot_util->m_nbJoints));
361  m_taskBlockedJoints->setReference(ref_zero);
362  m_invDyn->addMotionTask(*m_taskBlockedJoints, 1.0, 0);
363  }
364  } else if (!active_joints_sot.any()) {
365  /* from some ON to all OFF */
366  m_enabled = false ;
367  }
368  if (m_enabled == false)
369  for (unsigned int i = 0; i < m_robot_util->m_nbJoints; i++)
370  s(i) = false;
371  return s;
372 }
373 
374 DEFINE_SIGNAL_OUT_FUNCTION(tau_des, dynamicgraph::Vector) {
375  if (!m_initSucceeded) {
376  SEND_WARNING_STREAM_MSG("Cannot compute signal tau_des before initialization!");
377  return s;
378  }
379  if (s.size() != static_cast<Eigen::VectorXd::Index>(m_robot_util->m_nbJoints))
380  s.resize(m_robot_util->m_nbJoints);
381 
382  getProfiler().start(PROFILE_TAU_DES_COMPUTATION);
383 
384  getProfiler().start(PROFILE_READ_INPUT_SIGNALS);
385 
386  m_active_joints_checkedSINNER(iter);
387 
388  const VectorN6& q_robot = m_qSIN(iter);
389  assert(q_robot.size() == static_cast<Eigen::VectorXd::Index>(m_robot_util->m_nbJoints + 6));
390  const VectorN6& v_robot = m_vSIN(iter);
391  assert(v_robot.size() == static_cast<Eigen::VectorXd::Index>(m_robot_util->m_nbJoints + 6));
392 
393  const Vector3& x_com_ref = m_com_ref_posSIN(iter);
394  const Vector3& dx_com_ref = m_com_ref_velSIN(iter);
395  const Vector3& ddx_com_ref = m_com_ref_accSIN(iter);
396  const VectorN& x_waist_ref = m_waist_ref_posSIN(iter);
397  const Vector6& dx_waist_ref = m_waist_ref_velSIN(iter);
398  const Vector6& ddx_waist_ref = m_waist_ref_accSIN(iter);
399  const VectorN& q_ref = m_posture_ref_posSIN(iter);
400  assert(q_ref.size() == static_cast<Eigen::VectorXd::Index>(m_robot_util->m_nbJoints));
401  const VectorN& dq_ref = m_posture_ref_velSIN(iter);
402  assert(dq_ref.size() == static_cast<Eigen::VectorXd::Index>(m_robot_util->m_nbJoints));
403  const VectorN& ddq_ref = m_posture_ref_accSIN(iter);
404  assert(ddq_ref.size() == static_cast<Eigen::VectorXd::Index>(m_robot_util->m_nbJoints));
405 
406  const Vector6& kp_contact = m_kp_contactSIN(iter);
407  const Vector6& kd_contact = m_kd_contactSIN(iter);
408  const Vector3& kp_com = m_kp_comSIN(iter);
409  const Vector3& kd_com = m_kd_comSIN(iter);
410  const Vector6& kp_waist = m_kp_waistSIN(iter);
411  const Vector6& kd_waist = m_kd_waistSIN(iter);
412 
413  const VectorN& kp_posture = m_kp_postureSIN(iter);
414  assert(kp_posture.size() == static_cast<Eigen::VectorXd::Index>(m_robot_util->m_nbJoints));
415  const VectorN& kd_posture = m_kd_postureSIN(iter);
416  assert(kd_posture.size() == static_cast<Eigen::VectorXd::Index>(m_robot_util->m_nbJoints));
417 
418  /*const double & w_hands = m_w_handsSIN(iter);*/
419  const double & w_com = m_w_comSIN(iter);
420  const double & w_posture = m_w_postureSIN(iter);
421  const double & w_forces = m_w_forcesSIN(iter);
422  const double & w_waist = m_w_waistSIN(iter);
423 
424  getProfiler().stop(PROFILE_READ_INPUT_SIGNALS);
425 
426  getProfiler().start(PROFILE_PREPARE_INV_DYN);
427 
428  m_sampleCom.pos = x_com_ref - m_com_offset;
429  m_sampleCom.vel = dx_com_ref;
430  m_sampleCom.acc = ddx_com_ref;
431  m_taskCom->setReference(m_sampleCom);
432  m_taskCom->Kp(kp_com);
433  m_taskCom->Kd(kd_com);
434  if (m_w_com != w_com) {
435  m_w_com = w_com;
436  m_invDyn->updateTaskWeight(m_taskCom->name(), w_com);
437  }
438 
439  m_sampleWaist.pos = x_waist_ref;
440  m_sampleWaist.vel = dx_waist_ref;
441  m_sampleWaist.acc = ddx_waist_ref;
442  m_taskWaist->setReference(m_sampleWaist);
443  m_taskWaist->Kp(kp_waist);
444  m_taskWaist->Kd(kd_waist);
445  if (m_w_waist != w_waist) {
446  m_w_waist = w_waist;
447  m_invDyn->updateTaskWeight(m_taskWaist->name(), w_waist);
448  }
449 
450  m_robot_util->joints_sot_to_urdf(q_ref, m_samplePosture.pos);
451  m_robot_util->joints_sot_to_urdf(dq_ref, m_samplePosture.vel);
452  m_robot_util->joints_sot_to_urdf(ddq_ref, m_samplePosture.acc);
453  m_taskPosture->setReference(m_samplePosture);
454  m_taskPosture->Kp(kp_posture);
455  m_taskPosture->Kd(kd_posture);
456  if (m_w_posture != w_posture) {
457  m_w_posture = w_posture;
458  m_invDyn->updateTaskWeight(m_taskPosture->name(), w_posture);
459  }
460 
461  const double & fMin = m_f_minSIN(0);
462  const double & fMax = m_f_maxSIN(iter);
463  m_contactLF->setMinNormalForce(fMin);
464  m_contactRF->setMinNormalForce(fMin);
465  m_contactLF->setMaxNormalForce(fMax);
466  m_contactRF->setMaxNormalForce(fMax);
467  m_contactLF->Kp(kp_contact);
468  m_contactLF->Kd(kd_contact);
469  m_contactRF->Kp(kp_contact);
470  m_contactRF->Kd(kd_contact);
471  m_invDyn->updateRigidContactWeights(m_contactLF->name(), w_forces);
472  m_invDyn->updateRigidContactWeights(m_contactRF->name(), w_forces);
473 
474  if (m_firstTime) {
475  m_firstTime = false;
476  m_robot_util->config_sot_to_urdf(q_robot, m_q_urdf);
477  m_robot_util->velocity_sot_to_urdf(m_q_urdf, v_robot, m_v_urdf);
478 
479  m_invDyn->computeProblemData(m_t, m_q_urdf, m_v_urdf);
480  // m_robot->computeAllTerms(m_invDyn->data(), q, v);
481  pinocchio::SE3 H_lf = m_robot->position(m_invDyn->data(),
482  m_robot->model().getJointId(m_robot_util->m_foot_util.m_Left_Foot_Frame_Name));
483  m_contactLF->setReference(H_lf);
484  SEND_MSG("Setting left foot reference to " + toString(H_lf), MSG_TYPE_DEBUG);
485 
486  pinocchio::SE3 H_rf = m_robot->position(m_invDyn->data(),
487  m_robot->model().getJointId(m_robot_util->m_foot_util.m_Right_Foot_Frame_Name));
488  m_contactRF->setReference(H_rf);
489  SEND_MSG("Setting right foot reference to " + toString(H_rf), MSG_TYPE_DEBUG);
490  } else if (m_timeLast != static_cast<unsigned int>(iter - 1)) {
491  SEND_MSG("Last time " + toString(m_timeLast) + " is not current time-1: " + toString(iter), MSG_TYPE_ERROR);
492  if (m_timeLast == static_cast<unsigned int>(iter)) {
493  s = m_tau_sot;
494  return s;
495  }
496  }
497  // else if (m_ctrlMode == CONTROL_OUTPUT_TORQUE){
498  // // In velocity close the TSID loop on itself (v_des, q_des), in torque on the (q,v) of the robot.
499  // m_robot_util->config_sot_to_urdf(q_robot, m_q_urdf);
500  // m_robot_util->velocity_sot_to_urdf(m_q_urdf, v_robot, m_v_urdf);
501  // }
502  m_timeLast = static_cast<unsigned int>(iter);
503 
504  const HQPData & hqpData = m_invDyn->computeProblemData(m_t, m_q_urdf, m_v_urdf);
505 
506  getProfiler().stop(PROFILE_PREPARE_INV_DYN);
507 
508  getProfiler().start(PROFILE_HQP_SOLUTION);
509  SolverHQPBase * solver = m_hqpSolver;
510  getStatistics().store("solver dynamic size", 1.0);
511 
512  const HQPOutput & sol = solver->solve(hqpData);
513  getProfiler().stop(PROFILE_HQP_SOLUTION);
514 
515  if (sol.status != HQP_STATUS_OPTIMAL) {
516  SEND_ERROR_STREAM_MSG("HQP solver failed to find a solution: " + toString(sol.status));
517  SEND_DEBUG_STREAM_MSG(tsid::solvers::HQPDataToString(hqpData, false));
518  SEND_DEBUG_STREAM_MSG("q=" + toString(q_robot.transpose(), 1, 5));
519  SEND_DEBUG_STREAM_MSG("v=" + toString(v_robot.transpose(), 1, 5));
520  s.setZero();
521  return s;
522  }
523 
524  getStatistics().store("active inequalities", static_cast<double>(sol.activeSet.size()));
525  getStatistics().store("solver iterations", sol.iterations);
526 
527  m_dv_urdf = m_invDyn->getAccelerations(sol);
528  m_robot_util->velocity_urdf_to_sot(m_q_urdf, m_dv_urdf, m_dv_sot);
529  m_robot_util->joints_urdf_to_sot(m_invDyn->getActuatorForces(sol), m_tau_sot);
530 
531  getProfiler().stop(PROFILE_TAU_DES_COMPUTATION);
532  m_t += m_dt;
533 
534  s = m_tau_sot;
535 
536  return s;
537 }
538 
539 DEFINE_SIGNAL_OUT_FUNCTION(dv_des, dynamicgraph::Vector) {
540  if (!m_initSucceeded) {
541  SEND_WARNING_STREAM_MSG("Cannot compute signal dv_des before initialization!");
542  return s;
543  }
544  if (s.size() != m_robot->nv())
545  s.resize(m_robot->nv());
546  m_tau_desSOUT(iter);
547  s = m_dv_sot;
548  return s;
549 }
550 
551 DEFINE_SIGNAL_OUT_FUNCTION(v_des, dynamicgraph::Vector) {
552  if (!m_initSucceeded) {
553  SEND_WARNING_STREAM_MSG("Cannot compute signal v_des before initialization!");
554  return s;
555  }
556  if (s.size() != m_robot->nv())
557  s.resize(m_robot->nv());
558  m_dv_desSOUT(iter);
559  tsid::math::Vector v_mean;
560  v_mean = m_v_urdf + 0.5 * m_dt * m_dv_urdf;
561  m_v_urdf = m_v_urdf + m_dt * m_dv_urdf;
562  m_q_urdf = pinocchio::integrate(m_robot->model(), m_q_urdf, m_dt * v_mean);
563  m_robot_util->velocity_urdf_to_sot(m_q_urdf, m_v_urdf, m_v_sot);
564  s = m_v_sot;
565  return s;
566 }
567 
568 DEFINE_SIGNAL_OUT_FUNCTION(q_des, dynamicgraph::Vector) {
569  if (!m_initSucceeded) {
570  SEND_WARNING_STREAM_MSG("Cannot compute signal q_des before initialization!");
571  return s;
572  }
573  if (s.size() != m_robot->nv())
574  s.resize(m_robot->nv());
575  m_v_desSOUT(iter);
576  m_robot_util->config_urdf_to_sot(m_q_urdf, m_q_sot);
577  s = m_q_sot;
578  return s;
579 }
580 
581 DEFINE_SIGNAL_OUT_FUNCTION(u, dynamicgraph::Vector) {
582  if (!m_initSucceeded) {
583  SEND_WARNING_STREAM_MSG("Cannot compute signal u before initialization!");
584  return s;
585  }
586  if (s.size() != static_cast<Eigen::VectorXd::Index>(m_robot_util->m_nbJoints))
587  s.resize(m_robot_util->m_nbJoints);
588 
589  const VectorN& kp_pos = m_kp_posSIN(iter);
590  assert(kp_pos.size() == static_cast<Eigen::VectorXd::Index>(m_robot_util->m_nbJoints));
591  const VectorN& kd_pos = m_kd_posSIN(iter);
592  assert(kd_pos.size() == static_cast<Eigen::VectorXd::Index>(m_robot_util->m_nbJoints));
593 
594  const VectorN6& q_robot = m_qSIN(iter);
595  assert(q_robot.size() == static_cast<Eigen::VectorXd::Index>(m_robot_util->m_nbJoints + 6));
596  const VectorN6& v_robot = m_vSIN(iter);
597  assert(v_robot.size() == static_cast<Eigen::VectorXd::Index>(m_robot_util->m_nbJoints + 6));
598 
599  m_q_desSOUT(iter);
600 
601  s = m_tau_sot + kp_pos.cwiseProduct(m_q_sot.tail(m_robot_util->m_nbJoints) - q_robot.tail(m_robot_util->m_nbJoints)) +
602  kd_pos.cwiseProduct(m_v_sot.tail(m_robot_util->m_nbJoints) - v_robot.tail(m_robot_util->m_nbJoints));
603 
604  return s;
605 }
606 
607 /* --- COMMANDS ---------------------------------------------------------- */
608 
609 /* ------------------------------------------------------------------- */
610 /* --- ENTITY -------------------------------------------------------- */
611 /* ------------------------------------------------------------------- */
612 
613 void SimpleInverseDyn::display(std::ostream& os) const {
614  os << "SimpleInverseDyn " << getName();
615  try {
616  getProfiler().report_all(3, os);
617  getStatistics().report_all(1, os);
618  os << "QP size: nVar " << m_invDyn->nVar() << " nEq " << m_invDyn->nEq() << " nIn " << m_invDyn->nIn() << "\n";
619  } catch (ExceptionSignal e) {}
620 }
621 } // namespace torquecontrol
622 } // namespace sot
623 } // namespace dynamicgraph
double m_w_com
Weights of the Tasks (which can be changed)
tsid::solvers::SolverHQPBase * m_hqpSolver
Solver and problem formulation.
RobotUtilShrPtr m_robot_util
Final time of the control loop.
tsid::math::Vector m_dv_sot
Computed solutions (accelerations and torques) and their derivatives.
EIGEN_MAKE_ALIGNED_OPERATOR_NEW SimpleInverseDyn(const std::string &name)
tsid::InverseDynamicsFormulationAccForce * m_invDyn
tsid::math::Vector m_v_urdf
desired velocities (sot order)
tsid::robots::RobotWrapper * m_robot
True at the first iteration of the controller.
tsid::math::Vector m_tau_sot
desired and current positions (urdf order) (close the TSID loop on it)
tsid::math::Vector m_q_urdf
desired positions (sot order)
tsid::math::Vector m_q_sot
desired and current velocities (urdf order) (close the TSID loop on it)
tsid::math::Vector m_v_sot
desired accelerations (urdf order)
tsid::trajectories::TrajectorySample m_samplePosture
tsid::trajectories::TrajectorySample m_sampleCom
Trajectories of the tasks.
tsid::math::Vector m_dv_urdf
desired accelerations (sot order)
tsid::math::Vector3 m_com_offset
desired torques (sot order)
ControlOutput m_ctrlMode
Share pointer to the robot utils methods.
void init(const double &dt, const std::string &robotRef)
tsid::trajectories::TrajectorySample m_sampleWaist
virtual void setControlOutputType(const std::string &type)
DEFINE_SIGNAL_OUT_FUNCTION(u, dynamicgraph::Vector)
Eigen::Matrix< double, 3, 1 > Vector3
DEFINE_SIGNAL_INNER_FUNCTION(kinematics_computations, dynamicgraph::Vector)
Eigen::Matrix< double, Eigen::Dynamic, 1 > VectorN
Eigen::Matrix< double, 6, 1 > Vector6
Eigen::Matrix< double, Eigen::Dynamic, 1 > VectorN6
DYNAMICGRAPH_FACTORY_ENTITY_PLUGIN(AdmittanceController, "AdmittanceController")
to read text file
Definition: treeview.dox:22
#define PROFILE_TAU_DES_COMPUTATION
#define PROFILE_HQP_SOLUTION
#define RESETDEBUG5()
#define INPUT_SIGNALS
#define PROFILE_PREPARE_INV_DYN
#define OUTPUT_SIGNALS
#define PROFILE_READ_INPUT_SIGNALS