sot-talos-balance  1.6.0
foot-force-difference-controller.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 <sot/core/stop-watch.hh>
21 #include <dynamic-graph/factory.h>
22 #include <dynamic-graph/command-bind.h>
23 #include <dynamic-graph/all-commands.h>
24 
25 namespace dynamicgraph
26 {
27  namespace sot
28  {
29  namespace talos_balance
30  {
31  namespace dg = ::dynamicgraph;
32  using namespace dg;
33  using namespace dg::command;
34 
35 #define INPUT_SIGNALS m_phaseSIN << m_gainSwingSIN << m_gainStanceSIN << m_gainDoubleSIN << m_dfzAdmittanceSIN << m_vdcFrequencySIN << m_vdcDampingSIN << m_wrenchRightDesSIN << m_wrenchLeftDesSIN << m_wrenchRightSIN << m_wrenchLeftSIN << m_posRightDesSIN << m_posLeftDesSIN << m_posRightSIN << m_posLeftSIN
36 
37 #define INNER_SIGNALS m_dz_ctrlSOUT << m_dz_posSOUT
38 
39 #define OUTPUT_SIGNALS m_vRightSOUT << m_vLeftSOUT << m_gainRightSOUT << m_gainLeftSOUT
40 
43  typedef FootForceDifferenceController EntityClassName;
44 
45  /* --- DG FACTORY ---------------------------------------------------- */
46  DYNAMICGRAPH_FACTORY_ENTITY_PLUGIN(FootForceDifferenceController,
47  "FootForceDifferenceController");
48 
49  /* ------------------------------------------------------------------- */
50  /* --- CONSTRUCTION -------------------------------------------------- */
51  /* ------------------------------------------------------------------- */
53  : Entity(name)
54  , CONSTRUCT_SIGNAL_IN(phase, int)
55  , CONSTRUCT_SIGNAL_IN(gainSwing, double)
56  , CONSTRUCT_SIGNAL_IN(gainStance, double)
57  , CONSTRUCT_SIGNAL_IN(gainDouble, double)
58  , CONSTRUCT_SIGNAL_IN(dfzAdmittance, double)
59  , CONSTRUCT_SIGNAL_IN(vdcFrequency, double)
60  , CONSTRUCT_SIGNAL_IN(vdcDamping, double)
61  , CONSTRUCT_SIGNAL_IN(wrenchRightDes, dynamicgraph::Vector)
62  , CONSTRUCT_SIGNAL_IN(wrenchLeftDes, dynamicgraph::Vector)
63  , CONSTRUCT_SIGNAL_IN(wrenchRight, dynamicgraph::Vector)
64  , CONSTRUCT_SIGNAL_IN(wrenchLeft, dynamicgraph::Vector)
65  , CONSTRUCT_SIGNAL_IN(posRightDes, MatrixHomogeneous)
66  , CONSTRUCT_SIGNAL_IN(posLeftDes, MatrixHomogeneous)
67  , CONSTRUCT_SIGNAL_IN(posRight, MatrixHomogeneous)
68  , CONSTRUCT_SIGNAL_IN(posLeft, MatrixHomogeneous)
69  , CONSTRUCT_SIGNAL_INNER(dz_ctrl, double, m_dfzAdmittanceSIN << m_vdcDampingSIN << m_wrenchRightDesSIN << m_wrenchLeftDesSIN << m_wrenchRightSIN << m_wrenchLeftSIN << m_posRightSIN << m_posLeftSIN)
70  , CONSTRUCT_SIGNAL_INNER(dz_pos, double, m_vdcFrequencySIN << m_posRightDesSIN << m_posLeftDesSIN << m_posRightSIN << m_posLeftSIN)
71  , CONSTRUCT_SIGNAL_OUT(vRight, dynamicgraph::Vector, m_phaseSIN << m_dz_ctrlSINNER << m_dz_posSINNER)
72  , CONSTRUCT_SIGNAL_OUT(vLeft, dynamicgraph::Vector, m_phaseSIN << m_dz_ctrlSINNER << m_dz_posSINNER)
73  , CONSTRUCT_SIGNAL_OUT(gainRight, double, m_phaseSIN << m_gainSwingSIN << m_gainStanceSIN << m_gainDoubleSIN)
74  , CONSTRUCT_SIGNAL_OUT(gainLeft, double, m_phaseSIN << m_gainSwingSIN << m_gainStanceSIN << m_gainDoubleSIN)
75  , m_initSucceeded(false)
76  {
77  Entity::signalRegistration( INPUT_SIGNALS << OUTPUT_SIGNALS );
78 
79  /* Commands. */
80  addCommand("init", makeCommandVoid0(*this, &FootForceDifferenceController::init, docCommandVoid0("Initialize the entity.")));
81  }
82 
84  {
85  m_initSucceeded = true;
86  }
87 
88  /* ------------------------------------------------------------------- */
89  /* --- SIGNALS ------------------------------------------------------- */
90  /* ------------------------------------------------------------------- */
91 
93  {
94  if(!m_initSucceeded)
95  {
96  SEND_WARNING_STREAM_MSG("Can't compute dz_ctrl before initialization!");
97  return s;
98  }
99 
100  const double & dfzAdmittance = m_dfzAdmittanceSIN(iter);
101  const double & vdcDamping = m_vdcDampingSIN(iter);
102 
103  const Eigen::VectorXd & wrenchRightDes = m_wrenchRightDesSIN(iter);
104  const Eigen::VectorXd & wrenchLeftDes = m_wrenchLeftDesSIN(iter);
105  const Eigen::VectorXd & wrenchRight = m_wrenchRightSIN(iter);
106  const Eigen::VectorXd & wrenchLeft = m_wrenchLeftSIN(iter);
107 
108  const MatrixHomogeneous & posRight = m_posRightSIN(iter);
109  const MatrixHomogeneous & posLeft = m_posLeftSIN(iter);
110 
111  const double RTz = posRight.translation()[2];
112  const double LTz = posLeft.translation()[2];
113 
114  const double RFz_d = wrenchRightDes[2];
115  const double LFz_d = wrenchLeftDes[2];
116 
117  const double RFz = wrenchRight[2];
118  const double LFz = wrenchLeft[2];
119 
120  const double dz_ctrl = dfzAdmittance * ((LFz_d - RFz_d) - (LFz - RFz)) - vdcDamping * (RTz - LTz);
121 
122  s = dz_ctrl;
123 
124  return s;
125  }
126 
128  {
129  if(!m_initSucceeded)
130  {
131  SEND_WARNING_STREAM_MSG("Can't compute dz_pos before initialization!");
132  return s;
133  }
134 
135  const double & vdcFrequency = m_vdcFrequencySIN(iter);
136 
137  const MatrixHomogeneous & posRightDes = m_posRightDesSIN(iter);
138  const MatrixHomogeneous & posLeftDes = m_posLeftDesSIN(iter);
139  const MatrixHomogeneous & posRight = m_posRightSIN(iter);
140  const MatrixHomogeneous & posLeft = m_posLeftSIN(iter);
141 
142  const double RTz_d = posRightDes.translation()[2];
143  const double LTz_d = posLeftDes.translation()[2];
144 
145  const double RTz = posRight.translation()[2];
146  const double LTz = posLeft.translation()[2];
147 
148  const double dz_pos = vdcFrequency * ((LTz_d + RTz_d) - (LTz + RTz));
149 
150  s = dz_pos;
151 
152  return s;
153  }
154 
156  {
157  if(!m_initSucceeded)
158  {
159  SEND_WARNING_STREAM_MSG("Can't compute vRight before initialization!");
160  return s;
161  }
162  if(s.size()!=6)
163  s.resize(6);
164 
165  const double & dz_ctrl = m_dz_ctrlSINNER(iter);
166  const double & dz_pos = m_dz_posSINNER(iter);
167  const int & phase = m_phaseSIN(iter);
168 
169  s.setZero(6);
170 
171  if(phase==0)
172  s[2] = 0.5 * (dz_pos + dz_ctrl);
173 
174  return s;
175  }
176 
178  {
179  if(!m_initSucceeded)
180  {
181  SEND_WARNING_STREAM_MSG("Can't compute vLeft before initialization!");
182  return s;
183  }
184  if(s.size()!=6)
185  s.resize(6);
186 
187  const double & dz_ctrl = m_dz_ctrlSINNER(iter);
188  const double & dz_pos = m_dz_posSINNER(iter);
189  const int & phase = m_phaseSIN(iter);
190 
191  s.setZero(6);
192 
193  if(phase==0)
194  s[2] = 0.5 * (dz_pos - dz_ctrl);
195 
196  return s;
197  }
198 
199  DEFINE_SIGNAL_OUT_FUNCTION(gainRight, double)
200  {
201  if(!m_initSucceeded)
202  {
203  SEND_WARNING_STREAM_MSG("Can't compute gainRight before initialization!");
204  return s;
205  }
206 
207  const int & phase = m_phaseSIN(iter);
208  const double & gainSwing = m_gainSwingSIN(iter);
209  const double & gainStance = m_gainStanceSIN(iter);
210  const double & gainDouble = m_gainDoubleSIN(iter);
211 
212  if(phase>0)
213  s = gainSwing;
214  else if (phase<0)
215  s = gainStance;
216  else
217  s = gainDouble;
218 
219  return s;
220  }
221 
222  DEFINE_SIGNAL_OUT_FUNCTION(gainLeft, double)
223  {
224  if(!m_initSucceeded)
225  {
226  SEND_WARNING_STREAM_MSG("Can't compute gainLeft before initialization!");
227  return s;
228  }
229 
230  const int & phase = m_phaseSIN(iter);
231  const double & gainSwing = m_gainSwingSIN(iter);
232  const double & gainStance = m_gainStanceSIN(iter);
233  const double & gainDouble = m_gainDoubleSIN(iter);
234 
235  if(phase>0)
236  s = gainStance;
237  else if (phase<0)
238  s = gainSwing;
239  else
240  s = gainDouble;
241 
242  return s;
243  }
244 
245  /* --- COMMANDS ---------------------------------------------------------- */
246 
247  /* ------------------------------------------------------------------- */
248  /* --- ENTITY -------------------------------------------------------- */
249  /* ------------------------------------------------------------------- */
250 
251  void FootForceDifferenceController::display(std::ostream& os) const
252  {
253  os << "FootForceDifferenceController " << getName();
254  try
255  {
256  getProfiler().report_all(3, os);
257  }
258  catch (ExceptionSignal e) {}
259  }
260  } // namespace talos_balance
261  } // namespace sot
262 } // namespace dynamicgraph
263 
Eigen::Matrix< Scalar, Eigen::Dynamic, 1 > Vector
Definition: math/fwd.hh:40
DYNAMICGRAPH_FACTORY_ENTITY_PLUGIN(AdmittanceControllerEndEffector, "AdmittanceControllerEndEffector")
DEFINE_SIGNAL_INNER_FUNCTION(w_force, dynamicgraph::Vector)
EIGEN_MAKE_ALIGNED_OPERATOR_NEW FootForceDifferenceController(const std::string &name)