sot-torque-control  1.6.4
Collection of dynamic-graph entities aimed at implementing torque control on different robots.
 
Loading...
Searching...
No Matches
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 <boost/assign.hpp>
27#include <map>
28#include <pinocchio/fwd.hpp>
29
30/* HELPER */
31#include <dynamic-graph/signal-helper.h>
32
33#include <sot/core/matrix-geometry.hh>
34#include <sot/core/robot-utils.hh>
37
38namespace dynamicgraph {
39namespace sot {
40namespace torque_control {
41
42/* --------------------------------------------------------------------- */
43/* --- CLASS ----------------------------------------------------------- */
44/* --------------------------------------------------------------------- */
45
47 : public ::dynamicgraph::Entity {
49 DYNAMIC_GRAPH_ENTITY_DECL();
50
51 public:
52 /* --- CONSTRUCTOR ---- */
53 JointTrajectoryGenerator(const std::string& name);
54
55 void init(const double& dt, const std::string& robotRef);
56
57 /* --- SIGNALS --- */
58 DECLARE_SIGNAL_IN(base6d_encoders, dynamicgraph::Vector);
59 DECLARE_SIGNAL_OUT(q, dynamicgraph::Vector);
60 DECLARE_SIGNAL_OUT(dq, dynamicgraph::Vector);
61 DECLARE_SIGNAL_OUT(ddq, dynamicgraph::Vector);
62 DECLARE_SIGNAL(fRightFoot, OUT, dynamicgraph::Vector);
63 DECLARE_SIGNAL(fLeftFoot, OUT, dynamicgraph::Vector);
64 DECLARE_SIGNAL(fRightHand, OUT, dynamicgraph::Vector);
65 DECLARE_SIGNAL(fLeftHand, OUT, dynamicgraph::Vector);
66
67 protected:
68 DECLARE_SIGNAL_OUT_FUNCTION(fRightFoot, dynamicgraph::Vector);
69 DECLARE_SIGNAL_OUT_FUNCTION(fLeftFoot, dynamicgraph::Vector);
70 DECLARE_SIGNAL_OUT_FUNCTION(fRightHand, dynamicgraph::Vector);
71 DECLARE_SIGNAL_OUT_FUNCTION(fLeftHand, dynamicgraph::Vector);
72
73 public:
74 /* --- COMMANDS --- */
75
76 void playTrajectoryFile(const std::string& fileName);
77
79 void getJoint(const std::string& jointName);
80
82 bool isTrajectoryEnded();
83
89 void moveJoint(const std::string& jointName, const double& qFinal,
90 const double& time);
91 void moveForce(const std::string& forceName, const int& axis,
92 const double& fFinal, const double& time);
93
100 void startSinusoid(const std::string& jointName, const double& qFinal,
101 const double& time);
102
109 void startTriangle(const std::string& jointName, const double& qFinal,
110 const double& time, const double& Tacc);
111
119 void startConstAcc(const std::string& jointName, const double& qFinal,
120 const double& time);
121
128 // void startForceSinusoid(const std::string& forceName, const
129 // dynamicgraph::Vector& fFinal, const double& time);
130 void startForceSinusoid(const std::string& forceName, const int& axis,
131 const double& fFinal, const double& time);
132
142 void startLinearChirp(const std::string& jointName, const double& qFinal,
143 const double& f0, const double& f1, const double& time);
144
145 void startForceLinearChirp(const std::string& forceName, const int& axis,
146 const double& fFinal, const double& f0,
147 const double& f1, const double& time);
148
153 void stop(const std::string& jointName);
154
159 void stopForce(const std::string& forceName);
160
161 /* --- ENTITY INHERITANCE --- */
162 virtual void display(std::ostream& os) const;
163
164 void sendMsg(const std::string& msg, MsgType t = MSG_TYPE_INFO,
165 const char* = "", int = 0) {
166 logger_.stream(t) << ("[JointTrajectoryGenerator-" + name + "] " + msg)
167 << '\n';
168 }
169
170 protected:
178 JTG_TEXT_FILE
179 };
180
181 bool
184 double m_dt;
185
186 RobotUtilShrPtr m_robot_util;
187
188 std::vector<int> m_iterForceSignals;
189
190 std::vector<JTG_Status> m_status;
191 std::vector<AbstractTrajectoryGenerator*> m_currentTrajGen;
192 std::vector<NoTrajectoryGenerator*> m_noTrajGen;
193 std::vector<MinimumJerkTrajectoryGenerator*> m_minJerkTrajGen;
194 std::vector<SinusoidTrajectoryGenerator*> m_sinTrajGen;
195 std::vector<LinearChirpTrajectoryGenerator*> m_linChirpTrajGen;
196 std::vector<TriangleTrajectoryGenerator*> m_triangleTrajGen;
197 std::vector<ConstantAccelerationTrajectoryGenerator*> m_constAccTrajGen;
199
200 std::vector<JTG_Status> m_status_force;
201 std::vector<AbstractTrajectoryGenerator*> m_currentTrajGen_force;
202 std::vector<NoTrajectoryGenerator*> m_noTrajGen_force;
203 std::vector<SinusoidTrajectoryGenerator*> m_sinTrajGen_force;
204 std::vector<MinimumJerkTrajectoryGenerator*> m_minJerkTrajGen_force;
205 std::vector<LinearChirpTrajectoryGenerator*> m_linChirpTrajGen_force;
206
207 bool generateReferenceForceSignal(const std::string& forceName, int fid,
208 dynamicgraph::Vector& s, int iter);
209
210 bool convertJointNameToJointId(const std::string& name, unsigned int& id);
211 bool convertForceNameToForceId(const std::string& name, unsigned int& id);
212 bool isJointInRange(unsigned int id, double q);
213 bool isForceInRange(unsigned int id, const Eigen::VectorXd& f);
214 bool isForceInRange(unsigned int id, int axis, double f);
215
216}; // class JointTrajectoryGenerator
217
218} // namespace torque_control
219} // namespace sot
220} // namespace dynamicgraph
221
222#endif // #ifndef __sot_torque_control_joint_position_controller_H__
void sendMsg(const std::string &msg, MsgType t=MSG_TYPE_INFO, const char *="", int=0)
DECLARE_SIGNAL_OUT_FUNCTION(fLeftHand, dynamicgraph::Vector)
std::vector< AbstractTrajectoryGenerator * > m_currentTrajGen_force
status of the forces
std::vector< LinearChirpTrajectoryGenerator * > m_linChirpTrajGen
DECLARE_SIGNAL_OUT_FUNCTION(fLeftFoot, dynamicgraph::Vector)
std::vector< LinearChirpTrajectoryGenerator * > m_linChirpTrajGen_force
DECLARE_SIGNAL(fLeftHand, OUT, dynamicgraph::Vector)
std::vector< MinimumJerkTrajectoryGenerator * > m_minJerkTrajGen_force
double m_dt
true if it is the first iteration, false otherwise
std::vector< ConstantAccelerationTrajectoryGenerator * > m_constAccTrajGen
DECLARE_SIGNAL_OUT_FUNCTION(fRightFoot, dynamicgraph::Vector)
DECLARE_SIGNAL(fRightFoot, OUT, dynamicgraph::Vector)
bool m_firstIter
true if the entity has been successfully initialized
DECLARE_SIGNAL(fRightHand, OUT, dynamicgraph::Vector)
DECLARE_SIGNAL_IN(base6d_encoders, dynamicgraph::Vector)
std::vector< TriangleTrajectoryGenerator * > m_triangleTrajGen
std::vector< SinusoidTrajectoryGenerator * > m_sinTrajGen_force
DECLARE_SIGNAL(fLeftFoot, OUT, dynamicgraph::Vector)
std::vector< AbstractTrajectoryGenerator * > m_currentTrajGen
status of the joints
DECLARE_SIGNAL_OUT_FUNCTION(fRightHand, dynamicgraph::Vector)
std::vector< MinimumJerkTrajectoryGenerator * > m_minJerkTrajGen
#define SOTJOINTTRAJECTORYGENERATOR_EXPORT
to read text file
Definition: treeview.dox:22