1 |
|
|
/* |
2 |
|
|
* Copyright 2017, Andrea Del Prete, LAAS-CNRS |
3 |
|
|
* |
4 |
|
|
*/ |
5 |
|
|
|
6 |
|
|
#include <dynamic-graph/factory.h> |
7 |
|
|
|
8 |
|
|
#include <sot/core/debug.hh> |
9 |
|
|
#include <sot/core/stop-watch.hh> |
10 |
|
|
#include <sot/torque_control/commands-helper.hh> |
11 |
|
|
#include <sot/torque_control/se3-trajectory-generator.hh> |
12 |
|
|
|
13 |
|
|
namespace dynamicgraph { |
14 |
|
|
namespace sot { |
15 |
|
|
namespace torque_control { |
16 |
|
|
namespace dg = ::dynamicgraph; |
17 |
|
|
using namespace dg; |
18 |
|
|
using namespace dg::command; |
19 |
|
|
using namespace std; |
20 |
|
|
using namespace Eigen; |
21 |
|
|
|
22 |
|
|
#define PROFILE_SE3_POSITION_DESIRED_COMPUTATION "SE3TrajGen: traj computation" |
23 |
|
|
|
24 |
|
|
/// Define EntityClassName here rather than in the header file |
25 |
|
|
/// so that it can be used by the macros DEFINE_SIGNAL_**_FUNCTION. |
26 |
|
|
typedef SE3TrajectoryGenerator EntityClassName; |
27 |
|
|
|
28 |
|
|
/* --- DG FACTORY ---------------------------------------------------- */ |
29 |
|
|
DYNAMICGRAPH_FACTORY_ENTITY_PLUGIN(SE3TrajectoryGenerator, |
30 |
|
|
"SE3TrajectoryGenerator"); |
31 |
|
|
|
32 |
|
|
/* ------------------------------------------------------------------- */ |
33 |
|
|
/* --- CONSTRUCTION -------------------------------------------------- */ |
34 |
|
|
/* ------------------------------------------------------------------- */ |
35 |
|
|
SE3TrajectoryGenerator::SE3TrajectoryGenerator(const std::string& name) |
36 |
|
|
: Entity(name), |
37 |
|
|
CONSTRUCT_SIGNAL_IN(initial_value, dynamicgraph::Vector), |
38 |
|
|
CONSTRUCT_SIGNAL(x, OUT, dynamicgraph::Vector), |
39 |
|
|
CONSTRUCT_SIGNAL_IN(trigger, bool), |
40 |
|
|
CONSTRUCT_SIGNAL_OUT(dx, dynamicgraph::Vector, m_xSOUT), |
41 |
|
|
CONSTRUCT_SIGNAL_OUT(ddx, dynamicgraph::Vector, m_xSOUT), |
42 |
|
|
m_initSucceeded(false), |
43 |
|
|
m_firstIter(true), |
44 |
|
|
m_np(12), |
45 |
|
|
m_nv(6), |
46 |
|
|
m_iterLast(0), |
47 |
|
|
m_t(0), |
48 |
|
|
m_splineReady(false) { |
49 |
|
|
BIND_SIGNAL_TO_FUNCTION(x, OUT, dynamicgraph::Vector); |
50 |
|
|
|
51 |
|
|
Entity::signalRegistration(m_xSOUT << m_dxSOUT << m_ddxSOUT |
52 |
|
|
<< m_initial_valueSIN << m_triggerSIN); |
53 |
|
|
|
54 |
|
|
/* Commands. */ |
55 |
|
|
addCommand("init", makeCommandVoid1( |
56 |
|
|
*this, &SE3TrajectoryGenerator::init, |
57 |
|
|
docCommandVoid1("Initialize the entity.", |
58 |
|
|
"Time period in seconds (double)"))); |
59 |
|
|
|
60 |
|
|
addCommand( |
61 |
|
|
"getValue", |
62 |
|
|
makeCommandVoid1( |
63 |
|
|
*this, &SE3TrajectoryGenerator::getValue, |
64 |
|
|
docCommandVoid1("Get the current value of the specified index.", |
65 |
|
|
"index (int)"))); |
66 |
|
|
|
67 |
|
|
addCommand("playTrajectoryFile", |
68 |
|
|
makeCommandVoid1( |
69 |
|
|
*this, &SE3TrajectoryGenerator::playTrajectoryFile, |
70 |
|
|
docCommandVoid1( |
71 |
|
|
"Play the trajectory read from the specified text file.", |
72 |
|
|
"(string) File name, path included"))); |
73 |
|
|
addCommand( |
74 |
|
|
"setSpline", |
75 |
|
|
makeCommandVoid3(*this, &SE3TrajectoryGenerator::setSpline, |
76 |
|
|
docCommandVoid3("Load serialized spline from file", |
77 |
|
|
"(string) filename", |
78 |
|
|
"(double) time to initial conf", |
79 |
|
|
"(Matrix) orientation of the point"))); |
80 |
|
|
|
81 |
|
|
addCommand( |
82 |
|
|
"startSinusoid", |
83 |
|
|
makeCommandVoid3( |
84 |
|
|
*this, &SE3TrajectoryGenerator::startSinusoid, |
85 |
|
|
docCommandVoid3("Start an infinite sinusoid motion.", |
86 |
|
|
"(int) index", "(double) final value", |
87 |
|
|
"(double) time to reach the final value in sec"))); |
88 |
|
|
|
89 |
|
|
addCommand( |
90 |
|
|
"startTriangle", |
91 |
|
|
makeCommandVoid4( |
92 |
|
|
*this, &SE3TrajectoryGenerator::startTriangle, |
93 |
|
|
docCommandVoid4("Start an infinite triangle wave.", "(int) index", |
94 |
|
|
"(double) final values", |
95 |
|
|
"(double) time to reach the final value in sec", |
96 |
|
|
"(double) time to accelerate in sec"))); |
97 |
|
|
|
98 |
|
|
addCommand( |
99 |
|
|
"startConstAcc", |
100 |
|
|
makeCommandVoid3( |
101 |
|
|
*this, &SE3TrajectoryGenerator::startConstAcc, |
102 |
|
|
docCommandVoid3("Start an infinite trajectory with piece-wise " |
103 |
|
|
"constant acceleration.", |
104 |
|
|
"(int) index", "(double) final values", |
105 |
|
|
"(double) time to reach the final value in sec"))); |
106 |
|
|
|
107 |
|
|
addCommand("startLinChirp", |
108 |
|
|
makeCommandVoid5( |
109 |
|
|
*this, &SE3TrajectoryGenerator::startLinearChirp, |
110 |
|
|
docCommandVoid5("Start a linear-chirp motion.", |
111 |
|
|
"(int) index", "(double) final values", |
112 |
|
|
"(double) initial frequency [Hz]", |
113 |
|
|
"(double) final frequency [Hz]", |
114 |
|
|
"(double) trajectory time [sec]"))); |
115 |
|
|
addCommand("move", makeCommandVoid3( |
116 |
|
|
*this, &SE3TrajectoryGenerator::move, |
117 |
|
|
docCommandVoid3( |
118 |
|
|
"Move component corresponding to index to the " |
119 |
|
|
"specified value with a minimum jerk trajectory.", |
120 |
|
|
"(int) index", "(double) final values", |
121 |
|
|
"(double) time to reach the final value in sec"))); |
122 |
|
|
addCommand( |
123 |
|
|
"stop", |
124 |
|
|
makeCommandVoid1( |
125 |
|
|
*this, &SE3TrajectoryGenerator::stop, |
126 |
|
|
docCommandVoid1("Stop the motion of the specified index, or of all " |
127 |
|
|
"components of the vector if index is equal to -1.", |
128 |
|
|
"(int) index"))); |
129 |
|
|
} |
130 |
|
|
|
131 |
|
|
void SE3TrajectoryGenerator::init(const double& dt) { |
132 |
|
|
if (dt <= 0.0) return SEND_MSG("Timestep must be positive", MSG_TYPE_ERROR); |
133 |
|
|
m_firstIter = true; |
134 |
|
|
m_dt = dt; |
135 |
|
|
m_status.resize(m_np, TG_STOP); |
136 |
|
|
m_minJerkTrajGen.resize(m_np); |
137 |
|
|
m_sinTrajGen.resize(m_np); |
138 |
|
|
m_triangleTrajGen.resize(m_np); |
139 |
|
|
m_constAccTrajGen.resize(m_np); |
140 |
|
|
m_linChirpTrajGen.resize(m_np); |
141 |
|
|
m_currentTrajGen.resize(m_np); |
142 |
|
|
m_noTrajGen.resize(m_np); |
143 |
|
|
for (unsigned int i = 0; i < m_np; i++) { |
144 |
|
|
m_minJerkTrajGen[i] = new MinimumJerkTrajectoryGenerator(dt, 5.0, 1); |
145 |
|
|
m_sinTrajGen[i] = new SinusoidTrajectoryGenerator(dt, 5.0, 1); |
146 |
|
|
m_triangleTrajGen[i] = new TriangleTrajectoryGenerator(dt, 5.0, 1); |
147 |
|
|
m_constAccTrajGen[i] = |
148 |
|
|
new ConstantAccelerationTrajectoryGenerator(dt, 5.0, 1); |
149 |
|
|
m_linChirpTrajGen[i] = new LinearChirpTrajectoryGenerator(dt, 5.0, 1); |
150 |
|
|
m_noTrajGen[i] = new NoTrajectoryGenerator(1); |
151 |
|
|
m_currentTrajGen[i] = m_noTrajGen[i]; |
152 |
|
|
} |
153 |
|
|
m_splineTrajGen = new parametriccurves::Spline<double, Eigen::Dynamic>(); |
154 |
|
|
m_textFileTrajGen = new TextFileTrajectoryGenerator(dt, m_np); |
155 |
|
|
m_initSucceeded = true; |
156 |
|
|
} |
157 |
|
|
|
158 |
|
|
/* ------------------------------------------------------------------- */ |
159 |
|
|
/* --- SIGNALS ------------------------------------------------------- */ |
160 |
|
|
/* ------------------------------------------------------------------- */ |
161 |
|
|
|
162 |
|
|
DEFINE_SIGNAL_OUT_FUNCTION(x, dynamicgraph::Vector) { |
163 |
|
|
if (!m_initSucceeded) { |
164 |
|
|
SEND_WARNING_STREAM_MSG( |
165 |
|
|
"Cannot compute signal positionDes before initialization!"); |
166 |
|
|
return s; |
167 |
|
|
} |
168 |
|
|
|
169 |
|
|
getProfiler().start(PROFILE_SE3_POSITION_DESIRED_COMPUTATION); |
170 |
|
|
{ |
171 |
|
|
if (s.size() != m_np) s.resize(m_np); |
172 |
|
|
|
173 |
|
|
// at first iteration store initial values |
174 |
|
|
if (m_firstIter) { |
175 |
|
|
const dynamicgraph::Vector& initial_value = m_initial_valueSIN(iter); |
176 |
|
|
if (initial_value.size() != m_np) { |
177 |
|
|
SEND_ERROR_STREAM_MSG( |
178 |
|
|
"Unexpected size of input signal initial_value: " + |
179 |
|
|
toString(initial_value.size())); |
180 |
|
|
getProfiler().stop(PROFILE_SE3_POSITION_DESIRED_COMPUTATION); |
181 |
|
|
return s; |
182 |
|
|
} |
183 |
|
|
for (unsigned int i = 0; i < m_np; i++) |
184 |
|
|
m_currentTrajGen[i]->set_initial_point(initial_value(i)); |
185 |
|
|
m_firstIter = false; |
186 |
|
|
} else if (iter == static_cast<int>(m_iterLast)) { |
187 |
|
|
if (m_triggerSIN(iter) == true && m_splineReady) startSpline(); |
188 |
|
|
if (m_status[0] == TG_TEXT_FILE) { |
189 |
|
|
for (unsigned int i = 0; i < m_np; i++) |
190 |
|
|
s(i) = m_textFileTrajGen->getPos()[i]; |
191 |
|
|
} else if (m_status[0] == TG_SPLINE) { |
192 |
|
|
const Eigen::Vector3d& t = (*m_splineTrajGen)(m_t); |
193 |
|
|
s.head<3>() = t; |
194 |
|
|
s.segment<3>(3) = m_splineRotation.row(0); |
195 |
|
|
s.segment<3>(6) = m_splineRotation.row(1); |
196 |
|
|
s.segment<3>(9) = m_splineRotation.row(2); |
197 |
|
|
} else |
198 |
|
|
for (unsigned int i = 0; i < m_np; i++) |
199 |
|
|
s(i) = m_currentTrajGen[i]->getPos()(0); |
200 |
|
|
getProfiler().stop(PROFILE_SE3_POSITION_DESIRED_COMPUTATION); |
201 |
|
|
return s; |
202 |
|
|
} |
203 |
|
|
m_iterLast = iter; |
204 |
|
|
if (m_triggerSIN(iter) == true && m_splineReady) startSpline(); |
205 |
|
|
if (m_status[0] == TG_TEXT_FILE) { |
206 |
|
|
const VectorXd& xRef = m_textFileTrajGen->compute_next_point(); |
207 |
|
|
for (unsigned int i = 0; i < m_np; i++) { |
208 |
|
|
s(i) = xRef[i]; |
209 |
|
|
if (m_textFileTrajGen->isTrajectoryEnded()) { |
210 |
|
|
m_noTrajGen[i]->set_initial_point(s(i)); |
211 |
|
|
m_status[i] = TG_STOP; |
212 |
|
|
} |
213 |
|
|
} |
214 |
|
|
if (m_textFileTrajGen->isTrajectoryEnded()) |
215 |
|
|
SEND_MSG("Text file trajectory ended.", MSG_TYPE_INFO); |
216 |
|
|
} else if (m_status[0] == TG_SPLINE) { |
217 |
|
|
m_t += m_dt; |
218 |
|
|
if (!m_splineTrajGen->checkRange(m_t)) { |
219 |
|
|
const Eigen::Vector3d& t = (*m_splineTrajGen)(m_splineTrajGen->tmax()); |
220 |
|
|
s.head<3>() = t; |
221 |
|
|
s.segment<3>(3) = m_splineRotation.row(0); |
222 |
|
|
s.segment<3>(6) = m_splineRotation.row(1); |
223 |
|
|
s.segment<3>(9) = m_splineRotation.row(2); |
224 |
|
|
for (unsigned int i = 0; i < m_np; i++) { |
225 |
|
|
m_noTrajGen[i]->set_initial_point(s(i)); |
226 |
|
|
m_status[i] = TG_STOP; |
227 |
|
|
} |
228 |
|
|
m_splineReady = false; |
229 |
|
|
SEND_MSG("Spline trajectory ended. Remember to turn off the trigger.", |
230 |
|
|
MSG_TYPE_INFO); |
231 |
|
|
m_t = 0; |
232 |
|
|
} else { |
233 |
|
|
const Eigen::Vector3d& t = (*m_splineTrajGen)(m_t); |
234 |
|
|
s.head<3>() = t; |
235 |
|
|
s.segment<3>(3) = m_splineRotation.row(0); |
236 |
|
|
s.segment<3>(6) = m_splineRotation.row(1); |
237 |
|
|
s.segment<3>(9) = m_splineRotation.row(2); |
238 |
|
|
} |
239 |
|
|
} else { |
240 |
|
|
for (unsigned int i = 0; i < m_np; i++) { |
241 |
|
|
s(i) = m_currentTrajGen[i]->compute_next_point()(0); |
242 |
|
|
if (m_currentTrajGen[i]->isTrajectoryEnded()) { |
243 |
|
|
m_currentTrajGen[i] = m_noTrajGen[i]; |
244 |
|
|
m_noTrajGen[i]->set_initial_point(s(i)); |
245 |
|
|
m_status[i] = TG_STOP; |
246 |
|
|
SEND_MSG("Trajectory of index " + toString(i) + " ended.", |
247 |
|
|
MSG_TYPE_INFO); |
248 |
|
|
} |
249 |
|
|
} |
250 |
|
|
} |
251 |
|
|
} |
252 |
|
|
getProfiler().stop(PROFILE_SE3_POSITION_DESIRED_COMPUTATION); |
253 |
|
|
|
254 |
|
|
return s; |
255 |
|
|
} |
256 |
|
|
|
257 |
|
|
DEFINE_SIGNAL_OUT_FUNCTION(dx, dynamicgraph::Vector) { |
258 |
|
|
if (!m_initSucceeded) { |
259 |
|
|
std::ostringstream oss( |
260 |
|
|
"Cannot compute signal positionDes before initialization! iter:"); |
261 |
|
|
oss << iter; |
262 |
|
|
SEND_WARNING_STREAM_MSG(oss.str()); |
263 |
|
|
return s; |
264 |
|
|
} |
265 |
|
|
|
266 |
|
|
if (s.size() != m_nv) s.resize(m_nv); |
267 |
|
|
if (m_status[0] == TG_TEXT_FILE) { |
268 |
|
|
for (unsigned int i = 0; i < m_nv; i++) |
269 |
|
|
s(i) = m_textFileTrajGen->getVel()[i]; |
270 |
|
|
} else if (m_status[0] == TG_SPLINE) { |
271 |
|
|
const Eigen::Vector3d& t = m_splineTrajGen->derivate(m_t, 1); |
272 |
|
|
s.segment<3>(0) = t; |
273 |
|
|
s.segment<3>(3).setZero(); |
274 |
|
|
} else |
275 |
|
|
for (unsigned int i = 0; i < m_nv; i++) |
276 |
|
|
s(i) = m_currentTrajGen[i]->getVel()(0); |
277 |
|
|
|
278 |
|
|
return s; |
279 |
|
|
} |
280 |
|
|
|
281 |
|
|
DEFINE_SIGNAL_OUT_FUNCTION(ddx, dynamicgraph::Vector) { |
282 |
|
|
if (!m_initSucceeded) { |
283 |
|
|
std::ostringstream oss( |
284 |
|
|
"Cannot compute signal positionDes before initialization! iter:"); |
285 |
|
|
oss << iter; |
286 |
|
|
SEND_WARNING_STREAM_MSG(oss.str()); |
287 |
|
|
return s; |
288 |
|
|
} |
289 |
|
|
|
290 |
|
|
if (s.size() != m_nv) s.resize(m_nv); |
291 |
|
|
|
292 |
|
|
if (m_status[0] == TG_TEXT_FILE) { |
293 |
|
|
for (unsigned int i = 0; i < m_nv; i++) |
294 |
|
|
s(i) = m_textFileTrajGen->getAcc()[i]; |
295 |
|
|
} else if (m_status[0] == TG_SPLINE) { |
296 |
|
|
const Eigen::Vector3d& t = m_splineTrajGen->derivate(m_t, 2); |
297 |
|
|
s.segment<3>(0) = t; |
298 |
|
|
s.segment<3>(3).setZero(); |
299 |
|
|
} else |
300 |
|
|
for (unsigned int i = 0; i < m_nv; i++) |
301 |
|
|
s(i) = m_currentTrajGen[i]->getAcc()(0); |
302 |
|
|
|
303 |
|
|
return s; |
304 |
|
|
} |
305 |
|
|
|
306 |
|
|
/* ------------------------------------------------------------------- */ |
307 |
|
|
/* --- COMMANDS ------------------------------------------------------ */ |
308 |
|
|
/* ------------------------------------------------------------------- */ |
309 |
|
|
|
310 |
|
|
void SE3TrajectoryGenerator::getValue(const int& id) { |
311 |
|
|
if (id < 0 || id >= static_cast<int>(m_np)) |
312 |
|
|
return SEND_MSG("Index is out of bounds", MSG_TYPE_ERROR); |
313 |
|
|
|
314 |
|
|
SEND_MSG("Current value of component " + toString(id) + " is " + |
315 |
|
|
toString(m_currentTrajGen[id]->getPos()(0)), |
316 |
|
|
MSG_TYPE_INFO); |
317 |
|
|
} |
318 |
|
|
|
319 |
|
|
void SE3TrajectoryGenerator::playTrajectoryFile(const std::string& fileName) { |
320 |
|
|
if (!m_initSucceeded) |
321 |
|
|
return SEND_MSG("Cannot start sinusoid before initialization!", |
322 |
|
|
MSG_TYPE_ERROR); |
323 |
|
|
|
324 |
|
|
for (unsigned int i = 0; i < m_np; i++) |
325 |
|
|
if (m_status[i] != TG_STOP) |
326 |
|
|
return SEND_MSG("You cannot control component " + toString(i) + |
327 |
|
|
" because it is already controlled.", |
328 |
|
|
MSG_TYPE_ERROR); |
329 |
|
|
|
330 |
|
|
if (!m_textFileTrajGen->loadTextFile(fileName)) |
331 |
|
|
return SEND_MSG("Error while loading text file " + fileName, |
332 |
|
|
MSG_TYPE_ERROR); |
333 |
|
|
|
334 |
|
|
// check current configuration is not too far from initial trajectory |
335 |
|
|
// configuration |
336 |
|
|
bool needToMoveToInitConf = false; |
337 |
|
|
const VectorXd& xInit = m_textFileTrajGen->get_initial_point(); |
338 |
|
|
for (unsigned int i = 0; i < m_np; i++) |
339 |
|
|
if (fabs(xInit[i] - m_currentTrajGen[i]->getPos()(0)) > 0.001) { |
340 |
|
|
needToMoveToInitConf = true; |
341 |
|
|
SEND_MSG("Component " + toString(i) + |
342 |
|
|
" is too far from initial configuration so first i will " |
343 |
|
|
"move it there.", |
344 |
|
|
MSG_TYPE_WARNING); |
345 |
|
|
} |
346 |
|
|
|
347 |
|
|
// if necessary move joints to initial configuration |
348 |
|
|
if (needToMoveToInitConf) { |
349 |
|
|
for (unsigned int i = 0; i < m_np; i++) { |
350 |
|
|
m_minJerkTrajGen[i]->set_initial_point(m_noTrajGen[i]->getPos()); |
351 |
|
|
m_minJerkTrajGen[i]->set_final_point(xInit[i]); |
352 |
|
|
m_minJerkTrajGen[i]->set_trajectory_time(4.0); |
353 |
|
|
m_status[i] = TG_MIN_JERK; |
354 |
|
|
m_currentTrajGen[i] = m_minJerkTrajGen[i]; |
355 |
|
|
} |
356 |
|
|
return; |
357 |
|
|
} |
358 |
|
|
|
359 |
|
|
for (unsigned int i = 0; i < m_np; i++) { |
360 |
|
|
m_status[i] = TG_TEXT_FILE; |
361 |
|
|
} |
362 |
|
|
} |
363 |
|
|
|
364 |
|
|
void SE3TrajectoryGenerator::setSpline(const std::string& fileName, |
365 |
|
|
const double& timeToInitConf, |
366 |
|
|
const Eigen::MatrixXd& init_rotation) { |
367 |
|
|
if (!m_initSucceeded) |
368 |
|
|
return SEND_MSG("Cannot start sinusoid before initialization!", |
369 |
|
|
MSG_TYPE_ERROR); |
370 |
|
|
|
371 |
|
|
for (unsigned int i = 0; i < m_np; i++) |
372 |
|
|
if (m_status[i] != TG_STOP) |
373 |
|
|
return SEND_MSG("You cannot control component " + toString(i) + |
374 |
|
|
" because it is already controlled.", |
375 |
|
|
MSG_TYPE_ERROR); |
376 |
|
|
|
377 |
|
|
if (!m_splineTrajGen->loadFromFile(fileName)) |
378 |
|
|
return SEND_MSG("Error while loading text file " + fileName, |
379 |
|
|
MSG_TYPE_ERROR); |
380 |
|
|
|
381 |
|
|
// check current configuration is not too far from initial trajectory |
382 |
|
|
// configuration |
383 |
|
|
bool needToMoveToInitConf = false; |
384 |
|
|
m_splineReady = true; |
385 |
|
|
m_splineRotation = init_rotation; |
386 |
|
|
if (timeToInitConf < 0) { |
387 |
|
|
SEND_MSG("Spline Ready. Set trigger to true to start playing", |
388 |
|
|
MSG_TYPE_INFO); |
389 |
|
|
return; |
390 |
|
|
} |
391 |
|
|
|
392 |
|
|
const VectorXd& t = (*m_splineTrajGen)(0.0); |
393 |
|
|
VectorXd xInit(12); |
394 |
|
|
xInit.segment<3>(3) = m_splineRotation.row(0); |
395 |
|
|
xInit.segment<3>(6) = m_splineRotation.row(1); |
396 |
|
|
xInit.segment<3>(9) = m_splineRotation.row(2); |
397 |
|
|
xInit.head<3>() = t; |
398 |
|
|
|
399 |
|
|
for (unsigned int i = 0; i < m_np; i++) |
400 |
|
|
if (fabs(xInit[i] - m_currentTrajGen[i]->getPos()(0)) > 0.001) { |
401 |
|
|
needToMoveToInitConf = true; |
402 |
|
|
SEND_MSG("Component " + toString(i) + |
403 |
|
|
" is too far from initial configuration so first i will " |
404 |
|
|
"move it there.", |
405 |
|
|
MSG_TYPE_WARNING); |
406 |
|
|
} |
407 |
|
|
|
408 |
|
|
// if necessary move joints to initial configuration |
409 |
|
|
if (needToMoveToInitConf) { |
410 |
|
|
for (unsigned int i = 0; i < m_np; i++) { |
411 |
|
|
m_minJerkTrajGen[i]->set_initial_point(m_noTrajGen[i]->getPos()); |
412 |
|
|
m_minJerkTrajGen[i]->set_final_point(xInit[i]); |
413 |
|
|
m_minJerkTrajGen[i]->set_trajectory_time(timeToInitConf); |
414 |
|
|
m_status[i] = TG_MIN_JERK; |
415 |
|
|
m_currentTrajGen[i] = m_minJerkTrajGen[i]; |
416 |
|
|
} |
417 |
|
|
return; |
418 |
|
|
} |
419 |
|
|
|
420 |
|
|
SEND_MSG("Spline Ready. Set trigger to true to start playing", MSG_TYPE_INFO); |
421 |
|
|
} |
422 |
|
|
|
423 |
|
|
void SE3TrajectoryGenerator::startSpline() { |
424 |
|
|
if (m_status[0] == TG_SPLINE) return; |
425 |
|
|
m_t = 0.0; |
426 |
|
|
for (unsigned int i = 0; i < m_np; i++) { |
427 |
|
|
m_status[i] = TG_SPLINE; |
428 |
|
|
} |
429 |
|
|
} |
430 |
|
|
|
431 |
|
|
void SE3TrajectoryGenerator::startSinusoid(const int& id, const double& xFinal, |
432 |
|
|
const double& time) { |
433 |
|
|
if (!m_initSucceeded) |
434 |
|
|
return SEND_MSG("Cannot start sinusoid before initialization!", |
435 |
|
|
MSG_TYPE_ERROR); |
436 |
|
|
|
437 |
|
|
if (id < 0 || id >= static_cast<int>(m_np)) |
438 |
|
|
return SEND_MSG("Index is out of bounds", MSG_TYPE_ERROR); |
439 |
|
|
unsigned int i = id; |
440 |
|
|
if (time <= 0.0) |
441 |
|
|
return SEND_MSG("Trajectory time must be a positive number", |
442 |
|
|
MSG_TYPE_ERROR); |
443 |
|
|
if (m_status[i] != TG_STOP) |
444 |
|
|
return SEND_MSG( |
445 |
|
|
"You cannot move the specified component because it is already " |
446 |
|
|
"controlled.", |
447 |
|
|
MSG_TYPE_ERROR); |
448 |
|
|
|
449 |
|
|
m_sinTrajGen[i]->set_initial_point(m_noTrajGen[i]->getPos()); |
450 |
|
|
SEND_MSG( |
451 |
|
|
"Set initial point of sinusoid to " + toString(m_sinTrajGen[i]->getPos()), |
452 |
|
|
MSG_TYPE_DEBUG); |
453 |
|
|
m_sinTrajGen[i]->set_final_point(xFinal); |
454 |
|
|
m_sinTrajGen[i]->set_trajectory_time(time); |
455 |
|
|
m_status[i] = TG_SINUSOID; |
456 |
|
|
m_currentTrajGen[i] = m_sinTrajGen[i]; |
457 |
|
|
} |
458 |
|
|
|
459 |
|
|
void SE3TrajectoryGenerator::startTriangle(const int& id, const double& xFinal, |
460 |
|
|
const double& time, |
461 |
|
|
const double& Tacc) { |
462 |
|
|
if (!m_initSucceeded) |
463 |
|
|
return SEND_MSG("Cannot start triangle before initialization!", |
464 |
|
|
MSG_TYPE_ERROR); |
465 |
|
|
if (id < 0 || id >= static_cast<int>(m_np)) |
466 |
|
|
return SEND_MSG("Index is out of bounds", MSG_TYPE_ERROR); |
467 |
|
|
unsigned int i = id; |
468 |
|
|
if (m_status[i] != TG_STOP) |
469 |
|
|
return SEND_MSG( |
470 |
|
|
"You cannot move the specified component because it is already " |
471 |
|
|
"controlled.", |
472 |
|
|
MSG_TYPE_ERROR); |
473 |
|
|
|
474 |
|
|
m_triangleTrajGen[i]->set_initial_point(m_noTrajGen[i]->getPos()); |
475 |
|
|
SEND_MSG("Set initial point of triangular trajectory to " + |
476 |
|
|
toString(m_triangleTrajGen[i]->getPos()), |
477 |
|
|
MSG_TYPE_DEBUG); |
478 |
|
|
m_triangleTrajGen[i]->set_final_point(xFinal); |
479 |
|
|
|
480 |
|
|
if (!m_triangleTrajGen[i]->set_trajectory_time(time)) |
481 |
|
|
return SEND_MSG("Trajectory time cannot be negative.", MSG_TYPE_ERROR); |
482 |
|
|
|
483 |
|
|
if (!m_triangleTrajGen[i]->set_acceleration_time(Tacc)) |
484 |
|
|
return SEND_MSG( |
485 |
|
|
"Acceleration time cannot be negative or larger than half the " |
486 |
|
|
"trajectory time.", |
487 |
|
|
MSG_TYPE_ERROR); |
488 |
|
|
|
489 |
|
|
m_status[i] = TG_TRIANGLE; |
490 |
|
|
m_currentTrajGen[i] = m_triangleTrajGen[i]; |
491 |
|
|
} |
492 |
|
|
|
493 |
|
|
void SE3TrajectoryGenerator::startConstAcc(const int& id, const double& xFinal, |
494 |
|
|
const double& time) { |
495 |
|
|
if (!m_initSucceeded) |
496 |
|
|
return SEND_MSG( |
497 |
|
|
"Cannot start constant-acceleration trajectory before initialization!", |
498 |
|
|
MSG_TYPE_ERROR); |
499 |
|
|
if (id < 0 || id >= static_cast<int>(m_np)) |
500 |
|
|
return SEND_MSG("Index is out of bounds", MSG_TYPE_ERROR); |
501 |
|
|
unsigned int i = id; |
502 |
|
|
if (time <= 0.0) |
503 |
|
|
return SEND_MSG("Trajectory time must be a positive number", |
504 |
|
|
MSG_TYPE_ERROR); |
505 |
|
|
if (m_status[i] != TG_STOP) |
506 |
|
|
return SEND_MSG( |
507 |
|
|
"You cannot move the specified component because it is already " |
508 |
|
|
"controlled.", |
509 |
|
|
MSG_TYPE_ERROR); |
510 |
|
|
|
511 |
|
|
m_constAccTrajGen[i]->set_initial_point(m_noTrajGen[i]->getPos()); |
512 |
|
|
SEND_MSG("Set initial point of const-acc trajectory to " + |
513 |
|
|
toString(m_constAccTrajGen[i]->getPos()), |
514 |
|
|
MSG_TYPE_DEBUG); |
515 |
|
|
m_constAccTrajGen[i]->set_final_point(xFinal); |
516 |
|
|
m_constAccTrajGen[i]->set_trajectory_time(time); |
517 |
|
|
m_status[i] = TG_CONST_ACC; |
518 |
|
|
m_currentTrajGen[i] = m_constAccTrajGen[i]; |
519 |
|
|
} |
520 |
|
|
|
521 |
|
|
void SE3TrajectoryGenerator::startLinearChirp(const int& id, |
522 |
|
|
const double& xFinal, |
523 |
|
|
const double& f0, |
524 |
|
|
const double& f1, |
525 |
|
|
const double& time) { |
526 |
|
|
if (!m_initSucceeded) |
527 |
|
|
return SEND_MSG("Cannot start linear chirp before initialization!", |
528 |
|
|
MSG_TYPE_ERROR); |
529 |
|
|
if (id < 0 || id >= static_cast<int>(m_np)) |
530 |
|
|
return SEND_MSG("Index is out of bounds", MSG_TYPE_ERROR); |
531 |
|
|
unsigned int i = id; |
532 |
|
|
if (time <= 0.0) |
533 |
|
|
return SEND_MSG("Trajectory time must be a positive number", |
534 |
|
|
MSG_TYPE_ERROR); |
535 |
|
|
if (m_status[i] != TG_STOP) |
536 |
|
|
return SEND_MSG( |
537 |
|
|
"You cannot move the specified component because it is already " |
538 |
|
|
"controlled.", |
539 |
|
|
MSG_TYPE_ERROR); |
540 |
|
|
if (f0 > f1) |
541 |
|
|
return SEND_MSG( |
542 |
|
|
"f0 " + toString(f0) + " cannot to be more than f1 " + toString(f1), |
543 |
|
|
MSG_TYPE_ERROR); |
544 |
|
|
if (f0 <= 0.0) |
545 |
|
|
return SEND_MSG("Frequency has to be positive " + toString(f0), |
546 |
|
|
MSG_TYPE_ERROR); |
547 |
|
|
|
548 |
|
|
if (!m_linChirpTrajGen[i]->set_initial_point(m_noTrajGen[i]->getPos())) |
549 |
|
|
return SEND_MSG("Error while setting initial point " + |
550 |
|
|
toString(m_noTrajGen[i]->getPos()), |
551 |
|
|
MSG_TYPE_ERROR); |
552 |
|
|
if (!m_linChirpTrajGen[i]->set_final_point(xFinal)) |
553 |
|
|
return SEND_MSG("Error while setting final point " + toString(xFinal), |
554 |
|
|
MSG_TYPE_ERROR); |
555 |
|
|
if (!m_linChirpTrajGen[i]->set_trajectory_time(time)) |
556 |
|
|
return SEND_MSG("Error while setting trajectory time " + toString(time), |
557 |
|
|
MSG_TYPE_ERROR); |
558 |
|
|
if (!m_linChirpTrajGen[i]->set_initial_frequency(f0)) |
559 |
|
|
return SEND_MSG("Error while setting initial frequency " + toString(f0), |
560 |
|
|
MSG_TYPE_ERROR); |
561 |
|
|
if (!m_linChirpTrajGen[i]->set_final_frequency(f1)) |
562 |
|
|
return SEND_MSG("Error while setting final frequency " + toString(f1), |
563 |
|
|
MSG_TYPE_ERROR); |
564 |
|
|
m_status[i] = TG_LIN_CHIRP; |
565 |
|
|
m_currentTrajGen[i] = m_linChirpTrajGen[i]; |
566 |
|
|
} |
567 |
|
|
|
568 |
|
|
void SE3TrajectoryGenerator::move(const int& id, const double& xFinal, |
569 |
|
|
const double& time) { |
570 |
|
|
if (!m_initSucceeded) |
571 |
|
|
return SEND_MSG("Cannot move value before initialization!", MSG_TYPE_ERROR); |
572 |
|
|
unsigned int i = id; |
573 |
|
|
if (id < 0 || id >= static_cast<int>(m_np)) |
574 |
|
|
return SEND_MSG("Index is out of bounds", MSG_TYPE_ERROR); |
575 |
|
|
if (time <= 0.0) |
576 |
|
|
return SEND_MSG("Trajectory time must be a positive number", |
577 |
|
|
MSG_TYPE_ERROR); |
578 |
|
|
if (m_status[i] != TG_STOP) |
579 |
|
|
return SEND_MSG( |
580 |
|
|
"You cannot move the specified component because it is already " |
581 |
|
|
"controlled.", |
582 |
|
|
MSG_TYPE_ERROR); |
583 |
|
|
|
584 |
|
|
m_minJerkTrajGen[i]->set_initial_point(m_noTrajGen[i]->getPos()); |
585 |
|
|
m_minJerkTrajGen[i]->set_final_point(xFinal); |
586 |
|
|
m_minJerkTrajGen[i]->set_trajectory_time(time); |
587 |
|
|
m_status[i] = TG_MIN_JERK; |
588 |
|
|
m_currentTrajGen[i] = m_minJerkTrajGen[i]; |
589 |
|
|
} |
590 |
|
|
|
591 |
|
|
void SE3TrajectoryGenerator::stop(const int& id) { |
592 |
|
|
if (!m_initSucceeded) |
593 |
|
|
return SEND_MSG("Cannot stop value before initialization!", MSG_TYPE_ERROR); |
594 |
|
|
|
595 |
|
|
if (id == -1) // Stop entire vector |
596 |
|
|
{ |
597 |
|
|
for (unsigned int i = 0; i < m_np; i++) { |
598 |
|
|
m_status[i] = TG_STOP; |
599 |
|
|
// update the initial value |
600 |
|
|
m_noTrajGen[i]->set_initial_point(m_currentTrajGen[i]->getPos()); |
601 |
|
|
m_currentTrajGen[i] = m_noTrajGen[i]; |
602 |
|
|
} |
603 |
|
|
return; |
604 |
|
|
} |
605 |
|
|
if (id < 0 || id >= static_cast<int>(m_np)) |
606 |
|
|
return SEND_MSG("Index is out of bounds", MSG_TYPE_ERROR); |
607 |
|
|
unsigned int i = id; |
608 |
|
|
m_noTrajGen[i]->set_initial_point(m_currentTrajGen[i]->getPos()); |
609 |
|
|
m_status[i] = TG_STOP; |
610 |
|
|
m_currentTrajGen[i] = m_noTrajGen[i]; |
611 |
|
|
|
612 |
|
|
m_splineReady = false; |
613 |
|
|
m_t = 0.0; |
614 |
|
|
} |
615 |
|
|
|
616 |
|
|
/* ------------------------------------------------------------------- */ |
617 |
|
|
/* --- ENTITY -------------------------------------------------------- */ |
618 |
|
|
/* ------------------------------------------------------------------- */ |
619 |
|
|
|
620 |
|
|
void SE3TrajectoryGenerator::display(std::ostream& os) const { |
621 |
|
|
os << "SE3TrajectoryGenerator " << getName(); |
622 |
|
|
try { |
623 |
|
|
getProfiler().report_all(3, os); |
624 |
|
|
} catch (ExceptionSignal e) { |
625 |
|
|
} |
626 |
|
|
} |
627 |
|
|
} // namespace torque_control |
628 |
|
|
} // namespace sot |
629 |
|
|
} // namespace dynamicgraph |