sot-talos-balance  1.5.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 INPUT_SIGNALS m_wrenchDesSIN << m_qSIN
46 
47 #define INNER_SIGNALS m_kinematics_computations << m_qp_computations
48 
49 #define OUTPUT_SIGNALS m_wrenchLeftSOUT << m_copLeftSOUT << m_wrenchRightSOUT << m_copRightSOUT << m_wrenchRefSOUT << m_zmpRefSOUT << m_emergencyStopSOUT
50 
53  typedef DistributeWrench EntityClassName;
54 
55  /* --- DG FACTORY ---------------------------------------------------- */
56  DYNAMICGRAPH_FACTORY_ENTITY_PLUGIN(DistributeWrench,
57  "DistributeWrench");
58 
59  /* ------------------------------------------------------------------- */
60  /* --- CONSTRUCTION -------------------------------------------------- */
61  /* ------------------------------------------------------------------- */
62  DistributeWrench::DistributeWrench(const std::string& name)
63  : Entity(name)
64  , CONSTRUCT_SIGNAL_IN(wrenchDes, dynamicgraph::Vector)
65  , CONSTRUCT_SIGNAL_IN(q, dynamicgraph::Vector)
66  , CONSTRUCT_SIGNAL_INNER(kinematics_computations, int, m_qSIN)
67  , CONSTRUCT_SIGNAL_INNER(qp_computations, int, m_wrenchDesSIN << m_kinematics_computationsSINNER)
68  , CONSTRUCT_SIGNAL_OUT(wrenchLeft, dynamicgraph::Vector, m_qp_computationsSINNER)
69  , CONSTRUCT_SIGNAL_OUT(copLeft, dynamicgraph::Vector, m_wrenchLeftSOUT)
70  , CONSTRUCT_SIGNAL_OUT(wrenchRight, dynamicgraph::Vector, m_qp_computationsSINNER)
71  , CONSTRUCT_SIGNAL_OUT(copRight, dynamicgraph::Vector, m_wrenchRightSOUT)
72  , CONSTRUCT_SIGNAL_OUT(wrenchRef, dynamicgraph::Vector, m_wrenchLeftSOUT << m_wrenchRightSOUT)
73  , CONSTRUCT_SIGNAL_OUT(zmpRef, dynamicgraph::Vector, m_wrenchRefSOUT)
74  , CONSTRUCT_SIGNAL_OUT(emergencyStop, bool, m_zmpRefSOUT)
75  , m_initSucceeded(false)
76  , m_model()
77  , m_data(pinocchio::Model())
78  {
79  Entity::signalRegistration( INPUT_SIGNALS << OUTPUT_SIGNALS );
80 
81  /* Commands. */
82  addCommand("init", makeCommandVoid1(*this, &DistributeWrench::init, docCommandVoid1("Initialize the entity.","Robot name")));
83  }
84 
85  void DistributeWrench::init(const std::string& robotName)
86  {
87  if(!m_wrenchDesSIN.isPlugged())
88  return SEND_MSG("Init failed: signal wrenchDes is not plugged", MSG_TYPE_ERROR);
89  if(!m_qSIN.isPlugged())
90  return SEND_MSG("Init failed: signal q is not plugged", MSG_TYPE_ERROR);
91 
92  try
93  {
94  /* Retrieve m_robot_util informations */
95  std::string localName(robotName);
96  if (isNameInRobotUtil(localName))
97  {
98  m_robot_util = getRobotUtil(localName);
99 // std::cerr << "m_robot_util:" << m_robot_util << std::endl;
100  }
101  else
102  {
103  SEND_MSG("You should have a robotUtil pointer initialized before",MSG_TYPE_ERROR);
104  return;
105  }
106 
107  pinocchio::urdf::buildModel(m_robot_util->m_urdf_filename, pinocchio::JointModelFreeFlyer(), m_model);
108  m_data = pinocchio::Data(m_model);
109  }
110  catch (const std::exception& e)
111  {
112  std::cout << e.what();
113  SEND_MSG("Init failed: Could load URDF :" + m_robot_util->m_urdf_filename, MSG_TYPE_ERROR);
114  return;
115  }
116 
117 
118  assert(m_model.existFrame(m_robot_util->m_foot_util.m_Left_Foot_Frame_Name));
119  assert(m_model.existFrame(m_robot_util->m_foot_util.m_Right_Foot_Frame_Name));
120  m_left_foot_id = m_model.getFrameId(m_robot_util->m_foot_util.m_Left_Foot_Frame_Name);
121  m_right_foot_id = m_model.getFrameId(m_robot_util->m_foot_util.m_Right_Foot_Frame_Name);
122 
123  m_ankle_M_ftSens = pinocchio::SE3(Eigen::Matrix3d::Identity(), m_robot_util->m_foot_util.m_Right_Foot_Force_Sensor_XYZ.head<3>());
124 
125  // TODO: initialize m_qp1
126 
127  m_qp2.problem(12,6,0);
128 
129  m_initSucceeded = true;
130  }
131 
133  DistributeWrench::computeCoP(const dg::Vector & wrenchGlobal, const pinocchio::SE3 & pose) const
134  {
135  dg::Vector wrench = pose.actInv(pinocchio::Force(wrenchGlobal)).toVector();
136 
137  const double h = pose.translation()[2];
138 
139  const double fx = wrench[0];
140  const double fy = wrench[1];
141  const double fz = wrench[2];
142  const double tx = wrench[3];
143  const double ty = wrench[4];
144 
145  double m_eps = 0.1; // temporary
146 
147  double px, py;
148 
149  if(fz >= m_eps/2)
150  {
151  px = (- ty - fx*h)/fz;
152  py = ( tx - fy*h)/fz;
153  }
154  else
155  {
156  px = 0.0;
157  py = 0.0;
158  }
159  const double pz = 0.0;
160 
161  dg::Vector cop(3);
162  cop[0] = px;
163  cop[1] = py;
164  cop[2] = pz;
165 
166  return cop;
167  }
168 
169  /* ------------------------------------------------------------------- */
170  /* --- SIGNALS ------------------------------------------------------- */
171  /* ------------------------------------------------------------------- */
172 
173  DEFINE_SIGNAL_INNER_FUNCTION(kinematics_computations, int)
174  {
175  if(!m_initSucceeded)
176  {
177  SEND_WARNING_STREAM_MSG("Cannot compute signal kinematics_computations before initialization!");
178  return s;
179  }
180 
181  const Eigen::VectorXd & q = m_qSIN(iter);
182  assert(q.size()==m_model.nq && "Unexpected size of signal q");
183 
185 
186  /* Compute kinematics */
187 // m_q_pin.head<6>().setZero();
188 // m_q_pin(6) = 1.0;
189  pinocchio::framesForwardKinematics(m_model, m_data, q);
190 
192 
193  return s;
194  }
195 
196  DEFINE_SIGNAL_INNER_FUNCTION(qp_computations, int)
197  {
198  if(!m_initSucceeded)
199  {
200  SEND_WARNING_STREAM_MSG("Cannot compute signal qp_computations before initialization!");
201  return s;
202  }
203 
204  const Eigen::VectorXd & wrenchDes = m_wrenchDesSIN(iter);
205  m_kinematics_computationsSINNER(iter);
206  assert(wrenchDes.size()==6 && "Unexpected size of signal q");
207 
208  getProfiler().start(PROFILE_DISTRIBUTE_WRENCH_QP_COMPUTATIONS);
209 
210  // stub
211  Eigen::MatrixXd Q(12,12);
212  Q.setIdentity();
213 
214  Eigen::VectorXd C(12);
215  C.setZero();
216 
217  Eigen::MatrixXd Aeq(6,12);
218  Aeq.block<6,6>(0,0).setIdentity();
219  Aeq.block<6,6>(0,6).setIdentity();
220 
221  Eigen::VectorXd Beq(6);
222  Beq = wrenchDes;
223 
224  Eigen::MatrixXd Aineq(0,12);
225 
226  Eigen::VectorXd Bineq(0);
227 
228  bool success = m_qp2.solve(Q, C, Aeq, Beq, Aineq, Bineq);
229 
230  m_emergency_stop_triggered = !success;
231 
232  if(!success)
233  {
234  SEND_WARNING_STREAM_MSG("No solution to the QP problem!");
235  return s;
236  }
237 
238  Eigen::VectorXd result = m_qp2.result();
239 
240  m_wrenchLeft = result.head<6>();
241  m_wrenchRight = result.tail<6>();
242 
243  getProfiler().stop(PROFILE_DISTRIBUTE_WRENCH_QP_COMPUTATIONS);
244 
245  return s;
246  }
247 
249  {
250  if(!m_initSucceeded)
251  {
252  SEND_WARNING_STREAM_MSG("Cannot compute signal wrenchLeft before initialization!");
253  return s;
254  }
255 
256  m_qp_computationsSINNER(iter);
257  s = m_wrenchLeft;
258  return s;
259  }
260 
262  {
263  if(!m_initSucceeded)
264  {
265  SEND_WARNING_STREAM_MSG("Cannot compute signal wrenchRight before initialization!");
266  return s;
267  }
268 
269  m_qp_computationsSINNER(iter);
270  s = m_wrenchRight;
271  return s;
272  }
273 
275  {
276  if(!m_initSucceeded)
277  {
278  SEND_WARNING_STREAM_MSG("Cannot compute signal copLeft before initialization!");
279  return s;
280  }
281 
282  const Eigen::VectorXd & wrenchLeft = m_wrenchLeftSOUT(iter);
283 
284  // stub
285  const pinocchio::SE3 pose = m_data.oMf[m_left_foot_id] * m_ankle_M_ftSens;
286 
287  s = computeCoP(wrenchLeft,pose);
288 
289  return s;
290  }
291 
293  {
294  if(!m_initSucceeded)
295  {
296  SEND_WARNING_STREAM_MSG("Cannot compute signal copRight before initialization!");
297  return s;
298  }
299 
300  const Eigen::VectorXd & wrenchRight = m_wrenchRightSOUT(iter);
301 
302  const pinocchio::SE3 pose = m_data.oMf[m_right_foot_id] * m_ankle_M_ftSens;
303 
304  s = computeCoP(wrenchRight,pose);
305 
306  return s;
307  }
308 
310  {
311  if(!m_initSucceeded)
312  {
313  SEND_WARNING_STREAM_MSG("Cannot compute signal wrenchRef before initialization!");
314  return s;
315  }
316 
317  const Eigen::VectorXd & wrenchLeft = m_wrenchLeftSOUT(iter);
318  const Eigen::VectorXd & wrenchRight = m_wrenchRightSOUT(iter);
319 
320  s = wrenchLeft + wrenchRight;
321 
322  return s;
323  }
324 
326  {
327  if(!m_initSucceeded)
328  {
329  SEND_WARNING_STREAM_MSG("Cannot compute signal zmpRef before initialization!");
330  return s;
331  }
332 
333  const Eigen::VectorXd & wrenchRef = m_wrenchRefSOUT(iter);
334 
335  //const double fx = wrenchRef[0];
336  //const double fy = wrenchRef[1];
337  const double fz = wrenchRef[2];
338  const double tx = wrenchRef[3];
339  const double ty = wrenchRef[4];
340 
341  double m_eps = 0.1; // temporary
342 
343  double px, py;
344  if(fz >= m_eps/2)
345  {
346  px = -ty/fz;
347  py = tx/fz;
348  }
349  else
350  {
351  px = 0.0;
352  py = 0.0;
353  }
354  const double pz = 0.0;
355 
356  dg::Vector zmp(3);
357  zmp[0] = px;
358  zmp[1] = py;
359  zmp[2] = pz;
360 
361  s = zmp;
362 
363  return s;
364  }
365 
366  DEFINE_SIGNAL_OUT_FUNCTION(emergencyStop, bool)
367  {
368  const dynamicgraph::Vector & zmp = m_zmpRefSOUT(iter); // dummy to trigger zmp computation
369  (void) zmp; // disable unused variable warning
371  return s;
372  }
373 
374  /* --- COMMANDS ---------------------------------------------------------- */
375 
376  /* ------------------------------------------------------------------- */
377  /* --- ENTITY -------------------------------------------------------- */
378  /* ------------------------------------------------------------------- */
379 
380  void DistributeWrench::display(std::ostream& os) const
381  {
382  os << "DistributeWrench " << getName();
383  try
384  {
385  getProfiler().report_all(3, os);
386  }
387  catch (ExceptionSignal e) {}
388  }
389 
390  } // namespace talos_balance
391  } // namespace sot
392 } // namespace dynamicgraph
393 
#define PROFILE_DISTRIBUTE_WRENCH_KINEMATICS_COMPUTATIONS
dynamicgraph::Vector computeCoP(const dynamicgraph::Vector &wrench, const pinocchio::SE3 &pose) const
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 F/T sensor transformation
RobotUtilShrPtr m_robot_util
Pinocchio robot data.
#define PROFILE_DISTRIBUTE_WRENCH_QP_COMPUTATIONS
#define INPUT_SIGNALS
virtual void display(std::ostream &os) const
pinocchio::Model m_model
true if the entity has been successfully initialized