sot-talos-balance  1.5.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 
121  getProfiler().start(PROFILE_DCMCONTROLLER_ZMPREF_COMPUTATION);
122 
123 
124  const Vector & Kp = m_KpSIN(iter);
125  const Vector & Ki = m_KiSIN(iter);
126  const double & decayFactor = m_decayFactorSIN(iter);
127  const double & omega = m_omegaSIN(iter);
128  const Vector & dcm = m_dcmSIN(iter);
129  const Vector & dcmDes = m_dcmDesSIN(iter);
130  const Vector & zmpDes = m_zmpDesSIN(iter);
131 
132  assert(Kp.size()==3 && "Unexpected size of signal Kp");
133  assert(Ki.size()==3 && "Unexpected size of signal Ki");
134  assert(dcm.size()==3 && "Unexpected size of signal dcm");
135  assert(dcmDes.size()==3 && "Unexpected size of signal dcmDes");
136  assert(zmpDes.size()==3 && "Unexpected size of signal zmpDes");
137 
138  Vector dcmError = dcmDes - dcm;
139 
140  Vector zmpRef = zmpDes - (Vector::Constant(3,1,1.0) + Kp/omega).cwiseProduct(dcmError) - Ki.cwiseProduct(m_dcmIntegralError)/omega;
141  zmpRef[2] = 0.0; // maybe needs better way
142 
143  // update the integrator (AFTER using its value)
144  m_dcmIntegralError += ( dcmError - decayFactor*m_dcmIntegralError ) * m_dt;
145 
146  s = zmpRef;
147 
148  getProfiler().stop(PROFILE_DCMCONTROLLER_ZMPREF_COMPUTATION);
149 
150  return s;
151  }
152 
154  {
155  if(!m_initSucceeded)
156  {
157  SEND_WARNING_STREAM_MSG("Cannot compute signal wrenchRef before initialization!");
158  return s;
159  }
160 
161  getProfiler().start(PROFILE_DCMCONTROLLER_WRENCHREF_COMPUTATION);
162 
163  const double & omega = m_omegaSIN(iter);
164  const double & mass = m_massSIN(iter);
165  const Vector & com = m_comSIN(iter);
166 
167  const Vector & zmpRef = m_zmpRefSOUT(iter);
168 
169  assert(com.size()==3 && "Unexpected size of signal com");
170 
171  Eigen::Vector3d forceRef = mass*omega*omega*(com-zmpRef);
172  forceRef[2] = mass*9.81; // maybe needs better way
173 
174  Vector wrenchRef(6);
175  wrenchRef.head<3>() = forceRef;
176  Eigen::Vector3d com3 = com;
177  wrenchRef.tail<3>() = com3.cross(forceRef);
178 
179  s = wrenchRef;
180 
182 
183  return s;
184  }
185 
186  /* --- COMMANDS ---------------------------------------------------------- */
187 
188  /* ------------------------------------------------------------------- */
189  /* --- ENTITY -------------------------------------------------------- */
190  /* ------------------------------------------------------------------- */
191 
192  void DcmController::display(std::ostream& os) const
193  {
194  os << "DcmController " << getName();
195  try
196  {
197  getProfiler().report_all(3, os);
198  }
199  catch (ExceptionSignal e) {}
200  }
201  } // namespace talos_balance
202  } // namespace sot
203 } // namespace dynamicgraph
204 
#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