sot-torque-control  1.6.5
Collection of dynamic-graph entities aimed at implementing torque control on different robots.
 
Loading...
Searching...
No Matches
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
8#include <sot/core/debug.hh>
9#include <sot/core/stop-watch.hh>
12
13#include "../include/sot/torque_control/stc-commands.hh"
14
15namespace dynamicgraph {
16namespace sot {
17namespace torque_control {
18namespace dynamicgraph = ::dynamicgraph;
19using namespace dynamicgraph;
20using namespace dynamicgraph::command;
21using namespace std;
22using namespace Eigen;
23
24// Size to be aligned "-------------------------------------------------------"
25#define PROFILE_POSITION_DESIRED_COMPUTATION \
26 "TrajGen: reference joint traj computation "
27#define PROFILE_FORCE_DESIRED_COMPUTATION \
28 "TrajGen: reference force computation "
29
32typedef JointTrajectoryGenerator EntityClassName;
33
34/* --- DG FACTORY ---------------------------------------------------- */
36 "JointTrajectoryGenerator");
37
38/* ------------------------------------------------------------------- */
39/* --- CONSTRUCTION -------------------------------------------------- */
40/* ------------------------------------------------------------------- */
42 : Entity(name),
43 CONSTRUCT_SIGNAL_IN(base6d_encoders, dynamicgraph::Vector),
44 CONSTRUCT_SIGNAL_OUT(q, dynamicgraph::Vector, m_base6d_encodersSIN),
45 CONSTRUCT_SIGNAL_OUT(dq, dynamicgraph::Vector, m_qSOUT),
46 CONSTRUCT_SIGNAL_OUT(ddq, dynamicgraph::Vector, m_qSOUT),
47 CONSTRUCT_SIGNAL(fRightFoot, OUT, dynamicgraph::Vector),
48 CONSTRUCT_SIGNAL(fLeftFoot, OUT, dynamicgraph::Vector),
49 CONSTRUCT_SIGNAL(fRightHand, OUT, dynamicgraph::Vector),
50 CONSTRUCT_SIGNAL(fLeftHand, OUT, dynamicgraph::Vector),
51 m_initSucceeded(false),
52 m_firstIter(true),
53 m_robot_util(RefVoidRobotUtil()) {
54 BIND_SIGNAL_TO_FUNCTION(fRightFoot, OUT, dynamicgraph::Vector);
55 BIND_SIGNAL_TO_FUNCTION(fLeftFoot, OUT, dynamicgraph::Vector);
56 BIND_SIGNAL_TO_FUNCTION(fRightHand, OUT, dynamicgraph::Vector);
57 BIND_SIGNAL_TO_FUNCTION(fLeftHand, OUT, dynamicgraph::Vector);
58
59 m_status_force.resize(4, JTG_STOP);
60 m_minJerkTrajGen_force.resize(4);
61 m_sinTrajGen_force.resize(4);
63 m_currentTrajGen_force.resize(4);
64 m_noTrajGen_force.resize(4);
65
66 m_iterForceSignals.resize(4, 0);
67
68 Entity::signalRegistration(m_qSOUT << m_dqSOUT << m_ddqSOUT
69 << m_base6d_encodersSIN << m_fRightFootSOUT
70 << m_fLeftFootSOUT << m_fRightHandSOUT
71 << m_fLeftHandSOUT);
72
73 /* Commands. */
74 addCommand("init",
75 makeCommandVoid2(*this, &JointTrajectoryGenerator::init,
76 docCommandVoid2("Initialize the entity.",
77 "Time period in seconds (double)",
78 "robotRef (string)")));
79
80 addCommand(
81 "getJoint",
82 makeCommandVoid1(
84 docCommandVoid1("Get the current angle of the specified joint.",
85 "Joint name (string)")));
86
87 // const std::string& docstring = ;
88
89 addCommand("isTrajectoryEnded",
91 *this, "Return whether all joint trajectories have ended"));
92
93 addCommand("playTrajectoryFile",
94 makeCommandVoid1(
96 docCommandVoid1(
97 "Play the trajectory read from the specified text file.",
98 "(string) File name, path included")));
99
100 addCommand("startSinusoid",
101 makeCommandVoid3(
103 docCommandVoid3(
104 "Start an infinite sinusoid motion.",
105 "(string) joint name", "(double) final angle in radians",
106 "(double) time to reach the final angle in sec")));
107
108 addCommand("startTriangle",
109 makeCommandVoid4(
111 docCommandVoid4(
112 "Start an infinite triangle wave.", "(string) joint name",
113 "(double) final angle in radians",
114 "(double) time to reach the final angle in sec",
115 "(double) time to accelerate in sec")));
116
117 addCommand("startConstAcc",
118 makeCommandVoid3(
120 docCommandVoid3(
121 "Start an infinite trajectory with piece-wise constant "
122 "acceleration.",
123 "(string) joint name", "(double) final angle in radians",
124 "(double) time to reach the final angle in sec")));
125
126 addCommand(
127 "startForceSinusoid",
128 makeCommandVoid4(
130 docCommandVoid4("Start an infinite sinusoid force.",
131 "(string) force name", "(int) force axis in [0, 5]",
132 "(double) final 1d force in N or Nm",
133 "(double) time to reach the final force in sec")));
134
135 // addCommand("startForceSinusoid",
136 // makeCommandVoid3(*this,
137 // &JointTrajectoryGenerator::startForceSinusoid,
138 // docCommandVoid3("Start an infinite
139 // sinusoid force.",
140 // "(string) force name",
141 // "(Vector) final 6d force
142 // in N/Nm",
143 // "(double) time to reach
144 // the final force in
145 // sec")));
146
147 addCommand(
148 "startLinChirp",
149 makeCommandVoid5(
151 docCommandVoid5("Start a linear-chirp motion.", "(string) joint name",
152 "(double) final angle in radians",
153 "(double) initial frequency [Hz]",
154 "(double) final frequency [Hz]",
155 "(double) trajectory time [sec]")));
156
157 addCommand("startForceLinChirp",
158 makeCommandVoid6(
160 docCommandVoid6("Start a linear-chirp force traj.",
161 "(string) force name", "(int) force axis",
162 "(double) final force in N/Nm",
163 "(double) initial frequency [Hz]",
164 "(double) final frequency [Hz]",
165 "(double) trajectory time [sec]")));
166
167 addCommand("moveJoint",
168 makeCommandVoid3(
170 docCommandVoid3(
171 "Move the joint to the specified angle with a minimum "
172 "jerk trajectory.",
173 "(string) joint name", "(double) final angle in radians",
174 "(double) time to reach the final angle in sec")));
175
176 addCommand(
177 "moveForce",
178 makeCommandVoid4(
180 docCommandVoid4("Move the force to the specified value with a "
181 "minimum jerk trajectory.",
182 "(string) force name", "(int) force axis",
183 "(double) final force in N/Nm",
184 "(double) time to reach the final force in sec")));
185
186 addCommand("stop",
187 makeCommandVoid1(
189 docCommandVoid1("Stop the motion of the specified joint, or "
190 "of all joints if no joint name is specified.",
191 "(string) joint name")));
192
193 addCommand(
194 "stopForce",
195 makeCommandVoid1(*this, &JointTrajectoryGenerator::stopForce,
196 docCommandVoid1("Stop the specified force trajectort",
197 "(string) force name (rh,lh,lf,rf)")));
198}
199
200void JointTrajectoryGenerator::init(const double& dt,
201 const std::string& robotRef) {
202 /* Retrieve m_robot_util informations */
203 std::string localName(robotRef);
204 if (isNameInRobotUtil(localName))
205 m_robot_util = getRobotUtil(localName);
206 else {
207 SEND_MSG("You should have an entity controller manager initialized before",
208 MSG_TYPE_ERROR);
209 return;
210 }
211
212 if (dt <= 0.0) return SEND_MSG("Timestep must be positive", MSG_TYPE_ERROR);
213 m_firstIter = true;
214 m_dt = dt;
215
216 m_status.resize(m_robot_util->m_nbJoints, JTG_STOP);
217 m_minJerkTrajGen.resize(m_robot_util->m_nbJoints);
218 m_sinTrajGen.resize(m_robot_util->m_nbJoints);
219 m_triangleTrajGen.resize(m_robot_util->m_nbJoints);
220 m_constAccTrajGen.resize(m_robot_util->m_nbJoints);
221 m_linChirpTrajGen.resize(m_robot_util->m_nbJoints);
222 m_currentTrajGen.resize(m_robot_util->m_nbJoints);
223 m_noTrajGen.resize(m_robot_util->m_nbJoints);
224
225 for (long unsigned int i = 0; i < m_robot_util->m_nbJoints; i++) {
227 m_sinTrajGen[i] = new SinusoidTrajectoryGenerator(dt, 5.0, 1);
234 }
236 new TextFileTrajectoryGenerator(dt, m_robot_util->m_nbJoints);
237
238 for (int i = 0; i < 4; i++) {
244 }
245 m_initSucceeded = true;
246}
247
248/* ------------------------------------------------------------------- */
249/* --- SIGNALS ------------------------------------------------------- */
250/* ------------------------------------------------------------------- */
251
252DEFINE_SIGNAL_OUT_FUNCTION(q, dynamicgraph::Vector) {
253 if (!m_initSucceeded) {
254 SEND_WARNING_STREAM_MSG(
255 "Cannot compute signal positionDes before initialization!");
256 return s;
257 }
258
259 getProfiler().start(PROFILE_POSITION_DESIRED_COMPUTATION);
260 {
261 // at first iteration store current joints positions
262 if (m_firstIter) {
263 const dynamicgraph::Vector& base6d_encoders = m_base6d_encodersSIN(iter);
264 if (base6d_encoders.size() !=
265 static_cast<Eigen::VectorXd::Index>(m_robot_util->m_nbJoints + 6)) {
266 SEND_ERROR_STREAM_MSG("Unexpected size of signal base6d_encoder " +
267 toString(base6d_encoders.size()) + " " +
268 toString(m_robot_util->m_nbJoints + 6));
269 return s;
270 }
271 for (unsigned int i = 0; i < m_robot_util->m_nbJoints; i++)
272 m_noTrajGen[i]->set_initial_point(base6d_encoders(6 + i));
273 m_firstIter = false;
274 }
275
276 if (s.size() !=
277 static_cast<Eigen::VectorXd::Index>(m_robot_util->m_nbJoints))
278 s.resize(m_robot_util->m_nbJoints);
279
280 if (m_status[0] == JTG_TEXT_FILE) {
281 const VectorXd& qRef = m_textFileTrajGen->compute_next_point();
282 for (unsigned int i = 0; i < m_robot_util->m_nbJoints; i++) {
283 s(i) = qRef[i];
284 if (m_textFileTrajGen->isTrajectoryEnded()) {
285 m_noTrajGen[i]->set_initial_point(s(i));
286 m_status[i] = JTG_STOP;
287 }
288 }
289 if (m_textFileTrajGen->isTrajectoryEnded())
290 SEND_MSG("Text file trajectory ended.", MSG_TYPE_INFO);
291 } else {
292 for (unsigned int i = 0; i < m_robot_util->m_nbJoints; i++) {
293 s(i) = m_currentTrajGen[i]->compute_next_point()(0);
294 if (m_currentTrajGen[i]->isTrajectoryEnded()) {
295 m_currentTrajGen[i] = m_noTrajGen[i];
296 m_noTrajGen[i]->set_initial_point(s(i));
297 m_status[i] = JTG_STOP;
298 SEND_MSG("Trajectory of joint " + m_robot_util->get_name_from_id(i) +
299 " ended.",
300 MSG_TYPE_INFO);
301 }
302 }
303 }
304 }
305 getProfiler().stop(PROFILE_POSITION_DESIRED_COMPUTATION);
306
307 return s;
308}
309
310DEFINE_SIGNAL_OUT_FUNCTION(dq, dynamicgraph::Vector) {
311 if (!m_initSucceeded) {
312 SEND_WARNING_STREAM_MSG(
313 "Cannot compute signal positionDes before initialization! iter: " +
314 toString(iter));
315 return s;
316 }
317
318 if (s.size() != static_cast<Eigen::VectorXd::Index>(m_robot_util->m_nbJoints))
319 s.resize(m_robot_util->m_nbJoints);
320 if (m_status[0] == JTG_TEXT_FILE) {
321 for (unsigned int i = 0; i < m_robot_util->m_nbJoints; i++)
322 s(i) = m_textFileTrajGen->getVel()[i];
323 } else
324 for (unsigned int i = 0; i < m_robot_util->m_nbJoints; i++)
325 s(i) = m_currentTrajGen[i]->getVel()(0);
326
327 return s;
328}
329
330DEFINE_SIGNAL_OUT_FUNCTION(ddq, dynamicgraph::Vector) {
331 if (!m_initSucceeded) {
332 SEND_WARNING_STREAM_MSG(
333 "Cannot compute signal positionDes before initialization!" +
334 toString(iter));
335 return s;
336 }
337
338 if (s.size() != static_cast<Eigen::VectorXd::Index>(m_robot_util->m_nbJoints))
339 s.resize(m_robot_util->m_nbJoints);
340
341 if (m_status[0] == JTG_TEXT_FILE) {
342 for (unsigned int i = 0; i < m_robot_util->m_nbJoints; i++)
343 s(i) = m_textFileTrajGen->getAcc()[i];
344 } else
345 for (unsigned int i = 0; i < m_robot_util->m_nbJoints; i++)
346 s(i) = m_currentTrajGen[i]->getAcc()(0);
347
348 return s;
349}
350
351DEFINE_SIGNAL_OUT_FUNCTION(fRightFoot, dynamicgraph::Vector) {
352 // SEND_MSG("Compute force right foot iter "+toString(iter),
353 // MSG_TYPE_DEBUG);
354 generateReferenceForceSignal(
355 "fRightFoot",
356 static_cast<int>(m_robot_util->m_force_util.get_force_id_right_foot()), s,
357 iter);
358 return s;
359}
360
361DEFINE_SIGNAL_OUT_FUNCTION(fLeftFoot, dynamicgraph::Vector) {
362 generateReferenceForceSignal(
363 "fLeftFoot",
364 static_cast<int>(m_robot_util->m_force_util.get_force_id_left_foot()), s,
365 iter);
366 return s;
367}
368
369DEFINE_SIGNAL_OUT_FUNCTION(fRightHand, dynamicgraph::Vector) {
370 generateReferenceForceSignal(
371 "fRightHand",
372 static_cast<int>(m_robot_util->m_force_util.get_force_id_right_hand()), s,
373 iter);
374 return s;
375}
376
377DEFINE_SIGNAL_OUT_FUNCTION(fLeftHand, dynamicgraph::Vector) {
378 generateReferenceForceSignal(
379 "fLeftHand",
380 static_cast<int>(m_robot_util->m_force_util.get_force_id_left_hand()), s,
381 iter);
382 return s;
383}
384
386 const std::string& forceName, int fid, dynamicgraph::Vector& s, int iter) {
387 if (!m_initSucceeded) {
388 SEND_WARNING_STREAM_MSG("Cannot compute signal " + forceName +
389 " before initialization!");
390 return false;
391 }
392
393 getProfiler().start(PROFILE_FORCE_DESIRED_COMPUTATION);
394 {
395 if (s.size() != 6) s.resize(6);
396
397 if (iter > m_iterForceSignals[fid]) {
398 m_currentTrajGen_force[fid]->compute_next_point();
399 // SEND_MSG("Compute force "+forceName+" with id
400 // "+toString(fid)+": "+toString(fr.transpose()),
401 // MSG_TYPE_DEBUG);
402 m_iterForceSignals[fid]++;
403 }
404
405 const Eigen::VectorXd& fr = m_currentTrajGen_force[fid]->getPos();
406 for (unsigned int i = 0; i < 6; i++) s(i) = fr(i);
407
409 m_noTrajGen_force[fid]->set_initial_point(
410 m_currentTrajGen_force[fid]->getPos());
413 SEND_MSG("Trajectory of force " + forceName + " ended.", MSG_TYPE_INFO);
414 }
415 }
416 getProfiler().stop(PROFILE_FORCE_DESIRED_COMPUTATION);
417
418 return true;
419}
420
421/* ------------------------------------------------------------------- */
422/* --- COMMANDS ------------------------------------------------------ */
423/* ------------------------------------------------------------------- */
424
425void JointTrajectoryGenerator::getJoint(const std::string& jointName) {
426 unsigned int i;
427 if (convertJointNameToJointId(jointName, i) == false) return;
428 const dynamicgraph::Vector& base6d_encoders =
429 m_base6d_encodersSIN.accessCopy();
430 SEND_MSG("Current angle of joint " + jointName + " is " +
431 toString(base6d_encoders(6 + i)),
432 MSG_TYPE_INFO);
433}
434
436 bool output = true;
437 if (m_status[0] == JTG_TEXT_FILE) {
438 for (unsigned int i = 0; i < m_robot_util->m_nbJoints; i++) {
440 output = false;
441 SEND_MSG("Text file trajectory not ended.", MSG_TYPE_INFO);
442 return output;
443 }
444 }
445 } else {
446 for (unsigned int i = 0; i < m_robot_util->m_nbJoints; i++) {
448 output = false;
449 SEND_MSG("Trajectory of joint " + m_robot_util->get_name_from_id(i) +
450 "not ended.",
451 MSG_TYPE_INFO);
452 return output;
453 }
454 }
455 }
456 return output;
457}
458
459void JointTrajectoryGenerator::playTrajectoryFile(const std::string& fileName) {
460 if (!m_initSucceeded)
461 return SEND_MSG("Cannot start sinusoid before initialization!",
462 MSG_TYPE_ERROR);
463
464 for (unsigned int i = 0; i < m_robot_util->m_nbJoints; i++)
465 if (m_status[i] != JTG_STOP)
466 return SEND_MSG("You cannot joint " + m_robot_util->get_name_from_id(i) +
467 " because it is already controlled.",
468 MSG_TYPE_ERROR);
469
470 if (!m_textFileTrajGen->loadTextFile(fileName))
471 return SEND_MSG("Error while loading text file " + fileName,
472 MSG_TYPE_ERROR);
473
474 // check current configuration is not too far from initial trajectory
475 // configuration
476 bool needToMoveToInitConf = false;
477 const VectorXd& qInit = m_textFileTrajGen->get_initial_point();
478 for (unsigned int i = 0; i < m_robot_util->m_nbJoints; i++)
479 if (fabs(qInit[i] - m_currentTrajGen[i]->getPos()(0)) > 0.001) {
480 needToMoveToInitConf = true;
481 SEND_MSG("Joint " + m_robot_util->get_name_from_id(i) +
482 " is too far from initial configuration so first i will "
483 "move it there.",
484 MSG_TYPE_WARNING);
485 }
486
487 // if necessary move joints to initial configuration
488 if (needToMoveToInitConf) {
489 for (unsigned int i = 0; i < m_robot_util->m_nbJoints; i++) {
490 if (!isJointInRange(i, qInit[i])) return;
491
492 m_minJerkTrajGen[i]->set_initial_point(m_noTrajGen[i]->getPos());
493 m_minJerkTrajGen[i]->set_final_point(qInit[i]);
494 m_minJerkTrajGen[i]->set_trajectory_time(4.0);
497 }
498 return;
499 }
500
501 for (unsigned int i = 0; i < m_robot_util->m_nbJoints; i++) {
503 }
504}
505
506void JointTrajectoryGenerator::startSinusoid(const std::string& jointName,
507 const double& qFinal,
508 const double& time) {
509 if (!m_initSucceeded)
510 return SEND_MSG("Cannot start sinusoid before initialization!",
511 MSG_TYPE_ERROR);
512
513 unsigned int i;
514 if (convertJointNameToJointId(jointName, i) == false) return;
515 if (time <= 0.0)
516 return SEND_MSG("Trajectory time must be a positive number",
517 MSG_TYPE_ERROR);
518 if (m_status[i] != JTG_STOP)
519 return SEND_MSG(
520 "You cannot move the specified joint because it is already controlled.",
521 MSG_TYPE_ERROR);
522 if (!isJointInRange(i, qFinal)) return;
523
524 m_sinTrajGen[i]->set_initial_point(m_noTrajGen[i]->getPos());
525 SEND_MSG(
526 "Set initial point of sinusoid to " + toString(m_sinTrajGen[i]->getPos()),
527 MSG_TYPE_DEBUG);
528 m_sinTrajGen[i]->set_final_point(qFinal);
529 m_sinTrajGen[i]->set_trajectory_time(time);
532}
533
534void JointTrajectoryGenerator::startTriangle(const std::string& jointName,
535 const double& qFinal,
536 const double& time,
537 const double& Tacc) {
538 if (!m_initSucceeded)
539 return SEND_MSG("Cannot start triangle before initialization!",
540 MSG_TYPE_ERROR);
541
542 unsigned int i;
543 if (convertJointNameToJointId(jointName, i) == false) return;
544 if (m_status[i] != JTG_STOP)
545 return SEND_MSG(
546 "You cannot move the specified joint because it is already controlled.",
547 MSG_TYPE_ERROR);
548 if (!isJointInRange(i, qFinal)) return;
549
550 m_triangleTrajGen[i]->set_initial_point(m_noTrajGen[i]->getPos());
551 SEND_MSG("Set initial point of triangular trajectory to " +
552 toString(m_triangleTrajGen[i]->getPos()),
553 MSG_TYPE_DEBUG);
554 m_triangleTrajGen[i]->set_final_point(qFinal);
555
556 if (!m_triangleTrajGen[i]->set_trajectory_time(time))
557 return SEND_MSG("Trajectory time cannot be negative.", MSG_TYPE_ERROR);
558
559 if (!m_triangleTrajGen[i]->set_acceleration_time(Tacc))
560 return SEND_MSG(
561 "Acceleration time cannot be negative or larger than half the "
562 "trajectory time.",
563 MSG_TYPE_ERROR);
564
567}
568
569void JointTrajectoryGenerator::startConstAcc(const std::string& jointName,
570 const double& qFinal,
571 const double& time) {
572 if (!m_initSucceeded)
573 return SEND_MSG(
574 "Cannot start constant-acceleration trajectory before initialization!",
575 MSG_TYPE_ERROR);
576
577 unsigned int i;
578 if (convertJointNameToJointId(jointName, i) == false) return;
579 if (time <= 0.0)
580 return SEND_MSG("Trajectory time must be a positive number",
581 MSG_TYPE_ERROR);
582 if (m_status[i] != JTG_STOP)
583 return SEND_MSG(
584 "You cannot move the specified joint because it is already controlled.",
585 MSG_TYPE_ERROR);
586 if (!isJointInRange(i, qFinal)) return;
587
588 m_constAccTrajGen[i]->set_initial_point(m_noTrajGen[i]->getPos());
589 SEND_MSG("Set initial point of const-acc trajectory to " +
590 toString(m_constAccTrajGen[i]->getPos()),
591 MSG_TYPE_DEBUG);
592 m_constAccTrajGen[i]->set_final_point(qFinal);
593 m_constAccTrajGen[i]->set_trajectory_time(time);
596}
597
598void JointTrajectoryGenerator::startForceSinusoid(const std::string& forceName,
599 const int& axis,
600 const double& fFinal,
601 const double& time) {
602 if (!m_initSucceeded)
603 return SEND_MSG("Cannot start force sinusoid before initialization!",
604 MSG_TYPE_ERROR);
605
606 unsigned int i;
607 if (convertForceNameToForceId(forceName, i) == false) return;
608 if (time <= 0.0)
609 return SEND_MSG("Trajectory time must be a positive number",
610 MSG_TYPE_ERROR);
611 if (axis < 0 || axis > 5)
612 return SEND_MSG("Axis must be between 0 and 5", MSG_TYPE_ERROR);
613 if (m_status_force[i] != JTG_STOP)
614 return SEND_MSG(
615 "You cannot move the specified force because it is already controlled.",
616 MSG_TYPE_ERROR);
617 // EIGEN_CONST_VECTOR_FROM_SIGNAL(fFinal_eig, fFinal);
618 if (!isForceInRange(i, axis, fFinal)) return;
619
620 VectorXd currentF = m_noTrajGen_force[i]->getPos();
621 m_sinTrajGen_force[i]->set_initial_point(currentF);
622 SEND_MSG("Set initial point of sinusoid to " +
623 toString(m_sinTrajGen_force[i]->getPos()),
624 MSG_TYPE_DEBUG);
625 currentF[axis] = fFinal;
626 m_sinTrajGen_force[i]->set_final_point(currentF);
627 m_sinTrajGen_force[i]->set_trajectory_time(time);
630}
631
633 const double& qFinal,
634 const double& f0,
635 const double& f1,
636 const double& time) {
637 if (!m_initSucceeded)
638 return SEND_MSG("Cannot start linear chirp before initialization!",
639 MSG_TYPE_ERROR);
640
641 unsigned int i;
642 if (convertJointNameToJointId(jointName, i) == false) return;
643 if (time <= 0.0)
644 return SEND_MSG("Trajectory time must be a positive number",
645 MSG_TYPE_ERROR);
646 if (m_status[i] != JTG_STOP)
647 return SEND_MSG(
648 "You cannot move the specified joint because it is already controlled.",
649 MSG_TYPE_ERROR);
650 if (!isJointInRange(i, qFinal)) return;
651 if (f0 > f1)
652 return SEND_MSG(
653 "f0 " + toString(f0) + " cannot to be more than f1 " + toString(f1),
654 MSG_TYPE_ERROR);
655 if (f0 <= 0.0)
656 return SEND_MSG("Frequency has to be positive " + toString(f0),
657 MSG_TYPE_ERROR);
658
659 if (!m_linChirpTrajGen[i]->set_initial_point(m_noTrajGen[i]->getPos()))
660 return SEND_MSG("Error while setting initial point " +
661 toString(m_noTrajGen[i]->getPos()),
662 MSG_TYPE_ERROR);
663 if (!m_linChirpTrajGen[i]->set_final_point(qFinal))
664 return SEND_MSG("Error while setting final point " + toString(qFinal),
665 MSG_TYPE_ERROR);
666 if (!m_linChirpTrajGen[i]->set_trajectory_time(time))
667 return SEND_MSG("Error while setting trajectory time " + toString(time),
668 MSG_TYPE_ERROR);
669 if (!m_linChirpTrajGen[i]->set_initial_frequency(f0))
670 return SEND_MSG("Error while setting initial frequency " + toString(f0),
671 MSG_TYPE_ERROR);
672 if (!m_linChirpTrajGen[i]->set_final_frequency(f1))
673 return SEND_MSG("Error while setting final frequency " + toString(f1),
674 MSG_TYPE_ERROR);
677}
678
680 const string& forceName, const int& axis, const double& fFinal,
681 const double& f0, const double& f1, const double& time) {
682 if (!m_initSucceeded)
683 return SEND_MSG("Cannot start force linear chirp before initialization!",
684 MSG_TYPE_ERROR);
685
686 unsigned int i;
687 if (convertForceNameToForceId(forceName, i) == false) return;
688 if (time <= 0.0)
689 return SEND_MSG("Trajectory time must be a positive number",
690 MSG_TYPE_ERROR);
691 if (m_status_force[i] != JTG_STOP)
692 return SEND_MSG(
693 "You cannot move the specified force because it is already controlled.",
694 MSG_TYPE_ERROR);
695 if (!isForceInRange(i, axis, fFinal)) return;
696 if (f0 > f1)
697 return SEND_MSG(
698 "f0 " + toString(f0) + " cannot to be more than f1 " + toString(f1),
699 MSG_TYPE_ERROR);
700 if (f0 <= 0.0)
701 return SEND_MSG("Frequency has to be positive " + toString(f0),
702 MSG_TYPE_ERROR);
703
704 VectorXd currentF = m_noTrajGen_force[i]->getPos();
705 if (!m_linChirpTrajGen_force[i]->set_initial_point(currentF))
706 return SEND_MSG("Error while setting initial point " +
707 toString(m_noTrajGen_force[i]->getPos()),
708 MSG_TYPE_ERROR);
709 currentF[axis] = fFinal;
710 if (!m_linChirpTrajGen_force[i]->set_final_point(currentF))
711 return SEND_MSG("Error while setting final point " + toString(fFinal),
712 MSG_TYPE_ERROR);
713 if (!m_linChirpTrajGen_force[i]->set_trajectory_time(time))
714 return SEND_MSG("Error while setting trajectory time " + toString(time),
715 MSG_TYPE_ERROR);
716 if (!m_linChirpTrajGen_force[i]->set_initial_frequency(
717 Vector6d::Constant(f0)))
718 return SEND_MSG("Error while setting initial frequency " + toString(f0),
719 MSG_TYPE_ERROR);
720 if (!m_linChirpTrajGen_force[i]->set_final_frequency(Vector6d::Constant(f1)))
721 return SEND_MSG("Error while setting final frequency " + toString(f1),
722 MSG_TYPE_ERROR);
725}
726
727void JointTrajectoryGenerator::moveJoint(const string& jointName,
728 const double& qFinal,
729 const double& time) {
730 if (!m_initSucceeded)
731 return SEND_MSG("Cannot move joint before initialization!", MSG_TYPE_ERROR);
732
733 unsigned int i;
734 if (convertJointNameToJointId(jointName, i) == false) return;
735 if (time <= 0.0)
736 return SEND_MSG("Trajectory time must be a positive number",
737 MSG_TYPE_ERROR);
738 if (m_status[i] != JTG_STOP)
739 return SEND_MSG(
740 "You cannot move the specified joint because it is already controlled.",
741 MSG_TYPE_ERROR);
742 if (!isJointInRange(i, qFinal)) return;
743
744 m_minJerkTrajGen[i]->set_initial_point(m_noTrajGen[i]->getPos());
745 m_minJerkTrajGen[i]->set_final_point(qFinal);
746 m_minJerkTrajGen[i]->set_trajectory_time(time);
749}
750
751void JointTrajectoryGenerator::moveForce(const string& forceName,
752 const int& axis, const double& fFinal,
753 const double& time) {
754 if (!m_initSucceeded)
755 return SEND_MSG("Cannot move force before initialization!", MSG_TYPE_ERROR);
756
757 unsigned int i;
758 if (convertForceNameToForceId(forceName, i) == false) return;
759 if (time <= 0.0)
760 return SEND_MSG("Trajectory time must be a positive number",
761 MSG_TYPE_ERROR);
762 if (m_status_force[i] != JTG_STOP)
763 return SEND_MSG(
764 "You cannot move the specified force because it is already controlled.",
765 MSG_TYPE_ERROR);
766 if (!isForceInRange(i, axis, fFinal)) return;
767
768 VectorXd currentF = m_noTrajGen_force[i]->getPos();
769 m_minJerkTrajGen_force[i]->set_initial_point(currentF);
770 currentF[axis] = fFinal;
771 m_minJerkTrajGen_force[i]->set_final_point(currentF);
772 m_minJerkTrajGen_force[i]->set_trajectory_time(time);
775}
776
777void JointTrajectoryGenerator::stop(const std::string& jointName) {
778 if (!m_initSucceeded)
779 return SEND_MSG("Cannot stop joint before initialization!", MSG_TYPE_ERROR);
780
781 const dynamicgraph::Vector& base6d_encoders =
782 m_base6d_encodersSIN.accessCopy();
783 if (jointName == "all") {
784 for (unsigned int i = 0; i < m_robot_util->m_nbJoints; i++) {
785 m_status[i] = JTG_STOP;
786 // update the initial position
787 m_noTrajGen[i]->set_initial_point(base6d_encoders(6 + i));
789 }
790 return;
791 }
792
793 unsigned int i;
794 if (convertJointNameToJointId(jointName, i) == false) return;
795 m_noTrajGen[i]->set_initial_point(
796 base6d_encoders(6 + i)); // update the initial position
797 m_status[i] = JTG_STOP;
799}
800
801void JointTrajectoryGenerator::stopForce(const std::string& forceName) {
802 if (!m_initSucceeded)
803 return SEND_MSG("Cannot stop force before initialization!", MSG_TYPE_ERROR);
804
805 unsigned int i;
806 if (convertForceNameToForceId(forceName, i) == false) return;
807 m_noTrajGen_force[i]->set_initial_point(
808 m_currentTrajGen_force[i]->getPos()); // update the initial position
811}
812
813/* ------------------------------------------------------------------- */
814// ************************ PROTECTED MEMBER METHODS ********************
815/* ------------------------------------------------------------------- */
816
818 const std::string& name, unsigned int& id) {
819 // Check if the joint name exists
820 sot::Index jid = m_robot_util->get_id_from_name(name);
821 if (jid < 0) {
822 SEND_MSG("The specified joint name does not exist", MSG_TYPE_ERROR);
823 std::stringstream ss;
824 for (map<string, Index>::const_iterator it =
825 m_robot_util->m_name_to_id.begin();
826 it != m_robot_util->m_name_to_id.end(); it++)
827 ss << it->first << ", ";
828 SEND_MSG("Possible joint names are: " + ss.str(), MSG_TYPE_INFO);
829 return false;
830 }
831 id = static_cast<unsigned int>(jid);
832 return true;
833}
834
836 const std::string& name, unsigned int& id) {
837 // Check if the joint name exists
838 Index fid = m_robot_util->m_force_util.get_id_from_name(name);
839 if (fid < 0) {
840 SEND_MSG("The specified force name does not exist", MSG_TYPE_ERROR);
841 std::stringstream ss;
842 for (map<string, Index>::const_iterator it =
843 m_robot_util->m_force_util.m_name_to_force_id.begin();
844 it != m_robot_util->m_force_util.m_name_to_force_id.end(); it++)
845 ss << it->first << ", ";
846 SEND_MSG("Possible force names are: " + ss.str(), MSG_TYPE_INFO);
847 return false;
848 }
849 id = (unsigned int)fid;
850 return true;
851}
852
853bool JointTrajectoryGenerator::isJointInRange(unsigned int id, double q) {
854 JointLimits jl = m_robot_util->get_joint_limits_from_id(id);
855 if (q < jl.lower) {
856 SEND_MSG("Joint " + m_robot_util->get_name_from_id(id) +
857 ", desired angle " + toString(q) +
858 " is smaller than lower limit " + toString(jl.lower),
859 MSG_TYPE_ERROR);
860 return false;
861 }
862 if (q > jl.upper) {
863 SEND_MSG("Joint " + m_robot_util->get_name_from_id(id) +
864 ", desired angle " + toString(q) +
865 " is larger than upper limit " + toString(jl.upper),
866 MSG_TYPE_ERROR);
867 return false;
868 }
869 return true;
870}
871
873 const Eigen::VectorXd& f) {
874 ForceLimits fl = m_robot_util->m_force_util.get_limits_from_id(id);
875 for (unsigned int i = 0; i < 6; i++)
876 if (f[i] < fl.lower[i] || f[i] > fl.upper[i]) {
877 SEND_MSG("Desired force " + toString(i) +
878 " is out of range: " + toString(fl.lower[i]) + " < " +
879 toString(f[i]) + " < " + toString(fl.upper[i]),
880 MSG_TYPE_ERROR);
881 return false;
882 }
883 return true;
884}
885
886bool JointTrajectoryGenerator::isForceInRange(unsigned int id, int axis,
887 double f) {
888 ForceLimits fl = m_robot_util->m_force_util.get_limits_from_id(id);
889 if (f < fl.lower[axis] || f > fl.upper[axis]) {
890 SEND_MSG("Desired force " + toString(axis) +
891 " is out of range: " + toString(fl.lower[axis]) + " < " +
892 toString(f) + " < " + toString(fl.upper[axis]),
893 MSG_TYPE_ERROR);
894 return false;
895 }
896 return true;
897}
898
899/* ------------------------------------------------------------------- */
900/* --- ENTITY -------------------------------------------------------- */
901/* ------------------------------------------------------------------- */
902
903void JointTrajectoryGenerator::display(std::ostream& os) const {
904 os << "JointTrajectoryGenerator " << getName();
905 try {
906 getProfiler().report_all(3, os);
907 } catch (ExceptionSignal e) {
908 }
909}
910} // namespace torque_control
911} // namespace sot
912} // namespace dynamicgraph
void moveJoint(const std::string &jointName, const double &qFinal, const double &time)
void moveForce(const std::string &forceName, const int &axis, const double &fFinal, const double &time)
void startSinusoid(const std::string &jointName, const double &qFinal, const double &time)
std::vector< AbstractTrajectoryGenerator * > m_currentTrajGen_force
status of the forces
void startLinearChirp(const std::string &jointName, const double &qFinal, const double &f0, const double &f1, const double &time)
bool convertForceNameToForceId(const std::string &name, unsigned int &id)
std::vector< LinearChirpTrajectoryGenerator * > m_linChirpTrajGen
bool isForceInRange(unsigned int id, const Eigen::VectorXd &f)
bool generateReferenceForceSignal(const std::string &forceName, int fid, dynamicgraph::Vector &s, int iter)
std::vector< LinearChirpTrajectoryGenerator * > m_linChirpTrajGen_force
std::vector< MinimumJerkTrajectoryGenerator * > m_minJerkTrajGen_force
double m_dt
true if it is the first iteration, false otherwise
std::vector< ConstantAccelerationTrajectoryGenerator * > m_constAccTrajGen
void startTriangle(const std::string &jointName, const double &qFinal, const double &time, const double &Tacc)
bool m_firstIter
true if the entity has been successfully initialized
void startForceSinusoid(const std::string &forceName, const int &axis, const double &fFinal, const double &time)
void startConstAcc(const std::string &jointName, const double &qFinal, const double &time)
void startForceLinearChirp(const std::string &forceName, const int &axis, const double &fFinal, const double &f0, const double &f1, const double &time)
bool convertJointNameToJointId(const std::string &name, unsigned int &id)
std::vector< AbstractTrajectoryGenerator * > m_currentTrajGen
status of the joints
void init(const double &dt, const std::string &robotRef)
std::vector< MinimumJerkTrajectoryGenerator * > m_minJerkTrajGen
#define PROFILE_FORCE_DESIRED_COMPUTATION
#define PROFILE_POSITION_DESIRED_COMPUTATION
DEFINE_SIGNAL_OUT_FUNCTION(u, dynamicgraph::Vector)
DYNAMICGRAPH_FACTORY_ENTITY_PLUGIN(AdmittanceController, "AdmittanceController")
to read text file
Definition treeview.dox:22