sot-talos-balance  1.7.0
simple-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_SIMPLE_DISTRIBUTE_WRENCH_KINEMATICS_COMPUTATIONS "SimpleDistributeWrench: kinematics computations "
43 #define PROFILE_SIMPLE_DISTRIBUTE_WRENCH_WRENCHES_COMPUTATIONS "SimpleDistributeWrench: wrenches computations "
44 
45 #define INPUT_SIGNALS m_wrenchDesSIN << m_qSIN << m_rhoSIN << m_phaseSIN
46 
47 #define INNER_SIGNALS m_kinematics_computations << m_wrenches
48 
49 #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
50 
53  typedef SimpleDistributeWrench EntityClassName;
54 
55  /* --- DG FACTORY ---------------------------------------------------- */
56  DYNAMICGRAPH_FACTORY_ENTITY_PLUGIN(SimpleDistributeWrench,
57  "SimpleDistributeWrench");
58 
59  /* ------------------------------------------------------------------- */
60  /* --- CONSTRUCTION -------------------------------------------------- */
61  /* ------------------------------------------------------------------- */
63  : Entity(name)
64  , CONSTRUCT_SIGNAL_IN(wrenchDes, dynamicgraph::Vector)
65  , CONSTRUCT_SIGNAL_IN(q, dynamicgraph::Vector)
66  , CONSTRUCT_SIGNAL_IN(rho, double)
67  , CONSTRUCT_SIGNAL_IN(phase, int)
68  , CONSTRUCT_SIGNAL_INNER(kinematics_computations, int, m_qSIN)
69  , CONSTRUCT_SIGNAL_INNER(wrenches, int, m_wrenchDesSIN << m_rhoSIN << m_phaseSIN << m_kinematics_computationsSINNER)
70  , CONSTRUCT_SIGNAL_OUT(wrenchLeft, dynamicgraph::Vector, m_wrenchesSINNER)
71  , CONSTRUCT_SIGNAL_OUT(ankleWrenchLeft, dynamicgraph::Vector, m_wrenchLeftSOUT)
72  , CONSTRUCT_SIGNAL_OUT(surfaceWrenchLeft, dynamicgraph::Vector, m_wrenchLeftSOUT)
73  , CONSTRUCT_SIGNAL_OUT(copLeft, dynamicgraph::Vector, m_wrenchLeftSOUT)
74  , CONSTRUCT_SIGNAL_OUT(wrenchRight, dynamicgraph::Vector, m_wrenchesSINNER)
75  , CONSTRUCT_SIGNAL_OUT(ankleWrenchRight, dynamicgraph::Vector, m_wrenchRightSOUT)
76  , CONSTRUCT_SIGNAL_OUT(surfaceWrenchRight, dynamicgraph::Vector, m_wrenchRightSOUT)
77  , CONSTRUCT_SIGNAL_OUT(copRight, dynamicgraph::Vector, m_wrenchRightSOUT)
78  , CONSTRUCT_SIGNAL_OUT(wrenchRef, dynamicgraph::Vector, m_wrenchLeftSOUT << m_wrenchRightSOUT)
79  , CONSTRUCT_SIGNAL_OUT(zmpRef, dynamicgraph::Vector, m_wrenchRefSOUT)
80  , CONSTRUCT_SIGNAL_OUT(emergencyStop, bool, m_zmpRefSOUT)
81  , m_initSucceeded(false)
82  , m_model()
83  , m_data(pinocchio::Model())
84  {
85  Entity::signalRegistration( INPUT_SIGNALS << OUTPUT_SIGNALS );
86 
87  /* Commands. */
88  addCommand("init", makeCommandVoid1(*this, &SimpleDistributeWrench::init, docCommandVoid1("Initialize the entity.","Robot name")));
89  }
90 
91  void SimpleDistributeWrench::init(const std::string& robotName)
92  {
93  if(!m_wrenchDesSIN.isPlugged())
94  return SEND_MSG("Init failed: signal wrenchDes is not plugged", MSG_TYPE_ERROR);
95  if(!m_qSIN.isPlugged())
96  return SEND_MSG("Init failed: signal q is not plugged", MSG_TYPE_ERROR);
97 
98  try
99  {
100  /* Retrieve m_robot_util informations */
101  std::string localName(robotName);
102  if (isNameInRobotUtil(localName))
103  {
104  m_robot_util = getRobotUtil(localName);
105 // std::cerr << "m_robot_util:" << m_robot_util << std::endl;
106  }
107  else
108  {
109  SEND_MSG("You should have a robotUtil pointer initialized before",MSG_TYPE_ERROR);
110  return;
111  }
112 
113  pinocchio::urdf::buildModel(m_robot_util->m_urdf_filename, pinocchio::JointModelFreeFlyer(), m_model);
114  m_data = pinocchio::Data(m_model);
115  }
116  catch (const std::exception& e)
117  {
118  std::cout << e.what();
119  SEND_MSG("Init failed: Could load URDF :" + m_robot_util->m_urdf_filename, MSG_TYPE_ERROR);
120  return;
121  }
122 
123 
124  assert(m_model.existFrame(m_robot_util->m_foot_util.m_Left_Foot_Frame_Name));
125  assert(m_model.existFrame(m_robot_util->m_foot_util.m_Right_Foot_Frame_Name));
126  m_left_foot_id = m_model.getFrameId(m_robot_util->m_foot_util.m_Left_Foot_Frame_Name);
127  m_right_foot_id = m_model.getFrameId(m_robot_util->m_foot_util.m_Right_Foot_Frame_Name);
128 
129  //m_ankle_M_ftSens = pinocchio::SE3(Eigen::Matrix3d::Identity(), m_robot_util->m_foot_util.m_Right_Foot_Force_Sensor_XYZ.head<3>());
130  m_ankle_M_sole = pinocchio::SE3(Eigen::Matrix3d::Identity(), m_robot_util->m_foot_util.m_Right_Foot_Sole_XYZ.head<3>());
131 
132  m_initSucceeded = true;
133  }
134 
135  Eigen::Vector3d
136  SimpleDistributeWrench::computeCoP(const dg::Vector & wrenchGlobal, const pinocchio::SE3 & pose) const
137  {
138  const pinocchio::Force::Vector6 & wrench = pose.actInv(pinocchio::Force(wrenchGlobal)).toVector();
139 
140  const double h = pose.translation()[2];
141 
142  const double fx = wrench[0];
143  const double fy = wrench[1];
144  const double fz = wrench[2];
145  const double tx = wrench[3];
146  const double ty = wrench[4];
147 
148  double m_eps = 0.1; // temporary
149 
150  double px, py;
151 
152  if(fz >= m_eps/2)
153  {
154  px = (- ty - fx*h)/fz;
155  py = ( tx - fy*h)/fz;
156  }
157  else
158  {
159  px = 0.0;
160  py = 0.0;
161  }
162  const double pz = 0.0;
163 
164  Eigen::Vector3d cop;
165  cop[0] = px;
166  cop[1] = py;
167  cop[2] = pz;
168 
169  return cop;
170  }
171 
172  /* ------------------------------------------------------------------- */
173  /* --- SIGNALS ------------------------------------------------------- */
174  /* ------------------------------------------------------------------- */
175 
176  DEFINE_SIGNAL_INNER_FUNCTION(kinematics_computations, int)
177  {
178  if(!m_initSucceeded)
179  {
180  SEND_WARNING_STREAM_MSG("Cannot compute signal kinematics_computations before initialization!");
181  return s;
182  }
183 
184  const Eigen::VectorXd & q = m_qSIN(iter);
185  assert(q.size()==m_model.nq && "Unexpected size of signal q");
186 
188 
189  pinocchio::forwardKinematics(m_model, m_data, q);
190  pinocchio::updateFramePlacement(m_model, m_data, m_left_foot_id);
191  pinocchio::updateFramePlacement(m_model, m_data, m_right_foot_id);
192 
195 
197 
198  return s;
199  }
200 
201  void SimpleDistributeWrench::distributeWrench(const Eigen::VectorXd & wrenchDes, const double rho)
202  {
203  Eigen::Vector3d forceLeft = wrenchDes.head<3>()/2;
204  Eigen::Vector3d forceRight = forceLeft;
205  forceLeft[2] = rho * wrenchDes[2];
206  forceRight[2] = (1-rho) * wrenchDes[2];
207 
208  Eigen::Vector3d tauLeft = m_contactLeft.translation().cross(forceLeft);
209  Eigen::Vector3d tauRight = m_contactRight.translation().cross(forceRight);
210 
211  Eigen::Vector3d tauResidual = (wrenchDes.tail<3>() - tauLeft - tauRight)/2;
212  tauLeft += tauResidual;
213  tauRight += tauResidual;
214 
215  m_wrenchLeft << forceLeft, tauLeft;
216  m_wrenchRight << forceRight, tauRight;
217 
218  const bool success = true;
219 
220  m_emergency_stop_triggered = !success;
221  }
222 
223  void SimpleDistributeWrench::saturateWrench(const Eigen::VectorXd & wrenchDes, const int phase) {
224  const bool success = true;
225 
226  m_emergency_stop_triggered = !success;
227 
228  const Eigen::VectorXd & result = wrenchDes;
229 
230  if(phase>0) {
231  m_wrenchLeft = result;
232  m_wrenchRight.setZero(6);
233  }
234  else if(phase<0) {
235  m_wrenchRight = result;
236  m_wrenchLeft.setZero(6);
237  }
238  }
239 
241  {
242  if(!m_initSucceeded)
243  {
244  SEND_WARNING_STREAM_MSG("Cannot compute signal wrenches before initialization!");
245  return s;
246  }
247 
248  const Eigen::VectorXd & wrenchDes = m_wrenchDesSIN(iter);
249  const int & dummy = m_kinematics_computationsSINNER(iter);
250  (void) dummy;
251  const int & phase = m_phaseSIN(iter);
252 
253  assert(wrenchDes.size()==6 && "Unexpected size of signal wrenchDes");
254 
256 
257  if(phase==0)
258  {
259  const double & rho = m_rhoSIN(iter);
260 
261  distributeWrench(wrenchDes, rho);
262  }
263  else
264  {
265  saturateWrench(wrenchDes, phase);
266  }
267 
269 
271  {
272  SEND_WARNING_STREAM_MSG("Error in wrench distribution!");
273  return s;
274  }
275 
276  return s;
277  }
278 
280  {
281  if(!m_initSucceeded)
282  {
283  SEND_WARNING_STREAM_MSG("Cannot compute signal wrenchLeft before initialization!");
284  return s;
285  }
286  if(s.size()!=6)
287  s.resize(6);
288 
289  const int & dummy = m_wrenchesSINNER(iter);
290  (void) dummy;
291  s = m_wrenchLeft;
292  return s;
293  }
294 
296  {
297  if(!m_initSucceeded)
298  {
299  SEND_WARNING_STREAM_MSG("Cannot compute signal wrenchRight before initialization!");
300  return s;
301  }
302  if(s.size()!=6)
303  s.resize(6);
304 
305  const int & dummy = m_wrenchesSINNER(iter);
306  (void) dummy;
307  s = m_wrenchRight;
308  return s;
309  }
310 
312  {
313  if(!m_initSucceeded)
314  {
315  SEND_WARNING_STREAM_MSG("Cannot compute signal ankleWrenchLeft before initialization!");
316  return s;
317  }
318  if(s.size()!=6)
319  s.resize(6);
320 
321  const Eigen::VectorXd & wrenchLeft = m_wrenchLeftSOUT(iter);
322 
323  s = m_data.oMf[m_left_foot_id].actInv(pinocchio::Force(wrenchLeft)).toVector();
324 
325  return s;
326  }
327 
329  {
330  if(!m_initSucceeded)
331  {
332  SEND_WARNING_STREAM_MSG("Cannot compute signal ankleWrenchRight before initialization!");
333  return s;
334  }
335  if(s.size()!=6)
336  s.resize(6);
337 
338  const Eigen::VectorXd & wrenchRight = m_wrenchRightSOUT(iter);
339 
340  s = m_data.oMf[m_right_foot_id].actInv(pinocchio::Force(wrenchRight)).toVector();
341 
342  return s;
343  }
344 
346  {
347  if(!m_initSucceeded)
348  {
349  SEND_WARNING_STREAM_MSG("Cannot compute signal surfaceWrenchLeft before initialization!");
350  return s;
351  }
352  if(s.size()!=6)
353  s.resize(6);
354 
355  const Eigen::VectorXd & wrenchLeft = m_wrenchLeftSOUT(iter);
356 
357  s = m_contactLeft.actInv(pinocchio::Force(wrenchLeft)).toVector();
358 
359  return s;
360  }
361 
363  {
364  if(!m_initSucceeded)
365  {
366  SEND_WARNING_STREAM_MSG("Cannot compute signal surfaceWrenchRight before initialization!");
367  return s;
368  }
369  if(s.size()!=6)
370  s.resize(6);
371 
372  const Eigen::VectorXd & wrenchRight = m_wrenchRightSOUT(iter);
373 
374  s = m_contactRight.actInv(pinocchio::Force(wrenchRight)).toVector();
375 
376  return s;
377  }
378 
380  {
381  if(!m_initSucceeded)
382  {
383  SEND_WARNING_STREAM_MSG("Cannot compute signal copLeft before initialization!");
384  return s;
385  }
386  if(s.size()!=3)
387  s.resize(3);
388 
389  const Eigen::VectorXd & wrenchLeft = m_wrenchLeftSOUT(iter);
390 
392  {
393  s.setZero(3);
394  return s;
395  }
396 
397  s = computeCoP(wrenchLeft, m_contactLeft);
398 
399  return s;
400  }
401 
403  {
404  if(!m_initSucceeded)
405  {
406  SEND_WARNING_STREAM_MSG("Cannot compute signal copRight before initialization!");
407  return s;
408  }
409  if(s.size()!=3)
410  s.resize(3);
411 
412  const Eigen::VectorXd & wrenchRight = m_wrenchRightSOUT(iter);
413 
415  {
416  s.setZero(3);
417  return s;
418  }
419 
420  s = computeCoP(wrenchRight, m_contactRight);
421 
422  return s;
423  }
424 
426  {
427  if(!m_initSucceeded)
428  {
429  SEND_WARNING_STREAM_MSG("Cannot compute signal wrenchRef before initialization!");
430  return s;
431  }
432  if(s.size()!=6)
433  s.resize(6);
434 
435  const Eigen::VectorXd & wrenchLeft = m_wrenchLeftSOUT(iter);
436  const Eigen::VectorXd & wrenchRight = m_wrenchRightSOUT(iter);
437 
438  s = wrenchLeft + wrenchRight;
439 
440  return s;
441  }
442 
444  {
445  if(!m_initSucceeded)
446  {
447  SEND_WARNING_STREAM_MSG("Cannot compute signal zmpRef before initialization!");
448  return s;
449  }
450  if(s.size()!=3)
451  s.resize(3);
452 
453  const Eigen::VectorXd & wrenchRef = m_wrenchRefSOUT(iter);
454 
456  {
457  s.setZero(3);
458  return s;
459  }
460 
461  //const double fx = wrenchRef[0];
462  //const double fy = wrenchRef[1];
463  const double fz = wrenchRef[2];
464  const double tx = wrenchRef[3];
465  const double ty = wrenchRef[4];
466 
467  double m_eps = 0.1; // temporary
468 
469  double px, py;
470  if(fz >= m_eps/2)
471  {
472  px = -ty/fz;
473  py = tx/fz;
474  }
475  else
476  {
477  px = 0.0;
478  py = 0.0;
479  }
480  const double pz = 0.0;
481 
482  Eigen::Vector3d zmp(3);
483  zmp[0] = px;
484  zmp[1] = py;
485  zmp[2] = pz;
486 
487  s = zmp;
488 
489  return s;
490  }
491 
492  DEFINE_SIGNAL_OUT_FUNCTION(emergencyStop, bool)
493  {
494  const dynamicgraph::Vector & zmp = m_zmpRefSOUT(iter); // dummy to trigger zmp computation
495  (void) zmp; // disable unused variable warning
497  return s;
498  }
499 
500 
501  /* --- COMMANDS ---------------------------------------------------------- */
502 
503  /* ------------------------------------------------------------------- */
504  /* --- ENTITY -------------------------------------------------------- */
505  /* ------------------------------------------------------------------- */
506 
507  void SimpleDistributeWrench::display(std::ostream& os) const
508  {
509  os << "SimpleDistributeWrench " << getName();
510  try
511  {
512  getProfiler().report_all(3, os);
513  }
514  catch (ExceptionSignal e) {}
515  }
516 
517  } // namespace talos_balance
518  } // namespace sot
519 } // namespace dynamicgraph
520 
pinocchio::Model m_model
true if the entity has been successfully initialized
#define PROFILE_SIMPLE_DISTRIBUTE_WRENCH_WRENCHES_COMPUTATIONS
void saturateWrench(const Eigen::VectorXd &wrenchDes, const int phase)
Eigen::Matrix< Scalar, Eigen::Dynamic, 1 > Vector
Definition: math/fwd.hh:40
DYNAMICGRAPH_FACTORY_ENTITY_PLUGIN(AdmittanceControllerEndEffector, "AdmittanceControllerEndEffector")
#define PROFILE_SIMPLE_DISTRIBUTE_WRENCH_KINEMATICS_COMPUTATIONS
Eigen::Vector3d computeCoP(const dynamicgraph::Vector &wrench, const pinocchio::SE3 &pose) const
DEFINE_SIGNAL_INNER_FUNCTION(w_force, dynamicgraph::Vector)
void distributeWrench(const Eigen::VectorXd &wrenchDes, const double rho)
EIGEN_MAKE_ALIGNED_OPERATOR_NEW SimpleDistributeWrench(const std::string &name)
pinocchio::FrameIndex m_left_foot_id
ankle to sole transformation
#define INPUT_SIGNALS
#define OUTPUT_SIGNALS
Eigen::Matrix< Scalar, 6, 1 > Vector6
Definition: math/fwd.hh:46