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 namespace dg = dynamicgraph;
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  dg::Vector &getNextPosition(dg::Vector &pos, const int &time);
61 
63  dg::Vector &getNextCoM(dg::Vector &com, const int &time);
64 
66  dg::Vector &getNextCoP(dg::Vector &cop, const int &time);
67 
70  const int &time);
71 
73  unsigned int &getSeqId(unsigned int &seqid, const int &time);
74 
77  XYZThetaToMatrixHomogeneous(const dg::Vector &xyztheta);
78 
80  int &OneStepOfUpdate(int &dummy, const int &time);
81 
84  virtual void display(std::ostream &os) const;
86  friend std::ostream &operator<<(std::ostream &os,
87  const SotJointTrajectoryEntity &r) {
88  r.display(os);
89  return os;
90  }
92 
93 public:
94  typedef int Dummy;
95 
99  dynamicgraph::SignalTimeDependent<int, int> refresherSINTERN;
100 
102  SignalTimeDependent<Dummy, int> OneStepOfUpdateS;
103 
105  dynamicgraph::SignalTimeDependent<dg::Vector, int> positionSOUT;
106 
108  dynamicgraph::SignalTimeDependent<dg::Vector, int> comSOUT;
109 
111  dynamicgraph::SignalTimeDependent<dg::Vector, int> zmpSOUT;
112 
114  dynamicgraph::SignalTimeDependent<sot::MatrixHomogeneous, int> waistSOUT;
115 
117  dynamicgraph::SignalTimeDependent<unsigned int, int> seqIdSOUT;
118 
120  dynamicgraph::SignalPtr<Trajectory, int> trajectorySIN;
122 
123 protected:
125  std::deque<sot::Trajectory>::size_type index_;
126 
129 
131  dg::Vector pose_;
132 
134  dg::Vector com_;
135 
137  dg::Vector cop_;
138 
141 
143  unsigned int seqid_;
144 
147 
149  std::deque<sot::Trajectory> deque_traj_;
150 
152  void UpdatePoint(const JointTrajectoryPoint &aJTP);
153 
155  void UpdateTrajectory(const Trajectory &aTrajectory);
156 
158  void setInitTraj(const std::string &os);
159 };
160 
161 } /* namespace sot */
162 } /* namespace dynamicgraph */
163 
164 #endif // SOT_JOINT_TRAJECTORY_ENTITY_HH
dynamicgraph::sot::SotJointTrajectoryEntity::~SotJointTrajectoryEntity
virtual ~SotJointTrajectoryEntity()
Definition: joint-trajectory-entity.hh:55
dynamicgraph::sot::SotJointTrajectoryEntity::pose_
dg::Vector pose_
Store the pos;.
Definition: joint-trajectory-entity.hh:131
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< dg::Vector, int > zmpSOUT
Publish zmp for each evaluation of the graph.
Definition: joint-trajectory-entity.hh:111
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:117
dynamicgraph::sot::timestamp
Definition: trajectory.hh:83
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:146
dynamicgraph::sot::SotJointTrajectoryEntity::operator<<
SOTJOINT_TRAJECTORY_ENTITY_EXPORT friend std::ostream & operator<<(std::ostream &os, const SotJointTrajectoryEntity &r)
Definition: joint-trajectory-entity.hh:86
dynamicgraph::sot::SotJointTrajectoryEntity::com_
dg::Vector com_
Store the center of mass.
Definition: joint-trajectory-entity.hh:134
dynamicgraph::sot::SotJointTrajectoryEntity::deque_traj_
std::deque< sot::Trajectory > deque_traj_
Queue of trajectories.
Definition: joint-trajectory-entity.hh:149
dynamicgraph::sot::SotJointTrajectoryEntity::index_
std::deque< sot::Trajectory >::size_type index_
Index on the point along the trajectory.
Definition: joint-trajectory-entity.hh:125
dynamicgraph::sot::SotJointTrajectoryEntity::positionSOUT
dynamicgraph::SignalTimeDependent< dg::Vector, int > positionSOUT
Publish pose for each evaluation of the graph.
Definition: joint-trajectory-entity.hh:105
dynamicgraph::sot::SotJointTrajectoryEntity::Dummy
int Dummy
Definition: joint-trajectory-entity.hh:94
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:120
dynamicgraph::sot::SotJointTrajectoryEntity::cop_
dg::Vector cop_
Store the center of pressure ZMP.
Definition: joint-trajectory-entity.hh:137
dynamicgraph::sot::Trajectory
Definition: trajectory.hh:177
dynamicgraph::sot::SotJointTrajectoryEntity::traj_timestamp_
timestamp traj_timestamp_
Keep the starting time as an identifier of the trajector.
Definition: joint-trajectory-entity.hh:128
dynamicgraph::sot::SotJointTrajectoryEntity::comSOUT
dynamicgraph::SignalTimeDependent< dg::Vector, int > comSOUT
Publish com for each evaluation of the graph.
Definition: joint-trajectory-entity.hh:108
dynamicgraph::sot::SotJointTrajectoryEntity::seqid_
unsigned int seqid_
Store the current seq identifier.
Definition: joint-trajectory-entity.hh:143
matrix-geometry.hh
dynamicgraph::sot::JointTrajectoryPoint
Definition: trajectory.hh:115
dynamicgraph::sot::SotJointTrajectoryEntity::waist_
sot::MatrixHomogeneous waist_
Store the waist position.
Definition: joint-trajectory-entity.hh:140
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:114
dynamicgraph::sot::SotJointTrajectoryEntity::OneStepOfUpdateS
SignalTimeDependent< Dummy, int > OneStepOfUpdateS
Internal signal to trigger one step of the algorithm.
Definition: joint-trajectory-entity.hh:102
dynamicgraph::sot::SotJointTrajectoryEntity::refresherSINTERN
dynamicgraph::SignalTimeDependent< int, int > refresherSINTERN
Definition: joint-trajectory-entity.hh:99