sot-talos-balance  1.6.0
dcm-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 <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_DCMCONTROLLER_ZMPREF_COMPUTATION "DcmController: zmpRef computation "
38 #define PROFILE_DCMCONTROLLER_WRENCHREF_COMPUTATION "DcmController: wrenchRef computation "
39 
40 #define INPUT_SIGNALS m_KpSIN << m_KiSIN << m_decayFactorSIN << m_omegaSIN << m_massSIN << m_comSIN << m_dcmSIN << m_dcmDesSIN << m_zmpDesSIN
41 
42 #define OUTPUT_SIGNALS m_zmpRefSOUT << m_wrenchRefSOUT
43 
46  typedef DcmController EntityClassName;
47 
48  /* --- DG FACTORY ---------------------------------------------------- */
50  "DcmController");
51 
52  /* ------------------------------------------------------------------- */
53  /* --- CONSTRUCTION -------------------------------------------------- */
54  /* ------------------------------------------------------------------- */
55  DcmController::DcmController(const std::string& name)
56  : Entity(name)
57  , CONSTRUCT_SIGNAL_IN(Kp, dynamicgraph::Vector)
58  , CONSTRUCT_SIGNAL_IN(Ki, dynamicgraph::Vector)
59  , CONSTRUCT_SIGNAL_IN(decayFactor, double)
60  , CONSTRUCT_SIGNAL_IN(omega, double)
61  , CONSTRUCT_SIGNAL_IN(mass, double)
62  , CONSTRUCT_SIGNAL_IN(com, dynamicgraph::Vector)
63  , CONSTRUCT_SIGNAL_IN(dcm, dynamicgraph::Vector)
64  , CONSTRUCT_SIGNAL_IN(dcmDes, dynamicgraph::Vector)
65  , CONSTRUCT_SIGNAL_IN(zmpDes, dynamicgraph::Vector)
66  , CONSTRUCT_SIGNAL_OUT(zmpRef, dynamicgraph::Vector, INPUT_SIGNALS)
67  , CONSTRUCT_SIGNAL_OUT(wrenchRef, dynamicgraph::Vector, m_zmpRefSOUT << m_comSIN << m_omegaSIN << m_massSIN)
68  // dcomRef is set to depend from comRefSOUT to ensure position is updated before velocity
69  , m_initSucceeded(false)
70  {
71  Entity::signalRegistration( INPUT_SIGNALS << OUTPUT_SIGNALS );
72 
73  /* Commands. */
74  addCommand("init", makeCommandVoid1(*this, &DcmController::init, docCommandVoid1("Initialize the entity.","time step")));
75  addCommand("resetDcmIntegralError", makeCommandVoid0(*this, &DcmController::resetDcmIntegralError, docCommandVoid0("Set dcm integral error to zero.")));
76  }
77 
78  void DcmController::init(const double & dt)
79  {
80  if(!m_KpSIN.isPlugged())
81  return SEND_MSG("Init failed: signal Kp is not plugged", MSG_TYPE_ERROR);
82  if(!m_KiSIN.isPlugged())
83  return SEND_MSG("Init failed: signal Ki is not plugged", MSG_TYPE_ERROR);
84  if(!m_decayFactorSIN.isPlugged())
85  return SEND_MSG("Init failed: signal decayFactor is not plugged", MSG_TYPE_ERROR);
86  if(!m_omegaSIN.isPlugged())
87  return SEND_MSG("Init failed: signal omega is not plugged", MSG_TYPE_ERROR);
88  if(!m_massSIN.isPlugged())
89  return SEND_MSG("Init failed: signal mass is not plugged", MSG_TYPE_ERROR);
90  if(!m_comSIN.isPlugged())
91  return SEND_MSG("Init failed: signal com is not plugged", MSG_TYPE_ERROR);
92  if(!m_dcmSIN.isPlugged())
93  return SEND_MSG("Init failed: signal dcm is not plugged", MSG_TYPE_ERROR);
94  if(!m_dcmDesSIN.isPlugged())
95  return SEND_MSG("Init failed: signal dcmDes is not plugged", MSG_TYPE_ERROR);
96  if(!m_zmpDesSIN.isPlugged())
97  return SEND_MSG("Init failed: signal zmpDes is not plugged", MSG_TYPE_ERROR);
98 
99  m_dt = dt;
101  m_initSucceeded = true;
102  }
103 
105  {
106  m_dcmIntegralError.setZero(3);
107  }
108 
109  /* ------------------------------------------------------------------- */
110  /* --- SIGNALS ------------------------------------------------------- */
111  /* ------------------------------------------------------------------- */
112 
114  {
115  if(!m_initSucceeded)
116  {
117  SEND_WARNING_STREAM_MSG("Cannot compute signal zmpRef before initialization!");
118  return s;
119  }
120  if(s.size()!=3)
121  s.resize(3);
122 
123  getProfiler().start(PROFILE_DCMCONTROLLER_ZMPREF_COMPUTATION);
124 
125 
126  const Vector & Kp = m_KpSIN(iter);
127  const Vector & Ki = m_KiSIN(iter);
128  const double & decayFactor = m_decayFactorSIN(iter);
129  const double & omega = m_omegaSIN(iter);
130  const Vector & dcm = m_dcmSIN(iter);
131  const Vector & dcmDes = m_dcmDesSIN(iter);
132  const Vector & zmpDes = m_zmpDesSIN(iter);
133 
134  assert(Kp.size()==3 && "Unexpected size of signal Kp");
135  assert(Ki.size()==3 && "Unexpected size of signal Ki");
136  assert(dcm.size()==3 && "Unexpected size of signal dcm");
137  assert(dcmDes.size()==3 && "Unexpected size of signal dcmDes");
138  assert(zmpDes.size()==3 && "Unexpected size of signal zmpDes");
139 
140  const Eigen::Vector3d dcmError = dcmDes - dcm;
141 
142  Eigen::Vector3d zmpRef = zmpDes - (Eigen::Vector3d::Constant(1.0) + Kp/omega).cwiseProduct(dcmError) - Ki.cwiseProduct(m_dcmIntegralError)/omega;
143  zmpRef[2] = 0.0; // maybe needs better way
144 
145  // update the integrator (AFTER using its value)
146  m_dcmIntegralError += ( dcmError - decayFactor*m_dcmIntegralError ) * m_dt;
147 
148  s = zmpRef;
149 
150  getProfiler().stop(PROFILE_DCMCONTROLLER_ZMPREF_COMPUTATION);
151 
152  return s;
153  }
154 
156  {
157  if(!m_initSucceeded)
158  {
159  SEND_WARNING_STREAM_MSG("Cannot compute signal wrenchRef before initialization!");
160  return s;
161  }
162  if(s.size()!=6)
163  s.resize(6);
164 
165  getProfiler().start(PROFILE_DCMCONTROLLER_WRENCHREF_COMPUTATION);
166 
167  const double & omega = m_omegaSIN(iter);
168  const double & mass = m_massSIN(iter);
169  const Vector & com = m_comSIN(iter);
170 
171  const Vector & zmpRef = m_zmpRefSOUT(iter);
172 
173  assert(com.size()==3 && "Unexpected size of signal com");
174 
175  Eigen::Vector3d forceRef = mass*omega*omega*(com-zmpRef);
176  forceRef[2] = mass*9.81; // maybe needs better way
177 
178  Eigen::Matrix<double,6,1> wrenchRef;
179  wrenchRef.head<3>() = forceRef;
180  const Eigen::Vector3d com3 = com;
181  wrenchRef.tail<3>() = com3.cross(forceRef);
182 
183  s = wrenchRef;
184 
186 
187  return s;
188  }
189 
190  /* --- COMMANDS ---------------------------------------------------------- */
191 
192  /* ------------------------------------------------------------------- */
193  /* --- ENTITY -------------------------------------------------------- */
194  /* ------------------------------------------------------------------- */
195 
196  void DcmController::display(std::ostream& os) const
197  {
198  os << "DcmController " << getName();
199  try
200  {
201  getProfiler().report_all(3, os);
202  }
203  catch (ExceptionSignal e) {}
204  }
205  } // namespace talos_balance
206  } // namespace sot
207 } // namespace dynamicgraph
208 
#define OUTPUT_SIGNALS
Eigen::Matrix< Scalar, Eigen::Dynamic, 1 > Vector
Definition: math/fwd.hh:40
dynamicgraph::Vector m_dcmIntegralError
true if the entity has been successfully initialized
DYNAMICGRAPH_FACTORY_ENTITY_PLUGIN(AdmittanceControllerEndEffector, "AdmittanceControllerEndEffector")
#define PROFILE_DCMCONTROLLER_WRENCHREF_COMPUTATION
virtual void display(std::ostream &os) const
#define PROFILE_DCMCONTROLLER_ZMPREF_COMPUTATION
EIGEN_MAKE_ALIGNED_OPERATOR_NEW DcmController(const std::string &name)
#define INPUT_SIGNALS