sot-talos-balance  1.6.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_copLeftSOUT << m_wrenchRightSOUT << m_ankleWrenchRightSOUT << 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(copLeft, dynamicgraph::Vector, m_wrenchLeftSOUT)
73  , CONSTRUCT_SIGNAL_OUT(wrenchRight, dynamicgraph::Vector, m_wrenchesSINNER)
74  , CONSTRUCT_SIGNAL_OUT(ankleWrenchRight, dynamicgraph::Vector, m_wrenchRightSOUT)
75  , CONSTRUCT_SIGNAL_OUT(copRight, dynamicgraph::Vector, m_wrenchRightSOUT)
76  , CONSTRUCT_SIGNAL_OUT(wrenchRef, dynamicgraph::Vector, m_wrenchLeftSOUT << m_wrenchRightSOUT)
77  , CONSTRUCT_SIGNAL_OUT(zmpRef, dynamicgraph::Vector, m_wrenchRefSOUT)
78  , CONSTRUCT_SIGNAL_OUT(emergencyStop, bool, m_zmpRefSOUT)
79  , m_initSucceeded(false)
80  , m_model()
81  , m_data(pinocchio::Model())
82  {
83  Entity::signalRegistration( INPUT_SIGNALS << OUTPUT_SIGNALS );
84 
85  /* Commands. */
86  addCommand("init", makeCommandVoid1(*this, &SimpleDistributeWrench::init, docCommandVoid1("Initialize the entity.","Robot name")));
87  }
88 
89  void SimpleDistributeWrench::init(const std::string& robotName)
90  {
91  if(!m_wrenchDesSIN.isPlugged())
92  return SEND_MSG("Init failed: signal wrenchDes is not plugged", MSG_TYPE_ERROR);
93  if(!m_qSIN.isPlugged())
94  return SEND_MSG("Init failed: signal q is not plugged", MSG_TYPE_ERROR);
95 
96  try
97  {
98  /* Retrieve m_robot_util informations */
99  std::string localName(robotName);
100  if (isNameInRobotUtil(localName))
101  {
102  m_robot_util = getRobotUtil(localName);
103 // std::cerr << "m_robot_util:" << m_robot_util << std::endl;
104  }
105  else
106  {
107  SEND_MSG("You should have a robotUtil pointer initialized before",MSG_TYPE_ERROR);
108  return;
109  }
110 
111  pinocchio::urdf::buildModel(m_robot_util->m_urdf_filename, pinocchio::JointModelFreeFlyer(), m_model);
112  m_data = pinocchio::Data(m_model);
113  }
114  catch (const std::exception& e)
115  {
116  std::cout << e.what();
117  SEND_MSG("Init failed: Could load URDF :" + m_robot_util->m_urdf_filename, MSG_TYPE_ERROR);
118  return;
119  }
120 
121 
122  assert(m_model.existFrame(m_robot_util->m_foot_util.m_Left_Foot_Frame_Name));
123  assert(m_model.existFrame(m_robot_util->m_foot_util.m_Right_Foot_Frame_Name));
124  m_left_foot_id = m_model.getFrameId(m_robot_util->m_foot_util.m_Left_Foot_Frame_Name);
125  m_right_foot_id = m_model.getFrameId(m_robot_util->m_foot_util.m_Right_Foot_Frame_Name);
126 
127  //m_ankle_M_ftSens = pinocchio::SE3(Eigen::Matrix3d::Identity(), m_robot_util->m_foot_util.m_Right_Foot_Force_Sensor_XYZ.head<3>());
128  m_ankle_M_sole = pinocchio::SE3(Eigen::Matrix3d::Identity(), m_robot_util->m_foot_util.m_Right_Foot_Sole_XYZ.head<3>());
129 
130  m_initSucceeded = true;
131  }
132 
133  Eigen::Vector3d
134  SimpleDistributeWrench::computeCoP(const dg::Vector & wrenchGlobal, const pinocchio::SE3 & pose) const
135  {
136  const pinocchio::Force::Vector6 & wrench = pose.actInv(pinocchio::Force(wrenchGlobal)).toVector();
137 
138  const double h = pose.translation()[2];
139 
140  const double fx = wrench[0];
141  const double fy = wrench[1];
142  const double fz = wrench[2];
143  const double tx = wrench[3];
144  const double ty = wrench[4];
145 
146  double m_eps = 0.1; // temporary
147 
148  double px, py;
149 
150  if(fz >= m_eps/2)
151  {
152  px = (- ty - fx*h)/fz;
153  py = ( tx - fy*h)/fz;
154  }
155  else
156  {
157  px = 0.0;
158  py = 0.0;
159  }
160  const double pz = 0.0;
161 
162  Eigen::Vector3d cop;
163  cop[0] = px;
164  cop[1] = py;
165  cop[2] = pz;
166 
167  return cop;
168  }
169 
170  /* ------------------------------------------------------------------- */
171  /* --- SIGNALS ------------------------------------------------------- */
172  /* ------------------------------------------------------------------- */
173 
174  DEFINE_SIGNAL_INNER_FUNCTION(kinematics_computations, int)
175  {
176  if(!m_initSucceeded)
177  {
178  SEND_WARNING_STREAM_MSG("Cannot compute signal kinematics_computations before initialization!");
179  return s;
180  }
181 
182  const Eigen::VectorXd & q = m_qSIN(iter);
183  assert(q.size()==m_model.nq && "Unexpected size of signal q");
184 
186 
187  pinocchio::forwardKinematics(m_model, m_data, q);
188  pinocchio::updateFramePlacement(m_model, m_data, m_left_foot_id);
189  pinocchio::updateFramePlacement(m_model, m_data, m_right_foot_id);
190 
193 
195 
196  return s;
197  }
198 
199  bool SimpleDistributeWrench::distributeWrench(const Eigen::VectorXd & wrenchDes, const double rho)
200  {
201  Eigen::Vector3d forceLeft = wrenchDes.head<3>()/2;
202  Eigen::Vector3d forceRight = forceLeft;
203  forceLeft[2] = rho * wrenchDes[2];
204  forceRight[2] = (1-rho) * wrenchDes[2];
205 
206  Eigen::Vector3d tauLeft = m_contactLeft.translation().cross(forceLeft);
207  Eigen::Vector3d tauRight = m_contactRight.translation().cross(forceRight);
208 
209  Eigen::Vector3d tauResidual = (wrenchDes.tail<3>() - tauLeft - tauRight)/2;
210  tauLeft += tauResidual;
211  tauRight += tauResidual;
212 
213  m_wrenchLeft << forceLeft, tauLeft;
214  m_wrenchRight << forceRight, tauRight;
215 
216  bool success = true;
217 
218  m_emergency_stop_triggered = !success;
219 
220  return success;
221  }
222 
223  bool SimpleDistributeWrench::saturateWrench(const Eigen::VectorXd & wrenchDes, const int phase) {
224  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  return success;
240  }
241 
243  {
244  if(!m_initSucceeded)
245  {
246  SEND_WARNING_STREAM_MSG("Cannot compute signal wrenches before initialization!");
247  return s;
248  }
249 
250  const Eigen::VectorXd & wrenchDes = m_wrenchDesSIN(iter);
251  const int & dummy = m_kinematics_computationsSINNER(iter);
252  (void) dummy;
253  const int & phase = m_phaseSIN(iter);
254 
255  assert(wrenchDes.size()==6 && "Unexpected size of signal wrenchDes");
256 
258 
259  bool success;
260 
261  if(phase==0)
262  {
263  const double & rho = m_rhoSIN(iter);
264 
265  success = distributeWrench(wrenchDes, rho);
266  }
267  else
268  {
269  success = saturateWrench(wrenchDes, phase);
270  }
271 
273 
274  if(!success)
275  {
276  SEND_WARNING_STREAM_MSG("Error in wrench distribution!");
277  return s;
278  }
279 
280  return s;
281  }
282 
284  {
285  if(!m_initSucceeded)
286  {
287  SEND_WARNING_STREAM_MSG("Cannot compute signal wrenchLeft before initialization!");
288  return s;
289  }
290  if(s.size()!=6)
291  s.resize(6);
292 
293  m_wrenchesSINNER(iter);
294  s = m_wrenchLeft;
295  return s;
296  }
297 
299  {
300  if(!m_initSucceeded)
301  {
302  SEND_WARNING_STREAM_MSG("Cannot compute signal wrenchRight before initialization!");
303  return s;
304  }
305  if(s.size()!=6)
306  s.resize(6);
307 
308  m_wrenchesSINNER(iter);
309  s = m_wrenchRight;
310  return s;
311  }
312 
314  {
315  if(!m_initSucceeded)
316  {
317  SEND_WARNING_STREAM_MSG("Cannot compute signal ankleWrenchLeft before initialization!");
318  return s;
319  }
320  if(s.size()!=6)
321  s.resize(6);
322 
323  const Eigen::VectorXd & wrenchLeft = m_wrenchLeftSOUT(iter);
324 
325  s = m_data.oMf[m_left_foot_id].actInv(pinocchio::Force(wrenchLeft)).toVector();
326 
327  return s;
328  }
329 
331  {
332  if(!m_initSucceeded)
333  {
334  SEND_WARNING_STREAM_MSG("Cannot compute signal ankleWrenchRight before initialization!");
335  return s;
336  }
337  if(s.size()!=6)
338  s.resize(6);
339 
340  const Eigen::VectorXd & wrenchRight = m_wrenchRightSOUT(iter);
341 
342  s = m_data.oMf[m_right_foot_id].actInv(pinocchio::Force(wrenchRight)).toVector();
343 
344  return s;
345  }
346 
348  {
349  if(!m_initSucceeded)
350  {
351  SEND_WARNING_STREAM_MSG("Cannot compute signal copLeft before initialization!");
352  return s;
353  }
354  if(s.size()!=3)
355  s.resize(3);
356 
357  const Eigen::VectorXd & wrenchLeft = m_wrenchLeftSOUT(iter);
358 
359  s = computeCoP(wrenchLeft, m_contactLeft);
360 
361  return s;
362  }
363 
365  {
366  if(!m_initSucceeded)
367  {
368  SEND_WARNING_STREAM_MSG("Cannot compute signal copRight before initialization!");
369  return s;
370  }
371  if(s.size()!=3)
372  s.resize(3);
373 
374  const Eigen::VectorXd & wrenchRight = m_wrenchRightSOUT(iter);
375 
376  s = computeCoP(wrenchRight, m_contactRight);
377 
378  return s;
379  }
380 
382  {
383  if(!m_initSucceeded)
384  {
385  SEND_WARNING_STREAM_MSG("Cannot compute signal wrenchRef before initialization!");
386  return s;
387  }
388  if(s.size()!=6)
389  s.resize(6);
390 
391  const Eigen::VectorXd & wrenchLeft = m_wrenchLeftSOUT(iter);
392  const Eigen::VectorXd & wrenchRight = m_wrenchRightSOUT(iter);
393 
394  s = wrenchLeft + wrenchRight;
395 
396  return s;
397  }
398 
400  {
401  if(!m_initSucceeded)
402  {
403  SEND_WARNING_STREAM_MSG("Cannot compute signal zmpRef before initialization!");
404  return s;
405  }
406  if(s.size()!=3)
407  s.resize(3);
408 
409  const Eigen::VectorXd & wrenchRef = m_wrenchRefSOUT(iter);
410 
411  //const double fx = wrenchRef[0];
412  //const double fy = wrenchRef[1];
413  const double fz = wrenchRef[2];
414  const double tx = wrenchRef[3];
415  const double ty = wrenchRef[4];
416 
417  double m_eps = 0.1; // temporary
418 
419  double px, py;
420  if(fz >= m_eps/2)
421  {
422  px = -ty/fz;
423  py = tx/fz;
424  }
425  else
426  {
427  px = 0.0;
428  py = 0.0;
429  }
430  const double pz = 0.0;
431 
432  Eigen::Vector3d zmp(3);
433  zmp[0] = px;
434  zmp[1] = py;
435  zmp[2] = pz;
436 
437  s = zmp;
438 
439  return s;
440  }
441 
442  DEFINE_SIGNAL_OUT_FUNCTION(emergencyStop, bool)
443  {
444  const dynamicgraph::Vector & zmp = m_zmpRefSOUT(iter); // dummy to trigger zmp computation
445  (void) zmp; // disable unused variable warning
447  return s;
448  }
449 
450 
451  /* --- COMMANDS ---------------------------------------------------------- */
452 
453  /* ------------------------------------------------------------------- */
454  /* --- ENTITY -------------------------------------------------------- */
455  /* ------------------------------------------------------------------- */
456 
457  void SimpleDistributeWrench::display(std::ostream& os) const
458  {
459  os << "SimpleDistributeWrench " << getName();
460  try
461  {
462  getProfiler().report_all(3, os);
463  }
464  catch (ExceptionSignal e) {}
465  }
466 
467  } // namespace talos_balance
468  } // namespace sot
469 } // namespace dynamicgraph
470 
pinocchio::Model m_model
true if the entity has been successfully initialized
#define PROFILE_SIMPLE_DISTRIBUTE_WRENCH_WRENCHES_COMPUTATIONS
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)
EIGEN_MAKE_ALIGNED_OPERATOR_NEW SimpleDistributeWrench(const std::string &name)
bool distributeWrench(const Eigen::VectorXd &wrenchDes, const double rho)
pinocchio::FrameIndex m_left_foot_id
ankle to sole transformation
bool saturateWrench(const Eigen::VectorXd &wrenchDes, const int phase)
#define INPUT_SIGNALS
#define OUTPUT_SIGNALS
Eigen::Matrix< Scalar, 6, 1 > Vector6
Definition: math/fwd.hh:46