sot-talos-balance  1.5.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 <sot/core/debug.hh>
20 #include <dynamic-graph/factory.h>
21 #include <dynamic-graph/all-commands.h>
22 
23 #include <sot/core/stop-watch.hh>
24 
25 #include <pinocchio/parsers/urdf.hpp>
26 #include <pinocchio/algorithm/kinematics.hpp>
27 #include <pinocchio/algorithm/frames.hpp>
28 
29 namespace dynamicgraph
30 {
31  namespace sot
32  {
33  namespace talos_balance
34  {
35  namespace dg = ::dynamicgraph;
36  using namespace dg;
37  using namespace dg::command;
38 
39 //Size to be aligned "-------------------------------------------------------"
40 #define PROFILE_SIMPLE_DISTRIBUTE_WRENCH_KINEMATICS_COMPUTATIONS "SimpleDistributeWrench: kinematics computations "
41 #define PROFILE_SIMPLE_DISTRIBUTE_WRENCH_WRENCHES_COMPUTATIONS "SimpleDistributeWrench: wrenches computations "
42 
43 #define INPUT_SIGNALS m_wrenchDesSIN << m_qSIN << m_rhoSIN
44 
45 #define INNER_SIGNALS m_kinematics_computations << m_wrenches
46 
47 #define OUTPUT_SIGNALS m_wrenchLeftSOUT << m_wrenchRightSOUT << m_wrenchRefSOUT << m_zmpRefSOUT
48 
51  typedef SimpleDistributeWrench EntityClassName;
52 
53  /* --- DG FACTORY ---------------------------------------------------- */
54  DYNAMICGRAPH_FACTORY_ENTITY_PLUGIN(SimpleDistributeWrench,
55  "SimpleDistributeWrench");
56 
57  /* ------------------------------------------------------------------- */
58  /* --- CONSTRUCTION -------------------------------------------------- */
59  /* ------------------------------------------------------------------- */
61  : Entity(name)
62  , CONSTRUCT_SIGNAL_IN(wrenchDes, dynamicgraph::Vector)
63  , CONSTRUCT_SIGNAL_IN(q, dynamicgraph::Vector)
64  , CONSTRUCT_SIGNAL_IN(rho, double)
65  , CONSTRUCT_SIGNAL_INNER(kinematics_computations, int, m_qSIN)
66  , CONSTRUCT_SIGNAL_INNER(wrenches, int, m_wrenchDesSIN << m_kinematics_computationsSINNER)
67  , CONSTRUCT_SIGNAL_OUT(wrenchLeft, dynamicgraph::Vector, m_wrenchesSINNER)
68  , CONSTRUCT_SIGNAL_OUT(wrenchRight, dynamicgraph::Vector, m_wrenchesSINNER)
69  , CONSTRUCT_SIGNAL_OUT(wrenchRef, dynamicgraph::Vector, m_wrenchLeftSOUT << m_wrenchRightSOUT)
70  , CONSTRUCT_SIGNAL_OUT(zmpRef, dynamicgraph::Vector, m_wrenchRefSOUT)
71  , m_initSucceeded(false)
72  , m_model()
73  , m_data(pinocchio::Model())
74  {
75  Entity::signalRegistration( INPUT_SIGNALS << OUTPUT_SIGNALS );
76 
77  /* Commands. */
78  addCommand("init", makeCommandVoid1(*this, &SimpleDistributeWrench::init, docCommandVoid1("Initialize the entity.","Robot name")));
79  }
80 
81  void SimpleDistributeWrench::init(const std::string& robotName)
82  {
83  if(!m_wrenchDesSIN.isPlugged())
84  return SEND_MSG("Init failed: signal wrenchDes is not plugged", MSG_TYPE_ERROR);
85  if(!m_qSIN.isPlugged())
86  return SEND_MSG("Init failed: signal q is not plugged", MSG_TYPE_ERROR);
87 
88  try
89  {
90  /* Retrieve m_robot_util informations */
91  std::string localName(robotName);
92  if (isNameInRobotUtil(localName))
93  {
94  m_robot_util = getRobotUtil(localName);
95 // std::cerr << "m_robot_util:" << m_robot_util << std::endl;
96  }
97  else
98  {
99  SEND_MSG("You should have a robotUtil pointer initialized before",MSG_TYPE_ERROR);
100  return;
101  }
102 
103  pinocchio::urdf::buildModel(m_robot_util->m_urdf_filename, pinocchio::JointModelFreeFlyer(), m_model);
104  m_data = pinocchio::Data(m_model);
105  }
106  catch (const std::exception& e)
107  {
108  std::cout << e.what();
109  SEND_MSG("Init failed: Could load URDF :" + m_robot_util->m_urdf_filename, MSG_TYPE_ERROR);
110  return;
111  }
112 
113  m_initSucceeded = true;
114  }
115 
116  /* ------------------------------------------------------------------- */
117  /* --- SIGNALS ------------------------------------------------------- */
118  /* ------------------------------------------------------------------- */
119 
120  DEFINE_SIGNAL_INNER_FUNCTION(kinematics_computations, int)
121  {
122  if(!m_initSucceeded)
123  {
124  SEND_WARNING_STREAM_MSG("Cannot compute signal kinematics_computations before initialization!");
125  return s;
126  }
127 
128  const Eigen::VectorXd & q = m_qSIN(iter);
129  assert(q.size()==m_model.nq && "Unexpected size of signal q");
130 
132 
133  pinocchio::framesForwardKinematics(m_model, m_data, q);
134 
136 
137  return s;
138  }
139 
141  {
142  if(!m_initSucceeded)
143  {
144  SEND_WARNING_STREAM_MSG("Cannot compute signal wrenches before initialization!");
145  return s;
146  }
147 
148  const double rho_m = 0.1;
149  const double rho_M = 1.0 - rho_m;
150 
151  const Eigen::VectorXd & wrenchDes = m_wrenchDesSIN(iter);
152 
153  double rho = m_rhoSIN.isPlugged() ? m_rhoSIN(iter) : 0.5;
154  if(rho<rho_m)
155  rho = rho_m;
156  else if(rho>rho_M)
157  rho = rho_M;
158 
159  m_kinematics_computationsSINNER(iter);
160 
161  assert(wrenchDes.size()==6 && "Unexpected size of signal wrenchDes");
162 
164 
165  // stub
166  m_wrenchLeft = wrenchDes/2;
167  m_wrenchRight = wrenchDes/2;
168 
169  m_wrenchLeft[2] = (1-rho) * wrenchDes[2];
170  m_wrenchRight[2] = rho * wrenchDes[2];
171 
173 
174  return s;
175  }
176 
178  {
179  if(!m_initSucceeded)
180  {
181  SEND_WARNING_STREAM_MSG("Cannot compute signal wrenchLeft before initialization!");
182  return s;
183  }
184 
185  m_wrenchesSINNER(iter);
186  s = m_wrenchLeft;
187  return s;
188  }
189 
191  {
192  if(!m_initSucceeded)
193  {
194  SEND_WARNING_STREAM_MSG("Cannot compute signal wrenchRight before initialization!");
195  return s;
196  }
197 
198  m_wrenchesSINNER(iter);
199  s = m_wrenchRight;
200  return s;
201  }
202 
204  {
205  if(!m_initSucceeded)
206  {
207  SEND_WARNING_STREAM_MSG("Cannot compute signal wrenchRef before initialization!");
208  return s;
209  }
210 
211  const Eigen::VectorXd & wrenchLeft = m_wrenchLeftSOUT(iter);
212  const Eigen::VectorXd & wrenchRight = m_wrenchRightSOUT(iter);
213 
214  s = wrenchLeft + wrenchRight;
215 
216  return s;
217  }
218 
220  {
221  if(!m_initSucceeded)
222  {
223  SEND_WARNING_STREAM_MSG("Cannot compute signal zmpRef before initialization!");
224  return s;
225  }
226 
227  const Eigen::VectorXd & wrenchRef = m_wrenchRefSOUT(iter);
228 
229  const double fx = wrenchRef[0];
230  const double fy = wrenchRef[1];
231  const double fz = wrenchRef[2];
232  const double tx = wrenchRef[3];
233  const double ty = wrenchRef[4];
234 
235  double m_eps = 0.1; // temporary
236 
237  double px, py;
238  if(fz >= m_eps/2)
239  {
240  px = -ty/fz;
241  py = tx/fz;
242  }
243  else
244  {
245  px = 0.0;
246  py = 0.0;
247  }
248  const double pz = 0.0;
249 
250  dg::Vector zmp(3);
251  zmp[0] = px;
252  zmp[1] = py;
253  zmp[2] = pz;
254 
255  s = zmp;
256 
257  return s;
258  }
259 
260 
261  /* --- COMMANDS ---------------------------------------------------------- */
262 
263  /* ------------------------------------------------------------------- */
264  /* --- ENTITY -------------------------------------------------------- */
265  /* ------------------------------------------------------------------- */
266 
267  void SimpleDistributeWrench::display(std::ostream& os) const
268  {
269  os << "SimpleDistributeWrench " << getName();
270  try
271  {
272  getProfiler().report_all(3, os);
273  }
274  catch (ExceptionSignal e) {}
275  }
276 
277  } // namespace talos_balance
278  } // namespace sot
279 } // namespace dynamicgraph
280 
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
DEFINE_SIGNAL_INNER_FUNCTION(w_force, dynamicgraph::Vector)
EIGEN_MAKE_ALIGNED_OPERATOR_NEW SimpleDistributeWrench(const std::string &name)
#define INPUT_SIGNALS
#define OUTPUT_SIGNALS