6 #include <dynamic-graph/factory.h>
7 #include <sot/core/debug.hh>
11 #include <sot/core/stop-watch.hh>
16 namespace dg = ::dynamicgraph;
18 using namespace dg::command;
20 using namespace Eigen;
22 #define PROFILE_SE3_POSITION_DESIRED_COMPUTATION "SE3TrajGen: traj computation"
36 CONSTRUCT_SIGNAL_IN(initial_value,
dynamicgraph::Vector),
38 CONSTRUCT_SIGNAL_IN(trigger, bool),
40 CONSTRUCT_SIGNAL_OUT(ddx,
dynamicgraph::Vector, m_xSOUT),
41 m_initSucceeded(false),
47 m_splineReady(false) {
48 BIND_SIGNAL_TO_FUNCTION(x, OUT, dynamicgraph::Vector);
50 Entity::signalRegistration(m_xSOUT << m_dxSOUT << m_ddxSOUT << m_initial_valueSIN << m_triggerSIN);
54 docCommandVoid1(
"Initialize the entity.",
"Time period in seconds (double)")));
56 addCommand(
"getValue",
58 docCommandVoid1(
"Get the current value of the specified index.",
"index (int)")));
60 addCommand(
"playTrajectoryFile",
62 docCommandVoid1(
"Play the trajectory read from the specified text file.",
63 "(string) File name, path included")));
64 addCommand(
"setSpline",
66 docCommandVoid3(
"Load serialized spline from file",
"(string) filename",
67 "(double) time to initial conf",
"(Matrix) orientation of the point")));
70 docCommandVoid3(
"Start an infinite sinusoid motion.",
"(int) index",
71 "(double) final value",
72 "(double) time to reach the final value in sec")));
74 addCommand(
"startTriangle",
76 docCommandVoid4(
"Start an infinite triangle wave.",
"(int) index",
77 "(double) final values",
"(double) time to reach the final value in sec",
78 "(double) time to accelerate in sec")));
80 addCommand(
"startConstAcc",
82 docCommandVoid3(
"Start an infinite trajectory with piece-wise constant acceleration.",
83 "(int) index",
"(double) final values",
84 "(double) time to reach the final value in sec")));
86 addCommand(
"startLinChirp",
89 "(double) final values",
"(double) initial frequency [Hz]",
90 "(double) final frequency [Hz]",
"(double) trajectory time [sec]")));
95 "Move component corresponding to index to the specified value with a minimum jerk trajectory.",
96 "(int) index",
"(double) final values",
"(double) time to reach the final value in sec")));
102 "Stop the motion of the specified index, or of all components of the vector if index is equal to -1.",
107 if (dt <= 0.0)
return SEND_MSG(
"Timestep must be positive", MSG_TYPE_ERROR);
118 for (
unsigned int i = 0; i <
m_np; i++) {
127 m_splineTrajGen =
new parametriccurves::Spline<double, Eigen::Dynamic>();
137 if (!m_initSucceeded) {
138 SEND_WARNING_STREAM_MSG(
"Cannot compute signal positionDes before initialization!");
144 if (s.size() != m_np) s.resize(m_np);
148 const dynamicgraph::Vector& initial_value = m_initial_valueSIN(iter);
149 if (initial_value.size() != m_np) {
150 SEND_ERROR_STREAM_MSG(
"Unexpected size of input signal initial_value: " + toString(initial_value.size()));
154 for (
unsigned int i = 0; i < m_np; i++) m_currentTrajGen[i]->set_initial_point(initial_value(i));
156 }
else if (iter ==
static_cast<int>(m_iterLast)) {
157 if (m_triggerSIN(iter) ==
true && m_splineReady) startSpline();
158 if (m_status[0] == TG_TEXT_FILE) {
159 for (
unsigned int i = 0; i < m_np; i++) s(i) = m_textFileTrajGen->getPos()[i];
160 }
else if (m_status[0] == TG_SPLINE) {
161 const Eigen::Vector3d& t = (*m_splineTrajGen)(m_t);
163 s.segment<3>(3) = m_splineRotation.row(0);
164 s.segment<3>(6) = m_splineRotation.row(1);
165 s.segment<3>(9) = m_splineRotation.row(2);
167 for (
unsigned int i = 0; i < m_np; i++) s(i) = m_currentTrajGen[i]->getPos()(0);
172 if (m_triggerSIN(iter) ==
true && m_splineReady) startSpline();
173 if (m_status[0] == TG_TEXT_FILE) {
174 const VectorXd& xRef = m_textFileTrajGen->compute_next_point();
175 for (
unsigned int i = 0; i < m_np; i++) {
177 if (m_textFileTrajGen->isTrajectoryEnded()) {
178 m_noTrajGen[i]->set_initial_point(s(i));
179 m_status[i] = TG_STOP;
182 if (m_textFileTrajGen->isTrajectoryEnded()) SEND_MSG(
"Text file trajectory ended.", MSG_TYPE_INFO);
183 }
else if (m_status[0] == TG_SPLINE) {
185 if (!m_splineTrajGen->checkRange(m_t)) {
186 const Eigen::Vector3d& t = (*m_splineTrajGen)(m_splineTrajGen->tmax());
188 s.segment<3>(3) = m_splineRotation.row(0);
189 s.segment<3>(6) = m_splineRotation.row(1);
190 s.segment<3>(9) = m_splineRotation.row(2);
191 for (
unsigned int i = 0; i < m_np; i++) {
192 m_noTrajGen[i]->set_initial_point(s(i));
193 m_status[i] = TG_STOP;
195 m_splineReady =
false;
196 SEND_MSG(
"Spline trajectory ended. Remember to turn off the trigger.", MSG_TYPE_INFO);
199 const Eigen::Vector3d& t = (*m_splineTrajGen)(m_t);
201 s.segment<3>(3) = m_splineRotation.row(0);
202 s.segment<3>(6) = m_splineRotation.row(1);
203 s.segment<3>(9) = m_splineRotation.row(2);
206 for (
unsigned int i = 0; i < m_np; i++) {
207 s(i) = m_currentTrajGen[i]->compute_next_point()(0);
208 if (m_currentTrajGen[i]->isTrajectoryEnded()) {
209 m_currentTrajGen[i] = m_noTrajGen[i];
210 m_noTrajGen[i]->set_initial_point(s(i));
211 m_status[i] = TG_STOP;
212 SEND_MSG(
"Trajectory of index " + toString(i) +
" ended.", MSG_TYPE_INFO);
223 if (!m_initSucceeded) {
224 std::ostringstream oss(
"Cannot compute signal positionDes before initialization! iter:");
226 SEND_WARNING_STREAM_MSG(oss.str());
230 if (s.size() != m_nv) s.resize(m_nv);
231 if (m_status[0] == TG_TEXT_FILE) {
232 for (
unsigned int i = 0; i < m_nv; i++) s(i) = m_textFileTrajGen->getVel()[i];
233 }
else if (m_status[0] == TG_SPLINE) {
234 const Eigen::Vector3d& t = m_splineTrajGen->derivate(m_t, 1);
236 s.segment<3>(3).setZero();
238 for (
unsigned int i = 0; i < m_nv; i++) s(i) = m_currentTrajGen[i]->getVel()(0);
244 if (!m_initSucceeded) {
245 std::ostringstream oss(
"Cannot compute signal positionDes before initialization! iter:");
247 SEND_WARNING_STREAM_MSG(oss.str());
251 if (s.size() != m_nv) s.resize(m_nv);
253 if (m_status[0] == TG_TEXT_FILE) {
254 for (
unsigned int i = 0; i < m_nv; i++) s(i) = m_textFileTrajGen->getAcc()[i];
255 }
else if (m_status[0] == TG_SPLINE) {
256 const Eigen::Vector3d& t = m_splineTrajGen->derivate(m_t, 2);
258 s.segment<3>(3).setZero();
260 for (
unsigned int i = 0; i < m_nv; i++) s(i) = m_currentTrajGen[i]->getAcc()(0);
270 if (id < 0 || id >=
static_cast<int>(
m_np))
return SEND_MSG(
"Index is out of bounds", MSG_TYPE_ERROR);
272 SEND_MSG(
"Current value of component " + toString(
id) +
" is " + toString(
m_currentTrajGen[
id]->getPos()(0)),
277 if (!
m_initSucceeded)
return SEND_MSG(
"Cannot start sinusoid before initialization!", MSG_TYPE_ERROR);
279 for (
unsigned int i = 0; i <
m_np; i++)
281 return SEND_MSG(
"You cannot control component " + toString(i) +
" because it is already controlled.",
285 return SEND_MSG(
"Error while loading text file " + fileName, MSG_TYPE_ERROR);
288 bool needToMoveToInitConf =
false;
290 for (
unsigned int i = 0; i <
m_np; i++)
292 needToMoveToInitConf =
true;
293 SEND_MSG(
"Component " + toString(i) +
" is too far from initial configuration so first i will move it there.",
298 if (needToMoveToInitConf) {
299 for (
unsigned int i = 0; i <
m_np; i++) {
309 for (
unsigned int i = 0; i <
m_np; i++) {
315 const Eigen::MatrixXd& init_rotation) {
316 if (!
m_initSucceeded)
return SEND_MSG(
"Cannot start sinusoid before initialization!", MSG_TYPE_ERROR);
318 for (
unsigned int i = 0; i <
m_np; i++)
320 return SEND_MSG(
"You cannot control component " + toString(i) +
" because it is already controlled.",
324 return SEND_MSG(
"Error while loading text file " + fileName, MSG_TYPE_ERROR);
327 bool needToMoveToInitConf =
false;
330 if (timeToInitConf < 0) {
331 SEND_MSG(
"Spline Ready. Set trigger to true to start playing", MSG_TYPE_INFO);
335 const VectorXd& t = (*m_splineTrajGen)(0.0);
342 for (
unsigned int i = 0; i <
m_np; i++)
344 needToMoveToInitConf =
true;
345 SEND_MSG(
"Component " + toString(i) +
" is too far from initial configuration so first i will move it there.",
350 if (needToMoveToInitConf) {
351 for (
unsigned int i = 0; i <
m_np; i++) {
361 SEND_MSG(
"Spline Ready. Set trigger to true to start playing", MSG_TYPE_INFO);
367 for (
unsigned int i = 0; i <
m_np; i++) {
373 if (!
m_initSucceeded)
return SEND_MSG(
"Cannot start sinusoid before initialization!", MSG_TYPE_ERROR);
375 if (id < 0 || id >=
static_cast<int>(
m_np))
return SEND_MSG(
"Index is out of bounds", MSG_TYPE_ERROR);
377 if (time <= 0.0)
return SEND_MSG(
"Trajectory time must be a positive number", MSG_TYPE_ERROR);
379 return SEND_MSG(
"You cannot move the specified component because it is already controlled.", MSG_TYPE_ERROR);
382 SEND_MSG(
"Set initial point of sinusoid to " + toString(
m_sinTrajGen[i]->getPos()), MSG_TYPE_DEBUG);
390 const double& Tacc) {
391 if (!
m_initSucceeded)
return SEND_MSG(
"Cannot start triangle before initialization!", MSG_TYPE_ERROR);
392 if (id < 0 || id >=
static_cast<int>(
m_np))
return SEND_MSG(
"Index is out of bounds", MSG_TYPE_ERROR);
395 return SEND_MSG(
"You cannot move the specified component because it is already controlled.", MSG_TYPE_ERROR);
398 SEND_MSG(
"Set initial point of triangular trajectory to " + toString(
m_triangleTrajGen[i]->getPos()),
403 return SEND_MSG(
"Trajectory time cannot be negative.", MSG_TYPE_ERROR);
406 return SEND_MSG(
"Acceleration time cannot be negative or larger than half the trajectory time.", MSG_TYPE_ERROR);
414 return SEND_MSG(
"Cannot start constant-acceleration trajectory before initialization!", MSG_TYPE_ERROR);
415 if (id < 0 || id >=
static_cast<int>(
m_np))
return SEND_MSG(
"Index is out of bounds", MSG_TYPE_ERROR);
417 if (time <= 0.0)
return SEND_MSG(
"Trajectory time must be a positive number", MSG_TYPE_ERROR);
419 return SEND_MSG(
"You cannot move the specified component because it is already controlled.", MSG_TYPE_ERROR);
422 SEND_MSG(
"Set initial point of const-acc trajectory to " + toString(
m_constAccTrajGen[i]->getPos()), MSG_TYPE_DEBUG);
430 const double& time) {
431 if (!
m_initSucceeded)
return SEND_MSG(
"Cannot start linear chirp before initialization!", MSG_TYPE_ERROR);
432 if (id < 0 || id >=
static_cast<int>(
m_np))
return SEND_MSG(
"Index is out of bounds", MSG_TYPE_ERROR);
434 if (time <= 0.0)
return SEND_MSG(
"Trajectory time must be a positive number", MSG_TYPE_ERROR);
436 return SEND_MSG(
"You cannot move the specified component because it is already controlled.", MSG_TYPE_ERROR);
437 if (f0 > f1)
return SEND_MSG(
"f0 " + toString(f0) +
" cannot to be more than f1 " + toString(f1), MSG_TYPE_ERROR);
438 if (f0 <= 0.0)
return SEND_MSG(
"Frequency has to be positive " + toString(f0), MSG_TYPE_ERROR);
441 return SEND_MSG(
"Error while setting initial point " + toString(
m_noTrajGen[i]->getPos()), MSG_TYPE_ERROR);
443 return SEND_MSG(
"Error while setting final point " + toString(xFinal), MSG_TYPE_ERROR);
445 return SEND_MSG(
"Error while setting trajectory time " + toString(time), MSG_TYPE_ERROR);
447 return SEND_MSG(
"Error while setting initial frequency " + toString(f0), MSG_TYPE_ERROR);
449 return SEND_MSG(
"Error while setting final frequency " + toString(f1), MSG_TYPE_ERROR);
455 if (!
m_initSucceeded)
return SEND_MSG(
"Cannot move value before initialization!", MSG_TYPE_ERROR);
457 if (id < 0 || id >=
static_cast<int>(
m_np))
return SEND_MSG(
"Index is out of bounds", MSG_TYPE_ERROR);
458 if (time <= 0.0)
return SEND_MSG(
"Trajectory time must be a positive number", MSG_TYPE_ERROR);
460 return SEND_MSG(
"You cannot move the specified component because it is already controlled.", MSG_TYPE_ERROR);
470 if (!
m_initSucceeded)
return SEND_MSG(
"Cannot stop value before initialization!", MSG_TYPE_ERROR);
474 for (
unsigned int i = 0; i <
m_np; i++) {
482 if (id < 0 || id >=
static_cast<int>(
m_np))
return SEND_MSG(
"Index is out of bounds", MSG_TYPE_ERROR);
497 os <<
"SE3TrajectoryGenerator " << getName();
499 getProfiler().report_all(3, os);
500 }
catch (ExceptionSignal e) {
virtual const Eigen::VectorXd & get_initial_point()
void move(const int &id, const double &xFinal, const double &time)
Eigen::Matrix3d m_splineRotation
std::vector< SinusoidTrajectoryGenerator * > m_sinTrajGen
double m_t
last iter index
void setSpline(const std::string &filename, const double &timeToInitConf, const Eigen::MatrixXd &init_rotation)
TextFileTrajectoryGenerator * m_textFileTrajGen
void init(const double &dt)
std::vector< LinearChirpTrajectoryGenerator * > m_linChirpTrajGen
double m_dt
true if it is the first iteration, false otherwise
std::vector< ConstantAccelerationTrajectoryGenerator * > m_constAccTrajGen
void startTriangle(const int &id, const double &xFinal, const double &time, const double &Tacc)
virtual void display(std::ostream &os) const
parametriccurves::Spline< double, Eigen::Dynamic > * m_splineTrajGen
void startLinearChirp(const int &id, const double &xFinal, const double &f0, const double &f1, const double &time)
void startConstAcc(const int &id, const double &xFinal, const double &time)
void startSinusoid(const int &id, const double &xFinal, const double &time)
std::vector< TG_Status > m_status
bool m_firstIter
true if the entity has been successfully initialized
std::vector< TriangleTrajectoryGenerator * > m_triangleTrajGen
std::vector< NoTrajectoryGenerator * > m_noTrajGen
std::vector< AbstractTrajectoryGenerator * > m_currentTrajGen
status of the component
void getValue(const int &id)
std::vector< MinimumJerkTrajectoryGenerator * > m_minJerkTrajGen
unsigned int m_np
control loop time period
SE3TrajectoryGenerator(const std::string &name)
void playTrajectoryFile(const std::string &fileName)
virtual bool loadTextFile(const std::string &fileName)
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)
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")
#define PROFILE_SE3_POSITION_DESIRED_COMPUTATION