sot-torque-control  1.6.2
Collection of dynamic-graph entities aimed at implementing torque control on different robots.
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 
28 #include <pinocchio/fwd.hpp>
29 
30 #include <boost/assign.hpp>
31 
32 /* HELPER */
33 #include <dynamic-graph/signal-helper.h>
34 #include <sot/core/matrix-geometry.hh>
35 #include <sot/core/robot-utils.hh>
36 
39 
40 namespace dynamicgraph {
41 namespace sot {
42 namespace torque_control {
43 
44 /* --------------------------------------------------------------------- */
45 /* --- CLASS ----------------------------------------------------------- */
46 /* --------------------------------------------------------------------- */
47 
48 class SOTJOINTTRAJECTORYGENERATOR_EXPORT JointTrajectoryGenerator : public ::dynamicgraph::Entity {
50  DYNAMIC_GRAPH_ENTITY_DECL();
51 
52  public:
53  /* --- CONSTRUCTOR ---- */
54  JointTrajectoryGenerator(const std::string& name);
55 
56  void init(const double& dt, const std::string& robotRef);
57 
58  /* --- SIGNALS --- */
59  DECLARE_SIGNAL_IN(base6d_encoders, dynamicgraph::Vector);
60  DECLARE_SIGNAL_OUT(q, dynamicgraph::Vector);
61  DECLARE_SIGNAL_OUT(dq, dynamicgraph::Vector);
62  DECLARE_SIGNAL_OUT(ddq, dynamicgraph::Vector);
63  DECLARE_SIGNAL(fRightFoot, OUT, dynamicgraph::Vector);
64  DECLARE_SIGNAL(fLeftFoot, OUT, dynamicgraph::Vector);
65  DECLARE_SIGNAL(fRightHand, OUT, dynamicgraph::Vector);
66  DECLARE_SIGNAL(fLeftHand, OUT, dynamicgraph::Vector);
67 
68  protected:
69  DECLARE_SIGNAL_OUT_FUNCTION(fRightFoot, dynamicgraph::Vector);
70  DECLARE_SIGNAL_OUT_FUNCTION(fLeftFoot, dynamicgraph::Vector);
71  DECLARE_SIGNAL_OUT_FUNCTION(fRightHand, dynamicgraph::Vector);
72  DECLARE_SIGNAL_OUT_FUNCTION(fLeftHand, dynamicgraph::Vector);
73 
74  public:
75  /* --- COMMANDS --- */
76 
77  void playTrajectoryFile(const std::string& fileName);
78 
80  void getJoint(const std::string& jointName);
81 
83  bool isTrajectoryEnded();
84 
90  void moveJoint(const std::string& jointName, const double& qFinal, const double& time);
91  void moveForce(const std::string& forceName, const int& axis, const double& fFinal, const double& time);
92 
98  void startSinusoid(const std::string& jointName, const double& qFinal, const double& time);
99 
105  void startTriangle(const std::string& jointName, const double& qFinal, const double& time, const double& Tacc);
106 
113  void startConstAcc(const std::string& jointName, const double& qFinal, const double& time);
114 
120  // void startForceSinusoid(const std::string& forceName, const dynamicgraph::Vector& fFinal, const double&
121  // time);
122  void startForceSinusoid(const std::string& forceName, const int& axis, const double& fFinal, const double& time);
123 
132  void startLinearChirp(const std::string& jointName, const double& qFinal, const double& f0, const double& f1,
133  const double& time);
134 
135  void startForceLinearChirp(const std::string& forceName, const int& axis, const double& fFinal, const double& f0,
136  const double& f1, const double& time);
137 
142  void stop(const std::string& jointName);
143 
148  void stopForce(const std::string& forceName);
149 
150  /* --- ENTITY INHERITANCE --- */
151  virtual void display(std::ostream& os) const;
152 
153  void sendMsg(const std::string& msg, MsgType t = MSG_TYPE_INFO, const char* = "", int = 0) {
154  logger_.stream(t) << ("[JointTrajectoryGenerator-" + name + "] " + msg) << '\n';
155  }
156 
157  protected:
158  enum JTG_Status { JTG_STOP, JTG_SINUSOID, JTG_MIN_JERK, JTG_LIN_CHIRP, JTG_TRIANGLE, JTG_CONST_ACC, JTG_TEXT_FILE };
159 
161  bool m_firstIter;
162  double m_dt;
163 
164  RobotUtilShrPtr m_robot_util;
165 
166  std::vector<int> m_iterForceSignals;
167 
168  std::vector<JTG_Status> m_status;
169  std::vector<AbstractTrajectoryGenerator*> m_currentTrajGen;
170  std::vector<NoTrajectoryGenerator*> m_noTrajGen;
171  std::vector<MinimumJerkTrajectoryGenerator*> m_minJerkTrajGen;
172  std::vector<SinusoidTrajectoryGenerator*> m_sinTrajGen;
173  std::vector<LinearChirpTrajectoryGenerator*> m_linChirpTrajGen;
174  std::vector<TriangleTrajectoryGenerator*> m_triangleTrajGen;
175  std::vector<ConstantAccelerationTrajectoryGenerator*> m_constAccTrajGen;
177 
178  std::vector<JTG_Status> m_status_force;
179  std::vector<AbstractTrajectoryGenerator*> m_currentTrajGen_force;
180  std::vector<NoTrajectoryGenerator*> m_noTrajGen_force;
181  std::vector<SinusoidTrajectoryGenerator*> m_sinTrajGen_force;
182  std::vector<MinimumJerkTrajectoryGenerator*> m_minJerkTrajGen_force;
183  std::vector<LinearChirpTrajectoryGenerator*> m_linChirpTrajGen_force;
184 
185  bool generateReferenceForceSignal(const std::string& forceName, int fid, dynamicgraph::Vector& s, int iter);
186 
187  bool convertJointNameToJointId(const std::string& name, unsigned int& id);
188  bool convertForceNameToForceId(const std::string& name, unsigned int& id);
189  bool isJointInRange(unsigned int id, double q);
190  bool isForceInRange(unsigned int id, const Eigen::VectorXd& f);
191  bool isForceInRange(unsigned int id, int axis, double f);
192 
193 }; // class JointTrajectoryGenerator
194 
195 } // namespace torque_control
196 } // namespace sot
197 } // namespace dynamicgraph
198 
199 #endif // #ifndef __sot_torque_control_joint_position_controller_H__
std::vector< ConstantAccelerationTrajectoryGenerator * > m_constAccTrajGen
void sendMsg(const std::string &msg, MsgType t=MSG_TYPE_INFO, const char *="", int=0)
std::vector< AbstractTrajectoryGenerator * > m_currentTrajGen_force
status of the forces
std::vector< LinearChirpTrajectoryGenerator * > m_linChirpTrajGen
std::vector< MinimumJerkTrajectoryGenerator * > m_minJerkTrajGen_force
std::vector< SinusoidTrajectoryGenerator * > m_sinTrajGen_force
bool m_firstIter
true if the entity has been successfully initialized
double m_dt
true if it is the first iteration, false otherwise
#define SOTJOINTTRAJECTORYGENERATOR_EXPORT
std::vector< LinearChirpTrajectoryGenerator * > m_linChirpTrajGen_force
std::vector< TriangleTrajectoryGenerator * > m_triangleTrajGen
std::vector< AbstractTrajectoryGenerator * > m_currentTrajGen
status of the joints
std::vector< MinimumJerkTrajectoryGenerator * > m_minJerkTrajGen
to read text file
Definition: treeview.dox:22