sot-talos-balance  1.5.0
com-admittance-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_COMADMITTANCECONTROLLER_DDCOMREF_COMPUTATION "ComAdmittanceController: ddcomRef computation "
38 #define PROFILE_COMADMITTANCECONTROLLER_STATEREF_COMPUTATION "ComAdmittanceController: stateRef computation "
39 #define PROFILE_COMADMITTANCECONTROLLER_DCOMREF_COMPUTATION "ComAdmittanceController: dcomRef computation "
40 #define PROFILE_COMADMITTANCECONTROLLER_COMREF_COMPUTATION "ComAdmittanceController: comRef computation "
41 
42 #define INPUT_SIGNALS m_KpSIN << m_zmpSIN << m_zmpDesSIN << m_ddcomDesSIN
43 
44 #define INNER_SIGNALS m_stateRefSINNER
45 
46 #define OUTPUT_SIGNALS m_comRefSOUT << m_dcomRefSOUT << m_ddcomRefSOUT
47 
50  typedef ComAdmittanceController EntityClassName;
51 
52  /* --- DG FACTORY ---------------------------------------------------- */
53  DYNAMICGRAPH_FACTORY_ENTITY_PLUGIN(ComAdmittanceController,
54  "ComAdmittanceController");
55 
56  /* ------------------------------------------------------------------- */
57  /* --- CONSTRUCTION -------------------------------------------------- */
58  /* ------------------------------------------------------------------- */
60  : Entity(name)
61  , CONSTRUCT_SIGNAL_IN(Kp, dynamicgraph::Vector)
62  , CONSTRUCT_SIGNAL_IN(zmp, dynamicgraph::Vector)
63  , CONSTRUCT_SIGNAL_IN(zmpDes, dynamicgraph::Vector)
64  , CONSTRUCT_SIGNAL_IN(ddcomDes, dynamicgraph::Vector)
65  , CONSTRUCT_SIGNAL_OUT(ddcomRef, dynamicgraph::Vector, INPUT_SIGNALS)
66  , CONSTRUCT_SIGNAL_INNER(stateRef, dynamicgraph::Vector, m_ddcomRefSOUT)
67  , CONSTRUCT_SIGNAL_OUT(comRef, dynamicgraph::Vector, m_stateRefSINNER)
68  , CONSTRUCT_SIGNAL_OUT(dcomRef, dynamicgraph::Vector, m_stateRefSINNER)
69  // dcomRef is set to depend from comRefSOUT to ensure position is updated before velocity
70  , m_initSucceeded(false)
71  {
72  Entity::signalRegistration( INPUT_SIGNALS << INNER_SIGNALS << OUTPUT_SIGNALS );
73 
74  /* Commands. */
75  addCommand("init", makeCommandVoid1(*this, &ComAdmittanceController::init, docCommandVoid1("Initialize the entity.","time step")));
76  addCommand("setPosition", makeCommandVoid1(*this, &ComAdmittanceController::setPosition, docCommandVoid1("Set initial reference position.","Initial position")));
77  addCommand("setVelocity", makeCommandVoid1(*this, &ComAdmittanceController::setVelocity, docCommandVoid1("Set initial reference velocity.","Initial velocity")));
78  addCommand("setState", makeCommandVoid2(*this, &ComAdmittanceController::setState, docCommandVoid2("Set initial reference position and velocity.","Initial position","Initial velocity")));
79  }
80 
81  void ComAdmittanceController::init(const double & dt)
82  {
83  if(!m_KpSIN.isPlugged())
84  return SEND_MSG("Init failed: signal Kp is not plugged", MSG_TYPE_ERROR);
85  if(!m_ddcomDesSIN.isPlugged())
86  return SEND_MSG("Init failed: signal ddcomDes is not plugged", MSG_TYPE_ERROR);
87  if(!m_zmpSIN.isPlugged())
88  return SEND_MSG("Init failed: signal zmp is not plugged", MSG_TYPE_ERROR);
89  if(!m_zmpDesSIN.isPlugged())
90  return SEND_MSG("Init failed: signal zmpDes is not plugged", MSG_TYPE_ERROR);
91 
92  m_dt = dt;
93  m_state.setZero(6);
94  m_initSucceeded = true;
95  }
96 
98  {
99  m_state.head<3>() = com;
100  }
101 
103  {
104  m_state.tail<3>() = dcom;
105  }
106 
108  {
109  setPosition(com);
110  setVelocity(dcom);
111  }
112 
113  /* ------------------------------------------------------------------- */
114  /* --- SIGNALS ------------------------------------------------------- */
115  /* ------------------------------------------------------------------- */
116 
118  {
119  if(!m_initSucceeded)
120  {
121  SEND_WARNING_STREAM_MSG("Cannot compute signal ddcomRef before initialization!");
122  return s;
123  }
124 
126 
127  const Vector & ddcomDes = m_ddcomDesSIN(iter);
128  const Vector & zmp = m_zmpSIN(iter);
129  const Vector & zmpDes = m_zmpDesSIN(iter);
130  const Vector & Kp = m_KpSIN(iter);
131 
132  assert(ddcomDes.size()==3 && "Unexpected size of signal ddcomDes");
133  assert(zmp.size()==3 && "Unexpected size of signal zmp");
134  assert(zmpDes.size()==3 && "Unexpected size of signal zmpDes");
135  assert(Kp.size()==3 && "Unexpected size of signal Kp");
136 
137  const Vector & ddcomRef = ddcomDes + Kp.cwiseProduct(zmp-zmpDes);
138 
139  s = ddcomRef;
140 
142 
143  return s;
144  }
145 
147  {
148  if(!m_initSucceeded)
149  {
150  SEND_WARNING_STREAM_MSG("Cannot compute signal stateRef before initialization!");
151  return s;
152  }
153 
155 
156  const Vector & ddcomRef = m_ddcomRefSOUT(iter);
157 
158  assert(ddcomRef.size()==3 && "Unexpected size of signal ddcomRef");
159 
160  const Vector & dcomRef = m_state.tail<3>();
161 
162  m_state.head<3>() += dcomRef*m_dt + 0.5*ddcomRef*m_dt*m_dt;
163  m_state.tail<3>() += ddcomRef*m_dt;
164 
165  s = m_state;
166 
168 
169  return s;
170  }
171 
173  {
174  if(!m_initSucceeded)
175  {
176  SEND_WARNING_STREAM_MSG("Cannot compute signal dcomRef before initialization!");
177  return s;
178  }
179 
181 
182  const Vector & stateRef = m_stateRefSINNER(iter);
183 
184  assert(stateRef.size()==3 && "Unexpected size of signal stateRef");
185 
186  s = stateRef.head<3>();
187 
189 
190  return s;
191  }
192 
194  {
195  if(!m_initSucceeded)
196  {
197  SEND_WARNING_STREAM_MSG("Cannot compute signal dcomRef before initialization!");
198  return s;
199  }
200 
202 
203  const Vector & stateRef = m_stateRefSINNER(iter);
204 
205  assert(stateRef.size()==3 && "Unexpected size of signal stateRef");
206 
207  s = stateRef.tail<3>();
208 
210 
211  return s;
212  }
213 
214  /* --- COMMANDS ---------------------------------------------------------- */
215 
216  /* ------------------------------------------------------------------- */
217  /* --- ENTITY -------------------------------------------------------- */
218  /* ------------------------------------------------------------------- */
219 
220  void ComAdmittanceController::display(std::ostream& os) const
221  {
222  os << "ComAdmittanceController " << getName();
223  try
224  {
225  getProfiler().report_all(3, os);
226  }
227  catch (ExceptionSignal e) {}
228  }
229  } // namespace talos_balance
230  } // namespace sot
231 } // namespace dynamicgraph
232 
#define PROFILE_COMADMITTANCECONTROLLER_DDCOMREF_COMPUTATION
EIGEN_MAKE_ALIGNED_OPERATOR_NEW ComAdmittanceController(const std::string &name)
Eigen::Matrix< Scalar, Eigen::Dynamic, 1 > Vector
Definition: math/fwd.hh:40
#define INNER_SIGNALS
#define INPUT_SIGNALS
DYNAMICGRAPH_FACTORY_ENTITY_PLUGIN(AdmittanceControllerEndEffector, "AdmittanceControllerEndEffector")
#define PROFILE_COMADMITTANCECONTROLLER_COMREF_COMPUTATION
dynamicgraph::Vector m_state
true if the entity has been successfully initialized
#define PROFILE_COMADMITTANCECONTROLLER_DCOMREF_COMPUTATION
DEFINE_SIGNAL_INNER_FUNCTION(w_force, dynamicgraph::Vector)
#define OUTPUT_SIGNALS
void setState(const dynamicgraph::Vector &, const dynamicgraph::Vector &)
#define PROFILE_COMADMITTANCECONTROLLER_STATEREF_COMPUTATION