sot-torque-control  1.5.2
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__
dynamicgraph::sot::torque_control::SE3TrajectoryGenerator::m_initSucceeded
bool m_initSucceeded
Definition: se3-trajectory-generator.hh:140
dynamicgraph::sot::torque_control::SE3TrajectoryGenerator::m_status
std::vector< TG_Status > m_status
Definition: se3-trajectory-generator.hh:152
dynamicgraph::sot::torque_control::SE3TrajectoryGenerator::TG_TRIANGLE
@ TG_TRIANGLE
Definition: se3-trajectory-generator.hh:134
dynamicgraph::sot::torque_control::SE3TrajectoryGenerator::TG_LIN_CHIRP
@ TG_LIN_CHIRP
Definition: se3-trajectory-generator.hh:133
dynamicgraph
to read text file
Definition: treeview.dox:22
dynamicgraph::sot::torque_control::SE3TrajectoryGenerator::m_np
unsigned int m_np
control loop time period
Definition: se3-trajectory-generator.hh:143
dynamicgraph::sot::torque_control::SE3TrajectoryGenerator::m_currentTrajGen
std::vector< AbstractTrajectoryGenerator * > m_currentTrajGen
status of the component
Definition: se3-trajectory-generator.hh:153
dynamicgraph::sot::torque_control::SE3TrajectoryGenerator::m_nv
unsigned int m_nv
size of position vector
Definition: se3-trajectory-generator.hh:144
dynamicgraph::sot::torque_control::SE3TrajectoryGenerator::m_triangleTrajGen
std::vector< TriangleTrajectoryGenerator * > m_triangleTrajGen
Definition: se3-trajectory-generator.hh:158
vector-conversions.hh
dynamicgraph::sot::torque_control::SE3TrajectoryGenerator
Definition: se3-trajectory-generator.hh:46
trajectory-generators.hh
dynamicgraph::sot::torque_control::SE3TrajectoryGenerator::m_dt
double m_dt
true if it is the first iteration, false otherwise
Definition: se3-trajectory-generator.hh:142
dynamicgraph::sot::torque_control::SE3TrajectoryGenerator::m_t
double m_t
last iter index
Definition: se3-trajectory-generator.hh:147
dynamicgraph::sot::torque_control::SE3TrajectoryGenerator::TG_Status
TG_Status
Definition: se3-trajectory-generator.hh:129
dynamicgraph::sot::torque_control::SE3TrajectoryGenerator::m_noTrajGen
std::vector< NoTrajectoryGenerator * > m_noTrajGen
Definition: se3-trajectory-generator.hh:154
dynamicgraph::sot::torque_control::SE3TrajectoryGenerator::m_firstIter
bool m_firstIter
true if the entity has been successfully initialized
Definition: se3-trajectory-generator.hh:141
dynamicgraph::sot::torque_control::SE3TrajectoryGenerator::m_splineRotation
Eigen::Matrix3d m_splineRotation
Definition: se3-trajectory-generator.hh:148
dynamicgraph::sot::torque_control::SE3TrajectoryGenerator::m_splineTrajGen
parametriccurves::Spline< double, Eigen::Dynamic > * m_splineTrajGen
Definition: se3-trajectory-generator.hh:149
dynamicgraph::sot::torque_control::SE3TrajectoryGenerator::TG_STOP
@ TG_STOP
Definition: se3-trajectory-generator.hh:130
dynamicgraph::sot::torque_control::SE3TrajectoryGenerator::m_linChirpTrajGen
std::vector< LinearChirpTrajectoryGenerator * > m_linChirpTrajGen
Definition: se3-trajectory-generator.hh:157
torque_control
Definition: __init__.py:1
dynamicgraph::sot::torque_control::SE3TrajectoryGenerator::sendMsg
void sendMsg(const std::string &msg, MsgType t=MSG_TYPE_INFO, const char *="", int=0)
Definition: se3-trajectory-generator.hh:124
dynamicgraph::sot::torque_control::SE3TrajectoryGenerator::TG_MIN_JERK
@ TG_MIN_JERK
Definition: se3-trajectory-generator.hh:132
dynamicgraph::sot::torque_control::SE3TrajectoryGenerator::TG_SINUSOID
@ TG_SINUSOID
Definition: se3-trajectory-generator.hh:131
dynamicgraph::sot::torque_control::SE3TrajectoryGenerator::m_minJerkTrajGen
std::vector< MinimumJerkTrajectoryGenerator * > m_minJerkTrajGen
Definition: se3-trajectory-generator.hh:155
dynamicgraph::sot::torque_control::SE3TrajectoryGenerator::m_constAccTrajGen
std::vector< ConstantAccelerationTrajectoryGenerator * > m_constAccTrajGen
Definition: se3-trajectory-generator.hh:159
dynamicgraph::sot::torque_control::TextFileTrajectoryGenerator
Definition: trajectory-generators.hh:193
dynamicgraph::sot::torque_control::EntityClassName
AdmittanceController EntityClassName
Definition: admittance-controller.cpp:44
dynamicgraph::sot::torque_control::SE3TrajectoryGenerator::m_sinTrajGen
std::vector< SinusoidTrajectoryGenerator * > m_sinTrajGen
Definition: se3-trajectory-generator.hh:156
dynamicgraph::sot::torque_control::SE3TrajectoryGenerator::m_iterLast
unsigned int m_iterLast
size of velocity vector
Definition: se3-trajectory-generator.hh:145
dynamicgraph::sot::torque_control::SE3TrajectoryGenerator::TG_TEXT_FILE
@ TG_TEXT_FILE
Definition: se3-trajectory-generator.hh:136
dynamicgraph::sot::torque_control::SE3TrajectoryGenerator::m_splineReady
bool m_splineReady
Definition: se3-trajectory-generator.hh:150
SOTSE3TRAJECTORYGENERATOR_EXPORT
#define SOTSE3TRAJECTORYGENERATOR_EXPORT
Definition: se3-trajectory-generator.hh:20
dynamicgraph::sot::torque_control::SE3TrajectoryGenerator::TG_CONST_ACC
@ TG_CONST_ACC
Definition: se3-trajectory-generator.hh:135
dynamicgraph::sot::torque_control::SE3TrajectoryGenerator::m_textFileTrajGen
TextFileTrajectoryGenerator * m_textFileTrajGen
Definition: se3-trajectory-generator.hh:160