#include <sot/talos_balance/nd-trajectory-generator.hh>
Public Member Functions | |
NdTrajectoryGenerator (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 (dx, dynamicgraph::Vector) | |
DECLARE_SIGNAL_OUT (ddx, dynamicgraph::Vector) | |
virtual void | display (std::ostream &os) const |
void | getValue (const int &id) |
void | init (const double &dt, const unsigned int &n) |
void | move (const int &id, const double &xFinal, const double &time) |
void | playSpline (const std::string &fileName) |
void | playTrajectoryFile (const std::string &fileName) |
void | set (const int &id, const double &xVal) |
void | setSpline (const std::string &filename, const double &timeToInitConf) |
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 | stop (const int &id) |
Protected Types | |
enum | JTG_Status { JTG_STOP, JTG_SINUSOID, JTG_MIN_JERK, JTG_LIN_CHIRP, JTG_CONST_ACC, JTG_TEXT_FILE, JTG_SPLINE } |
Protected Member Functions | |
DECLARE_SIGNAL_OUT_FUNCTION (x, dynamicgraph::Vector) | |
Protected Attributes | |
std::vector< parametriccurves::InfiniteConstAcc< double, 1 > *> | m_constAccTrajGen |
std::vector< parametriccurves::AbstractCurve< double, dynamicgraph::sot::Vector1d > *> | 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 ouput vector More... | |
std::vector< parametriccurves::LinearChirp< double, 1 > * > | m_linChirpTrajGen |
std::vector< parametriccurves::MinimumJerk< double, 1 > *> | m_minJerkTrajGen |
unsigned int | m_n |
current control loop time. More... | |
std::vector< parametriccurves::Constant< double, 1 > *> | m_noTrajGen |
std::vector< parametriccurves::InfiniteSinusoid< double, 1 > *> | m_sinTrajGen |
bool | m_splineReady |
last iter index More... | |
parametriccurves::Spline< double, Eigen::Dynamic > * | m_splineTrajGen |
std::vector< JTG_Status > | m_status |
true if the spline has been successfully loaded. More... | |
double | m_t |
control loop time step. More... | |
parametriccurves::TextFile< double, Eigen::Dynamic > * | m_textFileTrajGen |
Definition at line 65 of file nd-trajectory-generator.hh.
|
protected |
Enumerator | |
---|---|
JTG_STOP | |
JTG_SINUSOID | |
JTG_MIN_JERK | |
JTG_LIN_CHIRP | |
JTG_CONST_ACC | |
JTG_TEXT_FILE | |
JTG_SPLINE |
Definition at line 156 of file nd-trajectory-generator.hh.
NdTrajectoryGenerator | ( | const std::string & | name | ) |
Definition at line 52 of file nd-trajectory-generator.cpp.
References dynamicgraph::command::docCommandVoid5(), NdTrajectoryGenerator::getValue(), NdTrajectoryGenerator::init(), dynamicgraph::command::makeCommandVoid5(), NdTrajectoryGenerator::move(), NdTrajectoryGenerator::playTrajectoryFile(), NdTrajectoryGenerator::set(), NdTrajectoryGenerator::setSpline(), NdTrajectoryGenerator::startConstAcc(), NdTrajectoryGenerator::startLinearChirp(), NdTrajectoryGenerator::startSinusoid(), and NdTrajectoryGenerator::stop().
DECLARE_SIGNAL | ( | x | , |
OUT | , | ||
dynamicgraph::Vector | |||
) |
DECLARE_SIGNAL_IN | ( | initial_value | , |
dynamicgraph::Vector | |||
) |
DECLARE_SIGNAL_IN | ( | trigger | , |
bool | |||
) |
DECLARE_SIGNAL_OUT | ( | dx | , |
dynamicgraph::Vector | |||
) |
DECLARE_SIGNAL_OUT | ( | ddx | , |
dynamicgraph::Vector | |||
) |
|
protected |
|
virtual |
Definition at line 692 of file nd-trajectory-generator.cpp.
void getValue | ( | const int & | id | ) |
Print the current value of the specified component.
Definition at line 369 of file nd-trajectory-generator.cpp.
References NdTrajectoryGenerator::m_currentTrajGen, NdTrajectoryGenerator::m_n, and NdTrajectoryGenerator::m_t.
Referenced by NdTrajectoryGenerator::NdTrajectoryGenerator().
void init | ( | const double & | dt, |
const unsigned int & | n | ||
) |
Definition at line 142 of file nd-trajectory-generator.cpp.
References DOUBLE_INF, NdTrajectoryGenerator::JTG_STOP, NdTrajectoryGenerator::m_constAccTrajGen, NdTrajectoryGenerator::m_currentTrajGen, NdTrajectoryGenerator::m_dt, NdTrajectoryGenerator::m_firstIter, NdTrajectoryGenerator::m_initSucceeded, NdTrajectoryGenerator::m_linChirpTrajGen, NdTrajectoryGenerator::m_minJerkTrajGen, NdTrajectoryGenerator::m_n, NdTrajectoryGenerator::m_noTrajGen, NdTrajectoryGenerator::m_sinTrajGen, NdTrajectoryGenerator::m_splineTrajGen, NdTrajectoryGenerator::m_status, and NdTrajectoryGenerator::m_textFileTrajGen.
Referenced by NdTrajectoryGenerator::NdTrajectoryGenerator().
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 600 of file nd-trajectory-generator.cpp.
References NdTrajectoryGenerator::JTG_MIN_JERK, NdTrajectoryGenerator::JTG_STOP, NdTrajectoryGenerator::m_currentTrajGen, NdTrajectoryGenerator::m_initSucceeded, NdTrajectoryGenerator::m_minJerkTrajGen, NdTrajectoryGenerator::m_n, NdTrajectoryGenerator::m_noTrajGen, NdTrajectoryGenerator::m_status, and NdTrajectoryGenerator::m_t.
Referenced by NdTrajectoryGenerator::NdTrajectoryGenerator().
void playSpline | ( | const std::string & | fileName | ) |
void playTrajectoryFile | ( | const std::string & | fileName | ) |
Definition at line 377 of file nd-trajectory-generator.cpp.
References NdTrajectoryGenerator::JTG_STOP, NdTrajectoryGenerator::JTG_TEXT_FILE, NdTrajectoryGenerator::m_currentTrajGen, NdTrajectoryGenerator::m_initSucceeded, NdTrajectoryGenerator::m_n, NdTrajectoryGenerator::m_status, NdTrajectoryGenerator::m_t, and NdTrajectoryGenerator::m_textFileTrajGen.
Referenced by NdTrajectoryGenerator::NdTrajectoryGenerator().
void set | ( | const int & | id, |
const double & | xVal | ||
) |
Instantaneously set a component to a given position.
id | integer index. |
xVal | The desired position of the component. |
Definition at line 622 of file nd-trajectory-generator.cpp.
References NdTrajectoryGenerator::JTG_STOP, NdTrajectoryGenerator::m_initSucceeded, NdTrajectoryGenerator::m_n, NdTrajectoryGenerator::m_noTrajGen, and NdTrajectoryGenerator::m_status.
Referenced by NdTrajectoryGenerator::NdTrajectoryGenerator().
void setSpline | ( | const std::string & | filename, |
const double & | timeToInitConf | ||
) |
Definition at line 433 of file nd-trajectory-generator.cpp.
References NdTrajectoryGenerator::JTG_MIN_JERK, NdTrajectoryGenerator::JTG_STOP, NdTrajectoryGenerator::m_currentTrajGen, NdTrajectoryGenerator::m_initSucceeded, NdTrajectoryGenerator::m_minJerkTrajGen, NdTrajectoryGenerator::m_n, NdTrajectoryGenerator::m_noTrajGen, NdTrajectoryGenerator::m_splineReady, NdTrajectoryGenerator::m_splineTrajGen, NdTrajectoryGenerator::m_status, and NdTrajectoryGenerator::m_t.
Referenced by NdTrajectoryGenerator::NdTrajectoryGenerator().
void startConstAcc | ( | const int & | id, |
const double & | xFinal, | ||
const double & | time | ||
) |
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]. 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 545 of file nd-trajectory-generator.cpp.
References NdTrajectoryGenerator::JTG_CONST_ACC, NdTrajectoryGenerator::JTG_STOP, NdTrajectoryGenerator::m_constAccTrajGen, NdTrajectoryGenerator::m_currentTrajGen, NdTrajectoryGenerator::m_initSucceeded, NdTrajectoryGenerator::m_n, NdTrajectoryGenerator::m_noTrajGen, NdTrajectoryGenerator::m_status, and NdTrajectoryGenerator::m_t.
Referenced by NdTrajectoryGenerator::NdTrajectoryGenerator().
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 567 of file nd-trajectory-generator.cpp.
References NdTrajectoryGenerator::JTG_LIN_CHIRP, NdTrajectoryGenerator::JTG_STOP, NdTrajectoryGenerator::m_currentTrajGen, NdTrajectoryGenerator::m_initSucceeded, NdTrajectoryGenerator::m_linChirpTrajGen, NdTrajectoryGenerator::m_n, NdTrajectoryGenerator::m_noTrajGen, NdTrajectoryGenerator::m_status, and NdTrajectoryGenerator::m_t.
Referenced by NdTrajectoryGenerator::NdTrajectoryGenerator().
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 494 of file nd-trajectory-generator.cpp.
References NdTrajectoryGenerator::JTG_SINUSOID, NdTrajectoryGenerator::JTG_STOP, NdTrajectoryGenerator::m_currentTrajGen, NdTrajectoryGenerator::m_initSucceeded, NdTrajectoryGenerator::m_n, NdTrajectoryGenerator::m_noTrajGen, NdTrajectoryGenerator::m_sinTrajGen, NdTrajectoryGenerator::m_status, and NdTrajectoryGenerator::m_t.
Referenced by NdTrajectoryGenerator::NdTrajectoryGenerator().
void startSpline | ( | ) |
Definition at line 484 of file nd-trajectory-generator.cpp.
References NdTrajectoryGenerator::JTG_SPLINE, NdTrajectoryGenerator::m_n, NdTrajectoryGenerator::m_status, and NdTrajectoryGenerator::m_t.
Referenced by dynamicgraph::sot::talos_balance::DEFINE_SIGNAL_OUT_FUNCTION().
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 640 of file nd-trajectory-generator.cpp.
References NdTrajectoryGenerator::JTG_STOP, NdTrajectoryGenerator::m_currentTrajGen, NdTrajectoryGenerator::m_initSucceeded, NdTrajectoryGenerator::m_n, NdTrajectoryGenerator::m_noTrajGen, NdTrajectoryGenerator::m_splineReady, NdTrajectoryGenerator::m_status, and NdTrajectoryGenerator::m_t.
Referenced by NdTrajectoryGenerator::NdTrajectoryGenerator().
|
protected |
Definition at line 183 of file nd-trajectory-generator.hh.
Referenced by NdTrajectoryGenerator::init(), and NdTrajectoryGenerator::startConstAcc().
|
protected |
status of the component
Definition at line 177 of file nd-trajectory-generator.hh.
Referenced by dynamicgraph::sot::talos_balance::DEFINE_SIGNAL_OUT_FUNCTION(), NdTrajectoryGenerator::getValue(), NdTrajectoryGenerator::init(), NdTrajectoryGenerator::move(), NdTrajectoryGenerator::playTrajectoryFile(), NdTrajectoryGenerator::setSpline(), NdTrajectoryGenerator::startConstAcc(), NdTrajectoryGenerator::startLinearChirp(), NdTrajectoryGenerator::startSinusoid(), and NdTrajectoryGenerator::stop().
|
protected |
true if it is the first iteration, false otherwise
Definition at line 170 of file nd-trajectory-generator.hh.
Referenced by dynamicgraph::sot::talos_balance::DEFINE_SIGNAL_OUT_FUNCTION(), and NdTrajectoryGenerator::init().
|
protected |
true if the entity has been successfully initialized
Definition at line 169 of file nd-trajectory-generator.hh.
Referenced by dynamicgraph::sot::talos_balance::DEFINE_SIGNAL_OUT_FUNCTION(), and NdTrajectoryGenerator::init().
|
protected |
Definition at line 168 of file nd-trajectory-generator.hh.
Referenced by dynamicgraph::sot::talos_balance::DEFINE_SIGNAL_OUT_FUNCTION(), NdTrajectoryGenerator::init(), NdTrajectoryGenerator::move(), NdTrajectoryGenerator::playTrajectoryFile(), NdTrajectoryGenerator::set(), NdTrajectoryGenerator::setSpline(), NdTrajectoryGenerator::startConstAcc(), NdTrajectoryGenerator::startLinearChirp(), NdTrajectoryGenerator::startSinusoid(), and NdTrajectoryGenerator::stop().
|
protected |
size of ouput vector
Definition at line 173 of file nd-trajectory-generator.hh.
Referenced by dynamicgraph::sot::talos_balance::DEFINE_SIGNAL_OUT_FUNCTION().
|
protected |
Definition at line 181 of file nd-trajectory-generator.hh.
Referenced by NdTrajectoryGenerator::init(), and NdTrajectoryGenerator::startLinearChirp().
|
protected |
Definition at line 179 of file nd-trajectory-generator.hh.
Referenced by NdTrajectoryGenerator::init(), NdTrajectoryGenerator::move(), and NdTrajectoryGenerator::setSpline().
|
protected |
current control loop time.
Definition at line 172 of file nd-trajectory-generator.hh.
Referenced by dynamicgraph::sot::talos_balance::DEFINE_SIGNAL_OUT_FUNCTION(), NdTrajectoryGenerator::getValue(), NdTrajectoryGenerator::init(), NdTrajectoryGenerator::move(), NdTrajectoryGenerator::playTrajectoryFile(), NdTrajectoryGenerator::set(), NdTrajectoryGenerator::setSpline(), NdTrajectoryGenerator::startConstAcc(), NdTrajectoryGenerator::startLinearChirp(), NdTrajectoryGenerator::startSinusoid(), NdTrajectoryGenerator::startSpline(), and NdTrajectoryGenerator::stop().
|
protected |
Definition at line 178 of file nd-trajectory-generator.hh.
Referenced by dynamicgraph::sot::talos_balance::DEFINE_SIGNAL_OUT_FUNCTION(), NdTrajectoryGenerator::init(), NdTrajectoryGenerator::move(), NdTrajectoryGenerator::set(), NdTrajectoryGenerator::setSpline(), NdTrajectoryGenerator::startConstAcc(), NdTrajectoryGenerator::startLinearChirp(), NdTrajectoryGenerator::startSinusoid(), and NdTrajectoryGenerator::stop().
|
protected |
Definition at line 180 of file nd-trajectory-generator.hh.
Referenced by NdTrajectoryGenerator::init(), and NdTrajectoryGenerator::startSinusoid().
|
protected |
last iter index
Definition at line 174 of file nd-trajectory-generator.hh.
Referenced by dynamicgraph::sot::talos_balance::DEFINE_SIGNAL_OUT_FUNCTION(), NdTrajectoryGenerator::setSpline(), and NdTrajectoryGenerator::stop().
|
protected |
Definition at line 185 of file nd-trajectory-generator.hh.
Referenced by dynamicgraph::sot::talos_balance::DEFINE_SIGNAL_OUT_FUNCTION(), NdTrajectoryGenerator::init(), and NdTrajectoryGenerator::setSpline().
|
protected |
true if the spline has been successfully loaded.
Definition at line 176 of file nd-trajectory-generator.hh.
Referenced by dynamicgraph::sot::talos_balance::DEFINE_SIGNAL_OUT_FUNCTION(), NdTrajectoryGenerator::init(), NdTrajectoryGenerator::move(), NdTrajectoryGenerator::playTrajectoryFile(), NdTrajectoryGenerator::set(), NdTrajectoryGenerator::setSpline(), NdTrajectoryGenerator::startConstAcc(), NdTrajectoryGenerator::startLinearChirp(), NdTrajectoryGenerator::startSinusoid(), NdTrajectoryGenerator::startSpline(), and NdTrajectoryGenerator::stop().
|
protected |
control loop time step.
Definition at line 171 of file nd-trajectory-generator.hh.
Referenced by dynamicgraph::sot::talos_balance::DEFINE_SIGNAL_OUT_FUNCTION(), NdTrajectoryGenerator::getValue(), NdTrajectoryGenerator::move(), NdTrajectoryGenerator::playTrajectoryFile(), NdTrajectoryGenerator::setSpline(), NdTrajectoryGenerator::startConstAcc(), NdTrajectoryGenerator::startLinearChirp(), NdTrajectoryGenerator::startSinusoid(), NdTrajectoryGenerator::startSpline(), and NdTrajectoryGenerator::stop().
|
protected |
Definition at line 184 of file nd-trajectory-generator.hh.
Referenced by dynamicgraph::sot::talos_balance::DEFINE_SIGNAL_OUT_FUNCTION(), NdTrajectoryGenerator::init(), and NdTrajectoryGenerator::playTrajectoryFile().