6 #include <dynamic-graph/factory.h>
7 #include <sot/core/debug.hh>
10 #include <sot/core/stop-watch.hh>
12 #include "../include/sot/torque_control/stc-commands.hh"
21 using namespace Eigen;
24 #define PROFILE_POSITION_DESIRED_COMPUTATION "TrajGen: reference joint traj computation "
25 #define PROFILE_FORCE_DESIRED_COMPUTATION "TrajGen: reference force computation "
39 CONSTRUCT_SIGNAL_IN(base6d_encoders,
dynamicgraph::Vector),
40 CONSTRUCT_SIGNAL_OUT(q,
dynamicgraph::Vector, m_base6d_encodersSIN),
42 CONSTRUCT_SIGNAL_OUT(ddq,
dynamicgraph::Vector, m_qSOUT),
47 m_initSucceeded(false),
49 m_robot_util(RefVoidRobotUtil()) {
50 BIND_SIGNAL_TO_FUNCTION(fRightFoot, OUT, dynamicgraph::Vector);
51 BIND_SIGNAL_TO_FUNCTION(fLeftFoot, OUT, dynamicgraph::Vector);
52 BIND_SIGNAL_TO_FUNCTION(fRightHand, OUT, dynamicgraph::Vector);
53 BIND_SIGNAL_TO_FUNCTION(fLeftHand, OUT, dynamicgraph::Vector);
64 Entity::signalRegistration(m_qSOUT << m_dqSOUT << m_ddqSOUT << m_base6d_encodersSIN << m_fRightFootSOUT
65 << m_fLeftFootSOUT << m_fRightHandSOUT << m_fLeftHandSOUT);
69 docCommandVoid2(
"Initialize the entity.",
"Time period in seconds (double)",
70 "robotRef (string)")));
72 addCommand(
"getJoint", makeCommandVoid1(
74 docCommandVoid1(
"Get the current angle of the specified joint.",
"Joint name (string)")));
78 addCommand(
"isTrajectoryEnded",
81 addCommand(
"playTrajectoryFile",
83 docCommandVoid1(
"Play the trajectory read from the specified text file.",
84 "(string) File name, path included")));
86 addCommand(
"startSinusoid",
89 docCommandVoid3(
"Start an infinite sinusoid motion.",
"(string) joint name",
90 "(double) final angle in radians",
"(double) time to reach the final angle in sec")));
96 docCommandVoid4(
"Start an infinite triangle wave.",
"(string) joint name",
"(double) final angle in radians",
97 "(double) time to reach the final angle in sec",
"(double) time to accelerate in sec")));
99 addCommand(
"startConstAcc",
101 docCommandVoid3(
"Start an infinite trajectory with piece-wise constant acceleration.",
102 "(string) joint name",
"(double) final angle in radians",
103 "(double) time to reach the final angle in sec")));
105 addCommand(
"startForceSinusoid",
107 docCommandVoid4(
"Start an infinite sinusoid force.",
"(string) force name",
108 "(int) force axis in [0, 5]",
"(double) final 1d force in N or Nm",
109 "(double) time to reach the final force in sec")));
118 addCommand(
"startLinChirp",
121 "(double) final angle in radians",
"(double) initial frequency [Hz]",
122 "(double) final frequency [Hz]",
"(double) trajectory time [sec]")));
125 "startForceLinChirp",
127 docCommandVoid6(
"Start a linear-chirp force traj.",
"(string) force name",
"(int) force axis",
128 "(double) final force in N/Nm",
"(double) initial frequency [Hz]",
129 "(double) final frequency [Hz]",
"(double) trajectory time [sec]")));
131 addCommand(
"moveJoint",
133 docCommandVoid3(
"Move the joint to the specified angle with a minimum jerk trajectory.",
134 "(string) joint name",
"(double) final angle in radians",
135 "(double) time to reach the final angle in sec")));
140 docCommandVoid4(
"Move the force to the specified value with a minimum jerk trajectory.",
141 "(string) force name",
"(int) force axis",
"(double) final force in N/Nm",
142 "(double) time to reach the final force in sec")));
144 addCommand(
"stop", makeCommandVoid1(
147 "Stop the motion of the specified joint, or of all joints if no joint name is specified.",
148 "(string) joint name")));
151 docCommandVoid1(
"Stop the specified force trajectort",
152 "(string) force name (rh,lh,lf,rf)")));
157 std::string localName(robotRef);
158 if (isNameInRobotUtil(localName))
161 SEND_MSG(
"You should have an entity controller manager initialized before", MSG_TYPE_ERROR);
165 if (dt <= 0.0)
return SEND_MSG(
"Timestep must be positive", MSG_TYPE_ERROR);
178 for (
long unsigned int i = 0; i <
m_robot_util->m_nbJoints; i++) {
189 for (
int i = 0; i < 4; i++) {
204 if (!m_initSucceeded) {
205 SEND_WARNING_STREAM_MSG(
"Cannot compute signal positionDes before initialization!");
213 const dynamicgraph::Vector& base6d_encoders = m_base6d_encodersSIN(iter);
214 if (base6d_encoders.size() !=
static_cast<Eigen::VectorXd::Index
>(m_robot_util->m_nbJoints + 6)) {
215 SEND_ERROR_STREAM_MSG(
"Unexpected size of signal base6d_encoder " + toString(base6d_encoders.size()) +
" " +
216 toString(m_robot_util->m_nbJoints + 6));
219 for (
unsigned int i = 0; i < m_robot_util->m_nbJoints; i++)
220 m_noTrajGen[i]->set_initial_point(base6d_encoders(6 + i));
224 if (s.size() !=
static_cast<Eigen::VectorXd::Index
>(m_robot_util->m_nbJoints)) s.resize(m_robot_util->m_nbJoints);
226 if (m_status[0] == JTG_TEXT_FILE) {
227 const VectorXd& qRef = m_textFileTrajGen->compute_next_point();
228 for (
unsigned int i = 0; i < m_robot_util->m_nbJoints; i++) {
230 if (m_textFileTrajGen->isTrajectoryEnded()) {
231 m_noTrajGen[i]->set_initial_point(s(i));
232 m_status[i] = JTG_STOP;
235 if (m_textFileTrajGen->isTrajectoryEnded()) SEND_MSG(
"Text file trajectory ended.", MSG_TYPE_INFO);
237 for (
unsigned int i = 0; i < m_robot_util->m_nbJoints; i++) {
238 s(i) = m_currentTrajGen[i]->compute_next_point()(0);
239 if (m_currentTrajGen[i]->isTrajectoryEnded()) {
240 m_currentTrajGen[i] = m_noTrajGen[i];
241 m_noTrajGen[i]->set_initial_point(s(i));
242 m_status[i] = JTG_STOP;
243 SEND_MSG(
"Trajectory of joint " + m_robot_util->get_name_from_id(i) +
" ended.", MSG_TYPE_INFO);
254 if (!m_initSucceeded) {
255 SEND_WARNING_STREAM_MSG(
"Cannot compute signal positionDes before initialization! iter: " + toString(iter));
259 if (s.size() !=
static_cast<Eigen::VectorXd::Index
>(m_robot_util->m_nbJoints)) s.resize(m_robot_util->m_nbJoints);
260 if (m_status[0] == JTG_TEXT_FILE) {
261 for (
unsigned int i = 0; i < m_robot_util->m_nbJoints; i++) s(i) = m_textFileTrajGen->getVel()[i];
263 for (
unsigned int i = 0; i < m_robot_util->m_nbJoints; i++) s(i) = m_currentTrajGen[i]->getVel()(0);
269 if (!m_initSucceeded) {
270 SEND_WARNING_STREAM_MSG(
"Cannot compute signal positionDes before initialization!" + toString(iter));
274 if (s.size() !=
static_cast<Eigen::VectorXd::Index
>(m_robot_util->m_nbJoints)) s.resize(m_robot_util->m_nbJoints);
276 if (m_status[0] == JTG_TEXT_FILE) {
277 for (
unsigned int i = 0; i < m_robot_util->m_nbJoints; i++) s(i) = m_textFileTrajGen->getAcc()[i];
279 for (
unsigned int i = 0; i < m_robot_util->m_nbJoints; i++) s(i) = m_currentTrajGen[i]->getAcc()(0);
286 generateReferenceForceSignal(
"fRightFoot",
static_cast<int>(m_robot_util->m_force_util.get_force_id_right_foot()), s,
292 generateReferenceForceSignal(
"fLeftFoot",
static_cast<int>(m_robot_util->m_force_util.get_force_id_left_foot()), s,
298 generateReferenceForceSignal(
"fRightHand",
static_cast<int>(m_robot_util->m_force_util.get_force_id_right_hand()), s,
304 generateReferenceForceSignal(
"fLeftHand",
static_cast<int>(m_robot_util->m_force_util.get_force_id_left_hand()), s,
310 dynamicgraph::Vector& s,
int iter) {
312 SEND_WARNING_STREAM_MSG(
"Cannot compute signal " + forceName +
" before initialization!");
318 if (s.size() != 6) s.resize(6);
328 for (
unsigned int i = 0; i < 6; i++) s(i) = fr(i);
334 SEND_MSG(
"Trajectory of force " + forceName +
" ended.", MSG_TYPE_INFO);
349 const dynamicgraph::Vector& base6d_encoders = m_base6d_encodersSIN.accessCopy();
350 SEND_MSG(
"Current angle of joint " + jointName +
" is " + toString(base6d_encoders(6 + i)), MSG_TYPE_INFO);
356 for (
unsigned int i = 0; i <
m_robot_util->m_nbJoints; i++) {
359 SEND_MSG(
"Text file trajectory not ended.", MSG_TYPE_INFO);
364 for (
unsigned int i = 0; i <
m_robot_util->m_nbJoints; i++) {
367 SEND_MSG(
"Trajectory of joint " +
m_robot_util->get_name_from_id(i) +
"not ended.", MSG_TYPE_INFO);
376 if (!
m_initSucceeded)
return SEND_MSG(
"Cannot start sinusoid before initialization!", MSG_TYPE_ERROR);
378 for (
unsigned int i = 0; i <
m_robot_util->m_nbJoints; i++)
380 return SEND_MSG(
"You cannot joint " +
m_robot_util->get_name_from_id(i) +
" because it is already controlled.",
384 return SEND_MSG(
"Error while loading text file " + fileName, MSG_TYPE_ERROR);
387 bool needToMoveToInitConf =
false;
389 for (
unsigned int i = 0; i <
m_robot_util->m_nbJoints; i++)
391 needToMoveToInitConf =
true;
393 " is too far from initial configuration so first i will move it there.",
398 if (needToMoveToInitConf) {
399 for (
unsigned int i = 0; i <
m_robot_util->m_nbJoints; i++) {
411 for (
unsigned int i = 0; i <
m_robot_util->m_nbJoints; i++) {
417 if (!
m_initSucceeded)
return SEND_MSG(
"Cannot start sinusoid before initialization!", MSG_TYPE_ERROR);
421 if (time <= 0.0)
return SEND_MSG(
"Trajectory time must be a positive number", MSG_TYPE_ERROR);
423 return SEND_MSG(
"You cannot move the specified joint because it is already controlled.", MSG_TYPE_ERROR);
427 SEND_MSG(
"Set initial point of sinusoid to " + toString(
m_sinTrajGen[i]->getPos()), MSG_TYPE_DEBUG);
435 const double& Tacc) {
436 if (!
m_initSucceeded)
return SEND_MSG(
"Cannot start triangle before initialization!", MSG_TYPE_ERROR);
441 return SEND_MSG(
"You cannot move the specified joint because it is already controlled.", MSG_TYPE_ERROR);
445 SEND_MSG(
"Set initial point of triangular trajectory to " + toString(
m_triangleTrajGen[i]->getPos()),
450 return SEND_MSG(
"Trajectory time cannot be negative.", MSG_TYPE_ERROR);
453 return SEND_MSG(
"Acceleration time cannot be negative or larger than half the trajectory time.", MSG_TYPE_ERROR);
461 return SEND_MSG(
"Cannot start constant-acceleration trajectory before initialization!", MSG_TYPE_ERROR);
465 if (time <= 0.0)
return SEND_MSG(
"Trajectory time must be a positive number", MSG_TYPE_ERROR);
467 return SEND_MSG(
"You cannot move the specified joint because it is already controlled.", MSG_TYPE_ERROR);
471 SEND_MSG(
"Set initial point of const-acc trajectory to " + toString(
m_constAccTrajGen[i]->getPos()), MSG_TYPE_DEBUG);
479 const double& time) {
480 if (!
m_initSucceeded)
return SEND_MSG(
"Cannot start force sinusoid before initialization!", MSG_TYPE_ERROR);
484 if (time <= 0.0)
return SEND_MSG(
"Trajectory time must be a positive number", MSG_TYPE_ERROR);
485 if (axis < 0 || axis > 5)
return SEND_MSG(
"Axis must be between 0 and 5", MSG_TYPE_ERROR);
487 return SEND_MSG(
"You cannot move the specified force because it is already controlled.", MSG_TYPE_ERROR);
493 SEND_MSG(
"Set initial point of sinusoid to " + toString(
m_sinTrajGen_force[i]->getPos()), MSG_TYPE_DEBUG);
494 currentF[axis] = fFinal;
502 const double& f1,
const double& time) {
503 if (!
m_initSucceeded)
return SEND_MSG(
"Cannot start linear chirp before initialization!", MSG_TYPE_ERROR);
507 if (time <= 0.0)
return SEND_MSG(
"Trajectory time must be a positive number", MSG_TYPE_ERROR);
509 return SEND_MSG(
"You cannot move the specified joint because it is already controlled.", MSG_TYPE_ERROR);
511 if (f0 > f1)
return SEND_MSG(
"f0 " + toString(f0) +
" cannot to be more than f1 " + toString(f1), MSG_TYPE_ERROR);
512 if (f0 <= 0.0)
return SEND_MSG(
"Frequency has to be positive " + toString(f0), MSG_TYPE_ERROR);
515 return SEND_MSG(
"Error while setting initial point " + toString(
m_noTrajGen[i]->getPos()), MSG_TYPE_ERROR);
517 return SEND_MSG(
"Error while setting final point " + toString(qFinal), MSG_TYPE_ERROR);
519 return SEND_MSG(
"Error while setting trajectory time " + toString(time), MSG_TYPE_ERROR);
521 return SEND_MSG(
"Error while setting initial frequency " + toString(f0), MSG_TYPE_ERROR);
523 return SEND_MSG(
"Error while setting final frequency " + toString(f1), MSG_TYPE_ERROR);
529 const double& f0,
const double& f1,
const double& time) {
530 if (!
m_initSucceeded)
return SEND_MSG(
"Cannot start force linear chirp before initialization!", MSG_TYPE_ERROR);
534 if (time <= 0.0)
return SEND_MSG(
"Trajectory time must be a positive number", MSG_TYPE_ERROR);
536 return SEND_MSG(
"You cannot move the specified force because it is already controlled.", MSG_TYPE_ERROR);
538 if (f0 > f1)
return SEND_MSG(
"f0 " + toString(f0) +
" cannot to be more than f1 " + toString(f1), MSG_TYPE_ERROR);
539 if (f0 <= 0.0)
return SEND_MSG(
"Frequency has to be positive " + toString(f0), MSG_TYPE_ERROR);
543 return SEND_MSG(
"Error while setting initial point " + toString(
m_noTrajGen_force[i]->getPos()), MSG_TYPE_ERROR);
544 currentF[axis] = fFinal;
546 return SEND_MSG(
"Error while setting final point " + toString(fFinal), MSG_TYPE_ERROR);
548 return SEND_MSG(
"Error while setting trajectory time " + toString(time), MSG_TYPE_ERROR);
550 return SEND_MSG(
"Error while setting initial frequency " + toString(f0), MSG_TYPE_ERROR);
552 return SEND_MSG(
"Error while setting final frequency " + toString(f1), MSG_TYPE_ERROR);
558 if (!
m_initSucceeded)
return SEND_MSG(
"Cannot move joint before initialization!", MSG_TYPE_ERROR);
562 if (time <= 0.0)
return SEND_MSG(
"Trajectory time must be a positive number", MSG_TYPE_ERROR);
564 return SEND_MSG(
"You cannot move the specified joint because it is already controlled.", MSG_TYPE_ERROR);
575 const double& time) {
576 if (!
m_initSucceeded)
return SEND_MSG(
"Cannot move force before initialization!", MSG_TYPE_ERROR);
580 if (time <= 0.0)
return SEND_MSG(
"Trajectory time must be a positive number", MSG_TYPE_ERROR);
582 return SEND_MSG(
"You cannot move the specified force because it is already controlled.", MSG_TYPE_ERROR);
587 currentF[axis] = fFinal;
595 if (!
m_initSucceeded)
return SEND_MSG(
"Cannot stop joint before initialization!", MSG_TYPE_ERROR);
597 const dynamicgraph::Vector& base6d_encoders = m_base6d_encodersSIN.accessCopy();
598 if (jointName ==
"all") {
599 for (
unsigned int i = 0; i <
m_robot_util->m_nbJoints; i++) {
602 m_noTrajGen[i]->set_initial_point(base6d_encoders(6 + i));
610 m_noTrajGen[i]->set_initial_point(base6d_encoders(6 + i));
616 if (!
m_initSucceeded)
return SEND_MSG(
"Cannot stop force before initialization!", MSG_TYPE_ERROR);
633 SEND_MSG(
"The specified joint name does not exist", MSG_TYPE_ERROR);
634 std::stringstream ss;
635 for (map<string, Index>::const_iterator it =
m_robot_util->m_name_to_id.begin();
637 ss << it->first <<
", ";
638 SEND_MSG(
"Possible joint names are: " + ss.str(), MSG_TYPE_INFO);
641 id =
static_cast<unsigned int>(jid);
647 Index fid =
m_robot_util->m_force_util.get_id_from_name(name);
649 SEND_MSG(
"The specified force name does not exist", MSG_TYPE_ERROR);
650 std::stringstream ss;
651 for (map<string, Index>::const_iterator it =
m_robot_util->m_force_util.m_name_to_force_id.begin();
652 it !=
m_robot_util->m_force_util.m_name_to_force_id.end(); it++)
653 ss << it->first <<
", ";
654 SEND_MSG(
"Possible force names are: " + ss.str(), MSG_TYPE_INFO);
657 id = (
unsigned int)fid;
662 JointLimits jl =
m_robot_util->get_joint_limits_from_id(
id);
664 SEND_MSG(
"Joint " +
m_robot_util->get_name_from_id(
id) +
", desired angle " + toString(q) +
665 " is smaller than lower limit " + toString(jl.lower),
670 SEND_MSG(
"Joint " +
m_robot_util->get_name_from_id(
id) +
", desired angle " + toString(q) +
671 " is larger than upper limit " + toString(jl.upper),
679 ForceLimits fl =
m_robot_util->m_force_util.get_limits_from_id(
id);
680 for (
unsigned int i = 0; i < 6; i++)
681 if (f[i] < fl.lower[i] || f[i] > fl.upper[i]) {
682 SEND_MSG(
"Desired force " + toString(i) +
" is out of range: " + toString(fl.lower[i]) +
" < " + toString(f[i]) +
683 " < " + toString(fl.upper[i]),
691 ForceLimits fl =
m_robot_util->m_force_util.get_limits_from_id(
id);
692 if (f < fl.lower[axis] || f > fl.upper[axis]) {
693 SEND_MSG(
"Desired force " + toString(axis) +
" is out of range: " + toString(fl.lower[axis]) +
" < " +
694 toString(f) +
" < " + toString(fl.upper[axis]),
706 os <<
"JointTrajectoryGenerator " << getName();
708 getProfiler().report_all(3, os);
709 }
catch (ExceptionSignal e) {
virtual const Eigen::VectorXd & get_initial_point()
virtual bool isTrajectoryEnded()
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)
std::vector< SinusoidTrajectoryGenerator * > m_sinTrajGen
void startSinusoid(const std::string &jointName, const double &qFinal, const double &time)
std::vector< AbstractTrajectoryGenerator * > m_currentTrajGen_force
status of the forces
RobotUtilShrPtr m_robot_util
control loop time period
void startLinearChirp(const std::string &jointName, const double &qFinal, const double &f0, const double &f1, const double &time)
void stopForce(const std::string &forceName)
bool isJointInRange(unsigned int id, double q)
TextFileTrajectoryGenerator * m_textFileTrajGen
bool convertForceNameToForceId(const std::string &name, unsigned int &id)
std::vector< LinearChirpTrajectoryGenerator * > m_linChirpTrajGen
bool isForceInRange(unsigned int id, const Eigen::VectorXd &f)
std::vector< JTG_Status > m_status_force
bool generateReferenceForceSignal(const std::string &forceName, int fid, dynamicgraph::Vector &s, int iter)
std::vector< LinearChirpTrajectoryGenerator * > m_linChirpTrajGen_force
std::vector< NoTrajectoryGenerator * > m_noTrajGen_force
std::vector< MinimumJerkTrajectoryGenerator * > m_minJerkTrajGen_force
double m_dt
true if it is the first iteration, false otherwise
void stop(const std::string &jointName)
std::vector< ConstantAccelerationTrajectoryGenerator * > m_constAccTrajGen
virtual void display(std::ostream &os) const
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)
JointTrajectoryGenerator(const std::string &name)
std::vector< TriangleTrajectoryGenerator * > m_triangleTrajGen
std::vector< JTG_Status > m_status
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)
std::vector< SinusoidTrajectoryGenerator * > m_sinTrajGen_force
std::vector< NoTrajectoryGenerator * > m_noTrajGen
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)
void getJoint(const std::string &jointName)
std::vector< MinimumJerkTrajectoryGenerator * > m_minJerkTrajGen
std::vector< int > m_iterForceSignals
void playTrajectoryFile(const std::string &fileName)
virtual bool loadTextFile(const std::string &fileName)
#define PROFILE_FORCE_DESIRED_COMPUTATION
#define PROFILE_POSITION_DESIRED_COMPUTATION
CommandVoid5< E, T1, T2, T3, T4, T5 > * makeCommandVoid5(E &entity, typename CommandVoid5< E, T1, T2, T3, T4, T5 >::function_t function, const std::string &docString)
CommandVoid6< E, T1, T2, T3, T4, T5, T6 > * makeCommandVoid6(E &entity, typename CommandVoid6< E, T1, T2, T3, T4, T5, T6 >::function_t function, const std::string &docString)
std::string docCommandVoid6(const std::string &doc, const std::string &type1, const std::string &type2, const std::string &type3, const std::string &type4, const std::string &type5, const std::string &type6)
std::string docCommandVoid5(const std::string &doc, const std::string &type1, const std::string &type2, const std::string &type3, const std::string &type4, const std::string &type5)
DEFINE_SIGNAL_OUT_FUNCTION(u, dynamicgraph::Vector)
AdmittanceController EntityClassName
DYNAMICGRAPH_FACTORY_ENTITY_PLUGIN(AdmittanceController, "AdmittanceController")