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