sot-torque-control  1.5.3
Collection of dynamic-graph entities aimed at implementing torque control on different robots.
trajectory-generators.hh
Go to the documentation of this file.
1 /*
2  * Copyright 2015, Andrea Del Prete, LAAS-CNRS
3  *
4  */
5 
6 #ifndef __sot_torque_control_trajectory_generator_H__
7 #define __sot_torque_control_trajectory_generator_H__
8 
9 /* --------------------------------------------------------------------- */
10 /* --- API ------------------------------------------------------------- */
11 /* --------------------------------------------------------------------- */
12 
13 #if defined(WIN32)
14 #if defined(trajectory_generator_EXPORTS)
15 #define TRAJECTORYGENERATOR_EXPORT __declspec(dllexport)
16 #else
17 #define TRAJECTORYGENERATOR_EXPORT __declspec(dllimport)
18 #endif
19 #else
20 #define HRP2COMMON_EXPORT
21 #endif
22 
23 /* --------------------------------------------------------------------- */
24 /* --- INCLUDE --------------------------------------------------------- */
25 /* --------------------------------------------------------------------- */
26 
27 #include <iostream>
28 
29 #include <map>
30 #include <fstream>
31 #include "boost/assign.hpp"
32 #include <dynamic-graph/signal-helper.h>
34 
35 namespace dynamicgraph {
36 namespace sot {
37 namespace torque_control {
38 
39 #define MAXBUFSIZE ((int)1000000)
40 
41 Eigen::MatrixXd readMatrixFromFile(const char* filename) {
42  int cols = 0, rows = 0;
43  double buff[MAXBUFSIZE];
44 
45  // Read numbers from file into buffer.
46  std::ifstream infile;
47  infile.open(filename);
48  while (!infile.eof()) {
49  std::string line;
50  getline(infile, line);
51  // std::cout<<"Read line "<<rows<<"\n";
52 
53  int temp_cols = 0;
54  std::stringstream stream(line);
55  while (!stream.eof()) stream >> buff[cols * rows + temp_cols++];
56 
57  if (temp_cols == 0) continue;
58 
59  if (cols == 0)
60  cols = temp_cols;
61  else if (temp_cols != cols && !infile.eof()) {
62  std::cout << "Error while reading matrix from file, line " << rows << " has " << temp_cols
63  << " columnds, while preceding lines had " << cols << " columnds\n";
64  std::cout << line << "\n";
65  break;
66  }
67 
68  rows++;
69  if ((rows + 1) * cols >= MAXBUFSIZE) {
70  std::cout << "Max buffer size exceeded (" << rows << " rows, " << cols << " cols)\n";
71  break;
72  }
73  }
74  infile.close();
75  rows--;
76 
77  // Populate matrix with numbers.
78  Eigen::MatrixXd result(rows, cols);
79  for (int i = 0; i < rows; i++)
80  for (int j = 0; j < cols; j++) result(i, j) = buff[cols * i + j];
81 
82  return result;
83 }
84 
86  protected:
87  Eigen::VectorXd m_x;
88  Eigen::VectorXd m_dx;
89  Eigen::VectorXd m_ddx;
90  Eigen::VectorXd m_x_init;
91  Eigen::VectorXd m_x_final;
92 
93  double m_traj_time;
94  double m_dt;
95  double m_t;
96  Eigen::VectorXd::Index m_size;
97 
98  virtual void resizeAllData(Eigen::VectorXd::Index size) {
99  m_size = size;
100  m_x.setZero(size);
101  m_dx.setZero(size);
102  m_ddx.setZero(size);
103  m_x_init.setZero(size);
104  m_x_final.setZero(size);
105  }
106 
107  void sendMsg(const std::string& msg, MsgType t = MSG_TYPE_INFO, const char* file = "", int line = 0) {
108  sendMsg("[AbstrTrajGen] " + msg, t, file, line);
109  }
110 
111  public:
112  AbstractTrajectoryGenerator(double dt, double traj_time, Eigen::VectorXd::Index size) {
113  m_t = 0.0;
114  m_dt = dt;
115  m_traj_time = traj_time;
116  resizeAllData(size);
117  }
118 
119  AbstractTrajectoryGenerator(double dt, double traj_time, const Eigen::VectorXd& x_init,
120  const Eigen::VectorXd& x_final) {
121  assert(x_init.size() == x_final.size() && "Initial and final state must have the same size");
122  m_dt = dt;
123  m_traj_time = traj_time;
124  resizeAllData(x_init.size());
125  set_initial_point(x_init);
126  set_final_point(x_final);
127  }
128 
129  virtual bool set_initial_point(const Eigen::VectorXd& x_init) {
130  if (x_init.size() != m_x_init.size()) return false;
131  m_x_init = x_init;
132  m_x = x_init;
133  m_dx.setZero();
134  m_t = 0.0;
135  return true;
136  }
137 
138  virtual bool set_initial_point(const double& x_init) {
139  if (1 != m_x_init.size()) return false;
140  m_x_init(0) = x_init;
141  m_x(0) = x_init;
142  m_dx(0) = 0.0;
143  m_t = 0.0;
144  return true;
145  }
146 
147  virtual bool set_final_point(const Eigen::VectorXd& x_final) {
148  if (x_final.size() != m_x_final.size()) return false;
149  m_x_final = x_final;
150  return true;
151  }
152 
153  virtual bool set_final_point(const double& x_final) {
154  if (1 != m_x_final.size()) return false;
155  m_x_final(0) = x_final;
156  return true;
157  }
158 
159  virtual bool set_trajectory_time(double traj_time) {
160  if (traj_time <= 0.0) return false;
161  m_traj_time = traj_time;
162  return true;
163  }
164 
165  virtual const Eigen::VectorXd& compute_next_point() = 0;
166 
167  virtual const Eigen::VectorXd& getPos() { return m_x; }
168  virtual const Eigen::VectorXd& getVel() { return m_dx; }
169  virtual const Eigen::VectorXd& getAcc() { return m_ddx; }
170  virtual const Eigen::VectorXd& get_initial_point() { return m_x_init; }
171  virtual const Eigen::VectorXd& get_final_point() { return m_x_final; }
172 
173  virtual bool isTrajectoryEnded() { return m_t >= m_traj_time; }
174 };
175 
180  public:
181  NoTrajectoryGenerator(int size) : AbstractTrajectoryGenerator(0.001, 1.0, size) {}
182 
183  virtual const Eigen::VectorXd& compute_next_point() {
184  m_t += m_dt;
185  return m_x;
186  }
187 
188  virtual bool isTrajectoryEnded() { return false; }
189 };
190 
194  protected:
195  Eigen::MatrixXd m_posTraj;
196  Eigen::MatrixXd m_velTraj;
197  Eigen::MatrixXd m_accTraj;
198 
199  public:
200  TextFileTrajectoryGenerator(double dt, Eigen::VectorXd::Index size) : AbstractTrajectoryGenerator(dt, 1.0, size) {}
201 
202  virtual bool loadTextFile(const std::string& fileName) {
203  Eigen::MatrixXd data = readMatrixFromFile(fileName.c_str());
204  // std::cout<<"Read matrix with "<<data.rows()<<" rows and "<<data.cols()<<" cols from text file\n";
205  if (data.cols() != 3 * m_size) {
206  std::cout << "Unexpected number of columns (expected " << 3 * m_size << ", found " << data.cols() << ")\n";
207  return false;
208  }
209 
210  m_traj_time = m_dt * (double)data.rows();
211  m_t = 0.0;
212 
213  m_posTraj = data.leftCols(m_size);
214  m_velTraj = data.middleCols(m_size, m_size);
215  m_accTraj = data.rightCols(m_size);
216 
217  m_x_init = m_posTraj.row(0);
218 
219  return true;
220  }
221 
222  virtual const Eigen::VectorXd& compute_next_point() {
223  Eigen::VectorXd::Index i = (Eigen::VectorXd::Index)std::floor(m_t / m_dt);
224  if (i < m_posTraj.rows()) {
225  m_x = m_posTraj.row(i);
226  m_dx = m_velTraj.row(i);
227  m_ddx = m_accTraj.row(i);
228  }
229  m_t += m_dt;
230  return m_x;
231  }
232 };
233 
236  public:
237  MinimumJerkTrajectoryGenerator(double dt, double traj_time, int size)
238  : AbstractTrajectoryGenerator(dt, traj_time, size) {}
239 
240  virtual const Eigen::VectorXd& compute_next_point() {
241  if (m_t <= m_traj_time) {
242  double td = m_t / m_traj_time;
243  double td2 = td * td;
244  double td3 = td2 * td;
245  double td4 = td3 * td;
246  double td5 = td4 * td;
247  double p = 10 * td3 - 15 * td4 + 6 * td5;
248  double dp = (30 * td2 - 60 * td3 + 30 * td4) / m_traj_time;
249  double ddp = (60 * td - 180 * td2 + 120 * td3) / (m_traj_time * m_traj_time);
250  m_x = m_x_init + (m_x_final - m_x_init) * p;
251  m_dx = (m_x_final - m_x_init) * dp;
252  m_ddx = (m_x_final - m_x_init) * ddp;
253  }
254  m_t += m_dt;
255  return m_x;
256  }
257 };
258 
263  public:
264  SinusoidTrajectoryGenerator(double dt, double traj_time, int size)
265  : AbstractTrajectoryGenerator(dt, traj_time, size) {}
266 
267  const Eigen::VectorXd& compute_next_point() {
268  double f = 1.0 / (2.0 * m_traj_time);
269  double two_pi_f = 2 * M_PI * f;
270  double two_pi_f_t = two_pi_f * m_t;
271  double p = 0.5 * (1.0 - cos(two_pi_f_t));
272  double dp = 0.5 * two_pi_f * sin(two_pi_f_t);
273  double ddp = 0.5 * two_pi_f * two_pi_f * cos(two_pi_f_t);
274  m_x = m_x_init + (m_x_final - m_x_init) * p;
275  m_dx = (m_x_final - m_x_init) * dp;
276  m_ddx = (m_x_final - m_x_init) * ddp;
277 
278  m_t += m_dt;
279  return m_x;
280  }
281 
282  virtual bool isTrajectoryEnded() { return false; }
283 };
284 
288  protected:
290  double m_Tacc;
291 
292  public:
293  TriangleTrajectoryGenerator(double dt, double traj_time, int size)
294  : AbstractTrajectoryGenerator(dt, traj_time, size), m_comingBack(false) {
295  m_Tacc = 1.0;
296  }
297 
298  bool set_acceleration_time(const double Tacc) {
299  if (Tacc < 0.0 || Tacc > 0.5 * m_traj_time) return false;
300  m_Tacc = Tacc;
301  return true;
302  }
303 
304  const Eigen::VectorXd& compute_next_point() {
305  int way;
306  double t = m_t;
307  Eigen::VectorXd max_vel = (m_x_final - m_x_init) / (m_traj_time - m_Tacc);
308  if (m_t > m_traj_time) {
309  way = -1;
310  t = t - m_traj_time;
311  } else {
312  way = 1;
313  }
314  if (t < m_Tacc) {
315  m_ddx = way * max_vel / m_Tacc;
316  m_dx = t / m_Tacc * way * max_vel;
317  } else if (t > m_traj_time - m_Tacc) {
318  m_ddx = -way * max_vel / m_Tacc;
319  m_dx = (m_traj_time - t) / m_Tacc * way * max_vel;
320  } else {
321  m_ddx = 0.0 * max_vel;
322  m_dx = way * max_vel;
323  }
324  m_x += m_dt * m_dx;
325  m_t += m_dt;
326  if (m_t >= 2 * m_traj_time) m_t = m_t - 2 * m_traj_time;
327  return m_x;
328  }
329  virtual bool isTrajectoryEnded() { return false; }
330 };
331 
335  protected:
336  bool m_is_accelerating; // if true then apply ddx0, otherwise apply -ddx0
337  int m_counter; // counter to decide when to switch from ddx0 to -ddx0
338  int m_counter_max; // value of the counter at which to switch from ddx0 to -ddx0
339  Eigen::VectorXd m_ddx0; // acceleration to go halfway from x_init to x_final in half traj_time
340 
341  public:
342  ConstantAccelerationTrajectoryGenerator(double dt, double traj_time, int size)
343  : AbstractTrajectoryGenerator(dt, traj_time, size), m_is_accelerating(true) {}
344 
345  virtual bool set_trajectory_time(double traj_time) {
347  if (!res) return false;
348  m_counter_max = int(m_traj_time / m_dt);
349  m_counter = int(0.5 * m_counter_max);
350  m_is_accelerating = true;
351  return true;
352  }
353 
354  const Eigen::VectorXd& compute_next_point() {
355  m_ddx0 = 4.0 * (m_x_final - m_x_init) / (m_traj_time * m_traj_time);
356 
357  if (m_counter == m_counter_max) {
358  m_counter = 0;
360  }
361  m_counter += 1;
362 
363  m_ddx = m_ddx0;
364  if (m_is_accelerating == false) m_ddx *= -1.0;
365 
366  m_x += m_dt * m_dx + 0.5 * m_dt * m_dt * m_ddx;
367  m_dx += m_dt * m_ddx;
368 
369  m_t += m_dt;
370  return m_x;
371  }
372 
373  virtual bool isTrajectoryEnded() { return false; }
374 };
375 
382  protected:
383  Eigen::VectorXd m_f0;
384  Eigen::VectorXd m_f1;
385 
387  Eigen::VectorXd m_k;
388  Eigen::VectorXd m_f;
389  Eigen::VectorXd m_phi;
390  Eigen::VectorXd m_phi_0;
391  Eigen::VectorXd m_p;
392  Eigen::VectorXd m_dp;
393  Eigen::VectorXd m_ddp;
394 
395  public:
396  LinearChirpTrajectoryGenerator(double dt, double traj_time, int size)
397  : AbstractTrajectoryGenerator(dt, traj_time, size) {
398  m_f0.setZero(size);
399  m_f1.setZero(size);
400  m_k.setZero(size);
401  m_f.setZero(size);
402  m_phi.setZero(size);
403  m_phi_0.setZero(size);
404  m_p.setZero(size);
405  m_dp.setZero(size);
406  m_ddp.setZero(size);
407  }
408 
409  virtual bool set_initial_frequency(const Eigen::VectorXd& f0) {
410  if (f0.size() != m_f0.size()) return false;
411  m_f0 = f0;
412  return true;
413  }
414 
415  virtual bool set_initial_frequency(const double& f0) {
416  if (1 != m_f0.size()) return false;
417  m_f0[0] = f0;
418  return true;
419  }
420 
421  virtual bool set_final_frequency(const Eigen::VectorXd& f1) {
422  if (f1.size() != m_f1.size()) return false;
423  m_f1 = f1;
424  return true;
425  }
426 
427  virtual bool set_final_frequency(const double& f1) {
428  if (1 != m_f1.size()) return false;
429  m_f1[0] = f1;
430  return true;
431  }
432 
433  const Eigen::VectorXd& compute_next_point() {
434  if (m_t == 0.0) {
435  m_k = 2.0 * (m_f1 - m_f0) / m_traj_time;
436  m_phi_0 = M_PI * m_traj_time * (m_f0 - m_f1);
437  }
438 
439  if (m_t < 0.5 * m_traj_time) {
440  m_f = m_f0 + m_k * m_t;
441  m_phi = 2 * M_PI * m_t * (m_f0 + 0.5 * m_k * m_t);
442  } else {
443  m_f = m_f1 + m_k * (0.5 * m_traj_time - m_t);
444  m_phi = m_phi_0 + 2 * M_PI * m_t * (m_f1 + 0.5 * m_k * (m_traj_time - m_t));
445  }
446  m_p = 0.5 * (1.0 - m_phi.array().cos());
447  m_dp = M_PI * m_f.array() * m_phi.array().sin();
448  m_ddp = 2.0 * M_PI * M_PI * m_f.array() * m_f.array() * m_phi.array().cos();
449 
450  m_x = m_x_init.array() + (m_x_final.array() - m_x_init.array()) * m_p.array();
451  m_dx = (m_x_final - m_x_init).array() * m_dp.array();
452  m_ddx = (m_x_final - m_x_init).array() * m_ddp.array();
453 
454  m_t += m_dt;
455  return m_x;
456  }
457 };
458 
459 } // namespace torque_control
460 } // namespace sot
461 } // namespace dynamicgraph
462 
463 #endif // #ifndef __sot_torque_control_trajectory_generators_H__
dynamicgraph::sot::torque_control::LinearChirpTrajectoryGenerator::set_initial_frequency
virtual bool set_initial_frequency(const Eigen::VectorXd &f0)
Definition: trajectory-generators.hh:409
dynamicgraph::sot::torque_control::LinearChirpTrajectoryGenerator::LinearChirpTrajectoryGenerator
LinearChirpTrajectoryGenerator(double dt, double traj_time, int size)
Definition: trajectory-generators.hh:396
dynamicgraph::sot::torque_control::LinearChirpTrajectoryGenerator::m_dp
Eigen::VectorXd m_dp
Definition: trajectory-generators.hh:392
dynamicgraph::sot::torque_control::LinearChirpTrajectoryGenerator::set_final_frequency
virtual bool set_final_frequency(const double &f1)
Definition: trajectory-generators.hh:427
dynamicgraph::sot::torque_control::AbstractTrajectoryGenerator
Definition: trajectory-generators.hh:85
dynamicgraph::sot::torque_control::AbstractTrajectoryGenerator::getVel
virtual const Eigen::VectorXd & getVel()
Definition: trajectory-generators.hh:168
dynamicgraph::sot::torque_control::TextFileTrajectoryGenerator::compute_next_point
virtual const Eigen::VectorXd & compute_next_point()
Definition: trajectory-generators.hh:222
dynamicgraph::sot::torque_control::TriangleTrajectoryGenerator
Definition: trajectory-generators.hh:287
dynamicgraph
to read text file
Definition: treeview.dox:22
dynamicgraph::sot::torque_control::AbstractTrajectoryGenerator::m_x_init
Eigen::VectorXd m_x_init
current acceleration
Definition: trajectory-generators.hh:90
dynamicgraph::sot::torque_control::SinusoidTrajectoryGenerator
Definition: trajectory-generators.hh:262
dynamicgraph::sot::torque_control::TriangleTrajectoryGenerator::m_comingBack
bool m_comingBack
Definition: trajectory-generators.hh:289
dynamicgraph::sot::torque_control::TriangleTrajectoryGenerator::m_Tacc
double m_Tacc
Definition: trajectory-generators.hh:290
dynamicgraph::sot::torque_control::ConstantAccelerationTrajectoryGenerator::compute_next_point
const Eigen::VectorXd & compute_next_point()
Definition: trajectory-generators.hh:354
dynamicgraph::sot::torque_control::LinearChirpTrajectoryGenerator::m_f0
Eigen::VectorXd m_f0
Definition: trajectory-generators.hh:383
dynamicgraph::sot::torque_control::LinearChirpTrajectoryGenerator
Definition: trajectory-generators.hh:381
dynamicgraph::sot::torque_control::readMatrixFromFile
Eigen::MatrixXd readMatrixFromFile(const char *filename)
Definition: trajectory-generators.hh:41
vector-conversions.hh
dynamicgraph::sot::torque_control::ConstantAccelerationTrajectoryGenerator::isTrajectoryEnded
virtual bool isTrajectoryEnded()
Definition: trajectory-generators.hh:373
dynamicgraph::sot::torque_control::ConstantAccelerationTrajectoryGenerator::m_ddx0
Eigen::VectorXd m_ddx0
Definition: trajectory-generators.hh:339
dynamicgraph::sot::torque_control::NoTrajectoryGenerator
Definition: trajectory-generators.hh:179
dynamicgraph::sot::torque_control::LinearChirpTrajectoryGenerator::m_phi
Eigen::VectorXd m_phi
current frequency (i.e. time derivative of the phase over 2*pi)
Definition: trajectory-generators.hh:389
dynamicgraph::sot::torque_control::AbstractTrajectoryGenerator::m_size
Eigen::VectorXd::Index m_size
current time
Definition: trajectory-generators.hh:96
dynamicgraph::sot::torque_control::AbstractTrajectoryGenerator::set_trajectory_time
virtual bool set_trajectory_time(double traj_time)
Definition: trajectory-generators.hh:159
dynamicgraph::sot::torque_control::LinearChirpTrajectoryGenerator::m_p
Eigen::VectorXd m_p
phase shift for second half of trajectory
Definition: trajectory-generators.hh:391
dynamicgraph::sot::torque_control::SinusoidTrajectoryGenerator::compute_next_point
const Eigen::VectorXd & compute_next_point()
Definition: trajectory-generators.hh:267
dynamicgraph::sot::torque_control::AbstractTrajectoryGenerator::resizeAllData
virtual void resizeAllData(Eigen::VectorXd::Index size)
Definition: trajectory-generators.hh:98
dynamicgraph::sot::torque_control::LinearChirpTrajectoryGenerator::set_initial_frequency
virtual bool set_initial_frequency(const double &f0)
Definition: trajectory-generators.hh:415
dynamicgraph::sot::torque_control::LinearChirpTrajectoryGenerator::m_ddp
Eigen::VectorXd m_ddp
Definition: trajectory-generators.hh:393
dynamicgraph::sot::torque_control::LinearChirpTrajectoryGenerator::set_final_frequency
virtual bool set_final_frequency(const Eigen::VectorXd &f1)
Definition: trajectory-generators.hh:421
dynamicgraph::sot::torque_control::AbstractTrajectoryGenerator::sendMsg
void sendMsg(const std::string &msg, MsgType t=MSG_TYPE_INFO, const char *file="", int line=0)
Definition: trajectory-generators.hh:107
dynamicgraph::sot::torque_control::AbstractTrajectoryGenerator::isTrajectoryEnded
virtual bool isTrajectoryEnded()
Definition: trajectory-generators.hh:173
dynamicgraph::sot::torque_control::AbstractTrajectoryGenerator::m_ddx
Eigen::VectorXd m_ddx
current velocity
Definition: trajectory-generators.hh:89
dynamicgraph::sot::torque_control::AbstractTrajectoryGenerator::m_x
Eigen::VectorXd m_x
Definition: trajectory-generators.hh:87
dynamicgraph::sot::torque_control::LinearChirpTrajectoryGenerator::m_k
Eigen::VectorXd m_k
final frequency
Definition: trajectory-generators.hh:387
torque_control
Definition: __init__.py:1
dynamicgraph::sot::torque_control::TextFileTrajectoryGenerator::loadTextFile
virtual bool loadTextFile(const std::string &fileName)
Definition: trajectory-generators.hh:202
dynamicgraph::sot::torque_control::NoTrajectoryGenerator::compute_next_point
virtual const Eigen::VectorXd & compute_next_point()
Definition: trajectory-generators.hh:183
dynamicgraph::sot::torque_control::AbstractTrajectoryGenerator::AbstractTrajectoryGenerator
AbstractTrajectoryGenerator(double dt, double traj_time, const Eigen::VectorXd &x_init, const Eigen::VectorXd &x_final)
Definition: trajectory-generators.hh:119
dynamicgraph::sot::torque_control::SinusoidTrajectoryGenerator::isTrajectoryEnded
virtual bool isTrajectoryEnded()
Definition: trajectory-generators.hh:282
dynamicgraph::sot::torque_control::TextFileTrajectoryGenerator::m_posTraj
Eigen::MatrixXd m_posTraj
Definition: trajectory-generators.hh:195
dynamicgraph::sot::torque_control::LinearChirpTrajectoryGenerator::compute_next_point
const Eigen::VectorXd & compute_next_point()
Definition: trajectory-generators.hh:433
dynamicgraph::sot::torque_control::LinearChirpTrajectoryGenerator::m_phi_0
Eigen::VectorXd m_phi_0
current phase
Definition: trajectory-generators.hh:390
dynamicgraph::sot::torque_control::AbstractTrajectoryGenerator::AbstractTrajectoryGenerator
AbstractTrajectoryGenerator(double dt, double traj_time, Eigen::VectorXd::Index size)
Definition: trajectory-generators.hh:112
dynamicgraph::sot::torque_control::MinimumJerkTrajectoryGenerator
Definition: trajectory-generators.hh:235
dynamicgraph::sot::torque_control::AbstractTrajectoryGenerator::set_initial_point
virtual bool set_initial_point(const double &x_init)
Definition: trajectory-generators.hh:138
dynamicgraph::sot::torque_control::AbstractTrajectoryGenerator::m_x_final
Eigen::VectorXd m_x_final
initial position
Definition: trajectory-generators.hh:91
dynamicgraph::sot::torque_control::TextFileTrajectoryGenerator::TextFileTrajectoryGenerator
TextFileTrajectoryGenerator(double dt, Eigen::VectorXd::Index size)
Definition: trajectory-generators.hh:200
dynamicgraph::sot::torque_control::LinearChirpTrajectoryGenerator::m_f
Eigen::VectorXd m_f
frequency first derivative
Definition: trajectory-generators.hh:388
dynamicgraph::sot::torque_control::ConstantAccelerationTrajectoryGenerator::m_counter_max
int m_counter_max
Definition: trajectory-generators.hh:338
dynamicgraph::sot::torque_control::AbstractTrajectoryGenerator::m_dx
Eigen::VectorXd m_dx
current position
Definition: trajectory-generators.hh:88
dynamicgraph::sot::torque_control::MinimumJerkTrajectoryGenerator::compute_next_point
virtual const Eigen::VectorXd & compute_next_point()
Definition: trajectory-generators.hh:240
dynamicgraph::sot::torque_control::TextFileTrajectoryGenerator::m_velTraj
Eigen::MatrixXd m_velTraj
Definition: trajectory-generators.hh:196
dynamicgraph::sot::torque_control::AbstractTrajectoryGenerator::m_dt
double m_dt
time to go from x_init to x_final (sec)
Definition: trajectory-generators.hh:94
dynamicgraph::sot::torque_control::TextFileTrajectoryGenerator
Definition: trajectory-generators.hh:193
dynamicgraph::sot::torque_control::AbstractTrajectoryGenerator::set_initial_point
virtual bool set_initial_point(const Eigen::VectorXd &x_init)
Definition: trajectory-generators.hh:129
dynamicgraph::sot::torque_control::AbstractTrajectoryGenerator::compute_next_point
virtual const Eigen::VectorXd & compute_next_point()=0
dynamicgraph::sot::torque_control::TriangleTrajectoryGenerator::set_acceleration_time
bool set_acceleration_time(const double Tacc)
Definition: trajectory-generators.hh:298
dynamicgraph::sot::torque_control::ConstantAccelerationTrajectoryGenerator
Definition: trajectory-generators.hh:334
dynamicgraph::sot::torque_control::MinimumJerkTrajectoryGenerator::MinimumJerkTrajectoryGenerator
MinimumJerkTrajectoryGenerator(double dt, double traj_time, int size)
Definition: trajectory-generators.hh:237
dynamicgraph::sot::torque_control::AbstractTrajectoryGenerator::getAcc
virtual const Eigen::VectorXd & getAcc()
Definition: trajectory-generators.hh:169
dynamicgraph::sot::torque_control::ConstantAccelerationTrajectoryGenerator::m_is_accelerating
bool m_is_accelerating
Definition: trajectory-generators.hh:336
dynamicgraph::sot::torque_control::AbstractTrajectoryGenerator::get_initial_point
virtual const Eigen::VectorXd & get_initial_point()
Definition: trajectory-generators.hh:170
dynamicgraph::sot::torque_control::SinusoidTrajectoryGenerator::SinusoidTrajectoryGenerator
SinusoidTrajectoryGenerator(double dt, double traj_time, int size)
Definition: trajectory-generators.hh:264
dynamicgraph::sot::torque_control::TriangleTrajectoryGenerator::isTrajectoryEnded
virtual bool isTrajectoryEnded()
Definition: trajectory-generators.hh:329
dynamicgraph::sot::torque_control::ConstantAccelerationTrajectoryGenerator::m_counter
int m_counter
Definition: trajectory-generators.hh:337
dynamicgraph::sot::torque_control::AbstractTrajectoryGenerator::get_final_point
virtual const Eigen::VectorXd & get_final_point()
Definition: trajectory-generators.hh:171
dynamicgraph::sot::torque_control::NoTrajectoryGenerator::isTrajectoryEnded
virtual bool isTrajectoryEnded()
Definition: trajectory-generators.hh:188
dynamicgraph::sot::torque_control::AbstractTrajectoryGenerator::getPos
virtual const Eigen::VectorXd & getPos()
Definition: trajectory-generators.hh:167
dynamicgraph::sot::torque_control::TextFileTrajectoryGenerator::m_accTraj
Eigen::MatrixXd m_accTraj
Definition: trajectory-generators.hh:197
dynamicgraph::sot::torque_control::NoTrajectoryGenerator::NoTrajectoryGenerator
NoTrajectoryGenerator(int size)
Definition: trajectory-generators.hh:181
dynamicgraph::sot::torque_control::TriangleTrajectoryGenerator::TriangleTrajectoryGenerator
TriangleTrajectoryGenerator(double dt, double traj_time, int size)
Definition: trajectory-generators.hh:293
dynamicgraph::sot::torque_control::TriangleTrajectoryGenerator::compute_next_point
const Eigen::VectorXd & compute_next_point()
Definition: trajectory-generators.hh:304
dynamicgraph::sot::torque_control::AbstractTrajectoryGenerator::set_final_point
virtual bool set_final_point(const Eigen::VectorXd &x_final)
Definition: trajectory-generators.hh:147
dynamicgraph::sot::torque_control::AbstractTrajectoryGenerator::set_final_point
virtual bool set_final_point(const double &x_final)
Definition: trajectory-generators.hh:153
dynamicgraph::sot::torque_control::AbstractTrajectoryGenerator::m_t
double m_t
control dt (sampling period of the trajectory)
Definition: trajectory-generators.hh:95
dynamicgraph::sot::torque_control::AbstractTrajectoryGenerator::m_traj_time
double m_traj_time
final position
Definition: trajectory-generators.hh:93
dynamicgraph::sot::torque_control::ConstantAccelerationTrajectoryGenerator::ConstantAccelerationTrajectoryGenerator
ConstantAccelerationTrajectoryGenerator(double dt, double traj_time, int size)
Definition: trajectory-generators.hh:342
dynamicgraph::sot::torque_control::LinearChirpTrajectoryGenerator::m_f1
Eigen::VectorXd m_f1
initial frequency
Definition: trajectory-generators.hh:384
dynamicgraph::sot::torque_control::ConstantAccelerationTrajectoryGenerator::set_trajectory_time
virtual bool set_trajectory_time(double traj_time)
Definition: trajectory-generators.hh:345
MAXBUFSIZE
#define MAXBUFSIZE
Definition: trajectory-generators.hh:39