sot-torque-control  1.6.1
Collection of dynamic-graph entities aimed at implementing torque control on different robots.
admittance-controller.cpp
Go to the documentation of this file.
1 /*
2  * Copyright 2015, Andrea Del Prete, LAAS-CNRS
3  *
4  */
5 
6 #include <tsid/utils/stop-watch.hpp>
7 #include <dynamic-graph/factory.h>
8 #include <sot/core/debug.hh>
11 
12 namespace dynamicgraph {
13 namespace sot {
14 namespace torque_control {
15 namespace dg = ::dynamicgraph;
16 using namespace dg;
17 using namespace dg::command;
18 using namespace std;
19 using namespace Eigen;
20 using namespace tsid;
21 using namespace tsid::math;
22 using namespace tsid::tasks;
23 using namespace dg::sot;
24 
25 #define PROFILE_DQ_DES_COMPUTATION "Admittance control computation"
26 
27 #define REF_FORCE_SIGNALS m_fRightFootRefSIN << m_fLeftFootRefSIN
28 // m_fRightHandRefSIN << m_fLeftHandRefSIN
29 #define FORCE_SIGNALS m_fRightFootSIN << m_fLeftFootSIN << m_fRightFootFilteredSIN << m_fLeftFootFilteredSIN
30 // m_fRightHandSIN << m_fLeftHandSIN
31 #define GAIN_SIGNALS \
32  m_kp_forceSIN << m_ki_forceSIN << m_kp_velSIN << m_ki_velSIN << m_force_integral_saturationSIN \
33  << m_force_integral_deadzoneSIN
34 #define STATE_SIGNALS m_encodersSIN << m_jointsVelocitiesSIN
35 
36 #define INPUT_SIGNALS \
37  STATE_SIGNALS << REF_FORCE_SIGNALS << FORCE_SIGNALS << GAIN_SIGNALS << m_controlledJointsSIN << m_dampingSIN
38 
39 #define DES_VEL_SIGNALS m_vDesRightFootSOUT << m_vDesLeftFootSOUT //<< m_fRightHandErrorSOUT << m_fLeftHandErrorSOUT
40 #define OUTPUT_SIGNALS m_uSOUT << m_dqDesSOUT << DES_VEL_SIGNALS
41 
45 
46 typedef Eigen::Matrix<double, 3, 1> Vector3;
47 typedef Eigen::Matrix<double, 6, 1> Vector6;
48 /* --- DG FACTORY ---------------------------------------------------- */
50 
51 /* ------------------------------------------------------------------- */
52 /* --- CONSTRUCTION -------------------------------------------------- */
53 /* ------------------------------------------------------------------- */
55  : Entity(name),
56  CONSTRUCT_SIGNAL_IN(encoders, dynamicgraph::Vector),
57  CONSTRUCT_SIGNAL_IN(jointsVelocities, dynamicgraph::Vector),
58  CONSTRUCT_SIGNAL_IN(kp_force, dynamicgraph::Vector),
59  CONSTRUCT_SIGNAL_IN(ki_force, dynamicgraph::Vector),
60  CONSTRUCT_SIGNAL_IN(kp_vel, dynamicgraph::Vector),
61  CONSTRUCT_SIGNAL_IN(ki_vel, dynamicgraph::Vector),
62  CONSTRUCT_SIGNAL_IN(force_integral_saturation, dynamicgraph::Vector),
63  CONSTRUCT_SIGNAL_IN(force_integral_deadzone, dynamicgraph::Vector),
64  CONSTRUCT_SIGNAL_IN(fRightFootRef, dynamicgraph::Vector),
65  CONSTRUCT_SIGNAL_IN(fLeftFootRef, dynamicgraph::Vector),
66  CONSTRUCT_SIGNAL_IN(fRightFoot, dynamicgraph::Vector),
67  CONSTRUCT_SIGNAL_IN(fLeftFoot, dynamicgraph::Vector),
68  CONSTRUCT_SIGNAL_IN(fRightFootFiltered, dynamicgraph::Vector),
69  CONSTRUCT_SIGNAL_IN(fLeftFootFiltered, dynamicgraph::Vector),
70  CONSTRUCT_SIGNAL_IN(controlledJoints, dynamicgraph::Vector),
71  CONSTRUCT_SIGNAL_IN(damping, dynamicgraph::Vector)
72  // ,CONSTRUCT_SIGNAL_IN(fRightHandRef, dynamicgraph::Vector)
73  // ,CONSTRUCT_SIGNAL_IN(fLeftHandRef, dynamicgraph::Vector)
74  // ,CONSTRUCT_SIGNAL_IN(fRightHand, dynamicgraph::Vector)
75  // ,CONSTRUCT_SIGNAL_IN(fLeftHand, dynamicgraph::Vector)
76  ,
77  CONSTRUCT_SIGNAL_OUT(u, dynamicgraph::Vector, STATE_SIGNALS << m_controlledJointsSIN),
78  CONSTRUCT_SIGNAL_OUT(dqDes, dynamicgraph::Vector, STATE_SIGNALS << DES_VEL_SIGNALS << m_dampingSIN),
79  CONSTRUCT_SIGNAL_OUT(vDesRightFoot, dynamicgraph::Vector,
80  m_fRightFootSIN << m_fRightFootFilteredSIN << m_fRightFootRefSIN << GAIN_SIGNALS),
81  CONSTRUCT_SIGNAL_OUT(vDesLeftFoot, dynamicgraph::Vector,
82  m_fLeftFootSIN << m_fLeftFootFilteredSIN << m_fLeftFootRefSIN << GAIN_SIGNALS)
83  // ,CONSTRUCT_SIGNAL_OUT(fRightHandError, dynamicgraph::Vector, m_fRightHandSIN <<
84  // m_fRightHandRefSIN)
85  // ,CONSTRUCT_SIGNAL_OUT(fLeftHandError, dynamicgraph::Vector, m_fLeftHandSIN <<
86  // m_fLeftHandRefSIN)
87  ,
88  m_firstIter(true),
89  m_initSucceeded(false),
90  m_useJacobianTranspose(true) {
91  Entity::signalRegistration(INPUT_SIGNALS << OUTPUT_SIGNALS);
92 
93  m_v_RF_int.setZero();
94  m_v_LF_int.setZero();
95 
96  /* Commands. */
97  addCommand("getUseJacobianTranspose",
98  makeDirectGetter(
99  *this, &m_useJacobianTranspose,
100  docDirectGetter("If true it uses the Jacobian transpose, otherwise the pseudoinverse", "bool")));
101  addCommand("setUseJacobianTranspose",
102  makeDirectSetter(
103  *this, &m_useJacobianTranspose,
104  docDirectSetter("If true it uses the Jacobian transpose, otherwise the pseudoinverse", "bool")));
105  addCommand("init", makeCommandVoid2(*this, &AdmittanceController::init,
106  docCommandVoid2("Initialize the entity.", "Time period in seconds (double)",
107  "Robot name (string)")));
108 }
109 
110 void AdmittanceController::init(const double& dt, const std::string& robotRef) {
111  if (dt <= 0.0) return SEND_MSG("Timestep must be positive", MSG_TYPE_ERROR);
112  if (!m_encodersSIN.isPlugged()) return SEND_MSG("Init failed: signal encoders is not plugged", MSG_TYPE_ERROR);
113  if (!m_jointsVelocitiesSIN.isPlugged())
114  return SEND_MSG("Init failed: signal jointsVelocities is not plugged", MSG_TYPE_ERROR);
115  if (!m_controlledJointsSIN.isPlugged())
116  return SEND_MSG("Init failed: signal controlledJoints is not plugged", MSG_TYPE_ERROR);
117 
118  m_dt = dt;
119  m_initSucceeded = true;
120 
121  /* Retrieve m_robot_util informations */
122  std::string localName(robotRef);
123  if (isNameInRobotUtil(localName))
124  m_robot_util = getRobotUtil(localName);
125  else
126  return SEND_MSG("You should have an entity controller manager initialized before", MSG_TYPE_ERROR);
127 
128  try {
129  vector<string> package_dirs;
130  m_robot = new robots::RobotWrapper(m_robot_util->m_urdf_filename, package_dirs, pinocchio::JointModelFreeFlyer());
131  m_data = new pinocchio::Data(m_robot->model());
132 
133  assert(m_robot->nv() >= 6);
134  m_robot_util->m_nbJoints = m_robot->nv() - 6;
135  m_nj = m_robot->nv() - 6;
136  m_u.setZero(m_nj);
137  m_dq_des_urdf.setZero(m_nj);
138  m_dqErrIntegral.setZero(m_nj);
139  // m_dqDesIntegral.setZero(m_nj);
140 
141  m_q_urdf.setZero(m_robot->nq());
142  m_q_urdf(6) = 1.0;
143  m_v_urdf.setZero(m_robot->nv());
144  m_J_RF.setZero(6, m_robot->nv());
145  m_J_LF.setZero(6, m_robot->nv());
146 
147  m_frame_id_rf = (int)m_robot->model().getFrameId(m_robot_util->m_foot_util.m_Right_Foot_Frame_Name);
148  m_frame_id_lf = (int)m_robot->model().getFrameId(m_robot_util->m_foot_util.m_Left_Foot_Frame_Name);
149 
150  } catch (const std::exception& e) {
151  std::cout << e.what();
152  return SEND_MSG("Init failed: Could load URDF :" + m_robot_util->m_urdf_filename, MSG_TYPE_ERROR);
153  }
154 }
155 
156 /* ------------------------------------------------------------------- */
157 /* --- SIGNALS ------------------------------------------------------- */
158 /* ------------------------------------------------------------------- */
159 
160 DEFINE_SIGNAL_OUT_FUNCTION(u, dynamicgraph::Vector) {
161  if (!m_initSucceeded) {
162  SEND_WARNING_STREAM_MSG("Cannot compute signal u before initialization!");
163  return s;
164  }
165 
166  const Vector& dqDes = m_dqDesSOUT(iter); // n
167  const Vector& q = m_encodersSIN(iter); // n
168  const Vector& dq = m_jointsVelocitiesSIN(iter); // n
169  const Vector& kp = m_kp_velSIN(iter);
170  const Vector& ki = m_ki_velSIN(iter);
171 
172  if (m_firstIter) {
173  m_qPrev = q;
174  m_firstIter = false;
175  }
176 
177  // estimate joint velocities using finite differences
178  m_dq_fd = (q - m_qPrev) / m_dt;
179  m_qPrev = q;
180 
181  m_dqErrIntegral += m_dt * ki.cwiseProduct(dqDes - m_dq_fd);
182  s = kp.cwiseProduct(dqDes - dq) + m_dqErrIntegral;
183 
184  return s;
185 }
186 
187 DEFINE_SIGNAL_OUT_FUNCTION(dqDes, dynamicgraph::Vector) {
188  if (!m_initSucceeded) {
189  SEND_WARNING_STREAM_MSG("Cannot compute signal dqDes before initialization!");
190  return s;
191  }
192 
193  getProfiler().start(PROFILE_DQ_DES_COMPUTATION);
194  {
195  const dg::sot::Vector6d v_des_LF = m_vDesLeftFootSOUT(iter);
196  const dg::sot::Vector6d v_des_RF = m_vDesRightFootSOUT(iter);
197  const Vector& q_sot = m_encodersSIN(iter); // n
198  // const Vector& dq_sot = m_jointsVelocitiesSIN(iter); // n
199  // const Vector& qMask = m_controlledJointsSIN(iter); // n
200  // const Eigen::Vector4d& damping = m_dampingSIN(iter); // 4
201 
202  assert(q_sot.size() == m_nj && "Unexpected size of signal encoder");
203 
205  m_robot_util->joints_sot_to_urdf(q_sot, m_q_urdf.tail(m_nj));
206  m_robot->computeAllTerms(*m_data, m_q_urdf, m_v_urdf);
207  m_robot->frameJacobianLocal(*m_data, m_frame_id_rf, m_J_RF);
208  m_robot->frameJacobianLocal(*m_data, m_frame_id_lf, m_J_LF);
209 
210  // SEND_INFO_STREAM_MSG("RFoot Jacobian :" + toString(m_J_RF.rightCols(m_nj)));
211  // set to zero columns of Jacobian corresponding to unactive joints
212  // for(int i=0; i<m_nj; i++)
213  // if(qMask(i)==0)
214  // m_J_all.col(6+i).setZero();
215 
217  if (m_useJacobianTranspose) {
218  m_dq_des_urdf = m_J_RF.rightCols(m_nj).transpose() * v_des_RF;
219  m_dq_des_urdf += m_J_LF.rightCols(m_nj).transpose() * v_des_LF;
220  } else {
221  m_J_RF_QR.compute(m_J_RF.rightCols(m_nj));
222  m_J_LF_QR.compute(m_J_LF.rightCols(m_nj));
223 
224  m_dq_des_urdf = m_J_RF_QR.solve(v_des_RF);
225  m_dq_des_urdf += m_J_LF_QR.solve(v_des_LF);
226  }
227 
228  if (s.size() != m_nj) s.resize(m_nj);
229 
230  m_robot_util->joints_urdf_to_sot(m_dq_des_urdf, s);
231  }
232  getProfiler().stop(PROFILE_DQ_DES_COMPUTATION);
233 
234  return s;
235 }
236 
237 DEFINE_SIGNAL_OUT_FUNCTION(vDesRightFoot, dynamicgraph::Vector) {
238  if (!m_initSucceeded) {
239  SEND_MSG("Cannot compute signal vDesRightFoot before initialization!", MSG_TYPE_WARNING_STREAM);
240  return s;
241  }
242  const Vector6& f = m_fRightFootSIN(iter);
243  const Vector6& f_filt = m_fRightFootFilteredSIN(iter);
244  const Vector6& fRef = m_fRightFootRefSIN(iter);
245  const Vector6d& kp = m_kp_forceSIN(iter);
246  const Vector6d& ki = m_ki_forceSIN(iter);
247  const Vector6d& f_sat = m_force_integral_saturationSIN(iter);
248  const Vector6d& dz = m_force_integral_deadzoneSIN(iter);
249 
250  dg::sot::Vector6d err = fRef - f;
251  dg::sot::Vector6d err_filt = fRef - f_filt;
252  dg::sot::Vector6d v_des = -kp.cwiseProduct(err_filt);
253 
254  for (int i = 0; i < 6; i++) {
255  if (err(i) > dz(i))
256  m_v_RF_int(i) -= m_dt * ki(i) * (err(i) - dz(i));
257  else if (err(i) < -dz(i))
258  m_v_RF_int(i) -= m_dt * ki(i) * (err(i) + dz(i));
259  }
260 
261  // saturate
262  bool saturating = false;
263  for (int i = 0; i < 6; i++) {
264  if (m_v_RF_int(i) > f_sat(i)) {
265  saturating = true;
266  m_v_RF_int(i) = f_sat(i);
267  } else if (m_v_RF_int(i) < -f_sat(i)) {
268  saturating = true;
269  m_v_RF_int(i) = -f_sat(i);
270  }
271  }
272  if (saturating) SEND_INFO_STREAM_MSG("Saturate m_v_RF_int integral: " + toString(m_v_RF_int.transpose()));
273  s = v_des + m_v_RF_int;
274  return s;
275 }
276 
277 DEFINE_SIGNAL_OUT_FUNCTION(vDesLeftFoot, dynamicgraph::Vector) {
278  if (!m_initSucceeded) {
279  SEND_MSG("Cannot compute signal vDesLeftFoot before initialization!", MSG_TYPE_WARNING_STREAM);
280  return s;
281  }
282  const Vector6& f = m_fLeftFootSIN(iter);
283  const Vector6& f_filt = m_fLeftFootFilteredSIN(iter);
284  const Vector6& fRef = m_fLeftFootRefSIN(iter);
285  const Vector6d& kp = m_kp_forceSIN(iter);
286  const Vector6d& ki = m_ki_forceSIN(iter);
287  const Vector6d& f_sat = m_force_integral_saturationSIN(iter);
288  const Vector6d& dz = m_force_integral_deadzoneSIN(iter);
289 
290  dg::sot::Vector6d err = fRef - f;
291  dg::sot::Vector6d err_filt = fRef - f_filt;
292  dg::sot::Vector6d v_des = -kp.cwiseProduct(err_filt);
293 
294  for (int i = 0; i < 6; i++) {
295  if (err(i) > dz(i))
296  m_v_LF_int(i) -= m_dt * ki(i) * (err(i) - dz(i));
297  else if (err(i) < -dz(i))
298  m_v_LF_int(i) -= m_dt * ki(i) * (err(i) + dz(i));
299  }
300 
301  // saturate
302  bool saturating = false;
303  for (int i = 0; i < 6; i++) {
304  if (m_v_LF_int(i) > f_sat(i)) {
305  saturating = true;
306  m_v_LF_int(i) = f_sat(i);
307  } else if (m_v_LF_int(i) < -f_sat(i)) {
308  saturating = true;
309  m_v_LF_int(i) = -f_sat(i);
310  }
311  }
312  if (saturating) SEND_INFO_STREAM_MSG("Saturate m_v_LF_int integral: " + toString(m_v_LF_int.transpose()));
313  s = v_des + m_v_LF_int;
314  return s;
315 }
316 
317 // DEFINE_SIGNAL_OUT_FUNCTION(fRightHandError,dynamicgraph::Vector)
318 // {
319 // if(!m_initSucceeded)
320 // {
321 // SEND_MSG("Cannot compute signal fRightHandError before initialization!", MSG_TYPE_WARNING_STREAM);
322 // return s;
323 // }
324 
325 // const Eigen::Matrix<double,24,1>& Kf = m_KfSIN(iter); // 6*4
326 // const Vector6& f = m_fRightHandSIN(iter); // 6
327 // const Vector6& fRef = m_fRightHandRefSIN(iter); // 6
328 // assert(f.size()==6 && "Unexpected size of signal fRightHand");
329 // assert(fRef.size()==6 && "Unexpected size of signal fRightHandRef");
330 
331 // if(s.size()!=6)
332 // s.resize(6);
333 // s.tail<3>() = Kf.segment<3>(12).cwiseProduct(fRef.head<3>() - f.head<3>() );
334 // s.head<3>() = Kf.segment<3>(15).cwiseProduct(fRef.tail<3>() - f.tail<3>());
335 // return s;
336 // }
337 
338 // DEFINE_SIGNAL_OUT_FUNCTION(fLeftHandError,dynamicgraph::Vector)
339 // {
340 // if(!m_initSucceeded)
341 // {
342 // SEND_MSG("Cannot compute signal fLeftHandError before initialization!", MSG_TYPE_WARNING_STREAM);
343 // return s;
344 // }
345 
346 // const Eigen::Matrix<double,24,1>& Kf = m_KfSIN(iter); // 6*4
347 // const Vector6& f = m_fLeftHandSIN(iter); // 6
348 // const Vector6& fRef = m_fLeftHandRefSIN(iter); // 6
349 // assert(f.size()==6 && "Unexpected size of signal fLeftHand");
350 // assert(fRef.size()==6 && "Unexpected size of signal fLeftHandRef");
351 
352 // if(s.size()!=6)
353 // s.resize(6);
354 // s.tail<3>() = Kf.segment<3>(18).cwiseProduct(fRef.head<3>() - f.head<3>() );
355 // s.head<3>() = Kf.segment<3>(21).cwiseProduct(fRef.tail<3>() - f.tail<3>());
356 
357 // return s;
358 // }
359 
360 /* --- COMMANDS ---------------------------------------------------------- */
361 
362 /* ------------------------------------------------------------------- */
363 /* --- ENTITY -------------------------------------------------------- */
364 /* ------------------------------------------------------------------- */
365 
366 void AdmittanceController::display(std::ostream& os) const {
367  os << "AdmittanceController " << getName();
368  try {
369  getProfiler().report_all(3, os);
370  } catch (ExceptionSignal e) {
371  }
372 }
373 
374 //**************************************************************************************************
375 VectorXd svdSolveWithDamping(const JacobiSVD<MatrixXd>& A, const VectorXd& b, double damping) {
376  eigen_assert(A.computeU() && A.computeV() &&
377  "svdSolveWithDamping() requires both unitaries U and V to be computed (thin unitaries suffice).");
378  assert(A.rows() == b.size());
379 
380  // cout<<"b = "<<toString(b,1)<<endl;
381  VectorXd tmp(A.cols());
382  int nzsv = static_cast<int>(A.nonzeroSingularValues());
383  tmp.noalias() = A.matrixU().leftCols(nzsv).adjoint() * b;
384  // cout<<"U^T*b = "<<toString(tmp,1)<<endl;
385  double sv, d2 = damping * damping;
386  for (int i = 0; i < nzsv; i++) {
387  sv = A.singularValues()(i);
388  tmp(i) *= sv / (sv * sv + d2);
389  }
390  // cout<<"S^+ U^T b = "<<toString(tmp,1)<<endl;
391  VectorXd res = A.matrixV().leftCols(nzsv) * tmp;
392 
393  // getLogger().sendMsg("sing val = "+toString(A.singularValues(),3), MSG_STREAM_INFO);
394  // getLogger().sendMsg("solution with damp "+toString(damping)+" = "+toString(res.norm()), MSG_STREAM_INFO);
395  // getLogger().sendMsg("solution without damping ="+toString(A.solve(b).norm()), MSG_STREAM_INFO);
396 
397  return res;
398 }
399 
400 } // namespace torque_control
401 } // namespace sot
402 } // namespace dynamicgraph
DES_VEL_SIGNALS
#define DES_VEL_SIGNALS
Definition: admittance-controller.cpp:39
dynamicgraph::sot::torque_control::Vector3
Eigen::Matrix< double, 3, 1 > Vector3
Definition: admittance-controller.cpp:46
admittance-controller.hh
dynamicgraph
to read text file
Definition: treeview.dox:22
dynamicgraph::sot::torque_control::AdmittanceController::m_q_urdf
tsid::math::Vector m_q_urdf
desired 6d wrench left foot
Definition: admittance-controller.hh:115
dynamicgraph::sot::torque_control::AdmittanceController::m_initSucceeded
bool m_initSucceeded
Definition: admittance-controller.hh:100
STATE_SIGNALS
#define STATE_SIGNALS
Definition: admittance-controller.cpp:34
dynamicgraph::sot::torque_control::AdmittanceController::m_nj
int m_nj
control loop time period
Definition: admittance-controller.hh:103
dynamicgraph::sot::torque_control::AdmittanceController::m_robot_util
RobotUtilShrPtr m_robot_util
Definition: admittance-controller.hh:132
INPUT_SIGNALS
#define INPUT_SIGNALS
Definition: admittance-controller.cpp:36
dynamicgraph::sot::torque_control::AdmittanceController::m_J_LF
Matrix6x m_J_LF
Definition: admittance-controller.hh:126
commands-helper.hh
dynamicgraph::sot::torque_control::AdmittanceController::m_dqErrIntegral
tsid::math::Vector m_dqErrIntegral
Definition: admittance-controller.hh:119
dynamicgraph::sot::torque_control::AdmittanceController::init
void init(const double &dt, const std::string &robotRef)
Definition: admittance-controller.cpp:110
OUTPUT_SIGNALS
#define OUTPUT_SIGNALS
Definition: admittance-controller.cpp:40
torque_control
Definition: __init__.py:1
dynamicgraph::sot::torque_control::AdmittanceController::m_dq_des_urdf
tsid::math::Vector m_dq_des_urdf
Definition: admittance-controller.hh:118
dynamicgraph::sot::torque_control::svdSolveWithDamping
VectorXd svdSolveWithDamping(const JacobiSVD< MatrixXd > &A, const VectorXd &b, double damping)
Definition: admittance-controller.cpp:375
dynamicgraph::sot::torque_control::AdmittanceController::m_J_RF
Matrix6x m_J_RF
Definition: admittance-controller.hh:125
dynamicgraph::sot::torque_control::AdmittanceController::display
virtual void display(std::ostream &os) const
Definition: admittance-controller.cpp:366
dynamicgraph::sot::torque_control::AdmittanceController::AdmittanceController
EIGEN_MAKE_ALIGNED_OPERATOR_NEW AdmittanceController(const std::string &name)
Definition: admittance-controller.cpp:54
dynamicgraph::sot::torque_control::AdmittanceController
Definition: admittance-controller.hh:46
dynamicgraph::sot::torque_control::AdmittanceController::m_data
pinocchio::Data * m_data
Definition: admittance-controller.hh:111
dynamicgraph::sot::torque_control::AdmittanceController::m_useJacobianTranspose
bool m_useJacobianTranspose
true if the entity has been successfully initialized
Definition: admittance-controller.hh:101
dynamicgraph::sot::torque_control::DYNAMICGRAPH_FACTORY_ENTITY_PLUGIN
DYNAMICGRAPH_FACTORY_ENTITY_PLUGIN(AdmittanceController, "AdmittanceController")
dynamicgraph::sot::torque_control::AdmittanceController::m_frame_id_rf
int m_frame_id_rf
robot geometric/inertial data
Definition: admittance-controller.hh:106
dynamicgraph::sot::torque_control::Vector6
Eigen::Matrix< double, 6, 1 > Vector6
Definition: admittance-controller.cpp:47
dynamicgraph::sot::torque_control::AdmittanceController::m_frame_id_lf
int m_frame_id_lf
frame id of right foot
Definition: admittance-controller.hh:107
dynamicgraph::sot::torque_control::AdmittanceController::m_v_RF_int
tsid::math::Vector6 m_v_RF_int
Definition: admittance-controller.hh:129
dynamicgraph::sot::torque_control::EntityClassName
AdmittanceController EntityClassName
Definition: admittance-controller.cpp:44
dynamicgraph::sot::torque_control::AdmittanceController::m_v_urdf
tsid::math::Vector m_v_urdf
Definition: admittance-controller.hh:116
dynamicgraph::sot::torque_control::AdmittanceController::m_dt
double m_dt
if true it uses the Jacobian transpose rather than the pseudoinverse
Definition: admittance-controller.hh:102
PROFILE_DQ_DES_COMPUTATION
#define PROFILE_DQ_DES_COMPUTATION
Definition: admittance-controller.cpp:25
dynamicgraph::sot::torque_control::AdmittanceController::m_robot
tsid::robots::RobotWrapper * m_robot
frame id of left foot
Definition: admittance-controller.hh:110
dynamicgraph::sot::torque_control::DEFINE_SIGNAL_OUT_FUNCTION
DEFINE_SIGNAL_OUT_FUNCTION(u, dynamicgraph::Vector)
Definition: admittance-controller.cpp:160
dynamicgraph::sot::torque_control::AdmittanceController::m_u
Eigen::VectorXd m_u
Definition: admittance-controller.hh:98
GAIN_SIGNALS
#define GAIN_SIGNALS
Definition: admittance-controller.cpp:31
dynamicgraph::sot::torque_control::AdmittanceController::m_v_LF_int
tsid::math::Vector6 m_v_LF_int
Definition: admittance-controller.hh:130