GCC Code Coverage Report

Line	Exec	Source
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 <deque>
13		#include <list>
14
15		// Maal
16		#include <dynamic-graph/linear-algebra.h>
17
18		// SOT
19		#include <dynamic-graph/all-signals.h>
20		#include <dynamic-graph/entity.h>
21
22		#include <sot/core/matrix-geometry.hh>
23		#include <sot/core/trajectory.hh>
24		#include <sstream>
25		// API
26
27		#if defined(WIN32)
28		#if defined(joint_trajectory_entity_EXPORTS)
29		#define SOTJOINT_TRAJECTORY_ENTITY_EXPORT __declspec(dllexport)
30		#else
31		#define SOTJOINT_TRAJECTORY_ENTITY_EXPORT __declspec(dllimport)
32		#endif
33		#else
34		#define SOTJOINT_TRAJECTORY_ENTITY_EXPORT
35		#endif
36
37		// Class
38
39		namespace dynamicgraph {
40		namespace sot {
41
42		/** \brief This object handles trajectory of quantities and publish them as
43		signals.
44
45		*/
46
47		class SOTJOINT_TRAJECTORY_ENTITY_EXPORT SotJointTrajectoryEntity
48		: public dynamicgraph::Entity {
49		public:
50	✗	DYNAMIC_GRAPH_ENTITY_DECL();
51
52		/// \brief Constructor
53		SotJointTrajectoryEntity(const std::string &name);
54	✗	virtual ~SotJointTrajectoryEntity() {}
55
56		void loadFile(const std::string &name);
57
58		/// \brief Return the next pose for the legs.
59		dynamicgraph::Vector &getNextPosition(dynamicgraph::Vector &pos,
60		const int &time);
61
62		/// \brief Return the next com.
63		dynamicgraph::Vector &getNextCoM(dynamicgraph::Vector &com, const int &time);
64
65		/// \brief Return the next cop.
66		dynamicgraph::Vector &getNextCoP(dynamicgraph::Vector &cop, const int &time);
67
68		/// \brief Return the next waist.
69		sot::MatrixHomogeneous &getNextWaist(sot::MatrixHomogeneous &waist,
70		const int &time);
71
72		/// \brief Return the current seq identified of the current trajectory.
73		unsigned int &getSeqId(unsigned int &seqid, const int &time);
74
75		/// \brief Convert a xyztheta vector into an homogeneous matrix
76		sot::MatrixHomogeneous XYZThetaToMatrixHomogeneous(
77		const dynamicgraph::Vector &xyztheta);
78
79		/// \brief Perform one update of the signals.
80		int &OneStepOfUpdate(int &dummy, const int &time);
81
82		/// @name Display
83		/// @{
84		virtual void display(std::ostream &os) const;
85		SOTJOINT_TRAJECTORY_ENTITY_EXPORT
86		friend std::ostream &operator<<(std::ostream &os,
87		const SotJointTrajectoryEntity &r) {
88		r.display(os);
89		return os;
90		}
91		/// @}
92
93		public:
94		typedef int Dummy;
95
96		/// @name Signals
97		/// @{
98		/// \brief Internal signal for synchronisation.
99		dynamicgraph::SignalTimeDependent<int, int> refresherSINTERN;
100
101		/// \brief Internal signal to trigger one step of the algorithm.
102		SignalTimeDependent<Dummy, int> OneStepOfUpdateS;
103
104		/// \brief Publish pose for each evaluation of the graph.
105		dynamicgraph::SignalTimeDependent<dynamicgraph::Vector, int> positionSOUT;
106
107		/// \brief Publish com for each evaluation of the graph.
108		dynamicgraph::SignalTimeDependent<dynamicgraph::Vector, int> comSOUT;
109
110		/// \brief Publish zmp for each evaluation of the graph.
111		dynamicgraph::SignalTimeDependent<dynamicgraph::Vector, int> zmpSOUT;
112
113		/// \brief Publish waist for each evaluation of the graph.
114		dynamicgraph::SignalTimeDependent<sot::MatrixHomogeneous, int> waistSOUT;
115
116		/// \brief Publish ID of the trajectory currently realized.
117		dynamicgraph::SignalTimeDependent<unsigned int, int> seqIdSOUT;
118
119		/// \brief Read a trajectory.
120		dynamicgraph::SignalPtr<Trajectory, int> trajectorySIN;
121		///@}
122
123		protected:
124		/// \brief Index on the point along the trajectory.
125		std::deque<sot::Trajectory>::size_type index_;
126
127		/// \brief Keep the starting time as an identifier of the trajector
128		timestamp traj_timestamp_;
129
130		/// \brief Store the pos;
131		dynamicgraph::Vector pose_;
132
133		/// \brief Store the center of mass.
134		dynamicgraph::Vector com_;
135
136		/// \brief Store the center of pressure ZMP.
137		dynamicgraph::Vector cop_;
138
139		/// \brief Store the waist position.
140		sot::MatrixHomogeneous waist_;
141
142		/// \brief Store the current seq identifier.
143		unsigned int seqid_;
144
145		/// \brief Initial state of the trajectory.
146		sot::Trajectory init_traj_;
147
148		/// \brief Queue of trajectories.
149		std::deque<sot::Trajectory> deque_traj_;
150
151		/// \brief Update the entity with the current point of the trajectory.
152		void UpdatePoint(const JointTrajectoryPoint &aJTP);
153
154		/// \brief Update the entity with the trajectory aTrajectory.
155		void UpdateTrajectory(const Trajectory &aTrajectory);
156
157		/// \brief Implements the parsing and the affectation of initial trajectory.
158		void setInitTraj(const std::string &os);
159		};
160
161		} /* namespace sot */
162		} /* namespace dynamicgraph */
163
164		#endif // SOT_JOINT_TRAJECTORY_ENTITY_HH
165