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 docCommandVoid1(
"Stop the motion of the specified index, or of all components of the vector if index is equal to -1.",
140 return SEND_MSG(
"Timestep must be positive", MSG_TYPE_ERROR);
142 return SEND_MSG(
"n must be at least 1", MSG_TYPE_ERROR);
154 for(
unsigned i=0; i<
m_n; i++)
157 m_sinTrajGen[i] =
new parametriccurves::InfiniteSinusoid<double,1>(5.0);
161 m_noTrajGen[i] =
new parametriccurves::Constant<double,1>(5.0);
169 m_splineTrajGen =
new parametriccurves::Spline<double,Eigen::Dynamic>();
170 m_textFileTrajGen =
new parametriccurves::TextFile<double, Eigen::Dynamic>(dt, n);
172 NdTrajectoryGenerator::setLoggerVerbosityLevel(dynamicgraph::LoggerVerbosity::VERBOSITY_ALL);
184 SEND_WARNING_STREAM_MSG(
"Cannot compute signal positionDes before initialization!");
197 if(initial_value.size()!=
m_n)
199 SEND_MSG(
"Unexpected size of input signal initial_value: "+toString(initial_value.size()),MSG_TYPE_ERROR);
203 for(
unsigned int i=0; i<
m_n; i++)
212 s = (*m_textFileTrajGen)(
m_t);
216 s = (*m_splineTrajGen)(
m_t);
219 for(
unsigned int i=0; i<
m_n; i++)
226 const bool & isTriggered = m_triggerSIN(iter);
238 for(
unsigned int i=0; i<
m_n; i++)
246 for(
unsigned int i=0; i<
m_n; i++)
251 SEND_MSG(
"Text file trajectory ended.", MSG_TYPE_INFO);
255 s = (*m_textFileTrajGen)(
m_t);
262 for(
unsigned int i=0; i<
m_n; i++)
268 SEND_MSG(
"Spline trajectory ended. Remember to turn off the trigger.", MSG_TYPE_INFO);
272 s = (*m_splineTrajGen)(
m_t);
276 for(
unsigned int i=0; i<
m_n; i++)
284 SEND_MSG(
"Trajectory of index "+toString(i)+
" ended.", MSG_TYPE_INFO);
300 SEND_WARNING_STREAM_MSG(
"Cannot compute signal positionDes before initialization!");
310 const bool & isTriggered = m_triggerSIN(iter);
322 for(
unsigned int i=0; i<
m_n; i++)
332 SEND_WARNING_STREAM_MSG(
"Cannot compute signal positionDes before initialization!");
342 const bool & isTriggered = m_triggerSIN(iter);
354 for(
unsigned int i=0; i<
m_n; i++)
366 if(id<0 || id>=static_cast<int>(
m_n))
367 return SEND_MSG(
"Index is out of bounds", MSG_TYPE_ERROR);
369 SEND_MSG(
"Current value of component "+toString(
id)+
" is "+toString( (*
m_currentTrajGen[
id])(
m_t)[0]) , MSG_TYPE_INFO);
375 return SEND_MSG(
"Cannot start sinusoid before initialization!",MSG_TYPE_ERROR);
377 for(
unsigned int i=0; i<
m_n; i++)
379 return SEND_MSG(
"You cannot control component "+toString(i)+
" because it is already controlled.", MSG_TYPE_ERROR);
382 return SEND_MSG(
"Error while loading text file "+fileName, MSG_TYPE_ERROR);
385 bool needToMoveToInitConf =
false;
387 for(
unsigned int i=0; i<
m_n; i++)
390 if(fabs(xInit[i] - currentVal) > 0.001)
392 needToMoveToInitConf =
true;
394 SEND_MSG(
"Component "+ toString(i) +
" is far from initial configuration (" + toString(xInit[i]) +
"->" + toString(currentVal) +
")", MSG_TYPE_WARNING);
418 for(
unsigned int i=0; i<
m_n; i++)
427 return SEND_MSG(
"Cannot start spline before initialization!",MSG_TYPE_ERROR);
429 for(
unsigned int i=0; i<
m_n; i++)
431 return SEND_MSG(
"You cannot control component " +toString(i)+
" because it is already controlled.",MSG_TYPE_ERROR);
434 return SEND_MSG(
"Error while loading spline"+filename, MSG_TYPE_ERROR);
436 SEND_MSG(
"Spline set to "+filename+
". Now checking initial position", MSG_TYPE_INFO);
438 bool needToMoveToInitConf =
false;
439 if(timeToInitConf < 0)
442 SEND_MSG(
"Spline Ready. Set trigger to true to start playing", MSG_TYPE_INFO);
447 const VectorXd& xInit = (*m_splineTrajGen)(0.0);
448 assert(xInit.size() ==
m_n);
449 for(
unsigned int i=0; i<
m_n; i++)
452 needToMoveToInitConf =
true;
453 SEND_MSG(
"Component "+ toString(i) +
" is too far from initial configuration so first i will move it there.", MSG_TYPE_WARNING);
456 if(needToMoveToInitConf)
458 for(
unsigned int i=0; i<
m_n; i++)
472 SEND_MSG(
"Spline Ready. Set trigger to true to start playing", MSG_TYPE_INFO);
479 for(
unsigned int i=0; i<
m_n; i++)
488 return SEND_MSG(
"Cannot start sinusoid before initialization!",MSG_TYPE_ERROR);
490 if(id<0 || id>=static_cast<int>(
m_n))
491 return SEND_MSG(
"Index is out of bounds", MSG_TYPE_ERROR);
494 return SEND_MSG(
"Trajectory time must be a positive number", MSG_TYPE_ERROR);
496 return SEND_MSG(
"You cannot move the specified component because it is already controlled.", MSG_TYPE_ERROR);
503 SEND_MSG(
"Set initial point of sinusoid to "+toString((*
m_noTrajGen[i])(
m_t)[0]),MSG_TYPE_DEBUG);
539 return SEND_MSG(
"Cannot start constant-acceleration trajectory before initialization!",MSG_TYPE_ERROR);
540 if(id<0 || id>=static_cast<int>(
m_n))
541 return SEND_MSG(
"Index is out of bounds", MSG_TYPE_ERROR);
544 return SEND_MSG(
"Trajectory time must be a positive number", MSG_TYPE_ERROR);
546 return SEND_MSG(
"You cannot move the specified component because it is already controlled.", MSG_TYPE_ERROR);
551 SEND_MSG(
"Set initial point of const-acc trajectory to "+toString((*
m_noTrajGen[i])(
m_t)[0]),MSG_TYPE_DEBUG);
561 return SEND_MSG(
"Cannot start linear chirp before initialization!",MSG_TYPE_ERROR);
562 if(id<0 || id>=static_cast<int>(
m_n))
563 return SEND_MSG(
"Index is out of bounds", MSG_TYPE_ERROR);
566 return SEND_MSG(
"Trajectory time must be a positive number", MSG_TYPE_ERROR);
568 return SEND_MSG(
"You cannot move the specified component because it is already controlled.", MSG_TYPE_ERROR);
572 return SEND_MSG(
"f0 "+toString(f0)+
" cannot to be more than f1 "+toString(f1),MSG_TYPE_ERROR);
574 return SEND_MSG(
"Frequency has to be positive "+toString(f0),MSG_TYPE_ERROR);
577 return SEND_MSG(
"Error while setting initial point "+toString((*
m_noTrajGen[i])(
m_t)[0]), MSG_TYPE_ERROR);
579 return SEND_MSG(
"Error while setting final point "+toString(xFinal), MSG_TYPE_ERROR);
581 return SEND_MSG(
"Error while setting trajectory time "+toString(time), MSG_TYPE_ERROR);
583 return SEND_MSG(
"Error while setting initial frequency "+toString(f0), MSG_TYPE_ERROR);
585 return SEND_MSG(
"Error while setting final frequency "+toString(f1), MSG_TYPE_ERROR);
594 return SEND_MSG(
"Cannot move value before initialization!",MSG_TYPE_ERROR);
596 if(id<0 || id>=static_cast<int>(
m_n))
597 return SEND_MSG(
"Index is out of bounds", MSG_TYPE_ERROR);
599 return SEND_MSG(
"Trajectory time must be a positive number", MSG_TYPE_ERROR);
601 return SEND_MSG(
"You cannot move the specified component because it is already controlled.", MSG_TYPE_ERROR);
617 return SEND_MSG(
"Cannot stop value before initialization!",MSG_TYPE_ERROR);
621 for(
unsigned int i=0; i<
m_n; i++)
631 if(id<0 || id>=static_cast<int>(
m_n))
632 return SEND_MSG(
"Index is out of bounds", MSG_TYPE_ERROR);
668 os <<
"NdTrajectoryGenerator "<<getName();
671 getProfiler().report_all(3, os);
673 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 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.