sot-talos-balance  1.7.0
hip-flexibility-compensation.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 <limits>
20 #include <math.h>
21 #include <sot/core/debug.hh>
22 #include <dynamic-graph/factory.h>
23 #include <dynamic-graph/all-commands.h>
24 #include <sot/core/stop-watch.hh>
25 
26 
27 namespace dynamicgraph {
28 namespace sot {
29 namespace talos_balance {
30 namespace dg = ::dynamicgraph;
31 using namespace dg;
32 using namespace dg::command;
33 
34 #define PROFILE_HIPFLEXIBILITYCOMPENSATION_TAUFILT_COMPUTATION \
35  "HipFlexibilityCompensation: Torque filter computation "
36 #define PROFILE_HIPFLEXIBILITYCOMPENSATION_DELTAQ_COMPUTATION \
37  "HipFlexibilityCompensation: Angular correction computation "
38 #define PROFILE_HIPFLEXIBILITYCOMPENSATION_QCMD_COMPUTATION \
39  "HipFlexibilityCompensation: Corrected joint configuration computation "
40 
41 #define JOINT_DES_SIGNALS m_q_desSIN
42 
43 #define INPUT_SIGNALS m_tauSIN << m_K_rSIN << m_K_lSIN //<< m_K_dSIN
44 
45 #define OUTPUT_SIGNALS m_tau_filtSOUT << m_delta_qSOUT << m_q_cmdSOUT
46 
49 typedef HipFlexibilityCompensation EntityClassName;
50 
51 /* --- DG FACTORY ---------------------------------------------------- */
52 DYNAMICGRAPH_FACTORY_ENTITY_PLUGIN(HipFlexibilityCompensation,
53  "HipFlexibilityCompensation");
54 
55 /* ------------------------------------------------------------------- */
56 /* --- CONSTRUCTION -------------------------------------------------- */
57 /* ------------------------------------------------------------------- */
59  : Entity(name)
60  , CONSTRUCT_SIGNAL_IN(q_des, dynamicgraph::Vector)
61  , CONSTRUCT_SIGNAL_IN(tau, dynamicgraph::Vector)
62  , CONSTRUCT_SIGNAL_IN(K_l, double)
63  , CONSTRUCT_SIGNAL_IN(K_r, double)
64  , CONSTRUCT_SIGNAL_OUT(tau_filt, dynamicgraph::Vector, m_tauSIN)
65  , CONSTRUCT_SIGNAL_OUT(delta_q, dynamicgraph::Vector, INPUT_SIGNALS << m_tau_filtSOUT)
66  , CONSTRUCT_SIGNAL_OUT(q_cmd, dynamicgraph::Vector, JOINT_DES_SIGNALS << m_delta_qSOUT)
67  , m_initSucceeded(false)
68  , m_torqueLowPassFilterFrequency(1)
69  , m_delta_q_saturation(0.01)
70  , m_rate_limiter(0.003){
71 
72  Entity::signalRegistration( JOINT_DES_SIGNALS << INPUT_SIGNALS << OUTPUT_SIGNALS );
73 
74  /* Commands. */
75  addCommand("init", makeCommandVoid2(*this,
77  docCommandVoid2("Initialize the entity.",
78  "Robot time step",
79  "Robot name")));
80 
81  addCommand("setTorqueLowPassFilterFrequency", makeCommandVoid1(*this,
83  docCommandVoid1("Set the LowPassFilter frequency for the torque computation.",
84  "Value of the frequency")));
85  addCommand("setAngularSaturation", makeCommandVoid1(*this,
87  docCommandVoid1("Set the saturation for the angular correction computation.",
88  "Value of the saturation")));
89  addCommand("setRateLimiter", makeCommandVoid1(*this,
91  docCommandVoid1("Set the rate for the rate limiter of delta_q.",
92  "Value of the limiter")));
93 
94  addCommand("getTorqueLowPassFilterFrequency", makeDirectGetter(*this, &m_torqueLowPassFilterFrequency,
95  docDirectGetter("Get the current value of the torque LowPassFilter frequency.", "frequency (double)")));
96 
97  addCommand("getAngularSaturation", makeDirectGetter(*this, &m_delta_q_saturation,
98  docDirectGetter("Get the current value of the Angular Saturation.", "saturation (double)")));
99 
100  addCommand("getRateLimiter", makeDirectGetter(*this, &m_rate_limiter,
101  docDirectGetter("Get the current value of the rate limiter.", "rate (double)")));
102 }
103 
104 /* --- COMMANDS ---------------------------------------------------------- */
105 
106 void HipFlexibilityCompensation::init(const double &dt, const std::string& robotName) {
107  if (!m_q_desSIN.isPlugged())
108  return SEND_MSG("Init failed: signal q_des is not plugged", MSG_TYPE_ERROR);
109  if (!m_tauSIN.isPlugged())
110  return SEND_MSG("Init failed: signal tau is not plugged", MSG_TYPE_ERROR);
111  if (!m_K_rSIN.isPlugged())
112  return SEND_MSG("Init failed: signal K_r is not plugged", MSG_TYPE_ERROR);
113  if (!m_K_lSIN.isPlugged())
114  return SEND_MSG("Init failed: signal K_l is not plugged", MSG_TYPE_ERROR);
115 
116  m_dt = dt;
117  std::string robotName_nonconst(robotName);
118 
119  if (!isNameInRobotUtil(robotName_nonconst)) {
120  SEND_MSG("You should have a robotUtil pointer initialized before", MSG_TYPE_ERROR);
121  } else {
122  m_robot_util = getRobotUtil(robotName_nonconst);
123  std::cerr << "m_robot_util:" << m_robot_util << std::endl;
124  }
125 
126  m_initSucceeded = true;
127  const Vector& q_des = m_q_desSIN.accessCopy();
128  const Vector& tau = m_tauSIN.accessCopy();
129  m_previous_delta_q.resize(q_des.size());
130  m_previous_delta_q.setZero();
131  m_previous_tau.resize(tau.size());
132  m_previous_tau.setZero();
133 }
134 
136  m_torqueLowPassFilterFrequency = frequency;
137 }
138 
139 void HipFlexibilityCompensation::setAngularSaturation(const double& saturation) {
140  m_delta_q_saturation = saturation;
141 }
142 
144  m_rate_limiter = rate;
145 }
146 
147 Vector HipFlexibilityCompensation::lowPassFilter(const double& frequency, const Vector& signal, Vector& previous_signal){
148  // delta_q = alpha * previous_delta_q(-1) + (1-alpha) * delta_q_des
149  double alpha = exp(- m_dt * 2 * M_PI * frequency);
150  Vector output = alpha * previous_signal + signal * (1 - alpha);
151  return output;
152 }
153 
154 void HipFlexibilityCompensation::rateLimiter(const Vector& signal, Vector& previous_signal, Vector& output){
155  Vector rate = (signal - previous_signal)/m_dt;
156  // Falling slew rate = - Rising slew rate = - m_rate_limiter
157  for (unsigned int i=0; i<signal.size(); i++){
158  if (rate[i] > m_rate_limiter){
159  output[i] = m_dt * m_rate_limiter + previous_signal[i];
160  } else if (rate[i] < -m_rate_limiter){
161  output[i] = m_dt * (-m_rate_limiter) + previous_signal[i];
162  } else {
163  output[i] = signal[i];
164  }
165  }
166 }
167 /* ------------------------------------------------------------------- */
168 /* --- SIGNALS ------------------------------------------------------- */
169 /* ------------------------------------------------------------------- */
170 
172  if (!m_initSucceeded) {
173  SEND_WARNING_STREAM_MSG("Cannot compute signal tau_filt before initialization!");
174  return s;
175  }
176 
178 
179  const Vector& tau = m_tauSIN(iter);
180 
181  if(s.size() != tau.size())
182  s.resize(tau.size());
183 
184  if (iter < 5){
185  s = tau;
186  } else {
187  // Low pass filter
189  }
190  m_previous_tau = tau;
192 
193  return s;
194 }
195 
197  if (!m_initSucceeded) {
198  SEND_WARNING_STREAM_MSG("Cannot compute signal delta_q before initialization!");
199  return s;
200  }
201 
203 
204  const Vector &tau = m_tau_filtSOUT(iter);
205  double K_r = m_K_rSIN(iter);
206  double K_l = m_K_lSIN(iter);
207 
208  if(s.size() != tau.size())
209  s.setZero(tau.size());
210 
211  for (unsigned int i=0; i<tau.size(); i++){
212  if (i==1){
213  s[i] = tau[i]/K_l; // torque/flexibility of left hip (roll)
214  }
215  else if (i==7){
216  s[i] = tau[i]/K_r; // torque/flexibility of right hip (roll)
217  }
218  else {
219  s[i] = 0.0; // no flexibility for other joints
220  }
221  }
222 
223  // Angular Saturation
224  // left hip
225  if (s[1] > m_delta_q_saturation){
226  s[1] = m_delta_q_saturation;
227  }
228  else if (s[1] < -m_delta_q_saturation){
229  s[1] = -m_delta_q_saturation;
230  }
231  // right hip
232  if (s[7] > m_delta_q_saturation){
233  s[7] = m_delta_q_saturation;
234  }
235  else if (s[7] < -m_delta_q_saturation){
236  s[7] = -m_delta_q_saturation;
237  }
238 
240  return s;
241 }
242 
244  if (!m_initSucceeded) {
245  SEND_WARNING_STREAM_MSG("Cannot compute signal q_cmd before initialization!");
246  return s;
247  }
248 
250 
251  const Vector &q_des = m_q_desSIN(iter);
252  const Vector &delta_q = m_delta_qSOUT(iter);
253 
254  if(s.size() != q_des.size())
255  s.resize(q_des.size());
256 
257  Vector limitedSignal;
258  limitedSignal.resize(delta_q.size());
259  rateLimiter(delta_q, m_previous_delta_q, limitedSignal);
260  m_previous_delta_q = limitedSignal;
261 
262  if (iter < 5){
263  s = q_des;
264  } else {
265  s = q_des + limitedSignal;
266  }
267 
269  return s;
270 }
271 
272 
273 /* ------------------------------------------------------------------- */
274 /* --- ENTITY INHERITANCE -------------------------------------------- */
275 /* ------------------------------------------------------------------- */
276 
277 void HipFlexibilityCompensation::display(std::ostream& os) const {
278  os << "HipFlexibilityCompensation " << getName();
279  try {
280  getProfiler().report_all(3, os);
281  } catch (ExceptionSignal e) {}
282 }
283 
284 } // namespace talos_balance
285 } // namespace sot
286 } // namespace dynamicgraph
287 
void rateLimiter(const dynamicgraph::Vector &signal, dynamicgraph::Vector &previous_signal, dynamicgraph::Vector &output)
Compute the limiter of a signal given the previous signal (based on first derivative).
#define PROFILE_HIPFLEXIBILITYCOMPENSATION_TAUFILT_COMPUTATION
Eigen::Matrix< Scalar, Eigen::Dynamic, 1 > Vector
Definition: math/fwd.hh:40
DYNAMICGRAPH_FACTORY_ENTITY_PLUGIN(AdmittanceControllerEndEffector, "AdmittanceControllerEndEffector")
#define PROFILE_HIPFLEXIBILITYCOMPENSATION_DELTAQ_COMPUTATION
double m_dt
true if the entity has been successfully initialized
#define INPUT_SIGNALS
void setAngularSaturation(const double &saturation)
Set the value of the saturation for the angular correction computation.
#define JOINT_DES_SIGNALS
EIGEN_MAKE_ALIGNED_OPERATOR_NEW HipFlexibilityCompensation(const std::string &name)
#define PROFILE_HIPFLEXIBILITYCOMPENSATION_QCMD_COMPUTATION
#define OUTPUT_SIGNALS
void setRateLimiter(const double &rate)
Set the value of the limiter for the the rate limiter of delta_q.
void setTorqueLowPassFilterFrequency(const double &frequency)
Set the LowPassFilter frequency for the torque computation.
dynamicgraph::Vector lowPassFilter(const double &frequency, const dynamicgraph::Vector &signal, dynamicgraph::Vector &previous_signal)
Compute the low pass filter of a signal given a frequency and the previous signal.
void init(const double &dt, const std::string &robotName)
Initialize the entity.