20 #include <sot/core/debug.hh> 21 #include <dynamic-graph/factory.h> 24 #include <sot/core/stop-watch.hh> 30 namespace talos_balance
36 using namespace Eigen;
38 #define PROFILE_ND_POSITION_DESIRED_COMPUTATION "NdTrajGen: traj computation" 39 #define DOUBLE_INF std::numeric_limits<double>::max() 46 "NdTrajectoryGenerator");
55 ,CONSTRUCT_SIGNAL_IN(trigger,bool)
59 ,m_initSucceeded(false)
68 Entity::signalRegistration( m_xSOUT << m_dxSOUT << m_ddxSOUT << m_initial_valueSIN
74 docCommandVoid2(
"Initialize the entity.",
75 "Time period in seconds (double)",
76 "size of output vector signal (int)")));
78 addCommand(
"getValue",
80 docCommandVoid1(
"Get the current value of the specified index.",
83 addCommand(
"playTrajectoryFile",
85 docCommandVoid1(
"Play the trajectory read from the specified text file.",
86 "(string) File name, path included")));
88 addCommand(
"startSinusoid",
90 docCommandVoid3(
"Start an infinite sinusoid motion.",
92 "(double) final value",
93 "(double) time to reach the final value in sec")));
95 addCommand(
"setSpline",
97 docCommandVoid2(
"Load serialized spline from file",
99 "(double) time to initial conf")));
109 addCommand(
"startConstAcc",
111 docCommandVoid3(
"Start an infinite trajectory with piece-wise constant acceleration.",
113 "(double) final values",
114 "(double) time to reach the final value in sec")));
116 addCommand(
"startLinChirp",
120 "(double) final values",
121 "(double) initial frequency [Hz]",
122 "(double) final frequency [Hz]",
123 "(double) trajectory time [sec]")));
126 docCommandVoid3(
"Move component corresponding to index to the specified value with a minimum jerk trajectory.",
128 "(double) final values",
129 "(double) time to reach the final value in sec")));
132 docCommandVoid2(
"Instantaneously set component corresponding to index to the specified value.",
134 "(double) desired value")));
137 docCommandVoid1(
"Stop the motion of the specified index, or of all components of the vector if index is equal to -1.",
145 return SEND_MSG(
"Timestep must be positive", MSG_TYPE_ERROR);
147 return SEND_MSG(
"n must be at least 1", MSG_TYPE_ERROR);
159 for(
unsigned i=0; i<
m_n; i++)
162 m_sinTrajGen[i] =
new parametriccurves::InfiniteSinusoid<double,1>(5.0);
166 m_noTrajGen[i] =
new parametriccurves::Constant<double,1>(5.0);
174 m_splineTrajGen =
new parametriccurves::Spline<double,Eigen::Dynamic>();
175 m_textFileTrajGen =
new parametriccurves::TextFile<double, Eigen::Dynamic>(dt, n);
177 NdTrajectoryGenerator::setLoggerVerbosityLevel(dynamicgraph::LoggerVerbosity::VERBOSITY_ALL);
189 SEND_WARNING_STREAM_MSG(
"Cannot compute signal positionDes before initialization!");
202 if(initial_value.size()!=
m_n)
204 SEND_MSG(
"Unexpected size of input signal initial_value: "+toString(initial_value.size()),MSG_TYPE_ERROR);
208 for(
unsigned int i=0; i<
m_n; i++)
217 s = (*m_textFileTrajGen)(
m_t);
221 s = (*m_splineTrajGen)(
m_t);
224 for(
unsigned int i=0; i<
m_n; i++)
231 const bool & isTriggered = m_triggerSIN(iter);
243 for(
unsigned int i=0; i<
m_n; i++)
251 for(
unsigned int i=0; i<
m_n; i++)
256 SEND_MSG(
"Text file trajectory ended.", MSG_TYPE_INFO);
260 s = (*m_textFileTrajGen)(
m_t);
267 for(
unsigned int i=0; i<
m_n; i++)
273 SEND_MSG(
"Spline trajectory ended. Remember to turn off the trigger.", MSG_TYPE_INFO);
277 s = (*m_splineTrajGen)(
m_t);
281 for(
unsigned int i=0; i<
m_n; i++)
289 SEND_MSG(
"Trajectory of index "+toString(i)+
" ended.", MSG_TYPE_INFO);
305 SEND_WARNING_STREAM_MSG(
"Cannot compute signal positionDes before initialization!");
315 const bool & isTriggered = m_triggerSIN(iter);
327 for(
unsigned int i=0; i<
m_n; i++)
337 SEND_WARNING_STREAM_MSG(
"Cannot compute signal positionDes before initialization!");
347 const bool & isTriggered = m_triggerSIN(iter);
359 for(
unsigned int i=0; i<
m_n; i++)
371 if(id<0 || id>=static_cast<int>(
m_n))
372 return SEND_MSG(
"Index is out of bounds", MSG_TYPE_ERROR);
374 SEND_MSG(
"Current value of component "+toString(
id)+
" is "+toString( (*
m_currentTrajGen[
id])(
m_t)[0]) , MSG_TYPE_INFO);
380 return SEND_MSG(
"Cannot start trajectory before initialization!",MSG_TYPE_ERROR);
382 for(
unsigned int i=0; i<
m_n; i++)
384 return SEND_MSG(
"You cannot control component "+toString(i)+
" because it is already controlled.", MSG_TYPE_ERROR);
388 std::string msg(
"Error while loading text file "+fileName);
389 SEND_MSG(msg, MSG_TYPE_ERROR);
390 throw std::runtime_error(msg);
394 bool needToMoveToInitConf =
false;
396 for(
unsigned int i=0; i<
m_n; i++)
399 if(fabs(xInit[i] - currentVal) > 0.001)
401 needToMoveToInitConf =
true;
403 SEND_MSG(
"Component "+ toString(i) +
" is far from initial configuration (" + toString(xInit[i]) +
"->" + toString(currentVal) +
")", MSG_TYPE_WARNING);
427 for(
unsigned int i=0; i<
m_n; i++)
436 return SEND_MSG(
"Cannot start spline before initialization!",MSG_TYPE_ERROR);
438 for(
unsigned int i=0; i<
m_n; i++)
440 return SEND_MSG(
"You cannot control component " +toString(i)+
" because it is already controlled.",MSG_TYPE_ERROR);
443 return SEND_MSG(
"Error while loading spline"+filename, MSG_TYPE_ERROR);
445 SEND_MSG(
"Spline set to "+filename+
". Now checking initial position", MSG_TYPE_INFO);
447 bool needToMoveToInitConf =
false;
448 if(timeToInitConf < 0)
451 SEND_MSG(
"Spline Ready. Set trigger to true to start playing", MSG_TYPE_INFO);
456 const VectorXd& xInit = (*m_splineTrajGen)(0.0);
457 assert(xInit.size() ==
m_n);
458 for(
unsigned int i=0; i<
m_n; i++)
461 needToMoveToInitConf =
true;
462 SEND_MSG(
"Component "+ toString(i) +
" is too far from initial configuration so first i will move it there.", MSG_TYPE_WARNING);
465 if(needToMoveToInitConf)
467 for(
unsigned int i=0; i<
m_n; i++)
481 SEND_MSG(
"Spline Ready. Set trigger to true to start playing", MSG_TYPE_INFO);
488 for(
unsigned int i=0; i<
m_n; i++)
497 return SEND_MSG(
"Cannot start sinusoid before initialization!",MSG_TYPE_ERROR);
499 if(id<0 || id>=static_cast<int>(
m_n))
500 return SEND_MSG(
"Index is out of bounds", MSG_TYPE_ERROR);
503 return SEND_MSG(
"Trajectory time must be a positive number", MSG_TYPE_ERROR);
505 return SEND_MSG(
"You cannot move the specified component because it is already controlled.", MSG_TYPE_ERROR);
512 SEND_MSG(
"Set initial point of sinusoid to "+toString((*
m_noTrajGen[i])(
m_t)[0]),MSG_TYPE_DEBUG);
548 return SEND_MSG(
"Cannot start constant-acceleration trajectory before initialization!",MSG_TYPE_ERROR);
549 if(id<0 || id>=static_cast<int>(
m_n))
550 return SEND_MSG(
"Index is out of bounds", MSG_TYPE_ERROR);
553 return SEND_MSG(
"Trajectory time must be a positive number", MSG_TYPE_ERROR);
555 return SEND_MSG(
"You cannot move the specified component because it is already controlled.", MSG_TYPE_ERROR);
560 SEND_MSG(
"Set initial point of const-acc trajectory to "+toString((*
m_noTrajGen[i])(
m_t)[0]),MSG_TYPE_DEBUG);
570 return SEND_MSG(
"Cannot start linear chirp before initialization!",MSG_TYPE_ERROR);
571 if(id<0 || id>=static_cast<int>(
m_n))
572 return SEND_MSG(
"Index is out of bounds", MSG_TYPE_ERROR);
575 return SEND_MSG(
"Trajectory time must be a positive number", MSG_TYPE_ERROR);
577 return SEND_MSG(
"You cannot move the specified component because it is already controlled.", MSG_TYPE_ERROR);
581 return SEND_MSG(
"f0 "+toString(f0)+
" cannot to be more than f1 "+toString(f1),MSG_TYPE_ERROR);
583 return SEND_MSG(
"Frequency has to be positive "+toString(f0),MSG_TYPE_ERROR);
586 return SEND_MSG(
"Error while setting initial point "+toString((*
m_noTrajGen[i])(
m_t)[0]), MSG_TYPE_ERROR);
588 return SEND_MSG(
"Error while setting final point "+toString(xFinal), MSG_TYPE_ERROR);
590 return SEND_MSG(
"Error while setting trajectory time "+toString(time), MSG_TYPE_ERROR);
592 return SEND_MSG(
"Error while setting initial frequency "+toString(f0), MSG_TYPE_ERROR);
594 return SEND_MSG(
"Error while setting final frequency "+toString(f1), MSG_TYPE_ERROR);
603 return SEND_MSG(
"Cannot move value before initialization!",MSG_TYPE_ERROR);
605 if(id<0 || id>=static_cast<int>(
m_n))
606 return SEND_MSG(
"Index is out of bounds", MSG_TYPE_ERROR);
608 return SEND_MSG(
"Trajectory time must be a positive number", MSG_TYPE_ERROR);
610 return SEND_MSG(
"You cannot move the specified component because it is already controlled.", MSG_TYPE_ERROR);
625 return SEND_MSG(
"Cannot set value before initialization!",MSG_TYPE_ERROR);
627 if(id<0 || id>=static_cast<int>(
m_n))
628 return SEND_MSG(
"Index is out of bounds", MSG_TYPE_ERROR);
630 return SEND_MSG(
"You cannot set the specified component because it is already controlled.", MSG_TYPE_ERROR);
643 return SEND_MSG(
"Cannot stop value before initialization!",MSG_TYPE_ERROR);
647 for(
unsigned int i=0; i<
m_n; i++)
657 if(id<0 || id>=static_cast<int>(
m_n))
658 return SEND_MSG(
"Index is out of bounds", MSG_TYPE_ERROR);
694 os <<
"NdTrajectoryGenerator "<<getName();
697 getProfiler().report_all(3, os);
699 catch (ExceptionSignal e) {}
void startSinusoid(const int &id, const double &xFinal, const double &time)
std::vector< parametriccurves::LinearChirp< double, 1 > * > m_linChirpTrajGen
std::vector< parametriccurves::InfiniteConstAcc< double, 1 > *> m_constAccTrajGen
Eigen::Matrix< Scalar, Eigen::Dynamic, 1 > Vector
void getValue(const int &id)
void playTrajectoryFile(const std::string &fileName)
AdmittanceControllerEndEffector EntityClassName
DYNAMICGRAPH_FACTORY_ENTITY_PLUGIN(AdmittanceControllerEndEffector, "AdmittanceControllerEndEffector")
virtual void display(std::ostream &os) const
#define PROFILE_ND_POSITION_DESIRED_COMPUTATION
bool m_splineReady
last iter index
unsigned int m_n
current control loop time.
parametriccurves::Spline< double, Eigen::Dynamic > * m_splineTrajGen
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)
double m_dt
true if it is the first iteration, false otherwise
std::vector< parametriccurves::InfiniteSinusoid< double, 1 > *> m_sinTrajGen
std::vector< parametriccurves::AbstractCurve< double, dynamicgraph::sot::Vector1d > *> m_currentTrajGen
status of the component
parametriccurves::TextFile< double, Eigen::Dynamic > * m_textFileTrajGen
void set(const int &id, const double &xVal)
void move(const int &id, const double &xFinal, const double &time)
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)
void init(const double &dt, const unsigned int &n)
void startConstAcc(const int &id, const double &xFinal, const double &time)
unsigned int m_iterLast
size of ouput vector
void setSpline(const std::string &filename, const double &timeToInitConf)
void startLinearChirp(const int &id, const double &xFinal, const double &f0, const double &f1, const double &time)
NdTrajectoryGenerator(const std::string &name)
double m_t
control loop time step.
std::vector< parametriccurves::MinimumJerk< double, 1 > *> m_minJerkTrajGen
std::vector< parametriccurves::Constant< double, 1 > *> m_noTrajGen
bool m_firstIter
true if the entity has been successfully initialized
DEFINE_SIGNAL_OUT_FUNCTION(dq, dynamicgraph::Vector)
std::vector< JTG_Status > m_status
true if the spline has been successfully loaded.