This object handles trajectory of quantities and publish them as signals. More...
#include <sot/core/joint-trajectory-entity.hh>
Public Types | |
typedef int | Dummy |
Public Member Functions | |
DYNAMIC_GRAPH_ENTITY_DECL () | |
SotJointTrajectoryEntity (const std::string &name) | |
Constructor. | |
virtual | ~SotJointTrajectoryEntity () |
void | loadFile (const std::string &name) |
dg::Vector & | getNextPosition (dg::Vector &pos, const int &time) |
Return the next pose for the legs. | |
dg::Vector & | getNextCoM (dg::Vector &com, const int &time) |
Return the next com. | |
dg::Vector & | getNextCoP (dg::Vector &cop, const int &time) |
Return the next cop. | |
sot::MatrixHomogeneous & | getNextWaist (sot::MatrixHomogeneous &waist, const int &time) |
Return the next waist. | |
unsigned int & | getSeqId (unsigned int &seqid, const int &time) |
Return the current seq identified of the current trajectory. | |
sot::MatrixHomogeneous | XYZThetaToMatrixHomogeneous (const dg::Vector &xyztheta) |
Convert a xyztheta vector into an homogeneous matrix. | |
int & | OneStepOfUpdate (int &dummy, const int &time) |
Perform one update of the signals. | |
Public Attributes | |
Signals | |
Internal signal for synchronisation. | |
dynamicgraph::SignalTimeDependent < int, int > | refresherSINTERN |
SignalTimeDependent< Dummy, int > | OneStepOfUpdateS |
Internal signal to trigger one step of the algorithm. | |
dynamicgraph::SignalTimeDependent < dg::Vector, int > | positionSOUT |
Publish pose for each evaluation of the graph. | |
dynamicgraph::SignalTimeDependent < dg::Vector, int > | comSOUT |
Publish com for each evaluation of the graph. | |
dynamicgraph::SignalTimeDependent < dg::Vector, int > | zmpSOUT |
Publish zmp for each evaluation of the graph. | |
dynamicgraph::SignalTimeDependent < sot::MatrixHomogeneous, int > | waistSOUT |
Publish waist for each evaluation of the graph. | |
dynamicgraph::SignalTimeDependent < unsigned int, int > | seqIdSOUT |
Publish ID of the trajectory currently realized. | |
dynamicgraph::SignalPtr < Trajectory, int > | trajectorySIN |
Read a trajectory. | |
Protected Member Functions | |
void | UpdatePoint (const JointTrajectoryPoint &aJTP) |
Update the entity with the current point of the trajectory. | |
void | UpdateTrajectory (const Trajectory &aTrajectory) |
Update the entity with the trajectory aTrajectory. | |
virtual void | commandLine (const std::string &cmdLine, std::istringstream &cmdArs, std::ostream &os) |
Command Line to dynamically modify parameters: initTraj: specify the initial vector of data to send. | |
void | setInitTraj (const std::string &os) |
Implements the parsing and the affectation of initial trajectory. | |
Protected Attributes | |
unsigned int | index_ |
Index on the point along the trajectory. | |
timestamp | traj_timestamp_ |
Keep the starting time as an identifier of the trajector. | |
dg::Vector | pose_ |
Store the pos;. | |
dg::Vector | com_ |
Store the center of mass. | |
dg::Vector | cop_ |
Store the center of pressure ZMP. | |
sot::MatrixHomogeneous | waist_ |
Store the waist position. | |
unsigned int | seqid_ |
Store the current seq identifier. | |
sot::Trajectory | init_traj_ |
Initial state of the trajectory. | |
std::deque< sot::Trajectory > | deque_traj_ |
Queue of trajectories. | |
Display | |
virtual void | display (std::ostream &os) const |
SOTJOINT_TRAJECTORY_ENTITY_EXPORT friend std::ostream & | operator<< (std::ostream &os, const SotJointTrajectoryEntity &r) |
This object handles trajectory of quantities and publish them as signals.
dynamicgraph::sot::SotJointTrajectoryEntity::SotJointTrajectoryEntity | ( | const std::string & | name | ) |
Constructor.
virtual dynamicgraph::sot::SotJointTrajectoryEntity::~SotJointTrajectoryEntity | ( | ) | [inline, virtual] |
virtual void dynamicgraph::sot::SotJointTrajectoryEntity::commandLine | ( | const std::string & | cmdLine, |
std::istringstream & | cmdArs, | ||
std::ostream & | os | ||
) | [protected, virtual] |
Command Line to dynamically modify parameters: initTraj: specify the initial vector of data to send.
Reimplemented from dynamicgraph::Entity.
virtual void dynamicgraph::sot::SotJointTrajectoryEntity::display | ( | std::ostream & | os | ) | const [virtual] |
Reimplemented from dynamicgraph::Entity.
dynamicgraph::sot::SotJointTrajectoryEntity::DYNAMIC_GRAPH_ENTITY_DECL | ( | ) |
dg::Vector& dynamicgraph::sot::SotJointTrajectoryEntity::getNextCoM | ( | dg::Vector & | com, |
const int & | time | ||
) |
Return the next com.
dg::Vector& dynamicgraph::sot::SotJointTrajectoryEntity::getNextCoP | ( | dg::Vector & | cop, |
const int & | time | ||
) |
Return the next cop.
dg::Vector& dynamicgraph::sot::SotJointTrajectoryEntity::getNextPosition | ( | dg::Vector & | pos, |
const int & | time | ||
) |
Return the next pose for the legs.
sot::MatrixHomogeneous& dynamicgraph::sot::SotJointTrajectoryEntity::getNextWaist | ( | sot::MatrixHomogeneous & | waist, |
const int & | time | ||
) |
Return the next waist.
unsigned int& dynamicgraph::sot::SotJointTrajectoryEntity::getSeqId | ( | unsigned int & | seqid, |
const int & | time | ||
) |
Return the current seq identified of the current trajectory.
void dynamicgraph::sot::SotJointTrajectoryEntity::loadFile | ( | const std::string & | name | ) |
int& dynamicgraph::sot::SotJointTrajectoryEntity::OneStepOfUpdate | ( | int & | dummy, |
const int & | time | ||
) |
Perform one update of the signals.
void dynamicgraph::sot::SotJointTrajectoryEntity::setInitTraj | ( | const std::string & | os | ) | [protected] |
Implements the parsing and the affectation of initial trajectory.
void dynamicgraph::sot::SotJointTrajectoryEntity::UpdatePoint | ( | const JointTrajectoryPoint & | aJTP | ) | [protected] |
Update the entity with the current point of the trajectory.
void dynamicgraph::sot::SotJointTrajectoryEntity::UpdateTrajectory | ( | const Trajectory & | aTrajectory | ) | [protected] |
Update the entity with the trajectory aTrajectory.
sot::MatrixHomogeneous dynamicgraph::sot::SotJointTrajectoryEntity::XYZThetaToMatrixHomogeneous | ( | const dg::Vector & | xyztheta | ) |
Convert a xyztheta vector into an homogeneous matrix.
SOTJOINT_TRAJECTORY_ENTITY_EXPORT friend std::ostream& operator<< | ( | std::ostream & | os, |
const SotJointTrajectoryEntity & | r | ||
) | [friend] |
Store the center of mass.
dynamicgraph::SignalTimeDependent<dg::Vector,int> dynamicgraph::sot::SotJointTrajectoryEntity::comSOUT |
Publish com for each evaluation of the graph.
Store the center of pressure ZMP.
std::deque<sot::Trajectory> dynamicgraph::sot::SotJointTrajectoryEntity::deque_traj_ [protected] |
Queue of trajectories.
unsigned int dynamicgraph::sot::SotJointTrajectoryEntity::index_ [protected] |
Index on the point along the trajectory.
Initial state of the trajectory.
Internal signal to trigger one step of the algorithm.
Store the pos;.
dynamicgraph::SignalTimeDependent<dg::Vector,int> dynamicgraph::sot::SotJointTrajectoryEntity::positionSOUT |
Publish pose for each evaluation of the graph.
dynamicgraph::SignalTimeDependent<int,int> dynamicgraph::sot::SotJointTrajectoryEntity::refresherSINTERN |
unsigned int dynamicgraph::sot::SotJointTrajectoryEntity::seqid_ [protected] |
Store the current seq identifier.
dynamicgraph::SignalTimeDependent<unsigned int,int> dynamicgraph::sot::SotJointTrajectoryEntity::seqIdSOUT |
Publish ID of the trajectory currently realized.
Keep the starting time as an identifier of the trajector.
Read a trajectory.
Store the waist position.
dynamicgraph::SignalTimeDependent<sot::MatrixHomogeneous,int> dynamicgraph::sot::SotJointTrajectoryEntity::waistSOUT |
Publish waist for each evaluation of the graph.
dynamicgraph::SignalTimeDependent<dg::Vector,int> dynamicgraph::sot::SotJointTrajectoryEntity::zmpSOUT |
Publish zmp for each evaluation of the graph.