sot-core  4.11.4
Hierarchical task solver plug-in for dynamic-graph.
joint-trajectory-entity.hh
Go to the documentation of this file.
1 /*
2  * Copyright 2013,
3  * Olivier Stasse,
4  *
5  * CNRS
6  *
7  */
8 
9 #ifndef SOT_JOINT_TRAJECTORY_ENTITY_HH
10 #define SOT_JOINT_TRAJECTORY_ENTITY_HH
11 
12 #include <list>
13 
14 #include <deque>
15 
16 // Maal
17 #include <dynamic-graph/linear-algebra.h>
18 
19 // SOT
20 #include <dynamic-graph/all-signals.h>
21 #include <dynamic-graph/entity.h>
23 #include <sot/core/trajectory.hh>
24 
25 #include <sstream>
26 // API
27 
28 #if defined(WIN32)
29 #if defined(joint_trajectory_entity_EXPORTS)
30 #define SOTJOINT_TRAJECTORY_ENTITY_EXPORT __declspec(dllexport)
31 #else
32 #define SOTJOINT_TRAJECTORY_ENTITY_EXPORT __declspec(dllimport)
33 #endif
34 #else
35 #define SOTJOINT_TRAJECTORY_ENTITY_EXPORT
36 #endif
37 
38 // Class
39 
40 namespace dynamicgraph {
41 namespace sot {
42 
49  : public dynamicgraph::Entity {
50 public:
51  DYNAMIC_GRAPH_ENTITY_DECL();
52 
54  SotJointTrajectoryEntity(const std::string &name);
56 
57  void loadFile(const std::string &name);
58 
60  dynamicgraph::Vector &getNextPosition(dynamicgraph::Vector &pos,
61  const int &time);
62 
64  dynamicgraph::Vector &getNextCoM(dynamicgraph::Vector &com, const int &time);
65 
67  dynamicgraph::Vector &getNextCoP(dynamicgraph::Vector &cop, const int &time);
68 
71  const int &time);
72 
74  unsigned int &getSeqId(unsigned int &seqid, const int &time);
75 
78  XYZThetaToMatrixHomogeneous(const dynamicgraph::Vector &xyztheta);
79 
81  int &OneStepOfUpdate(int &dummy, const int &time);
82 
85  virtual void display(std::ostream &os) const;
87  friend std::ostream &operator<<(std::ostream &os,
88  const SotJointTrajectoryEntity &r) {
89  r.display(os);
90  return os;
91  }
93 
94 public:
95  typedef int Dummy;
96 
100  dynamicgraph::SignalTimeDependent<int, int> refresherSINTERN;
101 
103  SignalTimeDependent<Dummy, int> OneStepOfUpdateS;
104 
106  dynamicgraph::SignalTimeDependent<dynamicgraph::Vector, int> positionSOUT;
107 
109  dynamicgraph::SignalTimeDependent<dynamicgraph::Vector, int> comSOUT;
110 
112  dynamicgraph::SignalTimeDependent<dynamicgraph::Vector, int> zmpSOUT;
113 
115  dynamicgraph::SignalTimeDependent<sot::MatrixHomogeneous, int> waistSOUT;
116 
118  dynamicgraph::SignalTimeDependent<unsigned int, int> seqIdSOUT;
119 
121  dynamicgraph::SignalPtr<Trajectory, int> trajectorySIN;
123 
124 protected:
126  std::deque<sot::Trajectory>::size_type index_;
127 
130 
132  dynamicgraph::Vector pose_;
133 
135  dynamicgraph::Vector com_;
136 
138  dynamicgraph::Vector cop_;
139 
142 
144  unsigned int seqid_;
145 
148 
150  std::deque<sot::Trajectory> deque_traj_;
151 
153  void UpdatePoint(const JointTrajectoryPoint &aJTP);
154 
156  void UpdateTrajectory(const Trajectory &aTrajectory);
157 
159  void setInitTraj(const std::string &os);
160 };
161 
162 } /* namespace sot */
163 } /* namespace dynamicgraph */
164 
165 #endif // SOT_JOINT_TRAJECTORY_ENTITY_HH
dynamicgraph::sot::SotJointTrajectoryEntity::positionSOUT
dynamicgraph::SignalTimeDependent< dynamicgraph::Vector, int > positionSOUT
Publish pose for each evaluation of the graph.
Definition: joint-trajectory-entity.hh:106
dynamicgraph::sot::SotJointTrajectoryEntity::~SotJointTrajectoryEntity
virtual ~SotJointTrajectoryEntity()
Definition: joint-trajectory-entity.hh:55
trajectory.hh
dynamicgraph::sot::MatrixHomogeneous
Eigen::Transform< double, 3, Eigen::Affine > SOT_CORE_EXPORT MatrixHomogeneous
Definition: matrix-geometry.hh:74
dynamicgraph::sot::SotJointTrajectoryEntity::zmpSOUT
dynamicgraph::SignalTimeDependent< dynamicgraph::Vector, int > zmpSOUT
Publish zmp for each evaluation of the graph.
Definition: joint-trajectory-entity.hh:112
dynamicgraph
Definition: abstract-sot-external-interface.hh:17
dynamicgraph::sot::SotJointTrajectoryEntity::seqIdSOUT
dynamicgraph::SignalTimeDependent< unsigned int, int > seqIdSOUT
Publish ID of the trajectory currently realized.
Definition: joint-trajectory-entity.hh:118
dynamicgraph::sot::timestamp
Definition: trajectory.hh:85
dynamicgraph::sot::SotJointTrajectoryEntity
This object handles trajectory of quantities and publish them as signals.
Definition: joint-trajectory-entity.hh:48
dynamicgraph::sot::SotJointTrajectoryEntity::init_traj_
sot::Trajectory init_traj_
Initial state of the trajectory.
Definition: joint-trajectory-entity.hh:147
dynamicgraph::sot::SotJointTrajectoryEntity::operator<<
SOTJOINT_TRAJECTORY_ENTITY_EXPORT friend std::ostream & operator<<(std::ostream &os, const SotJointTrajectoryEntity &r)
Definition: joint-trajectory-entity.hh:87
dynamicgraph::sot::SotJointTrajectoryEntity::deque_traj_
std::deque< sot::Trajectory > deque_traj_
Queue of trajectories.
Definition: joint-trajectory-entity.hh:150
dynamicgraph::sot::SotJointTrajectoryEntity::index_
std::deque< sot::Trajectory >::size_type index_
Index on the point along the trajectory.
Definition: joint-trajectory-entity.hh:126
dynamicgraph::sot::SotJointTrajectoryEntity::Dummy
int Dummy
Definition: joint-trajectory-entity.hh:95
dynamicgraph::sot::SotJointTrajectoryEntity::com_
dynamicgraph::Vector com_
Store the center of mass.
Definition: joint-trajectory-entity.hh:135
SOTJOINT_TRAJECTORY_ENTITY_EXPORT
#define SOTJOINT_TRAJECTORY_ENTITY_EXPORT
Definition: joint-trajectory-entity.hh:35
dynamicgraph::sot::SotJointTrajectoryEntity::trajectorySIN
dynamicgraph::SignalPtr< Trajectory, int > trajectorySIN
Read a trajectory.
Definition: joint-trajectory-entity.hh:121
dynamicgraph::sot::SotJointTrajectoryEntity::comSOUT
dynamicgraph::SignalTimeDependent< dynamicgraph::Vector, int > comSOUT
Publish com for each evaluation of the graph.
Definition: joint-trajectory-entity.hh:109
dynamicgraph::sot::Trajectory
Definition: trajectory.hh:179
dynamicgraph::sot::SotJointTrajectoryEntity::traj_timestamp_
timestamp traj_timestamp_
Keep the starting time as an identifier of the trajector.
Definition: joint-trajectory-entity.hh:129
dynamicgraph::sot::SotJointTrajectoryEntity::seqid_
unsigned int seqid_
Store the current seq identifier.
Definition: joint-trajectory-entity.hh:144
matrix-geometry.hh
dynamicgraph::sot::JointTrajectoryPoint
Definition: trajectory.hh:117
dynamicgraph::sot::SotJointTrajectoryEntity::waist_
sot::MatrixHomogeneous waist_
Store the waist position.
Definition: joint-trajectory-entity.hh:141
dynamicgraph::sot::SotJointTrajectoryEntity::display
virtual void display(std::ostream &os) const
dynamicgraph::sot::SotJointTrajectoryEntity::waistSOUT
dynamicgraph::SignalTimeDependent< sot::MatrixHomogeneous, int > waistSOUT
Publish waist for each evaluation of the graph.
Definition: joint-trajectory-entity.hh:115
dynamicgraph::sot::SotJointTrajectoryEntity::pose_
dynamicgraph::Vector pose_
Store the pos;.
Definition: joint-trajectory-entity.hh:132
dynamicgraph::sot::SotJointTrajectoryEntity::OneStepOfUpdateS
SignalTimeDependent< Dummy, int > OneStepOfUpdateS
Internal signal to trigger one step of the algorithm.
Definition: joint-trajectory-entity.hh:103
dynamicgraph::sot::SotJointTrajectoryEntity::cop_
dynamicgraph::Vector cop_
Store the center of pressure ZMP.
Definition: joint-trajectory-entity.hh:138
dynamicgraph::sot::SotJointTrajectoryEntity::refresherSINTERN
dynamicgraph::SignalTimeDependent< int, int > refresherSINTERN
Definition: joint-trajectory-entity.hh:100