sot-torque-control  1.6.2
Collection of dynamic-graph entities aimed at implementing torque control on different robots.
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 "boost/assign.hpp"
29 
30 #include <parametric-curves/spline.hpp>
31 
32 /* HELPER */
33 #include <dynamic-graph/signal-helper.h>
34 #include <sot/core/matrix-geometry.hh>
37 
38 namespace dynamicgraph {
39 namespace sot {
40 namespace torque_control {
41 
42 /* --------------------------------------------------------------------- */
43 /* --- CLASS ----------------------------------------------------------- */
44 /* --------------------------------------------------------------------- */
45 
46 class SOTSE3TRAJECTORYGENERATOR_EXPORT SE3TrajectoryGenerator : public ::dynamicgraph::Entity {
48  DYNAMIC_GRAPH_ENTITY_DECL();
49 
50  public:
51  /* --- CONSTRUCTOR ---- */
52  SE3TrajectoryGenerator(const std::string& name);
53 
54  void init(const double& dt);
55 
56  /* --- SIGNALS --- */
57  DECLARE_SIGNAL_IN(initial_value, dynamicgraph::Vector);
58  DECLARE_SIGNAL(x, OUT, dynamicgraph::Vector);
59  DECLARE_SIGNAL_IN(trigger, bool);
60  DECLARE_SIGNAL_OUT(dx, dynamicgraph::Vector);
61  DECLARE_SIGNAL_OUT(ddx, dynamicgraph::Vector);
62 
63  protected:
64  DECLARE_SIGNAL_OUT_FUNCTION(x, dynamicgraph::Vector);
65 
66  public:
67  /* --- COMMANDS --- */
68 
69  void playTrajectoryFile(const std::string& fileName);
70  void startSpline();
71  void setSpline(const std::string& filename, const double& timeToInitConf, const Eigen::MatrixXd& init_rotation);
72 
74  void getValue(const int& id);
75 
81  void move(const int& id, const double& xFinal, const double& time);
82 
88  void startSinusoid(const int& id, const double& xFinal, const double& time);
89 
95  void startTriangle(const int& id, const double& xFinal, const double& time, const double& Tacc);
96 
103  void startConstAcc(const int& id, const double& xFinal, const double& time);
104 
113  void startLinearChirp(const int& id, const double& xFinal, const double& f0, const double& f1, const double& time);
114 
119  void stop(const int& id);
120 
121  /* --- ENTITY INHERITANCE --- */
122  virtual void display(std::ostream& os) const;
123 
124  void sendMsg(const std::string& msg, MsgType t = MSG_TYPE_INFO, const char* = "", int = 0) {
125  logger_.stream(t) << ("[SE3TrajectoryGenerator-" + name + "] " + msg) << '\n';
126  }
127 
128  protected:
129  enum TG_Status {
137  TG_SPLINE
138  };
139 
141  bool m_firstIter;
142  double m_dt;
143  unsigned int m_np;
144  unsigned int m_nv;
145  unsigned int m_iterLast;
146 
147  double m_t;
148  Eigen::Matrix3d m_splineRotation;
149  parametriccurves::Spline<double, Eigen::Dynamic>* m_splineTrajGen;
151 
152  std::vector<TG_Status> m_status;
153  std::vector<AbstractTrajectoryGenerator*> m_currentTrajGen;
154  std::vector<NoTrajectoryGenerator*> m_noTrajGen;
155  std::vector<MinimumJerkTrajectoryGenerator*> m_minJerkTrajGen;
156  std::vector<SinusoidTrajectoryGenerator*> m_sinTrajGen;
157  std::vector<LinearChirpTrajectoryGenerator*> m_linChirpTrajGen;
158  std::vector<TriangleTrajectoryGenerator*> m_triangleTrajGen;
159  std::vector<ConstantAccelerationTrajectoryGenerator*> m_constAccTrajGen;
161 
162 }; // class SE3TrajectoryGenerator
163 
164 } // namespace torque_control
165 } // namespace sot
166 } // namespace dynamicgraph
167 
168 #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