sot-torque-control  1.6.5
Collection of dynamic-graph entities aimed at implementing torque control on different robots.
 
Loading...
Searching...
No Matches
se3-trajectory-generator.hh
Go to the documentation of this file.
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>
38
39namespace dynamicgraph {
40namespace sot {
41namespace torque_control {
42
43/* --------------------------------------------------------------------- */
44/* --- CLASS ----------------------------------------------------------- */
45/* --------------------------------------------------------------------- */
46
48 : public ::dynamicgraph::Entity {
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
77 void getValue(const int& id);
78
84 void move(const int& id, const double& xFinal, const double& time);
85
92 void startSinusoid(const int& id, const double& xFinal, const double& time);
93
100 void startTriangle(const int& id, const double& xFinal, const double& time,
101 const double& Tacc);
102
110 void startConstAcc(const int& id, const double& xFinal, const double& time);
111
121 void startLinearChirp(const int& id, const double& xFinal, const double& f0,
122 const double& f1, const double& time);
123
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:
148 TG_SPLINE
149 };
150
151 bool
154 double m_dt;
155 unsigned int m_np;
156 unsigned int m_nv;
157 unsigned int m_iterLast;
158
159 double m_t;
160 Eigen::Matrix3d m_splineRotation;
161 parametriccurves::Spline<double, Eigen::Dynamic>* m_splineTrajGen;
163
164 std::vector<TG_Status> m_status;
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;
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__
void sendMsg(const std::string &msg, MsgType t=MSG_TYPE_INFO, const char *="", int=0)
DECLARE_SIGNAL_IN(initial_value, dynamicgraph::Vector)
std::vector< SinusoidTrajectoryGenerator * > m_sinTrajGen
std::vector< LinearChirpTrajectoryGenerator * > m_linChirpTrajGen
double m_dt
true if it is the first iteration, false otherwise
std::vector< ConstantAccelerationTrajectoryGenerator * > m_constAccTrajGen
parametriccurves::Spline< double, Eigen::Dynamic > * m_splineTrajGen
bool m_firstIter
true if the entity has been successfully initialized
std::vector< TriangleTrajectoryGenerator * > m_triangleTrajGen
std::vector< AbstractTrajectoryGenerator * > m_currentTrajGen
status of the component
std::vector< MinimumJerkTrajectoryGenerator * > m_minJerkTrajGen
to read text file
Definition: treeview.dox:22
#define SOTSE3TRAJECTORYGENERATOR_EXPORT