sot-talos-balance  1.5.0
dummy-walking-pattern-generator.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/command-bind.h>
22 
23 #include <dynamic-graph/all-commands.h>
24 #include <sot/core/stop-watch.hh>
25 
26 namespace dynamicgraph
27 {
28  namespace sot
29  {
30  namespace talos_balance
31  {
32  namespace dg = ::dynamicgraph;
33  using namespace dg;
34  using namespace dg::command;
35 
36 //Size to be aligned "-------------------------------------------------------"
37 #define PROFILE_DUMMYWALKINGPATTERNGENERATOR_DCM_COMPUTATION "DummyWalkingPatternGenerator: dcm computation "
38 
39 #define INPUT_SIGNALS m_omegaSIN << m_footLeftSIN << m_footRightSIN << m_waistSIN << m_comSIN << m_vcomSIN << m_acomSIN << m_zmpSIN << m_referenceFrameSIN
40 
41 #define INNER_SIGNALS m_rfSINNER
42 
43 #define OUTPUT_SIGNALS m_comDesSOUT << m_vcomDesSOUT << m_acomDesSOUT << m_dcmDesSOUT << m_zmpDesSOUT << m_footLeftDesSOUT << m_footRightDesSOUT << m_waistDesSOUT
44 
47  typedef DummyWalkingPatternGenerator EntityClassName;
48 
49  /* --- DG FACTORY ---------------------------------------------------- */
50  DYNAMICGRAPH_FACTORY_ENTITY_PLUGIN(DummyWalkingPatternGenerator,
51  "DummyWalkingPatternGenerator");
52 
53  /* ------------------------------------------------------------------- */
54  /* --- CONSTRUCTION -------------------------------------------------- */
55  /* ------------------------------------------------------------------- */
57  : Entity(name)
58  , CONSTRUCT_SIGNAL_IN(omega, double)
59  , CONSTRUCT_SIGNAL_IN(footLeft, MatrixHomogeneous)
60  , CONSTRUCT_SIGNAL_IN(footRight, MatrixHomogeneous)
61  , CONSTRUCT_SIGNAL_IN(waist, MatrixHomogeneous)
62  , CONSTRUCT_SIGNAL_IN(com, dynamicgraph::Vector)
63  , CONSTRUCT_SIGNAL_IN(vcom, dynamicgraph::Vector)
64  , CONSTRUCT_SIGNAL_IN(acom, dynamicgraph::Vector)
65  , CONSTRUCT_SIGNAL_IN(zmp, dynamicgraph::Vector)
66  , CONSTRUCT_SIGNAL_IN(referenceFrame, MatrixHomogeneous)
67  , CONSTRUCT_SIGNAL_INNER(rf, MatrixHomogeneous, m_referenceFrameSIN)
68  , CONSTRUCT_SIGNAL_OUT(comDes, dynamicgraph::Vector, m_comSIN << m_rfSINNER)
69  , CONSTRUCT_SIGNAL_OUT(vcomDes, dynamicgraph::Vector, m_vcomSIN << m_rfSINNER)
70  , CONSTRUCT_SIGNAL_OUT(acomDes, dynamicgraph::Vector, m_acomSIN << m_rfSINNER)
71  , CONSTRUCT_SIGNAL_OUT(dcmDes, dynamicgraph::Vector, m_omegaSIN << m_comDesSOUT << m_vcomDesSOUT)
72  , CONSTRUCT_SIGNAL_OUT(zmpDes, dynamicgraph::Vector, m_omegaSIN << m_comDesSOUT << m_acomDesSOUT << m_rfSINNER << m_zmpSIN)
73  , CONSTRUCT_SIGNAL_OUT(footLeftDes, MatrixHomogeneous, m_footLeftSIN << m_rfSINNER)
74  , CONSTRUCT_SIGNAL_OUT(footRightDes, MatrixHomogeneous, m_footRightSIN << m_rfSINNER)
75  , CONSTRUCT_SIGNAL_OUT(waistDes, MatrixHomogeneous, m_waistSIN << m_rfSINNER)
76  , m_initSucceeded(false)
77  {
78  Entity::signalRegistration( INPUT_SIGNALS << OUTPUT_SIGNALS );
79 
80  /* Commands. */
81  addCommand("init", makeCommandVoid0(*this, &DummyWalkingPatternGenerator::init, docCommandVoid0("Initialize the entity.")));
82  }
83 
85  {
86  return m.linear().transpose()*(v-m.translation());
87  }
88 
89  MatrixHomogeneous DummyWalkingPatternGenerator::actInv(MatrixHomogeneous m1, MatrixHomogeneous m2)
90  {
91  MatrixHomogeneous res;
92  res.linear() = m1.linear().transpose()*m2.linear();
93  res.translation() = m1.linear().transpose()*(m2.translation()-m1.translation());
94  return res;
95  }
96 
98  {
99  m_initSucceeded = true;
100  }
101 
102  /* ------------------------------------------------------------------- */
103  /* --- SIGNALS ------------------------------------------------------- */
104  /* ------------------------------------------------------------------- */
105 
106  DEFINE_SIGNAL_INNER_FUNCTION(rf, MatrixHomogeneous)
107  {
108  if(!m_initSucceeded)
109  {
110  SEND_WARNING_STREAM_MSG("Cannot compute signal rf before initialization!");
111  return s;
112  }
113 
114  s = m_referenceFrameSIN.isPlugged() ? m_referenceFrameSIN(iter) : MatrixHomogeneous::Identity();
115 
116  return s;
117  }
118 
120  {
121  if(!m_initSucceeded)
122  {
123  SEND_WARNING_STREAM_MSG("Cannot compute signal comDes before initialization!");
124  return s;
125  }
126 
127  const Vector & com = m_comSIN(iter);
128  const MatrixHomogeneous & referenceFrame = m_rfSINNER(iter);
129 
130  assert( com.size()==3 && "Unexpected size of signal com" );
131 
132  const Vector comDes = actInv(referenceFrame, com);
133 
134  s = comDes;
135 
136  return s;
137  }
138 
140  {
141  if(!m_initSucceeded)
142  {
143  SEND_WARNING_STREAM_MSG("Cannot compute signal vcomDes before initialization!");
144  return s;
145  }
146 
147  const Vector & vcom = m_vcomSIN(iter);
148  const MatrixHomogeneous & referenceFrame = m_rfSINNER(iter);
149 
150  assert( vcom.size()==3 && "Unexpected size of signal vcom" );
151 
152  const Vector vcomDes(referenceFrame.linear().transpose()*vcom);
153 
154  s = vcomDes;
155 
156  return s;
157  }
158 
160  {
161  if(!m_initSucceeded)
162  {
163  SEND_WARNING_STREAM_MSG("Cannot compute signal acomDes before initialization!");
164  return s;
165  }
166 
167  const Vector & acom = m_acomSIN(iter);
168  const MatrixHomogeneous & referenceFrame = m_rfSINNER(iter);
169 
170  assert( acom.size()==3 && "Unexpected size of signal acom" );
171 
172  const Vector acomDes(referenceFrame.linear().transpose()*acom);
173 
174  s = acomDes;
175 
176  return s;
177  }
178 
180  {
181  if(!m_initSucceeded)
182  {
183  SEND_WARNING_STREAM_MSG("Cannot compute signal dcmDes before initialization!");
184  return s;
185  }
186 
187  const double & omega = m_omegaSIN(iter);
188  const Vector & comDes = m_comDesSOUT(iter);
189  const Vector & vcomDes = m_vcomDesSOUT(iter);
190 
191  const Vector dcmDes = comDes + vcomDes/omega;
192 
193  s = dcmDes;
194 
195  return s;
196  }
197 
199  {
200  if(!m_initSucceeded)
201  {
202  SEND_WARNING_STREAM_MSG("Cannot compute signal zmpDes before initialization!");
203  return s;
204  }
205 
206  Vector zmpDes;
207 
208  if(m_zmpSIN.isPlugged())
209  {
210  const Vector & zmp = m_zmpSIN(iter);
211  const MatrixHomogeneous & referenceFrame = m_rfSINNER(iter);
212 
213  zmpDes.resize(3);
214  zmpDes[0] = zmp[0];
215  zmpDes[1] = zmp[1];
216  zmpDes[2] = zmp.size()>2 ? zmp[2] : 0.;
217 
218  zmpDes = actInv(referenceFrame, zmpDes);
219  }
220  else
221  {
222  const double & omega = m_omegaSIN(iter);
223  const Vector & comDes = m_comDesSOUT(iter);
224  const Vector & acomDes = m_acomDesSOUT(iter);
225 
226  zmpDes = comDes - acomDes/(omega*omega);
227  zmpDes[2] = 0.0;
228  }
229 
230  s = zmpDes;
231 
232  return s;
233  }
234 
235  DEFINE_SIGNAL_OUT_FUNCTION(footLeftDes, MatrixHomogeneous)
236  {
237  if(!m_initSucceeded)
238  {
239  SEND_WARNING_STREAM_MSG("Cannot compute signal footLeftDes before initialization!");
240  return s;
241  }
242 
243  MatrixHomogeneous footLeft = m_footLeftSIN(iter);
244 
245  const MatrixHomogeneous & referenceFrame = m_rfSINNER(iter);
246 
247  s = actInv(referenceFrame, footLeft);
248 
249  return s;
250  }
251 
252  DEFINE_SIGNAL_OUT_FUNCTION(footRightDes, MatrixHomogeneous)
253  {
254  if(!m_initSucceeded)
255  {
256  SEND_WARNING_STREAM_MSG("Cannot compute signal footRightDes before initialization!");
257  return s;
258  }
259 
260  MatrixHomogeneous footRight = m_footRightSIN(iter);
261 
262  const MatrixHomogeneous & referenceFrame = m_rfSINNER(iter);
263 
264  s = actInv(referenceFrame, footRight);
265 
266  return s;
267  }
268 
269  DEFINE_SIGNAL_OUT_FUNCTION(waistDes, MatrixHomogeneous)
270  {
271  if(!m_initSucceeded)
272  {
273  SEND_WARNING_STREAM_MSG("Cannot compute signal waistDes before initialization!");
274  return s;
275  }
276 
277  const MatrixHomogeneous & waist = m_waistSIN(iter);
278  const MatrixHomogeneous & referenceFrame = m_rfSINNER(iter);
279 
280  s = actInv(referenceFrame, waist);
281 
282  return s;
283  }
284 
285  /* --- COMMANDS ---------------------------------------------------------- */
286 
287  /* ------------------------------------------------------------------- */
288  /* --- ENTITY -------------------------------------------------------- */
289  /* ------------------------------------------------------------------- */
290 
291  void DummyWalkingPatternGenerator::display(std::ostream& os) const
292  {
293  os << "DummyWalkingPatternGenerator " << getName();
294  try
295  {
296  getProfiler().report_all(3, os);
297  }
298  catch (ExceptionSignal e) {}
299  }
300  } // namespace talos_balance
301  } // namespace sot
302 } // namespace dynamicgraph
303 
Eigen::Matrix< Scalar, Eigen::Dynamic, 1 > Vector
Definition: math/fwd.hh:40
DYNAMICGRAPH_FACTORY_ENTITY_PLUGIN(AdmittanceControllerEndEffector, "AdmittanceControllerEndEffector")
EIGEN_MAKE_ALIGNED_OPERATOR_NEW DummyWalkingPatternGenerator(const std::string &name)
DEFINE_SIGNAL_INNER_FUNCTION(w_force, dynamicgraph::Vector)
dynamicgraph::Vector actInv(MatrixHomogeneous m, dynamicgraph::Vector v)
true if the entity has been successfully initialized