sot-talos-balance  1.6.0
distribute-wrench.cpp
Go to the documentation of this file.
1 /*
2  * Copyright 2018, Gepetto team, LAAS-CNRS
3  *
4  * This file is part of sot-talos-balance.
5  * sot-talos-balance 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-talos-balance 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-talos-balance. If not, see <http://www.gnu.org/licenses/>.
15  */
16 
18 
19 #include <iostream>
20 
21 #include <sot/core/debug.hh>
22 #include <dynamic-graph/factory.h>
23 #include <dynamic-graph/all-commands.h>
24 
25 #include <sot/core/stop-watch.hh>
26 
27 #include <pinocchio/parsers/urdf.hpp>
28 #include <pinocchio/algorithm/kinematics.hpp>
29 #include <pinocchio/algorithm/frames.hpp>
30 
31 namespace dynamicgraph
32 {
33  namespace sot
34  {
35  namespace talos_balance
36  {
37  namespace dg = ::dynamicgraph;
38  using namespace dg;
39  using namespace dg::command;
40 
41 //Size to be aligned "-------------------------------------------------------"
42 #define PROFILE_DISTRIBUTE_WRENCH_KINEMATICS_COMPUTATIONS "DistributeWrench: kinematics computations "
43 #define PROFILE_DISTRIBUTE_WRENCH_QP_COMPUTATIONS "DistributeWrench: QP problem computations "
44 
45 #define WEIGHT_SIGNALS m_wSumSIN << m_wNormSIN << m_wRatioSIN << m_wAnkleSIN
46 
47 #define INPUT_SIGNALS m_wrenchDesSIN << m_qSIN << m_rhoSIN << m_phaseSIN << m_frictionCoefficientSIN << WEIGHT_SIGNALS
48 
49 #define INNER_SIGNALS m_kinematics_computations << m_qp_computations
50 
51 #define OUTPUT_SIGNALS m_wrenchLeftSOUT << m_ankleWrenchLeftSOUT << m_surfaceWrenchLeftSOUT << m_copLeftSOUT << m_wrenchRightSOUT << m_ankleWrenchRightSOUT << m_surfaceWrenchRightSOUT << m_copRightSOUT << m_wrenchRefSOUT << m_zmpRefSOUT << m_emergencyStopSOUT
52 
55  typedef DistributeWrench EntityClassName;
56 
57  /* --- DG FACTORY ---------------------------------------------------- */
58  DYNAMICGRAPH_FACTORY_ENTITY_PLUGIN(DistributeWrench,
59  "DistributeWrench");
60 
61  /* ------------------------------------------------------------------- */
62  /* --- CONSTRUCTION -------------------------------------------------- */
63  /* ------------------------------------------------------------------- */
64  DistributeWrench::DistributeWrench(const std::string& name)
65  : Entity(name)
66  , CONSTRUCT_SIGNAL_IN(wrenchDes, dynamicgraph::Vector)
67  , CONSTRUCT_SIGNAL_IN(q, dynamicgraph::Vector)
68  , CONSTRUCT_SIGNAL_IN(rho, double)
69  , CONSTRUCT_SIGNAL_IN(phase, int)
70  , CONSTRUCT_SIGNAL_IN(frictionCoefficient, double)
71  , CONSTRUCT_SIGNAL_IN(wSum, double)
72  , CONSTRUCT_SIGNAL_IN(wNorm, double)
73  , CONSTRUCT_SIGNAL_IN(wRatio, double)
74  , CONSTRUCT_SIGNAL_IN(wAnkle, dynamicgraph::Vector)
75  , CONSTRUCT_SIGNAL_INNER(kinematics_computations, int, m_qSIN)
76  , CONSTRUCT_SIGNAL_INNER(qp_computations, int, m_wrenchDesSIN << m_rhoSIN << m_phaseSIN << WEIGHT_SIGNALS << m_kinematics_computationsSINNER)
77  , CONSTRUCT_SIGNAL_OUT(wrenchLeft, dynamicgraph::Vector, m_qp_computationsSINNER)
78  , CONSTRUCT_SIGNAL_OUT(ankleWrenchLeft, dynamicgraph::Vector, m_wrenchLeftSOUT)
79  , CONSTRUCT_SIGNAL_OUT(surfaceWrenchLeft, dynamicgraph::Vector, m_wrenchLeftSOUT)
80  , CONSTRUCT_SIGNAL_OUT(copLeft, dynamicgraph::Vector, m_wrenchLeftSOUT)
81  , CONSTRUCT_SIGNAL_OUT(wrenchRight, dynamicgraph::Vector, m_qp_computationsSINNER)
82  , CONSTRUCT_SIGNAL_OUT(ankleWrenchRight, dynamicgraph::Vector, m_wrenchRightSOUT)
83  , CONSTRUCT_SIGNAL_OUT(surfaceWrenchRight, dynamicgraph::Vector, m_wrenchRightSOUT)
84  , CONSTRUCT_SIGNAL_OUT(copRight, dynamicgraph::Vector, m_wrenchRightSOUT)
85  , CONSTRUCT_SIGNAL_OUT(wrenchRef, dynamicgraph::Vector, m_wrenchLeftSOUT << m_wrenchRightSOUT)
86  , CONSTRUCT_SIGNAL_OUT(zmpRef, dynamicgraph::Vector, m_wrenchRefSOUT)
87  , CONSTRUCT_SIGNAL_OUT(emergencyStop, bool, m_zmpRefSOUT)
88  , m_initSucceeded(false)
89  , m_model()
90  , m_data(pinocchio::Model())
91  , m_Q1(6,6)
92  , m_C1(6)
93  , m_Aeq1(0,6)
94  , m_Beq1(0)
95  , m_Aineq1(16,6)
96  , m_Bineq1(16)
97  , m_Q2(12,12)
98  , m_C2(12)
99  , m_Aeq2(0,12)
100  , m_Beq2(0)
101  , m_Aineq2(34,12)
102  , m_Bineq2(34)
103  , m_wAnkle(6)
104  {
105  Entity::signalRegistration( INPUT_SIGNALS << OUTPUT_SIGNALS );
106 
107  m_qp1.problem(6,0,16);
108  m_qp2.problem(12,0,34);
109 
110  /* Commands. */
111  addCommand("init", makeCommandVoid1(*this, &DistributeWrench::init, docCommandVoid1("Initialize the entity.","Robot name")));
112 
113  addCommand("set_right_foot_sizes",
114  makeCommandVoid1(*this, &DistributeWrench::set_right_foot_sizes,
115  docCommandVoid1("Set the size of the right foot (pos x, neg x, pos y, neg y)",
116  "4d vector")));
117  addCommand("set_left_foot_sizes",
118  makeCommandVoid1(*this, &DistributeWrench::set_left_foot_sizes,
119  docCommandVoid1("Set the size of the left foot (pos x, neg x, pos y, neg y)",
120  "4d vector")));
121 
122  addCommand("getMinPressure", makeDirectGetter(*this,&m_eps, docDirectGetter("Get minimum pressure","double")));
123  addCommand("setMinPressure", makeDirectSetter(*this,&m_eps, docDirectSetter("Set minimum pressure","double")));
124 
125  m_eps = 15.; // TODO: signal/conf
126  }
127 
128  void DistributeWrench::init(const std::string& robotName)
129  {
130  if(!m_wrenchDesSIN.isPlugged())
131  return SEND_MSG("Init failed: signal wrenchDes is not plugged", MSG_TYPE_ERROR);
132  if(!m_qSIN.isPlugged())
133  return SEND_MSG("Init failed: signal q is not plugged", MSG_TYPE_ERROR);
134 
135  if(m_left_foot_sizes.size()==0)
136  return SEND_ERROR_STREAM_MSG("Init failed: left foot size is not initialized");
137  if(m_right_foot_sizes.size()==0)
138  return SEND_ERROR_STREAM_MSG("Init failed: right foot size is not initialized");
139 
140  try
141  {
142  /* Retrieve m_robot_util informations */
143  std::string localName(robotName);
144  if (isNameInRobotUtil(localName))
145  {
146  m_robot_util = getRobotUtil(localName);
147 // std::cerr << "m_robot_util:" << m_robot_util << std::endl;
148  }
149  else
150  {
151  SEND_MSG("You should have a robotUtil pointer initialized before",MSG_TYPE_ERROR);
152  return;
153  }
154 
155  pinocchio::urdf::buildModel(m_robot_util->m_urdf_filename, pinocchio::JointModelFreeFlyer(), m_model);
156  m_data = pinocchio::Data(m_model);
157  }
158  catch (const std::exception& e)
159  {
160  std::cout << e.what();
161  SEND_MSG("Init failed: Could load URDF :" + m_robot_util->m_urdf_filename, MSG_TYPE_ERROR);
162  return;
163  }
164 
165 
166  assert(m_model.existFrame(m_robot_util->m_foot_util.m_Left_Foot_Frame_Name));
167  assert(m_model.existFrame(m_robot_util->m_foot_util.m_Right_Foot_Frame_Name));
168  m_left_foot_id = m_model.getFrameId(m_robot_util->m_foot_util.m_Left_Foot_Frame_Name);
169  m_right_foot_id = m_model.getFrameId(m_robot_util->m_foot_util.m_Right_Foot_Frame_Name);
170 
171  //m_ankle_M_ftSens = pinocchio::SE3(Eigen::Matrix3d::Identity(), m_robot_util->m_foot_util.m_Right_Foot_Force_Sensor_XYZ.head<3>());
172  m_ankle_M_sole = pinocchio::SE3(Eigen::Matrix3d::Identity(), m_robot_util->m_foot_util.m_Right_Foot_Sole_XYZ.head<3>());
173 
174  m_initSucceeded = true;
175  }
176 
178  {
179  if(s.size()!=4)
180  return SEND_MSG("Foot size vector should have size 4, not "+toString(s.size()), MSG_TYPE_ERROR);
181  m_right_foot_sizes = s;
182  }
183 
185  {
186  if(s.size()!=4)
187  return SEND_MSG("Foot size vector should have size 4, not "+toString(s.size()), MSG_TYPE_ERROR);
188  m_left_foot_sizes = s;
189  }
190 
191  // WARNING: we are assuming wrench = right = symmetrical
193  {
194  const double X = m_right_foot_sizes[0];
195  const double Y = m_right_foot_sizes[2];
197  // fx, fy, fz, mx, my, mz,
198  -1, 0, -mu, 0, 0, 0,
199  +1, 0, -mu, 0, 0, 0,
200  0, -1, -mu, 0, 0, 0,
201  0, +1, -mu, 0, 0, 0,
202  0, 0, -Y, -1, 0, 0,
203  0, 0, -Y, +1, 0, 0,
204  0, 0, -X, 0, -1, 0,
205  0, 0, -X, 0, +1, 0,
206  -Y, -X, -(X + Y) * mu, +mu, +mu, -1,
207  -Y, +X, -(X + Y) * mu, +mu, -mu, -1,
208  +Y, -X, -(X + Y) * mu, -mu, +mu, -1,
209  +Y, +X, -(X + Y) * mu, -mu, -mu, -1,
210  +Y, +X, -(X + Y) * mu, +mu, +mu, +1,
211  +Y, -X, -(X + Y) * mu, +mu, -mu, +1,
212  -Y, +X, -(X + Y) * mu, -mu, +mu, +1,
213  -Y, -X, -(X + Y) * mu, -mu, -mu, +1;
214  }
215 
216  Eigen::Vector3d
217  DistributeWrench::computeCoP(const dg::Vector & wrenchGlobal, const pinocchio::SE3 & pose) const
218  {
219  const pinocchio::Force::Vector6 & wrench = pose.actInv(pinocchio::Force(wrenchGlobal)).toVector();
220 
221  const double h = pose.translation()[2];
222 
223  const double fx = wrench[0];
224  const double fy = wrench[1];
225  const double fz = wrench[2];
226  const double tx = wrench[3];
227  const double ty = wrench[4];
228 
229  double m_eps = 0.1; // temporary
230 
231  double px, py;
232 
233  if(fz >= m_eps/2)
234  {
235  px = (- ty - fx*h)/fz;
236  py = ( tx - fy*h)/fz;
237  }
238  else
239  {
240  px = 0.0;
241  py = 0.0;
242  }
243  const double pz = 0.0;
244 
245  Eigen::Vector3d cop;
246  cop[0] = px;
247  cop[1] = py;
248  cop[2] = pz;
249 
250  return cop;
251  }
252 
253  /* ------------------------------------------------------------------- */
254  /* --- SIGNALS ------------------------------------------------------- */
255  /* ------------------------------------------------------------------- */
256 
257  DEFINE_SIGNAL_INNER_FUNCTION(kinematics_computations, int)
258  {
259  if(!m_initSucceeded)
260  {
261  SEND_WARNING_STREAM_MSG("Cannot compute signal kinematics_computations before initialization!");
262  return s;
263  }
264 
265  const Eigen::VectorXd & q = m_qSIN(iter);
266  assert(q.size()==m_model.nq && "Unexpected size of signal q");
267 
269 
270  pinocchio::forwardKinematics(m_model, m_data, q);
271  pinocchio::updateFramePlacement(m_model, m_data, m_left_foot_id);
272  pinocchio::updateFramePlacement(m_model, m_data, m_right_foot_id);
273 
276 
278 
279  return s;
280  }
281 
282  void DistributeWrench::distributeWrench(const Eigen::VectorXd & wrenchDes, const double rho, const double mu)
283  {
284 
285  // --- COSTS
286 
287  // Initialize cost matrices
288  Eigen::MatrixXd & Q = m_Q2;
289  Eigen::VectorXd & C = m_C2;
290 
291  // min |wrenchLeft + wrenchRight - wrenchDes|^2
292  Q.topLeftCorner<6,6>().setIdentity();
293  Q.topRightCorner<6,6>().setIdentity();
294  Q.bottomLeftCorner<6,6>().setIdentity();
295  Q.bottomRightCorner<6,6>().setIdentity();
296  Q *= m_wSum;
297 
298  C.head<6>() = -wrenchDes;
299  C.tail<6>() = -wrenchDes;
300  C *= m_wSum;
301 
302  // min |wrenchLeft_a|^2 + |wrenchRight_a|^2
303  Eigen::Matrix<double,6,6> tmp = m_wAnkle.asDiagonal() * m_data.oMf[m_left_foot_id].inverse().toDualActionMatrix();
304  Q.topLeftCorner<6,6>().noalias() += tmp.transpose() * tmp * m_wNorm;
305 
306  tmp = m_wAnkle.asDiagonal() * m_data.oMf[m_right_foot_id].inverse().toDualActionMatrix();
307  Q.bottomRightCorner<6,6>().noalias() += tmp.transpose() * tmp * m_wNorm;
308 
309  // min |(1-rho)e_z^T*wrenchLeft_c - rho*e_z^T*wrenchLeft_c|
310  Eigen::Matrix<double,1,12> tmp2;
311  tmp2 << (1-rho) * ( m_contactLeft.inverse().toDualActionMatrix().row(2) ),
312  (-rho) * ( m_contactRight.inverse().toDualActionMatrix().row(2) );
313 
314  Q.noalias() += tmp2.transpose() * tmp2 * m_wRatio;
315 
316  // --- Equality constraints
317 
318  Eigen::MatrixXd & Aeq = m_Aeq2;
319 
320  Eigen::VectorXd & Beq = m_Beq2;
321 
322  // --- Inequality constraints
323 
325 
326  Eigen::MatrixXd & Aineq = m_Aineq2;
327 
328  Aineq.topLeftCorner<16,6>() = m_wrenchFaceMatrix * m_contactLeft.inverse().toDualActionMatrix();
329  Aineq.topRightCorner<16,6>().setZero();
330  Aineq.block<16,6>(16,0).setZero();
331  Aineq.block<16,6>(16,6) = m_wrenchFaceMatrix * m_contactRight.inverse().toDualActionMatrix();
332 
333  Aineq.block<1,6>(32,0) = - m_contactLeft.inverse().toDualActionMatrix().row(2);
334  Aineq.block<1,6>(32,6).setZero();
335  Aineq.block<1,6>(33,0).setZero();
336  Aineq.block<1,6>(33,6) = - m_contactRight.inverse().toDualActionMatrix().row(2);
337 
338  Eigen::VectorXd & Bineq = m_Bineq2;
339 
340  Bineq.setZero();
341  Bineq(32) = - m_eps;
342  Bineq(33) = - m_eps;
343 
344  const bool success = m_qp2.solve(Q, C, Aeq, Beq, Aineq, Bineq);
345 
346  m_emergency_stop_triggered = !success;
347 
349  {
350  m_wrenchLeft.setZero(6);
351  m_wrenchRight.setZero(6);
352  }
353  else
354  {
355  const Eigen::VectorXd & result = m_qp2.result();
356  m_wrenchLeft = result.head<6>();
357  m_wrenchRight = result.tail<6>();
358  }
359  }
360 
361  void DistributeWrench::saturateWrench(const Eigen::VectorXd & wrenchDes, const int phase, const double mu) {
362  // Initialize cost matrices
363  Eigen::MatrixXd & Q = m_Q1;
364  Eigen::VectorXd & C = m_C1;
365 
366  // min |wrench - wrenchDes|^2
367  Q.setIdentity();
368  C = -wrenchDes;
369 
370  // --- Equality constraints
371 
372  Eigen::MatrixXd & Aeq = m_Aeq1;
373 
374  Eigen::VectorXd & Beq = m_Beq1;
375 
376  // --- Inequality constraints
377 
379 
380  Eigen::MatrixXd & Aineq = m_Aineq1;
381  if(phase>0) {
382  Aineq = m_wrenchFaceMatrix * m_contactLeft.inverse().toDualActionMatrix();
383  }
384  else {
385  Aineq = m_wrenchFaceMatrix * m_contactRight.inverse().toDualActionMatrix();
386  }
387 
388  Eigen::VectorXd & Bineq = m_Bineq1;
389  Bineq.setZero();
390 
391  const bool success = m_qp1.solve(Q, C, Aeq, Beq, Aineq, Bineq);
392 
393  m_emergency_stop_triggered = !success;
394 
396  {
397  m_wrenchLeft.setZero(6);
398  m_wrenchRight.setZero(6);
399  }
400  else
401  {
402  const Eigen::VectorXd & result = m_qp1.result();
403 
404  if(phase>0) {
405  m_wrenchLeft = result;
406  m_wrenchRight.setZero(6);
407  }
408  else if(phase<0) {
409  m_wrenchRight = result;
410  m_wrenchLeft.setZero(6);
411  }
412  }
413  }
414 
415  DEFINE_SIGNAL_INNER_FUNCTION(qp_computations, int)
416  {
417  if(!m_initSucceeded)
418  {
419  SEND_WARNING_STREAM_MSG("Cannot compute signal qp_computations before initialization!");
420  return s;
421  }
422 
423  const Eigen::VectorXd & wrenchDes = m_wrenchDesSIN(iter);
424  const int & dummy = m_kinematics_computationsSINNER(iter);
425  (void) dummy;
426  const int & phase = m_phaseSIN(iter);
427 
428  const double & mu = m_frictionCoefficientSIN(iter); // 0.7
429 
430  assert(wrenchDes.size()==6 && "Unexpected size of signal wrenchDes");
431 
432  getProfiler().start(PROFILE_DISTRIBUTE_WRENCH_QP_COMPUTATIONS);
433 
434  if(phase==0)
435  {
436  const double & rho = m_rhoSIN(iter);
437 
438  m_wSum = m_wSumSIN(iter); // 10000.0
439  m_wNorm = m_wNormSIN(iter); // 10.0
440  m_wRatio = m_wRatioSIN(iter); // 1.0
441  m_wAnkle = m_wAnkleSIN(iter); // 1., 1., 1e-4, 1., 1., 1e-4
442 
443  distributeWrench(wrenchDes, rho, mu);
444  }
445  else
446  {
447  saturateWrench(wrenchDes, phase, mu);
448  }
449 
450  getProfiler().stop(PROFILE_DISTRIBUTE_WRENCH_QP_COMPUTATIONS);
451 
453  {
454  SEND_ERROR_STREAM_MSG("No solution to the QP problem!");
455  return s;
456  }
457 
458  return s;
459  }
460 
462  {
463  if(!m_initSucceeded)
464  {
465  SEND_WARNING_STREAM_MSG("Cannot compute signal wrenchLeft before initialization!");
466  return s;
467  }
468  if(s.size()!=6)
469  s.resize(6);
470 
471  const int & dummy = m_qp_computationsSINNER(iter);
472  (void) dummy;
473  s = m_wrenchLeft;
474  return s;
475  }
476 
478  {
479  if(!m_initSucceeded)
480  {
481  SEND_WARNING_STREAM_MSG("Cannot compute signal wrenchRight before initialization!");
482  return s;
483  }
484  if(s.size()!=6)
485  s.resize(6);
486 
487  const int & dummy = m_qp_computationsSINNER(iter);
488  (void) dummy;
489  s = m_wrenchRight;
490  return s;
491  }
492 
494  {
495  if(!m_initSucceeded)
496  {
497  SEND_WARNING_STREAM_MSG("Cannot compute signal ankleWrenchLeft before initialization!");
498  return s;
499  }
500  if(s.size()!=6)
501  s.resize(6);
502 
503  const Eigen::VectorXd & wrenchLeft = m_wrenchLeftSOUT(iter);
504 
505  s = m_data.oMf[m_left_foot_id].actInv(pinocchio::Force(wrenchLeft)).toVector();
506 
507  return s;
508  }
509 
511  {
512  if(!m_initSucceeded)
513  {
514  SEND_WARNING_STREAM_MSG("Cannot compute signal ankleWrenchRight before initialization!");
515  return s;
516  }
517  if(s.size()!=6)
518  s.resize(6);
519 
520  const Eigen::VectorXd & wrenchRight = m_wrenchRightSOUT(iter);
521 
522  s = m_data.oMf[m_right_foot_id].actInv(pinocchio::Force(wrenchRight)).toVector();
523 
524  return s;
525  }
526 
528  {
529  if(!m_initSucceeded)
530  {
531  SEND_WARNING_STREAM_MSG("Cannot compute signal surfaceWrenchLeft before initialization!");
532  return s;
533  }
534  if(s.size()!=6)
535  s.resize(6);
536 
537  const Eigen::VectorXd & wrenchLeft = m_wrenchLeftSOUT(iter);
538 
539  s = m_contactLeft.actInv(pinocchio::Force(wrenchLeft)).toVector();
540 
541  return s;
542  }
543 
545  {
546  if(!m_initSucceeded)
547  {
548  SEND_WARNING_STREAM_MSG("Cannot compute signal surfaceWrenchRight before initialization!");
549  return s;
550  }
551  if(s.size()!=6)
552  s.resize(6);
553 
554  const Eigen::VectorXd & wrenchRight = m_wrenchRightSOUT(iter);
555 
556  s = m_contactRight.actInv(pinocchio::Force(wrenchRight)).toVector();
557 
558  return s;
559  }
560 
562  {
563  if(!m_initSucceeded)
564  {
565  SEND_WARNING_STREAM_MSG("Cannot compute signal copLeft before initialization!");
566  return s;
567  }
568  if(s.size()!=3)
569  s.resize(3);
570 
571  const Eigen::VectorXd & wrenchLeft = m_wrenchLeftSOUT(iter);
572 
574  {
575  s.setZero(3);
576  return s;
577  }
578 
579  s = computeCoP(wrenchLeft, m_contactLeft);
580 
581  return s;
582  }
583 
585  {
586  if(!m_initSucceeded)
587  {
588  SEND_WARNING_STREAM_MSG("Cannot compute signal copRight before initialization!");
589  return s;
590  }
591  if(s.size()!=3)
592  s.resize(3);
593 
594  const Eigen::VectorXd & wrenchRight = m_wrenchRightSOUT(iter);
595 
597  {
598  s.setZero(3);
599  return s;
600  }
601 
602  s = computeCoP(wrenchRight, m_contactRight);
603 
604  return s;
605  }
606 
608  {
609  if(!m_initSucceeded)
610  {
611  SEND_WARNING_STREAM_MSG("Cannot compute signal wrenchRef before initialization!");
612  return s;
613  }
614  if(s.size()!=6)
615  s.resize(6);
616 
617  const Eigen::VectorXd & wrenchLeft = m_wrenchLeftSOUT(iter);
618  const Eigen::VectorXd & wrenchRight = m_wrenchRightSOUT(iter);
619 
620  s = wrenchLeft + wrenchRight;
621 
622  return s;
623  }
624 
626  {
627  if(!m_initSucceeded)
628  {
629  SEND_WARNING_STREAM_MSG("Cannot compute signal zmpRef before initialization!");
630  return s;
631  }
632  if(s.size()!=3)
633  s.resize(3);
634 
635  const Eigen::VectorXd & wrenchRef = m_wrenchRefSOUT(iter);
636 
638  {
639  s.setZero(3);
640  return s;
641  }
642 
643  //const double fx = wrenchRef[0];
644  //const double fy = wrenchRef[1];
645  const double fz = wrenchRef[2];
646  const double tx = wrenchRef[3];
647  const double ty = wrenchRef[4];
648 
649  double m_eps = 0.1; // temporary
650 
651  double px, py;
652  if(fz >= m_eps/2)
653  {
654  px = -ty/fz;
655  py = tx/fz;
656  }
657  else
658  {
659  px = 0.0;
660  py = 0.0;
661  }
662  const double pz = 0.0;
663 
664  Eigen::Vector3d zmp(3);
665  zmp[0] = px;
666  zmp[1] = py;
667  zmp[2] = pz;
668 
669  s = zmp;
670 
671  return s;
672  }
673 
674  DEFINE_SIGNAL_OUT_FUNCTION(emergencyStop, bool)
675  {
676  const dynamicgraph::Vector & zmp = m_zmpRefSOUT(iter); // dummy to trigger zmp computation
677  (void) zmp; // disable unused variable warning
679  return s;
680  }
681 
682  /* --- COMMANDS ---------------------------------------------------------- */
683 
684  /* ------------------------------------------------------------------- */
685  /* --- ENTITY -------------------------------------------------------- */
686  /* ------------------------------------------------------------------- */
687 
688  void DistributeWrench::display(std::ostream& os) const
689  {
690  os << "DistributeWrench " << getName();
691  try
692  {
693  getProfiler().report_all(3, os);
694  }
695  catch (ExceptionSignal e) {}
696  }
697 
698  } // namespace talos_balance
699  } // namespace sot
700 } // namespace dynamicgraph
701 
#define PROFILE_DISTRIBUTE_WRENCH_KINEMATICS_COMPUTATIONS
void saturateWrench(const Eigen::VectorXd &wrenchDes, const int phase, const double mu)
Eigen::Vector4d m_right_foot_sizes
sizes of the left foot (pos x, neg x, pos y, neg y)
void computeWrenchFaceMatrix(const double mu)
sizes of the left foot (pos x, neg x, pos y, neg y)
Eigen::Matrix< Scalar, Eigen::Dynamic, 1 > Vector
Definition: math/fwd.hh:40
EIGEN_MAKE_ALIGNED_OPERATOR_NEW DistributeWrench(const std::string &name)
pinocchio::Data m_data
Pinocchio robot model.
DYNAMICGRAPH_FACTORY_ENTITY_PLUGIN(AdmittanceControllerEndEffector, "AdmittanceControllerEndEffector")
#define OUTPUT_SIGNALS
DEFINE_SIGNAL_INNER_FUNCTION(w_force, dynamicgraph::Vector)
pinocchio::FrameIndex m_left_foot_id
ankle to sole transformation
#define WEIGHT_SIGNALS
void set_left_foot_sizes(const dynamicgraph::Vector &s)
RobotUtilShrPtr m_robot_util
Pinocchio robot data.
#define PROFILE_DISTRIBUTE_WRENCH_QP_COMPUTATIONS
void distributeWrench(const Eigen::VectorXd &wrenchDes, const double rho, const double mu)
#define INPUT_SIGNALS
void set_right_foot_sizes(const dynamicgraph::Vector &s)
virtual void display(std::ostream &os) const
pinocchio::Model m_model
true if the entity has been successfully initialized
Eigen::Vector3d computeCoP(const dynamicgraph::Vector &wrench, const pinocchio::SE3 &pose) const
Eigen::Matrix< Scalar, 6, 1 > Vector6
Definition: math/fwd.hh:46