#include <sot/torque_control/joint-trajectory-generator.hh>
Public Member Functions | |
JointTrajectoryGenerator (const std::string &name) | |
DECLARE_SIGNAL (fRightFoot, OUT, dynamicgraph::Vector) | |
DECLARE_SIGNAL (fLeftFoot, OUT, dynamicgraph::Vector) | |
DECLARE_SIGNAL (fRightHand, OUT, dynamicgraph::Vector) | |
DECLARE_SIGNAL (fLeftHand, OUT, dynamicgraph::Vector) | |
DECLARE_SIGNAL_IN (base6d_encoders, dynamicgraph::Vector) | |
DECLARE_SIGNAL_OUT (q, dynamicgraph::Vector) | |
DECLARE_SIGNAL_OUT (dq, dynamicgraph::Vector) | |
DECLARE_SIGNAL_OUT (ddq, dynamicgraph::Vector) | |
virtual void | display (std::ostream &os) const |
void | getJoint (const std::string &jointName) |
void | init (const double &dt, const std::string &robotRef) |
bool | isTrajectoryEnded () |
void | moveForce (const std::string &forceName, const int &axis, const double &fFinal, const double &time) |
void | moveJoint (const std::string &jointName, const double &qFinal, 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 | startConstAcc (const std::string &jointName, const double &qFinal, const double &time) |
void | startForceLinearChirp (const std::string &forceName, const int &axis, const double &fFinal, const double &f0, const double &f1, const double &time) |
void | startForceSinusoid (const std::string &forceName, const int &axis, const double &fFinal, const double &time) |
void | startLinearChirp (const std::string &jointName, const double &qFinal, const double &f0, const double &f1, const double &time) |
void | startSinusoid (const std::string &jointName, const double &qFinal, const double &time) |
void | startTriangle (const std::string &jointName, const double &qFinal, const double &time, const double &Tacc) |
void | stop (const std::string &jointName) |
void | stopForce (const std::string &forceName) |
Protected Types | |
enum | JTG_Status { JTG_STOP, JTG_SINUSOID, JTG_MIN_JERK, JTG_LIN_CHIRP, JTG_TRIANGLE, JTG_CONST_ACC, JTG_TEXT_FILE } |
Protected Member Functions | |
bool | convertForceNameToForceId (const std::string &name, unsigned int &id) |
bool | convertJointNameToJointId (const std::string &name, unsigned int &id) |
DECLARE_SIGNAL_OUT_FUNCTION (fRightFoot, dynamicgraph::Vector) | |
DECLARE_SIGNAL_OUT_FUNCTION (fLeftFoot, dynamicgraph::Vector) | |
DECLARE_SIGNAL_OUT_FUNCTION (fRightHand, dynamicgraph::Vector) | |
DECLARE_SIGNAL_OUT_FUNCTION (fLeftHand, dynamicgraph::Vector) | |
bool | generateReferenceForceSignal (const std::string &forceName, int fid, dynamicgraph::Vector &s, int iter) |
bool | isForceInRange (unsigned int id, const Eigen::VectorXd &f) |
bool | isForceInRange (unsigned int id, int axis, double f) |
bool | isJointInRange (unsigned int id, double q) |
Definition at line 48 of file joint-trajectory-generator.hh.
|
protected |
Enumerator | |
---|---|
JTG_STOP | |
JTG_SINUSOID | |
JTG_MIN_JERK | |
JTG_LIN_CHIRP | |
JTG_TRIANGLE | |
JTG_CONST_ACC | |
JTG_TEXT_FILE |
Definition at line 158 of file joint-trajectory-generator.hh.
JointTrajectoryGenerator | ( | const std::string & | name | ) |
Definition at line 37 of file joint-trajectory-generator.cpp.
|
protected |
Definition at line 645 of file joint-trajectory-generator.cpp.
|
protected |
Definition at line 629 of file joint-trajectory-generator.cpp.
DECLARE_SIGNAL | ( | fRightFoot | , |
OUT | , | ||
dynamicgraph::Vector | |||
) |
DECLARE_SIGNAL | ( | fLeftFoot | , |
OUT | , | ||
dynamicgraph::Vector | |||
) |
DECLARE_SIGNAL | ( | fRightHand | , |
OUT | , | ||
dynamicgraph::Vector | |||
) |
DECLARE_SIGNAL | ( | fLeftHand | , |
OUT | , | ||
dynamicgraph::Vector | |||
) |
DECLARE_SIGNAL_IN | ( | base6d_encoders | , |
dynamicgraph::Vector | |||
) |
DECLARE_SIGNAL_OUT | ( | q | , |
dynamicgraph::Vector | |||
) |
DECLARE_SIGNAL_OUT | ( | dq | , |
dynamicgraph::Vector | |||
) |
DECLARE_SIGNAL_OUT | ( | ddq | , |
dynamicgraph::Vector | |||
) |
|
protected |
|
protected |
|
protected |
|
protected |
|
virtual |
Definition at line 705 of file joint-trajectory-generator.cpp.
|
protected |
Definition at line 309 of file joint-trajectory-generator.cpp.
void getJoint | ( | const std::string & | jointName | ) |
Print the current angle of the specified joint.
Definition at line 346 of file joint-trajectory-generator.cpp.
void init | ( | const double & | dt, |
const std::string & | robotRef | ||
) |
Definition at line 155 of file joint-trajectory-generator.cpp.
|
protected |
Definition at line 678 of file joint-trajectory-generator.cpp.
|
protected |
Definition at line 690 of file joint-trajectory-generator.cpp.
|
protected |
Definition at line 661 of file joint-trajectory-generator.cpp.
bool isTrajectoryEnded | ( | ) |
Returns whether all given trajectories have ended
Definition at line 353 of file joint-trajectory-generator.cpp.
void moveForce | ( | const std::string & | forceName, |
const int & | axis, | ||
const double & | fFinal, | ||
const double & | time | ||
) |
Definition at line 574 of file joint-trajectory-generator.cpp.
void moveJoint | ( | const std::string & | jointName, |
const double & | qFinal, | ||
const double & | time | ||
) |
Move a joint to a position with a minimum-jerk trajectory.
jointName | The short name of the joint. |
qFinal | The desired final position of the joint [rad]. |
time | The time to go from the current position to qFinal [sec]. |
Definition at line 557 of file joint-trajectory-generator.cpp.
void playTrajectoryFile | ( | const std::string & | fileName | ) |
Definition at line 375 of file joint-trajectory-generator.cpp.
|
inline |
Definition at line 153 of file joint-trajectory-generator.hh.
void startConstAcc | ( | const std::string & | jointName, |
const double & | qFinal, | ||
const double & | time | ||
) |
Start an infinite trajectory with piece-wise constant acceleration.
jointName | The short name of the joint. |
qFinal | The position of the joint corresponding to the max amplitude of the trajectory [rad]. |
time | The time to go from the current position to qFinal [sec]. |
Tacc | The time during witch acceleration is keept constant [sec]. |
Definition at line 459 of file joint-trajectory-generator.cpp.
void startForceLinearChirp | ( | const std::string & | forceName, |
const int & | axis, | ||
const double & | fFinal, | ||
const double & | f0, | ||
const double & | f1, | ||
const double & | time | ||
) |
Definition at line 528 of file joint-trajectory-generator.cpp.
void startForceSinusoid | ( | const std::string & | forceName, |
const int & | axis, | ||
const double & | fFinal, | ||
const double & | time | ||
) |
Start an infinite sinusoidal trajectory for the specified force signal.
forceName | The short name of the force signal (rh, lh, rf, lf). |
fFinal | The 6d force corresponding to the max amplitude of the sinusoid [N/Nm]. |
time | The time to go from 0 to fFinal [sec]. |
Definition at line 478 of file joint-trajectory-generator.cpp.
void startLinearChirp | ( | const std::string & | jointName, |
const double & | qFinal, | ||
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.
jointName | The short name of the joint. |
qFinal | The position of the joint 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 501 of file joint-trajectory-generator.cpp.
void startSinusoid | ( | const std::string & | jointName, |
const double & | qFinal, | ||
const double & | time | ||
) |
Start an infinite sinusoidal trajectory.
jointName | The short name of the joint. |
qFinal | The position of the joint corresponding to the max amplitude of the sinusoid [rad]. |
time | The time to go from the current position to qFinal [sec]. |
Definition at line 416 of file joint-trajectory-generator.cpp.
void startTriangle | ( | const std::string & | jointName, |
const double & | qFinal, | ||
const double & | time, | ||
const double & | Tacc | ||
) |
Start an infinite triangle trajectory.
jointName | The short name of the joint. |
qFinal | The position of the joint corresponding to the max amplitude of the trajectory [rad]. |
time | The time to go from the current position to qFinal [sec]. |
Definition at line 434 of file joint-trajectory-generator.cpp.
void stop | ( | const std::string & | jointName | ) |
Stop the motion of the specified joint. If jointName is "all" it stops the motion of all joints.
jointName | A string identifying the joint to stop. |
Definition at line 594 of file joint-trajectory-generator.cpp.
void stopForce | ( | const std::string & | forceName | ) |
Stop the trajectory of the specified force. If forceName is "all" it stops all forces.
forceName | A string identifying the force to stop. |
Definition at line 615 of file joint-trajectory-generator.cpp.
|
protected |
Definition at line 175 of file joint-trajectory-generator.hh.
|
protected |
status of the joints
Definition at line 169 of file joint-trajectory-generator.hh.
|
protected |
status of the forces
Definition at line 179 of file joint-trajectory-generator.hh.
|
protected |
true if it is the first iteration, false otherwise
Definition at line 162 of file joint-trajectory-generator.hh.
|
protected |
true if the entity has been successfully initialized
Definition at line 161 of file joint-trajectory-generator.hh.
|
protected |
Definition at line 160 of file joint-trajectory-generator.hh.
|
protected |
Definition at line 166 of file joint-trajectory-generator.hh.
|
protected |
Definition at line 173 of file joint-trajectory-generator.hh.
|
protected |
Definition at line 183 of file joint-trajectory-generator.hh.
|
protected |
Definition at line 171 of file joint-trajectory-generator.hh.
|
protected |
Definition at line 182 of file joint-trajectory-generator.hh.
|
protected |
Definition at line 170 of file joint-trajectory-generator.hh.
|
protected |
Definition at line 180 of file joint-trajectory-generator.hh.
|
protected |
control loop time period
Definition at line 164 of file joint-trajectory-generator.hh.
|
protected |
Definition at line 172 of file joint-trajectory-generator.hh.
|
protected |
Definition at line 181 of file joint-trajectory-generator.hh.
|
protected |
Definition at line 168 of file joint-trajectory-generator.hh.
|
protected |
Definition at line 178 of file joint-trajectory-generator.hh.
|
protected |
Definition at line 176 of file joint-trajectory-generator.hh.
|
protected |
Definition at line 174 of file joint-trajectory-generator.hh.