#include <sot/torque_control/se3-trajectory-generator.hh>
Public Member Functions | |
SE3TrajectoryGenerator (const std::string &name) | |
DECLARE_SIGNAL (x, OUT, dynamicgraph::Vector) | |
DECLARE_SIGNAL_IN (initial_value, dynamicgraph::Vector) | |
DECLARE_SIGNAL_IN (trigger, bool) | |
DECLARE_SIGNAL_OUT (ddx, dynamicgraph::Vector) | |
DECLARE_SIGNAL_OUT (dx, dynamicgraph::Vector) | |
virtual void | display (std::ostream &os) const |
void | getValue (const int &id) |
void | init (const double &dt) |
void | move (const int &id, const double &xFinal, const double &time) |
void | playTrajectoryFile (const std::string &fileName) |
void | sendMsg (const std::string &msg, MsgType t=MSG_TYPE_INFO, const char *="", int=0) |
void | setSpline (const std::string &filename, const double &timeToInitConf, const Eigen::MatrixXd &init_rotation) |
void | startConstAcc (const int &id, const double &xFinal, const double &time) |
void | startLinearChirp (const int &id, const double &xFinal, const double &f0, const double &f1, const double &time) |
void | startSinusoid (const int &id, const double &xFinal, const double &time) |
void | startSpline () |
void | startTriangle (const int &id, const double &xFinal, const double &time, const double &Tacc) |
void | stop (const int &id) |
Protected Types | |
enum | TG_Status { TG_STOP, TG_SINUSOID, TG_MIN_JERK, TG_LIN_CHIRP, TG_TRIANGLE, TG_CONST_ACC, TG_TEXT_FILE, TG_SPLINE } |
Protected Member Functions | |
DECLARE_SIGNAL_OUT_FUNCTION (x, dynamicgraph::Vector) | |
Protected Attributes | |
std::vector< ConstantAccelerationTrajectoryGenerator * > | m_constAccTrajGen |
std::vector< AbstractTrajectoryGenerator * > | m_currentTrajGen |
status of the component More... | |
double | m_dt |
true if it is the first iteration, false otherwise More... | |
bool | m_firstIter |
true if the entity has been successfully initialized More... | |
bool | m_initSucceeded |
unsigned int | m_iterLast |
size of velocity vector More... | |
std::vector< LinearChirpTrajectoryGenerator * > | m_linChirpTrajGen |
std::vector< MinimumJerkTrajectoryGenerator * > | m_minJerkTrajGen |
std::vector< NoTrajectoryGenerator * > | m_noTrajGen |
unsigned int | m_np |
control loop time period More... | |
unsigned int | m_nv |
size of position vector More... | |
std::vector< SinusoidTrajectoryGenerator * > | m_sinTrajGen |
bool | m_splineReady |
Eigen::Matrix3d | m_splineRotation |
parametriccurves::Spline< double, Eigen::Dynamic > * | m_splineTrajGen |
std::vector< TG_Status > | m_status |
double | m_t |
last iter index More... | |
TextFileTrajectoryGenerator * | m_textFileTrajGen |
std::vector< TriangleTrajectoryGenerator * > | m_triangleTrajGen |
Definition at line 46 of file se3-trajectory-generator.hh.
|
protected |
Enumerator | |
---|---|
TG_STOP | |
TG_SINUSOID | |
TG_MIN_JERK | |
TG_LIN_CHIRP | |
TG_TRIANGLE | |
TG_CONST_ACC | |
TG_TEXT_FILE | |
TG_SPLINE |
Definition at line 129 of file se3-trajectory-generator.hh.
SE3TrajectoryGenerator | ( | const std::string & | name | ) |
Definition at line 34 of file se3-trajectory-generator.cpp.
References dynamicgraph::command::docCommandVoid5(), SE3TrajectoryGenerator::getValue(), SE3TrajectoryGenerator::init(), dynamicgraph::command::makeCommandVoid5(), SE3TrajectoryGenerator::move(), SE3TrajectoryGenerator::playTrajectoryFile(), SE3TrajectoryGenerator::setSpline(), SE3TrajectoryGenerator::startConstAcc(), SE3TrajectoryGenerator::startLinearChirp(), SE3TrajectoryGenerator::startSinusoid(), SE3TrajectoryGenerator::startTriangle(), and SE3TrajectoryGenerator::stop().
DECLARE_SIGNAL | ( | x | , |
OUT | , | ||
dynamicgraph::Vector | |||
) |
DECLARE_SIGNAL_IN | ( | initial_value | , |
dynamicgraph::Vector | |||
) |
DECLARE_SIGNAL_IN | ( | trigger | , |
bool | |||
) |
DECLARE_SIGNAL_OUT | ( | ddx | , |
dynamicgraph::Vector | |||
) |
DECLARE_SIGNAL_OUT | ( | dx | , |
dynamicgraph::Vector | |||
) |
|
protected |
|
virtual |
Definition at line 496 of file se3-trajectory-generator.cpp.
void getValue | ( | const int & | id | ) |
Print the current value of the specified component.
Definition at line 269 of file se3-trajectory-generator.cpp.
References SE3TrajectoryGenerator::m_currentTrajGen, and SE3TrajectoryGenerator::m_np.
Referenced by SE3TrajectoryGenerator::SE3TrajectoryGenerator().
void init | ( | const double & | dt | ) |
Definition at line 106 of file se3-trajectory-generator.cpp.
References SE3TrajectoryGenerator::m_constAccTrajGen, SE3TrajectoryGenerator::m_currentTrajGen, SE3TrajectoryGenerator::m_dt, SE3TrajectoryGenerator::m_firstIter, SE3TrajectoryGenerator::m_initSucceeded, SE3TrajectoryGenerator::m_linChirpTrajGen, SE3TrajectoryGenerator::m_minJerkTrajGen, SE3TrajectoryGenerator::m_noTrajGen, SE3TrajectoryGenerator::m_np, SE3TrajectoryGenerator::m_sinTrajGen, SE3TrajectoryGenerator::m_splineTrajGen, SE3TrajectoryGenerator::m_status, SE3TrajectoryGenerator::m_textFileTrajGen, SE3TrajectoryGenerator::m_triangleTrajGen, and SE3TrajectoryGenerator::TG_STOP.
Referenced by SE3TrajectoryGenerator::SE3TrajectoryGenerator().
void move | ( | const int & | id, |
const double & | xFinal, | ||
const double & | time | ||
) |
Move a component to a position with a minimum-jerk trajectory.
id | integer index. |
xFinal | The desired final position of the component. |
time | The time to go from the current position to xFinal [sec]. |
Definition at line 454 of file se3-trajectory-generator.cpp.
References SE3TrajectoryGenerator::m_currentTrajGen, SE3TrajectoryGenerator::m_initSucceeded, SE3TrajectoryGenerator::m_minJerkTrajGen, SE3TrajectoryGenerator::m_noTrajGen, SE3TrajectoryGenerator::m_np, SE3TrajectoryGenerator::m_status, SE3TrajectoryGenerator::TG_MIN_JERK, and SE3TrajectoryGenerator::TG_STOP.
Referenced by SE3TrajectoryGenerator::SE3TrajectoryGenerator().
void playTrajectoryFile | ( | const std::string & | fileName | ) |
Definition at line 276 of file se3-trajectory-generator.cpp.
References AbstractTrajectoryGenerator::get_initial_point(), TextFileTrajectoryGenerator::loadTextFile(), SE3TrajectoryGenerator::m_currentTrajGen, SE3TrajectoryGenerator::m_initSucceeded, SE3TrajectoryGenerator::m_minJerkTrajGen, SE3TrajectoryGenerator::m_noTrajGen, SE3TrajectoryGenerator::m_np, SE3TrajectoryGenerator::m_status, SE3TrajectoryGenerator::m_textFileTrajGen, SE3TrajectoryGenerator::TG_MIN_JERK, SE3TrajectoryGenerator::TG_STOP, and SE3TrajectoryGenerator::TG_TEXT_FILE.
Referenced by SE3TrajectoryGenerator::SE3TrajectoryGenerator().
|
inline |
Definition at line 124 of file se3-trajectory-generator.hh.
void setSpline | ( | const std::string & | filename, |
const double & | timeToInitConf, | ||
const Eigen::MatrixXd & | init_rotation | ||
) |
Definition at line 314 of file se3-trajectory-generator.cpp.
References SE3TrajectoryGenerator::m_currentTrajGen, SE3TrajectoryGenerator::m_initSucceeded, SE3TrajectoryGenerator::m_minJerkTrajGen, SE3TrajectoryGenerator::m_noTrajGen, SE3TrajectoryGenerator::m_np, SE3TrajectoryGenerator::m_splineReady, SE3TrajectoryGenerator::m_splineRotation, SE3TrajectoryGenerator::m_splineTrajGen, SE3TrajectoryGenerator::m_status, SE3TrajectoryGenerator::TG_MIN_JERK, and SE3TrajectoryGenerator::TG_STOP.
Referenced by SE3TrajectoryGenerator::SE3TrajectoryGenerator().
void startConstAcc | ( | const int & | id, |
const double & | xFinal, | ||
const double & | time | ||
) |
Start an infinite trajectory with piece-wise constant acceleration.
id | integer index. |
xFinal | The position of the component corresponding to the max amplitude of the trajectory. |
time | The time to go from the current position to xFinal [sec]. |
Tacc | The time during witch acceleration is keept constant [sec]. |
Definition at line 412 of file se3-trajectory-generator.cpp.
References SE3TrajectoryGenerator::m_constAccTrajGen, SE3TrajectoryGenerator::m_currentTrajGen, SE3TrajectoryGenerator::m_initSucceeded, SE3TrajectoryGenerator::m_noTrajGen, SE3TrajectoryGenerator::m_np, SE3TrajectoryGenerator::m_status, SE3TrajectoryGenerator::TG_CONST_ACC, and SE3TrajectoryGenerator::TG_STOP.
Referenced by SE3TrajectoryGenerator::SE3TrajectoryGenerator().
void startLinearChirp | ( | const int & | id, |
const double & | xFinal, | ||
const double & | f0, | ||
const double & | f1, | ||
const double & | time | ||
) |
Start a linear-chirp trajectory, that is a sinusoidal trajectory with frequency being a linear function of time.
id | integer index. |
xFinal | The position of the component corresponding to the max amplitude of the sinusoid [rad]. |
f0 | The initial (min) frequency of the sinusoid [Hz] |
f1 | The final (max) frequency of the sinusoid [Hz] |
time | The time to get from f0 to f1 [sec] |
Definition at line 429 of file se3-trajectory-generator.cpp.
References SE3TrajectoryGenerator::m_currentTrajGen, SE3TrajectoryGenerator::m_initSucceeded, SE3TrajectoryGenerator::m_linChirpTrajGen, SE3TrajectoryGenerator::m_noTrajGen, SE3TrajectoryGenerator::m_np, SE3TrajectoryGenerator::m_status, SE3TrajectoryGenerator::TG_LIN_CHIRP, and SE3TrajectoryGenerator::TG_STOP.
Referenced by SE3TrajectoryGenerator::SE3TrajectoryGenerator().
void startSinusoid | ( | const int & | id, |
const double & | xFinal, | ||
const double & | time | ||
) |
Start an infinite sinusoidal trajectory.
id | integer index. |
xFinal | The position of the component corresponding to the max amplitude of the sinusoid. |
time | The time to go from the current position to xFinal [sec]. |
Definition at line 372 of file se3-trajectory-generator.cpp.
References SE3TrajectoryGenerator::m_currentTrajGen, SE3TrajectoryGenerator::m_initSucceeded, SE3TrajectoryGenerator::m_noTrajGen, SE3TrajectoryGenerator::m_np, SE3TrajectoryGenerator::m_sinTrajGen, SE3TrajectoryGenerator::m_status, SE3TrajectoryGenerator::TG_SINUSOID, and SE3TrajectoryGenerator::TG_STOP.
Referenced by SE3TrajectoryGenerator::SE3TrajectoryGenerator().
void startSpline | ( | ) |
Definition at line 364 of file se3-trajectory-generator.cpp.
References SE3TrajectoryGenerator::m_np, SE3TrajectoryGenerator::m_status, SE3TrajectoryGenerator::m_t, and SE3TrajectoryGenerator::TG_SPLINE.
void startTriangle | ( | const int & | id, |
const double & | xFinal, | ||
const double & | time, | ||
const double & | Tacc | ||
) |
Start an infinite triangle trajectory.
id | integer index. |
xFinal | The position of the component corresponding to the max amplitude of the trajectory. |
time | The time to go from the current position to xFinal [sec]. |
Definition at line 389 of file se3-trajectory-generator.cpp.
References SE3TrajectoryGenerator::m_currentTrajGen, SE3TrajectoryGenerator::m_initSucceeded, SE3TrajectoryGenerator::m_noTrajGen, SE3TrajectoryGenerator::m_np, SE3TrajectoryGenerator::m_status, SE3TrajectoryGenerator::m_triangleTrajGen, SE3TrajectoryGenerator::TG_STOP, and SE3TrajectoryGenerator::TG_TRIANGLE.
Referenced by SE3TrajectoryGenerator::SE3TrajectoryGenerator().
void stop | ( | const int & | id | ) |
Stop the motion of the specified component. If id is -1 it stops the trajectory of all the vector.
id | integer index. |
Definition at line 469 of file se3-trajectory-generator.cpp.
References SE3TrajectoryGenerator::m_currentTrajGen, SE3TrajectoryGenerator::m_initSucceeded, SE3TrajectoryGenerator::m_noTrajGen, SE3TrajectoryGenerator::m_np, SE3TrajectoryGenerator::m_splineReady, SE3TrajectoryGenerator::m_status, SE3TrajectoryGenerator::m_t, and SE3TrajectoryGenerator::TG_STOP.
Referenced by SE3TrajectoryGenerator::SE3TrajectoryGenerator().
|
protected |
Definition at line 159 of file se3-trajectory-generator.hh.
Referenced by SE3TrajectoryGenerator::init(), and SE3TrajectoryGenerator::startConstAcc().
|
protected |
status of the component
Definition at line 153 of file se3-trajectory-generator.hh.
Referenced by SE3TrajectoryGenerator::getValue(), SE3TrajectoryGenerator::init(), SE3TrajectoryGenerator::move(), SE3TrajectoryGenerator::playTrajectoryFile(), SE3TrajectoryGenerator::setSpline(), SE3TrajectoryGenerator::startConstAcc(), SE3TrajectoryGenerator::startLinearChirp(), SE3TrajectoryGenerator::startSinusoid(), SE3TrajectoryGenerator::startTriangle(), and SE3TrajectoryGenerator::stop().
|
protected |
true if it is the first iteration, false otherwise
Definition at line 142 of file se3-trajectory-generator.hh.
Referenced by SE3TrajectoryGenerator::init().
|
protected |
true if the entity has been successfully initialized
Definition at line 141 of file se3-trajectory-generator.hh.
Referenced by SE3TrajectoryGenerator::init().
|
protected |
Definition at line 140 of file se3-trajectory-generator.hh.
Referenced by SE3TrajectoryGenerator::init(), SE3TrajectoryGenerator::move(), SE3TrajectoryGenerator::playTrajectoryFile(), SE3TrajectoryGenerator::setSpline(), SE3TrajectoryGenerator::startConstAcc(), SE3TrajectoryGenerator::startLinearChirp(), SE3TrajectoryGenerator::startSinusoid(), SE3TrajectoryGenerator::startTriangle(), and SE3TrajectoryGenerator::stop().
|
protected |
size of velocity vector
Definition at line 145 of file se3-trajectory-generator.hh.
|
protected |
Definition at line 157 of file se3-trajectory-generator.hh.
Referenced by SE3TrajectoryGenerator::init(), and SE3TrajectoryGenerator::startLinearChirp().
|
protected |
Definition at line 155 of file se3-trajectory-generator.hh.
Referenced by SE3TrajectoryGenerator::init(), SE3TrajectoryGenerator::move(), SE3TrajectoryGenerator::playTrajectoryFile(), and SE3TrajectoryGenerator::setSpline().
|
protected |
Definition at line 154 of file se3-trajectory-generator.hh.
Referenced by SE3TrajectoryGenerator::init(), SE3TrajectoryGenerator::move(), SE3TrajectoryGenerator::playTrajectoryFile(), SE3TrajectoryGenerator::setSpline(), SE3TrajectoryGenerator::startConstAcc(), SE3TrajectoryGenerator::startLinearChirp(), SE3TrajectoryGenerator::startSinusoid(), SE3TrajectoryGenerator::startTriangle(), and SE3TrajectoryGenerator::stop().
|
protected |
control loop time period
Definition at line 143 of file se3-trajectory-generator.hh.
Referenced by SE3TrajectoryGenerator::getValue(), SE3TrajectoryGenerator::init(), SE3TrajectoryGenerator::move(), SE3TrajectoryGenerator::playTrajectoryFile(), SE3TrajectoryGenerator::setSpline(), SE3TrajectoryGenerator::startConstAcc(), SE3TrajectoryGenerator::startLinearChirp(), SE3TrajectoryGenerator::startSinusoid(), SE3TrajectoryGenerator::startSpline(), SE3TrajectoryGenerator::startTriangle(), and SE3TrajectoryGenerator::stop().
|
protected |
size of position vector
Definition at line 144 of file se3-trajectory-generator.hh.
|
protected |
Definition at line 156 of file se3-trajectory-generator.hh.
Referenced by SE3TrajectoryGenerator::init(), and SE3TrajectoryGenerator::startSinusoid().
|
protected |
Definition at line 150 of file se3-trajectory-generator.hh.
Referenced by SE3TrajectoryGenerator::setSpline(), and SE3TrajectoryGenerator::stop().
|
protected |
Definition at line 148 of file se3-trajectory-generator.hh.
Referenced by SE3TrajectoryGenerator::setSpline().
|
protected |
Definition at line 149 of file se3-trajectory-generator.hh.
Referenced by SE3TrajectoryGenerator::init(), and SE3TrajectoryGenerator::setSpline().
|
protected |
Definition at line 152 of file se3-trajectory-generator.hh.
Referenced by SE3TrajectoryGenerator::init(), SE3TrajectoryGenerator::move(), SE3TrajectoryGenerator::playTrajectoryFile(), SE3TrajectoryGenerator::setSpline(), SE3TrajectoryGenerator::startConstAcc(), SE3TrajectoryGenerator::startLinearChirp(), SE3TrajectoryGenerator::startSinusoid(), SE3TrajectoryGenerator::startSpline(), SE3TrajectoryGenerator::startTriangle(), and SE3TrajectoryGenerator::stop().
|
protected |
last iter index
Definition at line 147 of file se3-trajectory-generator.hh.
Referenced by SE3TrajectoryGenerator::startSpline(), and SE3TrajectoryGenerator::stop().
|
protected |
Definition at line 160 of file se3-trajectory-generator.hh.
Referenced by SE3TrajectoryGenerator::init(), and SE3TrajectoryGenerator::playTrajectoryFile().
|
protected |
Definition at line 158 of file se3-trajectory-generator.hh.
Referenced by SE3TrajectoryGenerator::init(), and SE3TrajectoryGenerator::startTriangle().