sot-torque-control  1.5.2
joint-trajectory-generator.hh
Go to the documentation of this file.
1 /*
2  * Copyright 2014, Oscar E. Ramos Ponce, LAAS-CNRS
3  *
4  */
5 
6 #ifndef __sot_torque_control_joint_trajectory_generator_H__
7 #define __sot_torque_control_joint_trajectory_generator_H__
8 
9 /* --------------------------------------------------------------------- */
10 /* --- API ------------------------------------------------------------- */
11 /* --------------------------------------------------------------------- */
12 
13 #if defined(WIN32)
14 #if defined(joint_position_controller_EXPORTS)
15 #define SOTJOINTTRAJECTORYGENERATOR_EXPORT __declspec(dllexport)
16 #else
17 #define SOTJOINTTRAJECTORYGENERATOR_EXPORT __declspec(dllimport)
18 #endif
19 #else
20 #define SOTJOINTTRAJECTORYGENERATOR_EXPORT
21 #endif
22 
23 /* --------------------------------------------------------------------- */
24 /* --- INCLUDE --------------------------------------------------------- */
25 /* --------------------------------------------------------------------- */
26 #include <map>
27 #include "boost/assign.hpp"
28 
29 /* HELPER */
30 #include <dynamic-graph/signal-helper.h>
31 #include <sot/core/matrix-geometry.hh>
32 #include <sot/core/robot-utils.hh>
34 
36 
37 namespace dynamicgraph {
38 namespace sot {
39 namespace torque_control {
40 
41 /* --------------------------------------------------------------------- */
42 /* --- CLASS ----------------------------------------------------------- */
43 /* --------------------------------------------------------------------- */
44 
45 class SOTJOINTTRAJECTORYGENERATOR_EXPORT JointTrajectoryGenerator : public ::dynamicgraph::Entity {
47  DYNAMIC_GRAPH_ENTITY_DECL();
48 
49  public:
50  /* --- CONSTRUCTOR ---- */
51  JointTrajectoryGenerator(const std::string& name);
52 
53  void init(const double& dt, const std::string& robotRef);
54 
55  /* --- SIGNALS --- */
56  DECLARE_SIGNAL_IN(base6d_encoders, dynamicgraph::Vector);
57  DECLARE_SIGNAL_OUT(q, dynamicgraph::Vector);
58  DECLARE_SIGNAL_OUT(dq, dynamicgraph::Vector);
59  DECLARE_SIGNAL_OUT(ddq, dynamicgraph::Vector);
60  DECLARE_SIGNAL(fRightFoot, OUT, dynamicgraph::Vector);
61  DECLARE_SIGNAL(fLeftFoot, OUT, dynamicgraph::Vector);
62  DECLARE_SIGNAL(fRightHand, OUT, dynamicgraph::Vector);
63  DECLARE_SIGNAL(fLeftHand, OUT, dynamicgraph::Vector);
64 
65  protected:
66  DECLARE_SIGNAL_OUT_FUNCTION(fRightFoot, dynamicgraph::Vector);
67  DECLARE_SIGNAL_OUT_FUNCTION(fLeftFoot, dynamicgraph::Vector);
68  DECLARE_SIGNAL_OUT_FUNCTION(fRightHand, dynamicgraph::Vector);
69  DECLARE_SIGNAL_OUT_FUNCTION(fLeftHand, dynamicgraph::Vector);
70 
71  public:
72  /* --- COMMANDS --- */
73 
74  void playTrajectoryFile(const std::string& fileName);
75 
77  void getJoint(const std::string& jointName);
78 
80  bool isTrajectoryEnded();
81 
87  void moveJoint(const std::string& jointName, const double& qFinal, const double& time);
88  void moveForce(const std::string& forceName, const int& axis, const double& fFinal, const double& time);
89 
95  void startSinusoid(const std::string& jointName, const double& qFinal, const double& time);
96 
102  void startTriangle(const std::string& jointName, const double& qFinal, const double& time, const double& Tacc);
103 
110  void startConstAcc(const std::string& jointName, const double& qFinal, const double& time);
111 
117  // void startForceSinusoid(const std::string& forceName, const dynamicgraph::Vector& fFinal, const double&
118  // time);
119  void startForceSinusoid(const std::string& forceName, const int& axis, const double& fFinal, const double& time);
120 
129  void startLinearChirp(const std::string& jointName, const double& qFinal, const double& f0, const double& f1,
130  const double& time);
131 
132  void startForceLinearChirp(const std::string& forceName, const int& axis, const double& fFinal, const double& f0,
133  const double& f1, const double& time);
134 
139  void stop(const std::string& jointName);
140 
145  void stopForce(const std::string& forceName);
146 
147  /* --- ENTITY INHERITANCE --- */
148  virtual void display(std::ostream& os) const;
149 
150  void sendMsg(const std::string& msg, MsgType t = MSG_TYPE_INFO, const char* = "", int = 0) {
151  logger_.stream(t) << ("[JointTrajectoryGenerator-" + name + "] " + msg) << '\n';
152  }
153 
154  protected:
155  enum JTG_Status { JTG_STOP, JTG_SINUSOID, JTG_MIN_JERK, JTG_LIN_CHIRP, JTG_TRIANGLE, JTG_CONST_ACC, JTG_TEXT_FILE };
156 
158  bool m_firstIter;
159  double m_dt;
160 
161  RobotUtilShrPtr m_robot_util;
162 
163  std::vector<int> m_iterForceSignals;
164 
165  std::vector<JTG_Status> m_status;
166  std::vector<AbstractTrajectoryGenerator*> m_currentTrajGen;
167  std::vector<NoTrajectoryGenerator*> m_noTrajGen;
168  std::vector<MinimumJerkTrajectoryGenerator*> m_minJerkTrajGen;
169  std::vector<SinusoidTrajectoryGenerator*> m_sinTrajGen;
170  std::vector<LinearChirpTrajectoryGenerator*> m_linChirpTrajGen;
171  std::vector<TriangleTrajectoryGenerator*> m_triangleTrajGen;
172  std::vector<ConstantAccelerationTrajectoryGenerator*> m_constAccTrajGen;
174 
175  std::vector<JTG_Status> m_status_force;
176  std::vector<AbstractTrajectoryGenerator*> m_currentTrajGen_force;
177  std::vector<NoTrajectoryGenerator*> m_noTrajGen_force;
178  std::vector<SinusoidTrajectoryGenerator*> m_sinTrajGen_force;
179  std::vector<MinimumJerkTrajectoryGenerator*> m_minJerkTrajGen_force;
180  std::vector<LinearChirpTrajectoryGenerator*> m_linChirpTrajGen_force;
181 
182  bool generateReferenceForceSignal(const std::string& forceName, int fid, dynamicgraph::Vector& s, int iter);
183 
184  bool convertJointNameToJointId(const std::string& name, unsigned int& id);
185  bool convertForceNameToForceId(const std::string& name, unsigned int& id);
186  bool isJointInRange(unsigned int id, double q);
187  bool isForceInRange(unsigned int id, const Eigen::VectorXd& f);
188  bool isForceInRange(unsigned int id, int axis, double f);
189 
190 }; // class JointTrajectoryGenerator
191 
192 } // namespace torque_control
193 } // namespace sot
194 } // namespace dynamicgraph
195 
196 #endif // #ifndef __sot_torque_control_joint_position_controller_H__
dynamicgraph::sot::torque_control::JointTrajectoryGenerator::m_minJerkTrajGen_force
std::vector< MinimumJerkTrajectoryGenerator * > m_minJerkTrajGen_force
Definition: joint-trajectory-generator.hh:179
dynamicgraph::sot::torque_control::JointTrajectoryGenerator::m_triangleTrajGen
std::vector< TriangleTrajectoryGenerator * > m_triangleTrajGen
Definition: joint-trajectory-generator.hh:171
dynamicgraph::sot::torque_control::JointTrajectoryGenerator::m_status
std::vector< JTG_Status > m_status
Definition: joint-trajectory-generator.hh:165
dynamicgraph::sot::torque_control::JointTrajectoryGenerator::m_sinTrajGen
std::vector< SinusoidTrajectoryGenerator * > m_sinTrajGen
Definition: joint-trajectory-generator.hh:169
dynamicgraph
to read text file
Definition: treeview.dox:22
dynamicgraph::sot::torque_control::JointTrajectoryGenerator::m_firstIter
bool m_firstIter
true if the entity has been successfully initialized
Definition: joint-trajectory-generator.hh:158
dynamicgraph::sot::torque_control::JointTrajectoryGenerator::m_minJerkTrajGen
std::vector< MinimumJerkTrajectoryGenerator * > m_minJerkTrajGen
Definition: joint-trajectory-generator.hh:168
vector-conversions.hh
trajectory-generators.hh
dynamicgraph::sot::torque_control::JointTrajectoryGenerator::m_initSucceeded
bool m_initSucceeded
Definition: joint-trajectory-generator.hh:157
SOTJOINTTRAJECTORYGENERATOR_EXPORT
#define SOTJOINTTRAJECTORYGENERATOR_EXPORT
Definition: joint-trajectory-generator.hh:20
dynamicgraph::sot::torque_control::JointTrajectoryGenerator::m_noTrajGen
std::vector< NoTrajectoryGenerator * > m_noTrajGen
Definition: joint-trajectory-generator.hh:167
dynamicgraph::sot::torque_control::JointTrajectoryGenerator::m_status_force
std::vector< JTG_Status > m_status_force
Definition: joint-trajectory-generator.hh:175
dynamicgraph::sot::torque_control::JointTrajectoryGenerator::m_noTrajGen_force
std::vector< NoTrajectoryGenerator * > m_noTrajGen_force
Definition: joint-trajectory-generator.hh:177
dynamicgraph::sot::torque_control::JointTrajectoryGenerator::m_robot_util
RobotUtilShrPtr m_robot_util
control loop time period
Definition: joint-trajectory-generator.hh:161
dynamicgraph::sot::torque_control::JointTrajectoryGenerator::m_currentTrajGen_force
std::vector< AbstractTrajectoryGenerator * > m_currentTrajGen_force
status of the forces
Definition: joint-trajectory-generator.hh:176
torque_control
Definition: __init__.py:1
dynamicgraph::sot::torque_control::JointTrajectoryGenerator::m_textFileTrajGen
TextFileTrajectoryGenerator * m_textFileTrajGen
Definition: joint-trajectory-generator.hh:173
dynamicgraph::sot::torque_control::JointTrajectoryGenerator
Definition: joint-trajectory-generator.hh:45
dynamicgraph::sot::torque_control::JointTrajectoryGenerator::JTG_Status
JTG_Status
Definition: joint-trajectory-generator.hh:155
dynamicgraph::sot::torque_control::JointTrajectoryGenerator::m_currentTrajGen
std::vector< AbstractTrajectoryGenerator * > m_currentTrajGen
status of the joints
Definition: joint-trajectory-generator.hh:166
dynamicgraph::sot::torque_control::JointTrajectoryGenerator::sendMsg
void sendMsg(const std::string &msg, MsgType t=MSG_TYPE_INFO, const char *="", int=0)
Definition: joint-trajectory-generator.hh:150
dynamicgraph::sot::torque_control::JointTrajectoryGenerator::m_constAccTrajGen
std::vector< ConstantAccelerationTrajectoryGenerator * > m_constAccTrajGen
Definition: joint-trajectory-generator.hh:172
dynamicgraph::sot::torque_control::TextFileTrajectoryGenerator
Definition: trajectory-generators.hh:193
dynamicgraph::sot::torque_control::JointTrajectoryGenerator::JTG_TRIANGLE
@ JTG_TRIANGLE
Definition: joint-trajectory-generator.hh:155
dynamicgraph::sot::torque_control::EntityClassName
AdmittanceController EntityClassName
Definition: admittance-controller.cpp:44
dynamicgraph::sot::torque_control::JointTrajectoryGenerator::m_sinTrajGen_force
std::vector< SinusoidTrajectoryGenerator * > m_sinTrajGen_force
Definition: joint-trajectory-generator.hh:178
dynamicgraph::sot::torque_control::JointTrajectoryGenerator::m_linChirpTrajGen
std::vector< LinearChirpTrajectoryGenerator * > m_linChirpTrajGen
Definition: joint-trajectory-generator.hh:170
dynamicgraph::sot::torque_control::JointTrajectoryGenerator::m_linChirpTrajGen_force
std::vector< LinearChirpTrajectoryGenerator * > m_linChirpTrajGen_force
Definition: joint-trajectory-generator.hh:180
dynamicgraph::sot::torque_control::JointTrajectoryGenerator::m_iterForceSignals
std::vector< int > m_iterForceSignals
Definition: joint-trajectory-generator.hh:163
dynamicgraph::sot::torque_control::JointTrajectoryGenerator::m_dt
double m_dt
true if it is the first iteration, false otherwise
Definition: joint-trajectory-generator.hh:159