joint-trajectory-generator.hh 10.1 KB
Newer Older
andreadelprete's avatar
andreadelprete committed
1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
/*
 * Copyright 2014, Oscar E. Ramos Ponce, LAAS-CNRS
 *
 * This file is part of sot-torque-control.
 * sot-torque-control is free software: you can redistribute it and/or
 * modify it under the terms of the GNU Lesser General Public License
 * as published by the Free Software Foundation, either version 3 of
 * the License, or (at your option) any later version.
 * sot-torque-control is distributed in the hope that it will be
 * useful, but WITHOUT ANY WARRANTY; without even the implied warranty
 * of MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
 * GNU Lesser General Public License for more details.  You should
 * have received a copy of the GNU Lesser General Public License along
 * with sot-torque-control.  If not, see <http://www.gnu.org/licenses/>.
 */

#ifndef __sot_torque_control_joint_trajectory_generator_H__
#define __sot_torque_control_joint_trajectory_generator_H__

/* --------------------------------------------------------------------- */
/* --- API ------------------------------------------------------------- */
/* --------------------------------------------------------------------- */

#if defined (WIN32)
#  if defined (joint_position_controller_EXPORTS)
#    define SOTJOINTTRAJECTORYGENERATOR_EXPORT __declspec(dllexport)
#  else
#    define SOTJOINTTRAJECTORYGENERATOR_EXPORT __declspec(dllimport)
#  endif
#else
#  define SOTJOINTTRAJECTORYGENERATOR_EXPORT
#endif


/* --------------------------------------------------------------------- */
/* --- INCLUDE --------------------------------------------------------- */
/* --------------------------------------------------------------------- */

#include <sot/torque_control/signal-helper.hh>
40
#include <sot/torque_control/common.hh>
andreadelprete's avatar
andreadelprete committed
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
#include <sot/torque_control/utils/vector-conversions.hh>
#include <sot/torque_control/utils/logger.hh>
#include <sot/torque_control/utils/trajectory-generators.hh>
#include <map>
#include "boost/assign.hpp"


namespace dynamicgraph {
  namespace sot {
    namespace torque_control {

      /* --------------------------------------------------------------------- */
      /* --- CLASS ----------------------------------------------------------- */
      /* --------------------------------------------------------------------- */

