1 |
|
|
/* |
2 |
|
|
* Copyright 2017, Andrea Del Prete, LAAS-CNRS |
3 |
|
|
* |
4 |
|
|
*/ |
5 |
|
|
|
6 |
|
|
#ifndef __sot_torque_control_se3_trajectory_generator_H__ |
7 |
|
|
#define __sot_torque_control_se3_trajectory_generator_H__ |
8 |
|
|
|
9 |
|
|
/* --------------------------------------------------------------------- */ |
10 |
|
|
/* --- API ------------------------------------------------------------- */ |
11 |
|
|
/* --------------------------------------------------------------------- */ |
12 |
|
|
|
13 |
|
|
#if defined(WIN32) |
14 |
|
|
#if defined(se3_position_controller_EXPORTS) |
15 |
|
|
#define SOTSE3TRAJECTORYGENERATOR_EXPORT __declspec(dllexport) |
16 |
|
|
#else |
17 |
|
|
#define SOTSE3TRAJECTORYGENERATOR_EXPORT __declspec(dllimport) |
18 |
|
|
#endif |
19 |
|
|
#else |
20 |
|
|
#define SOTSE3TRAJECTORYGENERATOR_EXPORT |
21 |
|
|
#endif |
22 |
|
|
|
23 |
|
|
/* --------------------------------------------------------------------- */ |
24 |
|
|
/* --- INCLUDE --------------------------------------------------------- */ |
25 |
|
|
/* --------------------------------------------------------------------- */ |
26 |
|
|
|
27 |
|
|
#include <map> |
28 |
|
|
#include <parametric-curves/spline.hpp> |
29 |
|
|
|
30 |
|
|
#include "boost/assign.hpp" |
31 |
|
|
|
32 |
|
|
/* HELPER */ |
33 |
|
|
#include <dynamic-graph/signal-helper.h> |
34 |
|
|
|
35 |
|
|
#include <sot/core/matrix-geometry.hh> |
36 |
|
|
#include <sot/torque_control/utils/trajectory-generators.hh> |
37 |
|
|
#include <sot/torque_control/utils/vector-conversions.hh> |
38 |
|
|
|
39 |
|
|
namespace dynamicgraph { |
40 |
|
|
namespace sot { |
41 |
|
|
namespace torque_control { |
42 |
|
|
|
43 |
|
|
/* --------------------------------------------------------------------- */ |
44 |
|
|
/* --- CLASS ----------------------------------------------------------- */ |
45 |
|
|
/* --------------------------------------------------------------------- */ |
46 |
|
|
|
47 |
|
|
class SOTSE3TRAJECTORYGENERATOR_EXPORT SE3TrajectoryGenerator |
48 |
|
|
: public ::dynamicgraph::Entity { |
49 |
|
|
typedef SE3TrajectoryGenerator EntityClassName; |
50 |
|
|
DYNAMIC_GRAPH_ENTITY_DECL(); |
51 |
|
|
|
52 |
|
|
public: |
53 |
|
|
/* --- CONSTRUCTOR ---- */ |
54 |
|
|
SE3TrajectoryGenerator(const std::string& name); |
55 |
|
|
|
56 |
|
|
void init(const double& dt); |
57 |
|
|
|
58 |
|
|
/* --- SIGNALS --- */ |
59 |
|
|
DECLARE_SIGNAL_IN(initial_value, dynamicgraph::Vector); |
60 |
|
|
DECLARE_SIGNAL(x, OUT, dynamicgraph::Vector); |
61 |
|
|
DECLARE_SIGNAL_IN(trigger, bool); |
62 |
|
|
DECLARE_SIGNAL_OUT(dx, dynamicgraph::Vector); |
63 |
|
|
DECLARE_SIGNAL_OUT(ddx, dynamicgraph::Vector); |
64 |
|
|
|
65 |
|
|
protected: |
66 |
|
|
DECLARE_SIGNAL_OUT_FUNCTION(x, dynamicgraph::Vector); |
67 |
|
|
|
68 |
|
|
public: |
69 |
|
|
/* --- COMMANDS --- */ |
70 |
|
|
|
71 |
|
|
void playTrajectoryFile(const std::string& fileName); |
72 |
|
|
void startSpline(); |
73 |
|
|
void setSpline(const std::string& filename, const double& timeToInitConf, |
74 |
|
|
const Eigen::MatrixXd& init_rotation); |
75 |
|
|
|
76 |
|
|
/** Print the current value of the specified component. */ |
77 |
|
|
void getValue(const int& id); |
78 |
|
|
|
79 |
|
|
/** Move a component to a position with a minimum-jerk trajectory. |
80 |
|
|
* @param id integer index. |
81 |
|
|
* @param xFinal The desired final position of the component. |
82 |
|
|
* @param time The time to go from the current position to xFinal [sec]. |
83 |
|
|
*/ |
84 |
|
|
void move(const int& id, const double& xFinal, const double& time); |
85 |
|
|
|
86 |
|
|
/** Start an infinite sinusoidal trajectory. |
87 |
|
|
* @param id integer index. |
88 |
|
|
* @param xFinal The position of the component corresponding to the max |
89 |
|
|
* amplitude of the sinusoid. |
90 |
|
|
* @param time The time to go from the current position to xFinal [sec]. |
91 |
|
|
*/ |
92 |
|
|
void startSinusoid(const int& id, const double& xFinal, const double& time); |
93 |
|
|
|
94 |
|
|
/** Start an infinite triangle trajectory. |
95 |
|
|
* @param id integer index. |
96 |
|
|
* @param xFinal The position of the component corresponding to the max |
97 |
|
|
* amplitude of the trajectory. |
98 |
|
|
* @param time The time to go from the current position to xFinal [sec]. |
99 |
|
|
*/ |
100 |
|
|
void startTriangle(const int& id, const double& xFinal, const double& time, |
101 |
|
|
const double& Tacc); |
102 |
|
|
|
103 |
|
|
/** Start an infinite trajectory with piece-wise constant acceleration. |
104 |
|
|
* @param id integer index. |
105 |
|
|
* @param xFinal The position of the component corresponding to the max |
106 |
|
|
* amplitude of the trajectory. |
107 |
|
|
* @param time The time to go from the current position to xFinal [sec]. |
108 |
|
|
* @param Tacc The time during witch acceleration is keept constant [sec]. |
109 |
|
|
*/ |
110 |
|
|
void startConstAcc(const int& id, const double& xFinal, const double& time); |
111 |
|
|
|
112 |
|
|
/** Start a linear-chirp trajectory, that is a sinusoidal trajectory with |
113 |
|
|
* frequency being a linear function of time. |
114 |
|
|
* @param id integer index. |
115 |
|
|
* @param xFinal The position of the component corresponding to the max |
116 |
|
|
* amplitude of the sinusoid [rad]. |
117 |
|
|
* @param f0 The initial (min) frequency of the sinusoid [Hz] |
118 |
|
|
* @param f1 The final (max) frequency of the sinusoid [Hz] |
119 |
|
|
* @param time The time to get from f0 to f1 [sec] |
120 |
|
|
*/ |
121 |
|
|
void startLinearChirp(const int& id, const double& xFinal, const double& f0, |
122 |
|
|
const double& f1, const double& time); |
123 |
|
|
|
124 |
|
|
/** Stop the motion of the specified component. If id is -1 |
125 |
|
|
* it stops the trajectory of all the vector. |
126 |
|
|
* @param id integer index. |
127 |
|
|
* */ |
128 |
|
|
void stop(const int& id); |
129 |
|
|
|
130 |
|
|
/* --- ENTITY INHERITANCE --- */ |
131 |
|
|
virtual void display(std::ostream& os) const; |
132 |
|
|
|
133 |
|
|
void sendMsg(const std::string& msg, MsgType t = MSG_TYPE_INFO, |
134 |
|
|
const char* = "", int = 0) { |
135 |
|
|
logger_.stream(t) << ("[SE3TrajectoryGenerator-" + name + "] " + msg) |
136 |
|
|
<< '\n'; |
137 |
|
|
} |
138 |
|
|
|
139 |
|
|
protected: |
140 |
|
|
enum TG_Status { |
141 |
|
|
TG_STOP, |
142 |
|
|
TG_SINUSOID, |
143 |
|
|
TG_MIN_JERK, |
144 |
|
|
TG_LIN_CHIRP, |
145 |
|
|
TG_TRIANGLE, |
146 |
|
|
TG_CONST_ACC, |
147 |
|
|
TG_TEXT_FILE, |
148 |
|
|
TG_SPLINE |
149 |
|
|
}; |
150 |
|
|
|
151 |
|
|
bool |
152 |
|
|
m_initSucceeded; /// true if the entity has been successfully initialized |
153 |
|
|
bool m_firstIter; /// true if it is the first iteration, false otherwise |
154 |
|
|
double m_dt; /// control loop time period |
155 |
|
|
unsigned int m_np; /// size of position vector |
156 |
|
|
unsigned int m_nv; /// size of velocity vector |
157 |
|
|
unsigned int m_iterLast; /// last iter index |
158 |
|
|
|
159 |
|
|
double m_t; |
160 |
|
|
Eigen::Matrix3d m_splineRotation; |
161 |
|
|
parametriccurves::Spline<double, Eigen::Dynamic>* m_splineTrajGen; |
162 |
|
|
bool m_splineReady; |
163 |
|
|
|
164 |
|
|
std::vector<TG_Status> m_status; /// status of the component |
165 |
|
|
std::vector<AbstractTrajectoryGenerator*> m_currentTrajGen; |
166 |
|
|
std::vector<NoTrajectoryGenerator*> m_noTrajGen; |
167 |
|
|
std::vector<MinimumJerkTrajectoryGenerator*> m_minJerkTrajGen; |
168 |
|
|
std::vector<SinusoidTrajectoryGenerator*> m_sinTrajGen; |
169 |
|
|
std::vector<LinearChirpTrajectoryGenerator*> m_linChirpTrajGen; |
170 |
|
|
std::vector<TriangleTrajectoryGenerator*> m_triangleTrajGen; |
171 |
|
|
std::vector<ConstantAccelerationTrajectoryGenerator*> m_constAccTrajGen; |
172 |
|
|
TextFileTrajectoryGenerator* m_textFileTrajGen; |
173 |
|
|
|
174 |
|
|
}; // class SE3TrajectoryGenerator |
175 |
|
|
|
176 |
|
|
} // namespace torque_control |
177 |
|
|
} // namespace sot |
178 |
|
|
} // namespace dynamicgraph |
179 |
|
|
|
180 |
|
|
#endif // #ifndef __sot_torque_control_nd_trajectory_generator_H__ |