sot-torque-control  1.6.2
Collection of dynamic-graph entities aimed at implementing torque control on different robots.
joint-trajectory-generator.cpp
Go to the documentation of this file.
1 /*
2  * Copyright 2014, Oscar E. Ramos Ponce, LAAS-CNRS
3  *
4  */
5 
6 #include <dynamic-graph/factory.h>
7 #include <sot/core/debug.hh>
10 #include <sot/core/stop-watch.hh>
11 
12 #include "../include/sot/torque_control/stc-commands.hh"
13 
14 namespace dynamicgraph {
15 namespace sot {
16 namespace torque_control {
17 namespace dynamicgraph = ::dynamicgraph;
18 using namespace dynamicgraph;
19 using namespace dynamicgraph::command;
20 using namespace std;
21 using namespace Eigen;
22 
23 // Size to be aligned "-------------------------------------------------------"
24 #define PROFILE_POSITION_DESIRED_COMPUTATION "TrajGen: reference joint traj computation "
25 #define PROFILE_FORCE_DESIRED_COMPUTATION "TrajGen: reference force computation "
26 
29 typedef JointTrajectoryGenerator EntityClassName;
30 
31 /* --- DG FACTORY ---------------------------------------------------- */
32 DYNAMICGRAPH_FACTORY_ENTITY_PLUGIN(JointTrajectoryGenerator, "JointTrajectoryGenerator");
33 
34 /* ------------------------------------------------------------------- */
35 /* --- CONSTRUCTION -------------------------------------------------- */
36 /* ------------------------------------------------------------------- */
38  : Entity(name),
39  CONSTRUCT_SIGNAL_IN(base6d_encoders, dynamicgraph::Vector),
40  CONSTRUCT_SIGNAL_OUT(q, dynamicgraph::Vector, m_base6d_encodersSIN),
41  CONSTRUCT_SIGNAL_OUT(dq, dynamicgraph::Vector, m_qSOUT),
42  CONSTRUCT_SIGNAL_OUT(ddq, dynamicgraph::Vector, m_qSOUT),
43  CONSTRUCT_SIGNAL(fRightFoot, OUT, dynamicgraph::Vector),
44  CONSTRUCT_SIGNAL(fLeftFoot, OUT, dynamicgraph::Vector),
45  CONSTRUCT_SIGNAL(fRightHand, OUT, dynamicgraph::Vector),
46  CONSTRUCT_SIGNAL(fLeftHand, OUT, dynamicgraph::Vector),
47  m_initSucceeded(false),
48  m_firstIter(true),
49  m_robot_util(RefVoidRobotUtil()) {
50  BIND_SIGNAL_TO_FUNCTION(fRightFoot, OUT, dynamicgraph::Vector);
51  BIND_SIGNAL_TO_FUNCTION(fLeftFoot, OUT, dynamicgraph::Vector);
52  BIND_SIGNAL_TO_FUNCTION(fRightHand, OUT, dynamicgraph::Vector);
53  BIND_SIGNAL_TO_FUNCTION(fLeftHand, OUT, dynamicgraph::Vector);
54 
55  m_status_force.resize(4, JTG_STOP);
56  m_minJerkTrajGen_force.resize(4);
57  m_sinTrajGen_force.resize(4);
58  m_linChirpTrajGen_force.resize(4);
59  m_currentTrajGen_force.resize(4);
60  m_noTrajGen_force.resize(4);
61 
62  m_iterForceSignals.resize(4, 0);
63 
64  Entity::signalRegistration(m_qSOUT << m_dqSOUT << m_ddqSOUT << m_base6d_encodersSIN << m_fRightFootSOUT
65  << m_fLeftFootSOUT << m_fRightHandSOUT << m_fLeftHandSOUT);
66 
67  /* Commands. */
68  addCommand("init", makeCommandVoid2(*this, &JointTrajectoryGenerator::init,
69  docCommandVoid2("Initialize the entity.", "Time period in seconds (double)",
70  "robotRef (string)")));
71 
72  addCommand("getJoint", makeCommandVoid1(
74  docCommandVoid1("Get the current angle of the specified joint.", "Joint name (string)")));
75 
76  // const std::string& docstring = ;
77 
78  addCommand("isTrajectoryEnded",
79  new command::IsTrajectoryEnded(*this, "Return whether all joint trajectories have ended"));
80 
81  addCommand("playTrajectoryFile",
82  makeCommandVoid1(*this, &JointTrajectoryGenerator::playTrajectoryFile,
83  docCommandVoid1("Play the trajectory read from the specified text file.",
84  "(string) File name, path included")));
85 
86  addCommand("startSinusoid",
87  makeCommandVoid3(
89  docCommandVoid3("Start an infinite sinusoid motion.", "(string) joint name",
90  "(double) final angle in radians", "(double) time to reach the final angle in sec")));
91 
92  addCommand(
93  "startTriangle",
94  makeCommandVoid4(
96  docCommandVoid4("Start an infinite triangle wave.", "(string) joint name", "(double) final angle in radians",
97  "(double) time to reach the final angle in sec", "(double) time to accelerate in sec")));
98 
99  addCommand("startConstAcc",
100  makeCommandVoid3(*this, &JointTrajectoryGenerator::startConstAcc,
101  docCommandVoid3("Start an infinite trajectory with piece-wise constant acceleration.",
102  "(string) joint name", "(double) final angle in radians",
103  "(double) time to reach the final angle in sec")));
104 
105  addCommand("startForceSinusoid",
106  makeCommandVoid4(*this, &JointTrajectoryGenerator::startForceSinusoid,
107  docCommandVoid4("Start an infinite sinusoid force.", "(string) force name",
108  "(int) force axis in [0, 5]", "(double) final 1d force in N or Nm",
109  "(double) time to reach the final force in sec")));
110 
111  // addCommand("startForceSinusoid",
112  // makeCommandVoid3(*this, &JointTrajectoryGenerator::startForceSinusoid,
113  // docCommandVoid3("Start an infinite sinusoid force.",
114  // "(string) force name",
115  // "(Vector) final 6d force in N/Nm",
116  // "(double) time to reach the final force in sec")));
117 
118  addCommand("startLinChirp",
120  docCommandVoid5("Start a linear-chirp motion.", "(string) joint name",
121  "(double) final angle in radians", "(double) initial frequency [Hz]",
122  "(double) final frequency [Hz]", "(double) trajectory time [sec]")));
123 
124  addCommand(
125  "startForceLinChirp",
127  docCommandVoid6("Start a linear-chirp force traj.", "(string) force name", "(int) force axis",
128  "(double) final force in N/Nm", "(double) initial frequency [Hz]",
129  "(double) final frequency [Hz]", "(double) trajectory time [sec]")));
130 
131  addCommand("moveJoint",
132  makeCommandVoid3(*this, &JointTrajectoryGenerator::moveJoint,
133  docCommandVoid3("Move the joint to the specified angle with a minimum jerk trajectory.",
134  "(string) joint name", "(double) final angle in radians",
135  "(double) time to reach the final angle in sec")));
136 
137  addCommand(
138  "moveForce",
139  makeCommandVoid4(*this, &JointTrajectoryGenerator::moveForce,
140  docCommandVoid4("Move the force to the specified value with a minimum jerk trajectory.",
141  "(string) force name", "(int) force axis", "(double) final force in N/Nm",
142  "(double) time to reach the final force in sec")));
143 
144  addCommand("stop", makeCommandVoid1(
146  docCommandVoid1(
147  "Stop the motion of the specified joint, or of all joints if no joint name is specified.",
148  "(string) joint name")));
149 
150  addCommand("stopForce", makeCommandVoid1(*this, &JointTrajectoryGenerator::stopForce,
151  docCommandVoid1("Stop the specified force trajectort",
152  "(string) force name (rh,lh,lf,rf)")));
153 }
154 
155 void JointTrajectoryGenerator::init(const double& dt, const std::string& robotRef) {
156  /* Retrieve m_robot_util informations */
157  std::string localName(robotRef);
158  if (isNameInRobotUtil(localName))
159  m_robot_util = getRobotUtil(localName);
160  else {
161  SEND_MSG("You should have an entity controller manager initialized before", MSG_TYPE_ERROR);
162  return;
163  }
164 
165  if (dt <= 0.0) return SEND_MSG("Timestep must be positive", MSG_TYPE_ERROR);
166  m_firstIter = true;
167  m_dt = dt;
168 
169  m_status.resize(m_robot_util->m_nbJoints, JTG_STOP);
170  m_minJerkTrajGen.resize(m_robot_util->m_nbJoints);
171  m_sinTrajGen.resize(m_robot_util->m_nbJoints);
172  m_triangleTrajGen.resize(m_robot_util->m_nbJoints);
173  m_constAccTrajGen.resize(m_robot_util->m_nbJoints);
174  m_linChirpTrajGen.resize(m_robot_util->m_nbJoints);
175  m_currentTrajGen.resize(m_robot_util->m_nbJoints);
176  m_noTrajGen.resize(m_robot_util->m_nbJoints);
177 
178  for (long unsigned int i = 0; i < m_robot_util->m_nbJoints; i++) {
180  m_sinTrajGen[i] = new SinusoidTrajectoryGenerator(dt, 5.0, 1);
181  m_triangleTrajGen[i] = new TriangleTrajectoryGenerator(dt, 5.0, 1);
186  }
188 
189  for (int i = 0; i < 4; i++) {
191  m_sinTrajGen_force[i] = new SinusoidTrajectoryGenerator(dt, 5.0, 6);
195  }
196  m_initSucceeded = true;
197 }
198 
199 /* ------------------------------------------------------------------- */
200 /* --- SIGNALS ------------------------------------------------------- */
201 /* ------------------------------------------------------------------- */
202 
203 DEFINE_SIGNAL_OUT_FUNCTION(q, dynamicgraph::Vector) {
204  if (!m_initSucceeded) {
205  SEND_WARNING_STREAM_MSG("Cannot compute signal positionDes before initialization!");
206  return s;
207  }
208 
209  getProfiler().start(PROFILE_POSITION_DESIRED_COMPUTATION);
210  {
211  // at first iteration store current joints positions
212  if (m_firstIter) {
213  const dynamicgraph::Vector& base6d_encoders = m_base6d_encodersSIN(iter);
214  if (base6d_encoders.size() != static_cast<Eigen::VectorXd::Index>(m_robot_util->m_nbJoints + 6)) {
215  SEND_ERROR_STREAM_MSG("Unexpected size of signal base6d_encoder " + toString(base6d_encoders.size()) + " " +
216  toString(m_robot_util->m_nbJoints + 6));
217  return s;
218  }
219  for (unsigned int i = 0; i < m_robot_util->m_nbJoints; i++)
220  m_noTrajGen[i]->set_initial_point(base6d_encoders(6 + i));
221  m_firstIter = false;
222  }
223 
224  if (s.size() != static_cast<Eigen::VectorXd::Index>(m_robot_util->m_nbJoints)) s.resize(m_robot_util->m_nbJoints);
225 
226  if (m_status[0] == JTG_TEXT_FILE) {
227  const VectorXd& qRef = m_textFileTrajGen->compute_next_point();
228  for (unsigned int i = 0; i < m_robot_util->m_nbJoints; i++) {
229  s(i) = qRef[i];
230  if (m_textFileTrajGen->isTrajectoryEnded()) {
231  m_noTrajGen[i]->set_initial_point(s(i));
232  m_status[i] = JTG_STOP;
233  }
234  }
235  if (m_textFileTrajGen->isTrajectoryEnded()) SEND_MSG("Text file trajectory ended.", MSG_TYPE_INFO);
236  } else {
237  for (unsigned int i = 0; i < m_robot_util->m_nbJoints; i++) {
238  s(i) = m_currentTrajGen[i]->compute_next_point()(0);
239  if (m_currentTrajGen[i]->isTrajectoryEnded()) {
240  m_currentTrajGen[i] = m_noTrajGen[i];
241  m_noTrajGen[i]->set_initial_point(s(i));
242  m_status[i] = JTG_STOP;
243  SEND_MSG("Trajectory of joint " + m_robot_util->get_name_from_id(i) + " ended.", MSG_TYPE_INFO);
244  }
245  }
246  }
247  }
248  getProfiler().stop(PROFILE_POSITION_DESIRED_COMPUTATION);
249 
250  return s;
251 }
252 
253 DEFINE_SIGNAL_OUT_FUNCTION(dq, dynamicgraph::Vector) {
254  if (!m_initSucceeded) {
255  SEND_WARNING_STREAM_MSG("Cannot compute signal positionDes before initialization! iter: " + toString(iter));
256  return s;
257  }
258 
259  if (s.size() != static_cast<Eigen::VectorXd::Index>(m_robot_util->m_nbJoints)) s.resize(m_robot_util->m_nbJoints);
260  if (m_status[0] == JTG_TEXT_FILE) {
261  for (unsigned int i = 0; i < m_robot_util->m_nbJoints; i++) s(i) = m_textFileTrajGen->getVel()[i];
262  } else
263  for (unsigned int i = 0; i < m_robot_util->m_nbJoints; i++) s(i) = m_currentTrajGen[i]->getVel()(0);
264 
265  return s;
266 }
267 
268 DEFINE_SIGNAL_OUT_FUNCTION(ddq, dynamicgraph::Vector) {
269  if (!m_initSucceeded) {
270  SEND_WARNING_STREAM_MSG("Cannot compute signal positionDes before initialization!" + toString(iter));
271  return s;
272  }
273 
274  if (s.size() != static_cast<Eigen::VectorXd::Index>(m_robot_util->m_nbJoints)) s.resize(m_robot_util->m_nbJoints);
275 
276  if (m_status[0] == JTG_TEXT_FILE) {
277  for (unsigned int i = 0; i < m_robot_util->m_nbJoints; i++) s(i) = m_textFileTrajGen->getAcc()[i];
278  } else
279  for (unsigned int i = 0; i < m_robot_util->m_nbJoints; i++) s(i) = m_currentTrajGen[i]->getAcc()(0);
280 
281  return s;
282 }
283 
284 DEFINE_SIGNAL_OUT_FUNCTION(fRightFoot, dynamicgraph::Vector) {
285  // SEND_MSG("Compute force right foot iter "+toString(iter), MSG_TYPE_DEBUG);
286  generateReferenceForceSignal("fRightFoot", static_cast<int>(m_robot_util->m_force_util.get_force_id_right_foot()), s,
287  iter);
288  return s;
289 }
290 
291 DEFINE_SIGNAL_OUT_FUNCTION(fLeftFoot, dynamicgraph::Vector) {
292  generateReferenceForceSignal("fLeftFoot", static_cast<int>(m_robot_util->m_force_util.get_force_id_left_foot()), s,
293  iter);
294  return s;
295 }
296 
297 DEFINE_SIGNAL_OUT_FUNCTION(fRightHand, dynamicgraph::Vector) {
298  generateReferenceForceSignal("fRightHand", static_cast<int>(m_robot_util->m_force_util.get_force_id_right_hand()), s,
299  iter);
300  return s;
301 }
302 
303 DEFINE_SIGNAL_OUT_FUNCTION(fLeftHand, dynamicgraph::Vector) {
304  generateReferenceForceSignal("fLeftHand", static_cast<int>(m_robot_util->m_force_util.get_force_id_left_hand()), s,
305  iter);
306  return s;
307 }
308 
309 bool JointTrajectoryGenerator::generateReferenceForceSignal(const std::string& forceName, int fid,
310  dynamicgraph::Vector& s, int iter) {
311  if (!m_initSucceeded) {
312  SEND_WARNING_STREAM_MSG("Cannot compute signal " + forceName + " before initialization!");
313  return false;
314  }
315 
316  getProfiler().start(PROFILE_FORCE_DESIRED_COMPUTATION);
317  {
318  if (s.size() != 6) s.resize(6);
319 
320  if (iter > m_iterForceSignals[fid]) {
321  m_currentTrajGen_force[fid]->compute_next_point();
322  // SEND_MSG("Compute force "+forceName+" with id "+toString(fid)+": "+toString(fr.transpose()),
323  // MSG_TYPE_DEBUG);
324  m_iterForceSignals[fid]++;
325  }
326 
327  const Eigen::VectorXd& fr = m_currentTrajGen_force[fid]->getPos();
328  for (unsigned int i = 0; i < 6; i++) s(i) = fr(i);
329 
331  m_noTrajGen_force[fid]->set_initial_point(m_currentTrajGen_force[fid]->getPos());
333  m_status_force[fid] = JTG_STOP;
334  SEND_MSG("Trajectory of force " + forceName + " ended.", MSG_TYPE_INFO);
335  }
336  }
337  getProfiler().stop(PROFILE_FORCE_DESIRED_COMPUTATION);
338 
339  return true;
340 }
341 
342 /* ------------------------------------------------------------------- */
343 /* --- COMMANDS ------------------------------------------------------ */
344 /* ------------------------------------------------------------------- */
345 
346 void JointTrajectoryGenerator::getJoint(const std::string& jointName) {
347  unsigned int i;
348  if (convertJointNameToJointId(jointName, i) == false) return;
349  const dynamicgraph::Vector& base6d_encoders = m_base6d_encodersSIN.accessCopy();
350  SEND_MSG("Current angle of joint " + jointName + " is " + toString(base6d_encoders(6 + i)), MSG_TYPE_INFO);
351 }
352 
354  bool output = true;
355  if (m_status[0] == JTG_TEXT_FILE) {
356  for (unsigned int i = 0; i < m_robot_util->m_nbJoints; i++) {
358  output = false;
359  SEND_MSG("Text file trajectory not ended.", MSG_TYPE_INFO);
360  return output;
361  }
362  }
363  } else {
364  for (unsigned int i = 0; i < m_robot_util->m_nbJoints; i++) {
365  if (!m_currentTrajGen[i]->isTrajectoryEnded()) {
366  output = false;
367  SEND_MSG("Trajectory of joint " + m_robot_util->get_name_from_id(i) + "not ended.", MSG_TYPE_INFO);
368  return output;
369  }
370  }
371  }
372  return output;
373 }
374 
375 void JointTrajectoryGenerator::playTrajectoryFile(const std::string& fileName) {
376  if (!m_initSucceeded) return SEND_MSG("Cannot start sinusoid before initialization!", MSG_TYPE_ERROR);
377 
378  for (unsigned int i = 0; i < m_robot_util->m_nbJoints; i++)
379  if (m_status[i] != JTG_STOP)
380  return SEND_MSG("You cannot joint " + m_robot_util->get_name_from_id(i) + " because it is already controlled.",
381  MSG_TYPE_ERROR);
382 
383  if (!m_textFileTrajGen->loadTextFile(fileName))
384  return SEND_MSG("Error while loading text file " + fileName, MSG_TYPE_ERROR);
385 
386  // check current configuration is not too far from initial trajectory configuration
387  bool needToMoveToInitConf = false;
388  const VectorXd& qInit = m_textFileTrajGen->get_initial_point();
389  for (unsigned int i = 0; i < m_robot_util->m_nbJoints; i++)
390  if (fabs(qInit[i] - m_currentTrajGen[i]->getPos()(0)) > 0.001) {
391  needToMoveToInitConf = true;
392  SEND_MSG("Joint " + m_robot_util->get_name_from_id(i) +
393  " is too far from initial configuration so first i will move it there.",
394  MSG_TYPE_WARNING);
395  }
396 
397  // if necessary move joints to initial configuration
398  if (needToMoveToInitConf) {
399  for (unsigned int i = 0; i < m_robot_util->m_nbJoints; i++) {
400  if (!isJointInRange(i, qInit[i])) return;
401 
402  m_minJerkTrajGen[i]->set_initial_point(m_noTrajGen[i]->getPos());
403  m_minJerkTrajGen[i]->set_final_point(qInit[i]);
404  m_minJerkTrajGen[i]->set_trajectory_time(4.0);
405  m_status[i] = JTG_MIN_JERK;
407  }
408  return;
409  }
410 
411  for (unsigned int i = 0; i < m_robot_util->m_nbJoints; i++) {
412  m_status[i] = JTG_TEXT_FILE;
413  }
414 }
415 
416 void JointTrajectoryGenerator::startSinusoid(const std::string& jointName, const double& qFinal, const double& time) {
417  if (!m_initSucceeded) return SEND_MSG("Cannot start sinusoid before initialization!", MSG_TYPE_ERROR);
418 
419  unsigned int i;
420  if (convertJointNameToJointId(jointName, i) == false) return;
421  if (time <= 0.0) return SEND_MSG("Trajectory time must be a positive number", MSG_TYPE_ERROR);
422  if (m_status[i] != JTG_STOP)
423  return SEND_MSG("You cannot move the specified joint because it is already controlled.", MSG_TYPE_ERROR);
424  if (!isJointInRange(i, qFinal)) return;
425 
426  m_sinTrajGen[i]->set_initial_point(m_noTrajGen[i]->getPos());
427  SEND_MSG("Set initial point of sinusoid to " + toString(m_sinTrajGen[i]->getPos()), MSG_TYPE_DEBUG);
428  m_sinTrajGen[i]->set_final_point(qFinal);
429  m_sinTrajGen[i]->set_trajectory_time(time);
430  m_status[i] = JTG_SINUSOID;
432 }
433 
434 void JointTrajectoryGenerator::startTriangle(const std::string& jointName, const double& qFinal, const double& time,
435  const double& Tacc) {
436  if (!m_initSucceeded) return SEND_MSG("Cannot start triangle before initialization!", MSG_TYPE_ERROR);
437 
438  unsigned int i;
439  if (convertJointNameToJointId(jointName, i) == false) return;
440  if (m_status[i] != JTG_STOP)
441  return SEND_MSG("You cannot move the specified joint because it is already controlled.", MSG_TYPE_ERROR);
442  if (!isJointInRange(i, qFinal)) return;
443 
444  m_triangleTrajGen[i]->set_initial_point(m_noTrajGen[i]->getPos());
445  SEND_MSG("Set initial point of triangular trajectory to " + toString(m_triangleTrajGen[i]->getPos()),
446  MSG_TYPE_DEBUG);
447  m_triangleTrajGen[i]->set_final_point(qFinal);
448 
449  if (!m_triangleTrajGen[i]->set_trajectory_time(time))
450  return SEND_MSG("Trajectory time cannot be negative.", MSG_TYPE_ERROR);
451 
452  if (!m_triangleTrajGen[i]->set_acceleration_time(Tacc))
453  return SEND_MSG("Acceleration time cannot be negative or larger than half the trajectory time.", MSG_TYPE_ERROR);
454 
455  m_status[i] = JTG_TRIANGLE;
457 }
458 
459 void JointTrajectoryGenerator::startConstAcc(const std::string& jointName, const double& qFinal, const double& time) {
460  if (!m_initSucceeded)
461  return SEND_MSG("Cannot start constant-acceleration trajectory before initialization!", MSG_TYPE_ERROR);
462 
463  unsigned int i;
464  if (convertJointNameToJointId(jointName, i) == false) return;
465  if (time <= 0.0) return SEND_MSG("Trajectory time must be a positive number", MSG_TYPE_ERROR);
466  if (m_status[i] != JTG_STOP)
467  return SEND_MSG("You cannot move the specified joint because it is already controlled.", MSG_TYPE_ERROR);
468  if (!isJointInRange(i, qFinal)) return;
469 
470  m_constAccTrajGen[i]->set_initial_point(m_noTrajGen[i]->getPos());
471  SEND_MSG("Set initial point of const-acc trajectory to " + toString(m_constAccTrajGen[i]->getPos()), MSG_TYPE_DEBUG);
472  m_constAccTrajGen[i]->set_final_point(qFinal);
473  m_constAccTrajGen[i]->set_trajectory_time(time);
474  m_status[i] = JTG_CONST_ACC;
476 }
477 
478 void JointTrajectoryGenerator::startForceSinusoid(const std::string& forceName, const int& axis, const double& fFinal,
479  const double& time) {
480  if (!m_initSucceeded) return SEND_MSG("Cannot start force sinusoid before initialization!", MSG_TYPE_ERROR);
481 
482  unsigned int i;
483  if (convertForceNameToForceId(forceName, i) == false) return;
484  if (time <= 0.0) return SEND_MSG("Trajectory time must be a positive number", MSG_TYPE_ERROR);
485  if (axis < 0 || axis > 5) return SEND_MSG("Axis must be between 0 and 5", MSG_TYPE_ERROR);
486  if (m_status_force[i] != JTG_STOP)
487  return SEND_MSG("You cannot move the specified force because it is already controlled.", MSG_TYPE_ERROR);
488  // EIGEN_CONST_VECTOR_FROM_SIGNAL(fFinal_eig, fFinal);
489  if (!isForceInRange(i, axis, fFinal)) return;
490 
491  VectorXd currentF = m_noTrajGen_force[i]->getPos();
492  m_sinTrajGen_force[i]->set_initial_point(currentF);
493  SEND_MSG("Set initial point of sinusoid to " + toString(m_sinTrajGen_force[i]->getPos()), MSG_TYPE_DEBUG);
494  currentF[axis] = fFinal;
495  m_sinTrajGen_force[i]->set_final_point(currentF);
496  m_sinTrajGen_force[i]->set_trajectory_time(time);
499 }
500 
501 void JointTrajectoryGenerator::startLinearChirp(const string& jointName, const double& qFinal, const double& f0,
502  const double& f1, const double& time) {
503  if (!m_initSucceeded) return SEND_MSG("Cannot start linear chirp before initialization!", MSG_TYPE_ERROR);
504 
505  unsigned int i;
506  if (convertJointNameToJointId(jointName, i) == false) return;
507  if (time <= 0.0) return SEND_MSG("Trajectory time must be a positive number", MSG_TYPE_ERROR);
508  if (m_status[i] != JTG_STOP)
509  return SEND_MSG("You cannot move the specified joint because it is already controlled.", MSG_TYPE_ERROR);
510  if (!isJointInRange(i, qFinal)) return;
511  if (f0 > f1) return SEND_MSG("f0 " + toString(f0) + " cannot to be more than f1 " + toString(f1), MSG_TYPE_ERROR);
512  if (f0 <= 0.0) return SEND_MSG("Frequency has to be positive " + toString(f0), MSG_TYPE_ERROR);
513 
514  if (!m_linChirpTrajGen[i]->set_initial_point(m_noTrajGen[i]->getPos()))
515  return SEND_MSG("Error while setting initial point " + toString(m_noTrajGen[i]->getPos()), MSG_TYPE_ERROR);
516  if (!m_linChirpTrajGen[i]->set_final_point(qFinal))
517  return SEND_MSG("Error while setting final point " + toString(qFinal), MSG_TYPE_ERROR);
518  if (!m_linChirpTrajGen[i]->set_trajectory_time(time))
519  return SEND_MSG("Error while setting trajectory time " + toString(time), MSG_TYPE_ERROR);
520  if (!m_linChirpTrajGen[i]->set_initial_frequency(f0))
521  return SEND_MSG("Error while setting initial frequency " + toString(f0), MSG_TYPE_ERROR);
522  if (!m_linChirpTrajGen[i]->set_final_frequency(f1))
523  return SEND_MSG("Error while setting final frequency " + toString(f1), MSG_TYPE_ERROR);
524  m_status[i] = JTG_LIN_CHIRP;
526 }
527 
528 void JointTrajectoryGenerator::startForceLinearChirp(const string& forceName, const int& axis, const double& fFinal,
529  const double& f0, const double& f1, const double& time) {
530  if (!m_initSucceeded) return SEND_MSG("Cannot start force linear chirp before initialization!", MSG_TYPE_ERROR);
531 
532  unsigned int i;
533  if (convertForceNameToForceId(forceName, i) == false) return;
534  if (time <= 0.0) return SEND_MSG("Trajectory time must be a positive number", MSG_TYPE_ERROR);
535  if (m_status_force[i] != JTG_STOP)
536  return SEND_MSG("You cannot move the specified force because it is already controlled.", MSG_TYPE_ERROR);
537  if (!isForceInRange(i, axis, fFinal)) return;
538  if (f0 > f1) return SEND_MSG("f0 " + toString(f0) + " cannot to be more than f1 " + toString(f1), MSG_TYPE_ERROR);
539  if (f0 <= 0.0) return SEND_MSG("Frequency has to be positive " + toString(f0), MSG_TYPE_ERROR);
540 
541  VectorXd currentF = m_noTrajGen_force[i]->getPos();
542  if (!m_linChirpTrajGen_force[i]->set_initial_point(currentF))
543  return SEND_MSG("Error while setting initial point " + toString(m_noTrajGen_force[i]->getPos()), MSG_TYPE_ERROR);
544  currentF[axis] = fFinal;
545  if (!m_linChirpTrajGen_force[i]->set_final_point(currentF))
546  return SEND_MSG("Error while setting final point " + toString(fFinal), MSG_TYPE_ERROR);
547  if (!m_linChirpTrajGen_force[i]->set_trajectory_time(time))
548  return SEND_MSG("Error while setting trajectory time " + toString(time), MSG_TYPE_ERROR);
549  if (!m_linChirpTrajGen_force[i]->set_initial_frequency(Vector6d::Constant(f0)))
550  return SEND_MSG("Error while setting initial frequency " + toString(f0), MSG_TYPE_ERROR);
551  if (!m_linChirpTrajGen_force[i]->set_final_frequency(Vector6d::Constant(f1)))
552  return SEND_MSG("Error while setting final frequency " + toString(f1), MSG_TYPE_ERROR);
555 }
556 
557 void JointTrajectoryGenerator::moveJoint(const string& jointName, const double& qFinal, const double& time) {
558  if (!m_initSucceeded) return SEND_MSG("Cannot move joint before initialization!", MSG_TYPE_ERROR);
559 
560  unsigned int i;
561  if (convertJointNameToJointId(jointName, i) == false) return;
562  if (time <= 0.0) return SEND_MSG("Trajectory time must be a positive number", MSG_TYPE_ERROR);
563  if (m_status[i] != JTG_STOP)
564  return SEND_MSG("You cannot move the specified joint because it is already controlled.", MSG_TYPE_ERROR);
565  if (!isJointInRange(i, qFinal)) return;
566 
567  m_minJerkTrajGen[i]->set_initial_point(m_noTrajGen[i]->getPos());
568  m_minJerkTrajGen[i]->set_final_point(qFinal);
569  m_minJerkTrajGen[i]->set_trajectory_time(time);
570  m_status[i] = JTG_MIN_JERK;
572 }
573 
574 void JointTrajectoryGenerator::moveForce(const string& forceName, const int& axis, const double& fFinal,
575  const double& time) {
576  if (!m_initSucceeded) return SEND_MSG("Cannot move force before initialization!", MSG_TYPE_ERROR);
577 
578  unsigned int i;
579  if (convertForceNameToForceId(forceName, i) == false) return;
580  if (time <= 0.0) return SEND_MSG("Trajectory time must be a positive number", MSG_TYPE_ERROR);
581  if (m_status_force[i] != JTG_STOP)
582  return SEND_MSG("You cannot move the specified force because it is already controlled.", MSG_TYPE_ERROR);
583  if (!isForceInRange(i, axis, fFinal)) return;
584 
585  VectorXd currentF = m_noTrajGen_force[i]->getPos();
586  m_minJerkTrajGen_force[i]->set_initial_point(currentF);
587  currentF[axis] = fFinal;
588  m_minJerkTrajGen_force[i]->set_final_point(currentF);
589  m_minJerkTrajGen_force[i]->set_trajectory_time(time);
592 }
593 
594 void JointTrajectoryGenerator::stop(const std::string& jointName) {
595  if (!m_initSucceeded) return SEND_MSG("Cannot stop joint before initialization!", MSG_TYPE_ERROR);
596 
597  const dynamicgraph::Vector& base6d_encoders = m_base6d_encodersSIN.accessCopy();
598  if (jointName == "all") {
599  for (unsigned int i = 0; i < m_robot_util->m_nbJoints; i++) {
600  m_status[i] = JTG_STOP;
601  // update the initial position
602  m_noTrajGen[i]->set_initial_point(base6d_encoders(6 + i));
604  }
605  return;
606  }
607 
608  unsigned int i;
609  if (convertJointNameToJointId(jointName, i) == false) return;
610  m_noTrajGen[i]->set_initial_point(base6d_encoders(6 + i)); // update the initial position
611  m_status[i] = JTG_STOP;
613 }
614 
615 void JointTrajectoryGenerator::stopForce(const std::string& forceName) {
616  if (!m_initSucceeded) return SEND_MSG("Cannot stop force before initialization!", MSG_TYPE_ERROR);
617 
618  unsigned int i;
619  if (convertForceNameToForceId(forceName, i) == false) return;
620  m_noTrajGen_force[i]->set_initial_point(m_currentTrajGen_force[i]->getPos()); // update the initial position
623 }
624 
625 /* ------------------------------------------------------------------- */
626 // ************************ PROTECTED MEMBER METHODS ********************
627 /* ------------------------------------------------------------------- */
628 
629 bool JointTrajectoryGenerator::convertJointNameToJointId(const std::string& name, unsigned int& id) {
630  // Check if the joint name exists
631  sot::Index jid = m_robot_util->get_id_from_name(name);
632  if (jid < 0) {
633  SEND_MSG("The specified joint name does not exist", MSG_TYPE_ERROR);
634  std::stringstream ss;
635  for (map<string, Index>::const_iterator it = m_robot_util->m_name_to_id.begin();
636  it != m_robot_util->m_name_to_id.end(); it++)
637  ss << it->first << ", ";
638  SEND_MSG("Possible joint names are: " + ss.str(), MSG_TYPE_INFO);
639  return false;
640  }
641  id = static_cast<unsigned int>(jid);
642  return true;
643 }
644 
645 bool JointTrajectoryGenerator::convertForceNameToForceId(const std::string& name, unsigned int& id) {
646  // Check if the joint name exists
647  Index fid = m_robot_util->m_force_util.get_id_from_name(name);
648  if (fid < 0) {
649  SEND_MSG("The specified force name does not exist", MSG_TYPE_ERROR);
650  std::stringstream ss;
651  for (map<string, Index>::const_iterator it = m_robot_util->m_force_util.m_name_to_force_id.begin();
652  it != m_robot_util->m_force_util.m_name_to_force_id.end(); it++)
653  ss << it->first << ", ";
654  SEND_MSG("Possible force names are: " + ss.str(), MSG_TYPE_INFO);
655  return false;
656  }
657  id = (unsigned int)fid;
658  return true;
659 }
660 
661 bool JointTrajectoryGenerator::isJointInRange(unsigned int id, double q) {
662  JointLimits jl = m_robot_util->get_joint_limits_from_id(id);
663  if (q < jl.lower) {
664  SEND_MSG("Joint " + m_robot_util->get_name_from_id(id) + ", desired angle " + toString(q) +
665  " is smaller than lower limit " + toString(jl.lower),
666  MSG_TYPE_ERROR);
667  return false;
668  }
669  if (q > jl.upper) {
670  SEND_MSG("Joint " + m_robot_util->get_name_from_id(id) + ", desired angle " + toString(q) +
671  " is larger than upper limit " + toString(jl.upper),
672  MSG_TYPE_ERROR);
673  return false;
674  }
675  return true;
676 }
677 
678 bool JointTrajectoryGenerator::isForceInRange(unsigned int id, const Eigen::VectorXd& f) {
679  ForceLimits fl = m_robot_util->m_force_util.get_limits_from_id(id);
680  for (unsigned int i = 0; i < 6; i++)
681  if (f[i] < fl.lower[i] || f[i] > fl.upper[i]) {
682  SEND_MSG("Desired force " + toString(i) + " is out of range: " + toString(fl.lower[i]) + " < " + toString(f[i]) +
683  " < " + toString(fl.upper[i]),
684  MSG_TYPE_ERROR);
685  return false;
686  }
687  return true;
688 }
689 
690 bool JointTrajectoryGenerator::isForceInRange(unsigned int id, int axis, double f) {
691  ForceLimits fl = m_robot_util->m_force_util.get_limits_from_id(id);
692  if (f < fl.lower[axis] || f > fl.upper[axis]) {
693  SEND_MSG("Desired force " + toString(axis) + " is out of range: " + toString(fl.lower[axis]) + " < " +
694  toString(f) + " < " + toString(fl.upper[axis]),
695  MSG_TYPE_ERROR);
696  return false;
697  }
698  return true;
699 }
700 
701 /* ------------------------------------------------------------------- */
702 /* --- ENTITY -------------------------------------------------------- */
703 /* ------------------------------------------------------------------- */
704 
705 void JointTrajectoryGenerator::display(std::ostream& os) const {
706  os << "JointTrajectoryGenerator " << getName();
707  try {
708  getProfiler().report_all(3, os);
709  } catch (ExceptionSignal e) {
710  }
711 }
712 } // namespace torque_control
713 } // namespace sot
714 } // namespace dynamicgraph
dynamicgraph::sot::torque_control::JointTrajectoryGenerator::JTG_SINUSOID
@ JTG_SINUSOID
Definition: joint-trajectory-generator.hh:158
dynamicgraph::command::makeCommandVoid6
CommandVoid6< E, T1, T2, T3, T4, T5, T6 > * makeCommandVoid6(E &entity, typename CommandVoid6< E, T1, T2, T3, T4, T5, T6 >::function_t function, const std::string &docString)
Definition: commands-helper.hh:155
dynamicgraph::sot::torque_control::JointTrajectoryGenerator::JTG_LIN_CHIRP
@ JTG_LIN_CHIRP
Definition: joint-trajectory-generator.hh:158
dynamicgraph::sot::torque_control::JointTrajectoryGenerator::JTG_TEXT_FILE
@ JTG_TEXT_FILE
Definition: joint-trajectory-generator.hh:158
dynamicgraph::sot::torque_control::JointTrajectoryGenerator::startLinearChirp
void startLinearChirp(const std::string &jointName, const double &qFinal, const double &f0, const double &f1, const double &time)
Definition: joint-trajectory-generator.cpp:501
dynamicgraph::sot::torque_control::JointTrajectoryGenerator::m_minJerkTrajGen_force
std::vector< MinimumJerkTrajectoryGenerator * > m_minJerkTrajGen_force
Definition: joint-trajectory-generator.hh:182
dynamicgraph::sot::torque_control::JointTrajectoryGenerator::m_triangleTrajGen
std::vector< TriangleTrajectoryGenerator * > m_triangleTrajGen
Definition: joint-trajectory-generator.hh:174
dynamicgraph::sot::torque_control::JointTrajectoryGenerator::m_status
std::vector< JTG_Status > m_status
Definition: joint-trajectory-generator.hh:168
dynamicgraph::sot::torque_control::JointTrajectoryGenerator::startForceSinusoid
void startForceSinusoid(const std::string &forceName, const int &axis, const double &fFinal, const double &time)
Definition: joint-trajectory-generator.cpp:478
dynamicgraph::sot::torque_control::JointTrajectoryGenerator::moveForce
void moveForce(const std::string &forceName, const int &axis, const double &fFinal, const double &time)
Definition: joint-trajectory-generator.cpp:574
dynamicgraph::sot::torque_control::JointTrajectoryGenerator::m_sinTrajGen
std::vector< SinusoidTrajectoryGenerator * > m_sinTrajGen
Definition: joint-trajectory-generator.hh:172
dynamicgraph::sot::torque_control::TriangleTrajectoryGenerator
Definition: trajectory-generators.hh:287
dynamicgraph
to read text file
Definition: treeview.dox:22
dynamicgraph::sot::torque_control::JointTrajectoryGenerator::startTriangle
void startTriangle(const std::string &jointName, const double &qFinal, const double &time, const double &Tacc)
Definition: joint-trajectory-generator.cpp:434
dynamicgraph::sot::torque_control::JointTrajectoryGenerator::JTG_MIN_JERK
@ JTG_MIN_JERK
Definition: joint-trajectory-generator.hh:158
dynamicgraph::sot::torque_control::JointTrajectoryGenerator::startSinusoid
void startSinusoid(const std::string &jointName, const double &qFinal, const double &time)
Definition: joint-trajectory-generator.cpp:416
dynamicgraph::command
Definition: commands-helper.hh:62
dynamicgraph::sot::torque_control::JointTrajectoryGenerator::m_firstIter
bool m_firstIter
true if the entity has been successfully initialized
Definition: joint-trajectory-generator.hh:161
dynamicgraph::sot::torque_control::SinusoidTrajectoryGenerator
Definition: trajectory-generators.hh:262
dynamicgraph::sot::torque_control::JointTrajectoryGenerator::moveJoint
void moveJoint(const std::string &jointName, const double &qFinal, const double &time)
Definition: joint-trajectory-generator.cpp:557
dynamicgraph::sot::torque_control::JointTrajectoryGenerator::m_minJerkTrajGen
std::vector< MinimumJerkTrajectoryGenerator * > m_minJerkTrajGen
Definition: joint-trajectory-generator.hh:171
dynamicgraph::sot::torque_control::JointTrajectoryGenerator::isForceInRange
bool isForceInRange(unsigned int id, const Eigen::VectorXd &f)
Definition: joint-trajectory-generator.cpp:678
dynamicgraph::sot::torque_control::LinearChirpTrajectoryGenerator
Definition: trajectory-generators.hh:381
dynamicgraph::sot::torque_control::JointTrajectoryGenerator::m_initSucceeded
bool m_initSucceeded
Definition: joint-trajectory-generator.hh:160
dynamicgraph::sot::torque_control::NoTrajectoryGenerator
Definition: trajectory-generators.hh:179
dynamicgraph::sot::torque_control::JointTrajectoryGenerator::display
virtual void display(std::ostream &os) const
Definition: joint-trajectory-generator.cpp:705
dynamicgraph::sot::torque_control::JointTrajectoryGenerator::generateReferenceForceSignal
bool generateReferenceForceSignal(const std::string &forceName, int fid, dynamicgraph::Vector &s, int iter)
Definition: joint-trajectory-generator.cpp:309
dynamicgraph::sot::torque_control::JointTrajectoryGenerator::startForceLinearChirp
void startForceLinearChirp(const std::string &forceName, const int &axis, const double &fFinal, const double &f0, const double &f1, const double &time)
Definition: joint-trajectory-generator.cpp:528
commands-helper.hh
dynamicgraph::sot::command::IsTrajectoryEnded
Definition: stc-commands.hh:27
dynamicgraph::sot::torque_control::JointTrajectoryGenerator::convertForceNameToForceId
bool convertForceNameToForceId(const std::string &name, unsigned int &id)
Definition: joint-trajectory-generator.cpp:645
PROFILE_FORCE_DESIRED_COMPUTATION
#define PROFILE_FORCE_DESIRED_COMPUTATION
Definition: joint-trajectory-generator.cpp:25
dynamicgraph::sot::torque_control::JointTrajectoryGenerator::m_noTrajGen
std::vector< NoTrajectoryGenerator * > m_noTrajGen
Definition: joint-trajectory-generator.hh:170
dynamicgraph::sot::torque_control::JointTrajectoryGenerator::init
void init(const double &dt, const std::string &robotRef)
Definition: joint-trajectory-generator.cpp:155
dynamicgraph::sot::torque_control::AbstractTrajectoryGenerator::isTrajectoryEnded
virtual bool isTrajectoryEnded()
Definition: trajectory-generators.hh:173
dynamicgraph::sot::torque_control::JointTrajectoryGenerator::playTrajectoryFile
void playTrajectoryFile(const std::string &fileName)
Definition: joint-trajectory-generator.cpp:375
dynamicgraph::sot::torque_control::JointTrajectoryGenerator::isJointInRange
bool isJointInRange(unsigned int id, double q)
Definition: joint-trajectory-generator.cpp:661
dynamicgraph::sot::torque_control::JointTrajectoryGenerator::getJoint
void getJoint(const std::string &jointName)
Definition: joint-trajectory-generator.cpp:346
dynamicgraph::sot::torque_control::JointTrajectoryGenerator::JTG_CONST_ACC
@ JTG_CONST_ACC
Definition: joint-trajectory-generator.hh:158
dynamicgraph::sot::torque_control::JointTrajectoryGenerator::m_status_force
std::vector< JTG_Status > m_status_force
Definition: joint-trajectory-generator.hh:178
dynamicgraph::sot::torque_control::JointTrajectoryGenerator::m_noTrajGen_force
std::vector< NoTrajectoryGenerator * > m_noTrajGen_force
Definition: joint-trajectory-generator.hh:180
dynamicgraph::sot::torque_control::JointTrajectoryGenerator::m_robot_util
RobotUtilShrPtr m_robot_util
control loop time period
Definition: joint-trajectory-generator.hh:164
dynamicgraph::sot::torque_control::JointTrajectoryGenerator::m_currentTrajGen_force
std::vector< AbstractTrajectoryGenerator * > m_currentTrajGen_force
status of the forces
Definition: joint-trajectory-generator.hh:179
torque_control
Definition: __init__.py:1
dynamicgraph::sot::torque_control::JointTrajectoryGenerator::m_textFileTrajGen
TextFileTrajectoryGenerator * m_textFileTrajGen
Definition: joint-trajectory-generator.hh:176
dynamicgraph::sot::torque_control::TextFileTrajectoryGenerator::loadTextFile
virtual bool loadTextFile(const std::string &fileName)
Definition: trajectory-generators.hh:202
dynamicgraph::sot::torque_control::JointTrajectoryGenerator::JTG_STOP
@ JTG_STOP
Definition: joint-trajectory-generator.hh:158
PROFILE_POSITION_DESIRED_COMPUTATION
#define PROFILE_POSITION_DESIRED_COMPUTATION
Definition: joint-trajectory-generator.cpp:24
dynamicgraph::command::docCommandVoid5
std::string docCommandVoid5(const std::string &doc, const std::string &type1, const std::string &type2, const std::string &type3, const std::string &type4, const std::string &type5)
Definition: commands-helper.hh:117
dynamicgraph::sot::torque_control::JointTrajectoryGenerator::m_currentTrajGen
std::vector< AbstractTrajectoryGenerator * > m_currentTrajGen
status of the joints
Definition: joint-trajectory-generator.hh:169
dynamicgraph::sot::torque_control::MinimumJerkTrajectoryGenerator
Definition: trajectory-generators.hh:235
dynamicgraph::sot::torque_control::JointTrajectoryGenerator::convertJointNameToJointId
bool convertJointNameToJointId(const std::string &name, unsigned int &id)
Definition: joint-trajectory-generator.cpp:629
dynamicgraph::sot::torque_control::DYNAMICGRAPH_FACTORY_ENTITY_PLUGIN
DYNAMICGRAPH_FACTORY_ENTITY_PLUGIN(AdmittanceController, "AdmittanceController")
dynamicgraph::command::makeCommandVoid5
CommandVoid5< E, T1, T2, T3, T4, T5 > * makeCommandVoid5(E &entity, typename CommandVoid5< E, T1, T2, T3, T4, T5 >::function_t function, const std::string &docString)
Definition: commands-helper.hh:94
dynamicgraph::sot::torque_control::JointTrajectoryGenerator::startConstAcc
void startConstAcc(const std::string &jointName, const double &qFinal, const double &time)
Definition: joint-trajectory-generator.cpp:459
dynamicgraph::sot::torque_control::JointTrajectoryGenerator::stopForce
void stopForce(const std::string &forceName)
Definition: joint-trajectory-generator.cpp:615
dynamicgraph::sot::torque_control::JointTrajectoryGenerator::m_constAccTrajGen
std::vector< ConstantAccelerationTrajectoryGenerator * > m_constAccTrajGen
Definition: joint-trajectory-generator.hh:175
dynamicgraph::sot::torque_control::TextFileTrajectoryGenerator
Definition: trajectory-generators.hh:193
dynamicgraph::sot::torque_control::JointTrajectoryGenerator::JTG_TRIANGLE
@ JTG_TRIANGLE
Definition: joint-trajectory-generator.hh:158
joint-trajectory-generator.hh
dynamicgraph::sot::torque_control::EntityClassName
AdmittanceController EntityClassName
Definition: admittance-controller.cpp:44
dynamicgraph::sot::torque_control::JointTrajectoryGenerator::stop
void stop(const std::string &jointName)
Definition: joint-trajectory-generator.cpp:594
dynamicgraph::sot::torque_control::JointTrajectoryGenerator::m_sinTrajGen_force
std::vector< SinusoidTrajectoryGenerator * > m_sinTrajGen_force
Definition: joint-trajectory-generator.hh:181
dynamicgraph::sot::torque_control::DEFINE_SIGNAL_OUT_FUNCTION
DEFINE_SIGNAL_OUT_FUNCTION(u, dynamicgraph::Vector)
Definition: admittance-controller.cpp:160
dynamicgraph::sot::torque_control::ConstantAccelerationTrajectoryGenerator
Definition: trajectory-generators.hh:334
dynamicgraph::sot::torque_control::JointTrajectoryGenerator::m_linChirpTrajGen
std::vector< LinearChirpTrajectoryGenerator * > m_linChirpTrajGen
Definition: joint-trajectory-generator.hh:173
dynamicgraph::sot::torque_control::AbstractTrajectoryGenerator::get_initial_point
virtual const Eigen::VectorXd & get_initial_point()
Definition: trajectory-generators.hh:170
dynamicgraph::command::docCommandVoid6
std::string docCommandVoid6(const std::string &doc, const std::string &type1, const std::string &type2, const std::string &type3, const std::string &type4, const std::string &type5, const std::string &type6)
Definition: commands-helper.hh:178
dynamicgraph::sot::torque_control::JointTrajectoryGenerator::JointTrajectoryGenerator
JointTrajectoryGenerator(const std::string &name)
Definition: joint-trajectory-generator.cpp:37
dynamicgraph::sot::torque_control::JointTrajectoryGenerator::isTrajectoryEnded
bool isTrajectoryEnded()
Definition: joint-trajectory-generator.cpp:353
dynamicgraph::sot::torque_control::JointTrajectoryGenerator::m_linChirpTrajGen_force
std::vector< LinearChirpTrajectoryGenerator * > m_linChirpTrajGen_force
Definition: joint-trajectory-generator.hh:183
dynamicgraph::sot::torque_control::JointTrajectoryGenerator::m_iterForceSignals
std::vector< int > m_iterForceSignals
Definition: joint-trajectory-generator.hh:166
dynamicgraph::sot::torque_control::JointTrajectoryGenerator::m_dt
double m_dt
true if it is the first iteration, false otherwise
Definition: joint-trajectory-generator.hh:162