      class SOTJOINTTRAJECTORYGENERATOR_EXPORT JointTrajectoryGenerator
	:public::dynamicgraph::Entity
      {
        typedef JointTrajectoryGenerator EntityClassName;
        DYNAMIC_GRAPH_ENTITY_DECL();
Guilhem Saurel's avatar
Guilhem Saurel committed
61
62

      public:
andreadelprete's avatar
andreadelprete committed
63
64
65
        /* --- CONSTRUCTOR ---- */
        JointTrajectoryGenerator( const std::string & name );

66
        void init(const double& dt, const std::string &robotRef);
andreadelprete's avatar
andreadelprete committed
67
68

        /* --- SIGNALS --- */
69
70
71
72
73
74
75
76
        DECLARE_SIGNAL_IN(base6d_encoders,  dynamicgraph::Vector);
        DECLARE_SIGNAL_OUT(q,               dynamicgraph::Vector);
        DECLARE_SIGNAL_OUT(dq,              dynamicgraph::Vector);
        DECLARE_SIGNAL_OUT(ddq,             dynamicgraph::Vector);
        DECLARE_SIGNAL(fRightFoot, OUT,     dynamicgraph::Vector);
        DECLARE_SIGNAL(fLeftFoot,  OUT,     dynamicgraph::Vector);
        DECLARE_SIGNAL(fRightHand, OUT,     dynamicgraph::Vector);
        DECLARE_SIGNAL(fLeftHand,  OUT,     dynamicgraph::Vector);
andreadelprete's avatar
andreadelprete committed
77
78

      protected:
79
80
81
82
        DECLARE_SIGNAL_OUT_FUNCTION(fRightFoot, dynamicgraph::Vector);
        DECLARE_SIGNAL_OUT_FUNCTION(fLeftFoot,  dynamicgraph::Vector);
        DECLARE_SIGNAL_OUT_FUNCTION(fRightHand, dynamicgraph::Vector);
        DECLARE_SIGNAL_OUT_FUNCTION(fLeftHand,  dynamicgraph::Vector);
andreadelprete's avatar
andreadelprete committed
83
84
85
86
87
88
89
90
91
92

      public:

        /* --- COMMANDS --- */

        void playTrajectoryFile(const std::string& fileName);

        /** Print the current angle of the specified joint. */
        void getJoint(const std::string& jointName);

93
94
95
        /** Returns whether all given trajectories have ended **/
        bool isTrajectoryEnded();

andreadelprete's avatar
andreadelprete committed
96
97
98
99
100
101
102
103
104
105
106
107
108
109
110
111
112
113
114
115
116
117
118
119
120
121
122
123
124
125
126
127
128
129
130
        /** Move a joint to a position with a minimum-jerk trajectory.
         * @param jointName The short name of the joint.
         * @param qFinal The desired final position of the joint [rad].
         * @param time The time to go from the current position to qFinal [sec].
         */
        void moveJoint(const std::string& jointName, const double& qFinal, const double& time);
        void moveForce(const std::string& forceName, const int& axis, const double& fFinal, const double& time);

        /** Start an infinite sinusoidal trajectory.
         * @param jointName The short name of the joint.
         * @param qFinal The position of the joint corresponding to the max amplitude of the sinusoid [rad].
         * @param time The time to go from the current position to qFinal [sec].
         */
        void startSinusoid(const std::string& jointName, const double& qFinal, const double& time);

        /** Start an infinite triangle trajectory.
         * @param jointName The short name of the joint.
         * @param qFinal The position of the joint corresponding to the max amplitude of the trajectory [rad].
         * @param time The time to go from the current position to qFinal [sec].
         */
        void startTriangle(const std::string& jointName, const double& qFinal, const double& time, const double& Tacc);

        /** Start an infinite trajectory with piece-wise constant acceleration.
         * @param jointName The short name of the joint.
         * @param qFinal The position of the joint corresponding to the max amplitude of the trajectory [rad].
         * @param time The time to go from the current position to qFinal [sec].
         * @param Tacc The time during witch acceleration is keept constant [sec].
         */
        void startConstAcc(const std::string& jointName, const double& qFinal, const double& time);

        /** Start an infinite sinusoidal trajectory for the specified force signal.
         * @param forceName The short name of the force signal (rh, lh, rf, lf).
         * @param fFinal The 6d force corresponding to the max amplitude of the sinusoid [N/Nm].
         * @param time The time to go from 0 to fFinal [sec].
         */
131
//        void startForceSinusoid(const std::string& forceName, const dynamicgraph::Vector& fFinal, const double& time);
andreadelprete's avatar
andreadelprete committed
132
133
134
135
136
137
138
139
140
141
142
143
144
145
146
147
148
149
150
151
152
153
154
155
156
        void startForceSinusoid(const std::string& forceName, const int& axis, const double& fFinal, const double& time);

        /** Start a linear-chirp trajectory, that is a sinusoidal trajectory with frequency
         * being a linear function of time.
         * @param jointName The short name of the joint.
         * @param qFinal The position of the joint corresponding to the max amplitude of the sinusoid [rad].
         * @param f0 The initial (min) frequency of the sinusoid [Hz]
         * @param f1 The final (max) frequency of the sinusoid [Hz]
         * @param time The time to get from f0 to f1 [sec]
         */
        void startLinearChirp(const std::string& jointName, const double& qFinal, const double& f0, const double& f1, const double& time);

        void startForceLinearChirp(const std::string& forceName, const int& axis, const double& fFinal, const double& f0, const double& f1, const double& time);

        /** Stop the motion of the specified joint. If jointName is "all"
         * it stops the motion of all joints.
         * @param jointName A string identifying the joint to stop.
         * */
        void stop(const std::string& jointName);

        /** Stop the trajectory of the specified force. If forceName is "all"
         * it stops all forces.
         * @param forceName A string identifying the force to stop.
         * */
        void stopForce(const std::string& forceName);
Guilhem Saurel's avatar
Guilhem Saurel committed
157

andreadelprete's avatar
andreadelprete committed
158
159
160
161
162
163
164
        /* --- ENTITY INHERITANCE --- */
        virtual void display( std::ostream& os ) const;

        void sendMsg(const std::string& msg, MsgType t=MSG_TYPE_INFO, const char* file="", int line=0)
        {
          getLogger().sendMsg("[JointTrajectoryGenerator-"+name+"] "+msg, t, file, line);
        }
Guilhem Saurel's avatar
Guilhem Saurel committed
165

andreadelprete's avatar
andreadelprete committed
166
167
168
169
170
171
172
173
174
175
176
177
178
179
180
181
      protected:
        enum JTG_Status
        {
          JTG_STOP,
          JTG_SINUSOID,
          JTG_MIN_JERK,
          JTG_LIN_CHIRP,
          JTG_TRIANGLE,
          JTG_CONST_ACC,
          JTG_TEXT_FILE
        };

        bool              m_initSucceeded;    /// true if the entity has been successfully initialized
        bool              m_firstIter;        /// true if it is the first iteration, false otherwise
        double            m_dt;               /// control loop time period

182
	RobotUtil     *m_robot_util;
183

andreadelprete's avatar
andreadelprete committed
184
185
186
187
188
189
190
191
192
193
194
195
196
197
198
199
200
201
202
        std::vector<int>  m_iterForceSignals;

        std::vector<JTG_Status> m_status;     /// status of the joints
        std::vector<AbstractTrajectoryGenerator*>    m_currentTrajGen;
        std::vector<NoTrajectoryGenerator*>          m_noTrajGen;
        std::vector<MinimumJerkTrajectoryGenerator*> m_minJerkTrajGen;
        std::vector<SinusoidTrajectoryGenerator*>    m_sinTrajGen;
        std::vector<LinearChirpTrajectoryGenerator*> m_linChirpTrajGen;
        std::vector<TriangleTrajectoryGenerator*>    m_triangleTrajGen;
        std::vector<ConstantAccelerationTrajectoryGenerator*>    m_constAccTrajGen;
        TextFileTrajectoryGenerator*                 m_textFileTrajGen;

        std::vector<JTG_Status> m_status_force;     /// status of the forces
        std::vector<AbstractTrajectoryGenerator*>    m_currentTrajGen_force;
        std::vector<NoTrajectoryGenerator*>          m_noTrajGen_force;
        std::vector<SinusoidTrajectoryGenerator*>    m_sinTrajGen_force;
        std::vector<MinimumJerkTrajectoryGenerator*> m_minJerkTrajGen_force;
        std::vector<LinearChirpTrajectoryGenerator*> m_linChirpTrajGen_force;

203
        bool generateReferenceForceSignal(const std::string& forceName, int fid, dynamicgraph::Vector& s, int iter);
andreadelprete's avatar
andreadelprete committed
204
205
206
207
208
209
210
211

        bool convertJointNameToJointId(const std::string& name, unsigned int& id);
        bool convertForceNameToForceId(const std::string& name, unsigned int& id);
        bool isJointInRange(unsigned int id, double q);
        bool isForceInRange(unsigned int id, const Eigen::VectorXd& f);
        bool isForceInRange(unsigned int id, int axis, double f);

      }; // class JointTrajectoryGenerator
Guilhem Saurel's avatar
Guilhem Saurel committed
212

andreadelprete's avatar
andreadelprete committed
213
214
215
216
217
218
219
    }    // namespace torque_control
  }      // namespace sot
}        // namespace dynamicgraph



#endif // #ifndef __sot_torque_control_joint_position_controller_H__