patterngeneratorinterface.hh 13.4 KB
Newer Older
Thomas Moulard's avatar
Thomas Moulard 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
/*
 * Copyright 2007, 2008, 2009, 2010, 
 *
 * Andrei     Herdt
 * Fumio      Kanehiro
 * Francois   Keith
 * Alireza    Nakhaei
 * Olivier    Stasse
 *
 * JRL, CNRS/AIST
 *
 * This file is part of walkGenJrl.
 * walkGenJrl 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.
 *
 * walkGenJrl 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 General Lesser Public License for more details.
 * You should have received a copy of the GNU Lesser General Public License
 * along with walkGenJrl.  If not, see <http://www.gnu.org/licenses/>.
 *
 *  Research carried out within the scope of the 
 *  Joint Japanese-French Robotics Laboratory (JRL)
 */
/*! \file PatternGeneratorInterface.h
29
30
  \brief  This object provides a unified interface to access 
  the pattern generator.
Thomas Moulard's avatar
Thomas Moulard committed
31
32
33
34
35
36
37
38
  It allows to hide all the computation and hacking to the user.
*/


#ifndef _PATTERN_GENERATOR_INTERFACE_H_
#define _PATTERN_GENERATOR_INTERFACE_H_

#include <deque>
mnaveau's avatar
mnaveau committed
39
#include <jrl/walkgen/pinocchiorobot.hh>
40
#include <jrl/walkgen/pgtypes.hh>
Thomas Moulard's avatar
Thomas Moulard committed
41
42
43
44
45
46

namespace PatternGeneratorJRL
{

  /** @ingroup Interface
      This class is the interface between the Pattern Generator and the 
47
48
      external world. In addition to the classical setter and getter for various
      parameters
Thomas Moulard's avatar
Thomas Moulard committed
49
50
51
52
53
54
55
56
57
58
59
      there is the possibility to pass commands a string of stream to the method
      \a ParseCmd().
      
      There is a set of functionnalities directly supported by the API:
      
     
      
  */
  class WALK_GEN_JRL_EXPORT PatternGeneratorInterface 
    {    
    public:
60
61
    // overload the new[] eigen operator
    EIGEN_MAKE_ALIGNED_OPERATOR_NEW
62
63
64
65
66
67
    /*! Constructor 
      @param strm: Should provide the file to initialize the preview control,
      the path to the VRML model, and the name of the file containing the VRML
      model.
    */
    PatternGeneratorInterface(PinocchioRobot *) {};
Thomas Moulard's avatar
Thomas Moulard committed
68
    
69
70
    /*! Destructor */
    virtual ~PatternGeneratorInterface() {};
Thomas Moulard's avatar
Thomas Moulard committed
71
    
72
73
74
75
76
77
78
79
80
81
82
83
84
85
86
87
88
89
90
91
92
93
94
95
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
131
132
133
134
135
136
137
138
139
    /*! \brief Function to specify steps in the stack of the walking pattern 
      generator. 
      This method is different AddOnLineStep which is the default step add 
      when there is no policy,
      or no step available.
    */
    virtual void AddStepInStack(double dx, double dy, double theta)=0;
    
    
    /*! Common Initialization of walking. 
      @param[out] lStartingCOMPosition: For the starting position on the 
      articular space, returns
      the COM position.
      @param[out] lStartingZMPPosition: For the starting position on the 
      articular space, returns
      the ZMP position.
      @param[out] BodyAnglesIni: Basically it is a copy of CurrentJointValues
      but as a vector.
      @param[out] InitLeftFootAbsPos: Returns the current absolute position 
      of the left foot for
      the given posture of the robot.
      @param[out] InitRightFootAbsPos: Returns the current absolute position 
      of the right foot
      for the given posture of the robot.
      @param[out] lRelativeFootPositions: List of relative positions for the 
      support foot still in the
      stack of steps.
      @param[in] lCurrentJointValues: The vector of articular values in 
      classical C++ style.
      @param[in] ClearStepStackHandler: Clean the stack of steps after copy.
    */
    virtual void CommonInitializationOfWalking
    (COMState & lStartingCOMState,
     Eigen::Vector3d & lStartingZMPPosition,
     Eigen::VectorXd & BodyAnglesIni,
     FootAbsolutePosition & InitLeftFootAbsPos, 
     FootAbsolutePosition & InitRightFootAbsPos,
     std::deque<RelativeFootPosition> & lRelativeFootPositions,
     std::vector<double> & lCurrentJointValues,
     bool ClearStepStackHandler)=0;


    /*! \name Methods for the control part. 
      @{
    */
    
    /*! \brief Run One Step of the global control loop aka 
      The Main Method To Be Used.
      @param[out]  CurrentConfiguration 
      The current configuration of the robot according to 
      the implementation of dynamic-JRLJapan. 
      This should be first position and orientation
      of the waist, and then all the DOFs of your robot. 
      @param[out]  CurrentVelocity 
      The current velocity of the robot according to the 
      the implementation of dynamic-JRLJapan. 
      @param[out]  CurrentAcceleration  
      The current acceleration of the robot according to the 
      the implementation of dynamic-JRLJapan. 
      @param[out]  ZMPTarget  
      The target ZMP in the waist reference frame.
      @return True is there is still some data to send, false otherwise.
    */
    virtual bool RunOneStepOfTheControlLoop
    (Eigen::VectorXd & CurrentConfiguration,
     Eigen::VectorXd & CurrentVelocity,
     Eigen::VectorXd & CurrentAcceleration,
     Eigen::VectorXd & ZMPTarget)=0;
Thomas Moulard's avatar
Thomas Moulard committed
140
    
141
142
143
144
145
146
147
148
149
150
151
152
153
154
155
156
157
158
159
160
161
162
163
164
165
166
167
168
169
170
171
172
173
174
175
176
177
178
179
180
181
182
183
184
185
186
187
188
189
190
191
192
193
194
195
196
197
198
199
200
201
202
203
204
205
206
207
208
209
210
211
212
213
214
215
216
217
218
219
220
221
222
223
    /*! \brief Run One Step of the global control loop aka 
      The Main Method To Be Used.
      @param[out]  CurrentConfiguration The current configuration of the robot 
      according to 
      the implementation of dynamic-JRLJapan. This should be first position 
      and orientation
      of the waist, and then all the DOFs of your robot. 
      @param[out]  CurrentVelocity  The current velocity of the robot 
      according to the 
      the implementation of dynamic-JRLJapan. 
      @param[out]  CurrentAcceleration  The current acceleration of the 
      robot according to the 
      the implementation of dynamic-JRLJapan. 
      @param[out]  ZMPTarget  The target ZMP in the waist reference frame.
      @param[out] COMPosition The CoM position for this motion.
      @param[out] LeftFootPosition: Absolute position of the left foot.
      @param[out] RightFootPosition: Absolute position of the right foot.
      @return True is there is still some data to send, false otherwise.
    */
    virtual bool RunOneStepOfTheControlLoop
    (Eigen::VectorXd & CurrentConfiguration,
     Eigen::VectorXd & CurrentVelocity,
     Eigen::VectorXd & CurrentAcceleration,
     Eigen::VectorXd &ZMPTarget,
     COMPosition &COMPosition,
     FootAbsolutePosition &LeftFootPosition,
     FootAbsolutePosition &RightFootPosition)=0;

    /*! \brief Run One Step of the global control loop aka 
      The Main Method To Be Used.
      @param[out]  CurrentConfiguration The current configuration of the robot 
      according to 
      the implementation of dynamic-JRLJapan. This should be first position and 
      orientation
      of the waist, and then all the DOFs of your robot. 
      @param[out]  CurrentVelocity  The current velocity of the robot according 
      to the 
      the implementation of dynamic-JRLJapan. 
      @param[out]  CurrentAcceleration  The current acceleration of the robot 
      according to the 
      the implementation of dynamic-JRLJapan. 
      @param[out]  ZMPTarget  The target ZMP in the waist reference frame.
      @param[out] COMState The CoM state (up to the acceleration) for this 
      motion.
      @param[out] LeftFootPosition: Absolute position of the left foot.
      @param[out] RightFootPosition: Absolute position of the right foot.
      @return True is there is still some data to send, false otherwise.
    */
    virtual bool RunOneStepOfTheControlLoop
    (Eigen::VectorXd & CurrentConfiguration,
     Eigen::VectorXd & CurrentVelocity,
     Eigen::VectorXd & CurrentAcceleration,
     Eigen::VectorXd &ZMPTarget,
     COMState &COMState,
     FootAbsolutePosition &LeftFootPosition,
     FootAbsolutePosition &RightFootPosition)=0;



    /*! \brief Run One Step of the global control loop aka 
      The Main Method To Be Used.
      @param[out] LeftFootPosition: Absolute position of the left foot.
      @param[out] RightFootPosition: Absolute position of the right foot.
      @param[out] ZMPRefPos: ZMP position new reference 
      @param[out] COMRefPos: COM position new reference.
      @return True is there is still some data to send, false otherwise.
    */
    virtual bool RunOneStepOfTheControlLoop
    (FootAbsolutePosition &LeftFootPosition,
     FootAbsolutePosition &RightFootPosition,
     ZMPPosition &ZMPRefPos,
     COMPosition &COMRefPos)=0;
    /*! @} */

    /*! Set the current joint values of the robot.
      This method is used to properly initialize the pattern generator.
      It also updates the state of the robot if other control mechanisms 
      modifies the upper body part and if this should be taken into account
      into the pattern generator in the second loop of control. */
    virtual void SetCurrentJointValues(Eigen::VectorXd &lCurrentJointValues)=0;

    /*! \brief Returns the walking mode. */
    virtual int GetWalkMode() const=0;
Thomas Moulard's avatar
Thomas Moulard committed
224
    
225
226
227
    /*! \brief Get the leg joint velocity */
    virtual void GetLegJointVelocity(Eigen::VectorXd &dqr, 
                                     Eigen::VectorXd &dql) const=0;
Thomas Moulard's avatar
Thomas Moulard committed
228

229
230
    /*! \brief Read a sequence of steps. */
    virtual void ReadSequenceOfSteps(std::istringstream &strm)=0;
Thomas Moulard's avatar
Thomas Moulard committed
231
    
232
233
234
235
236
    /*! \name On-line steps related methods 
      @{
    */
    /*! \brief Start the creation of steps on line. */
    virtual void StartOnLineStepSequencing()=0;
Thomas Moulard's avatar
Thomas Moulard committed
237
    
238
239
    /*! \brief Stop the creation of steps on line. */
    virtual void StopOnLineStepSequencing()=0;
Thomas Moulard's avatar
Thomas Moulard committed
240

241
242
    /*! \brief Add an online step */
    virtual void AddOnLineStep(double X, double Y, double Theta)=0;
Thomas Moulard's avatar
Thomas Moulard committed
243
    
244
245
246
247
248
249
250
251
252
    /*! \brief Change online step.
      The strategy is the following: the step in single support phase at time t
      has its landing position changed to \f$ (X,Y,\theta) \f$ in absolute
      coordinates (i.e. in the world reference frame of the free flyer of the 
      robot).
      For stability reason there is no guarantee that this method can 
      realized the operation. Please see the documentation of the walking 
      pattern generator
      algorithm used. 
Thomas Moulard's avatar
Thomas Moulard committed
253
      
254
255
256
257
258
259
260
261
262
263
264
265
266
267
268
269
270
271
272
273
274
275
276
277
278
279
280
281
282
283
284
285
286
287
288
289
290
291
292
293
294
295
      If the time falls during a double support phase, the next single support 
      phase is chosen.
            
      @param[in] Time: Time information of the step. 
      @param[in] aFootAbsolutePosition: Absolute position of the foot.
      @return If the operation failed the method returns a negative number 
      related
      to an error, 0 otherwise.
    */
    virtual int ChangeOnLineStep(double Time, 
                                 FootAbsolutePosition &aFootAbsolutePosition,
                                 double &newtime)=0;

    /*! \brief Change online step.
      See the above method for the specifications.
      This method uses a different format with stream of strings.
      @param[in] Time: Time information of the step. 
      @return nothing */
    virtual void ChangeOnLineStep(std::istringstream &strm,double &newtime)=0;

    /*! @} */

    /*! \name For internal odometry.
      @{ */
    /*! \brief Update the current waist absolute position */
    virtual void UpdateAbsolutePosition(bool UpdateAbsMotionOrNot)=0;

    /*! \brief Get the waist position and orientation as a quaternion,
      and the planar X-Y orientation in Orientation. */
    virtual void getWaistPositionAndOrientation
    (double TQ[7],double &Orientation) const=0;

    /*! \brief Set Waist position and Orientation  */
    virtual void setWaistPositionAndOrientation(double TQ[7])=0;

    /*! \brief Get Waist velocity */
    virtual void getWaistVelocity(double &dx,
                                  double &dy,
                                  double &omega) const=0;

    /*! \brief An other method to get the waist position using a matrix. */
    virtual void getWaistPositionMatrix(Eigen::Matrix4d &lWaistAbsPos) const=0;
Thomas Moulard's avatar
Thomas Moulard committed
296
     
297
    /*!@} */
Thomas Moulard's avatar
Thomas Moulard committed
298
299


300
301
    /*! \brief Set the initial ZMP reference point. */
    virtual void setZMPInitialPoint(Eigen::Vector3d & lZMPInitialPoint)=0;
Thomas Moulard's avatar
Thomas Moulard committed
302

303
304
    /*! \brief Get the initial ZMP reference point. */
    virtual void getZMPInitialPoint(Eigen::Vector3d & lZMPInitialPoint) const=0;
Thomas Moulard's avatar
Thomas Moulard committed
305

306
307
308
    /*! \name System to call a given method based on registration of a method. 
      @{
    */
Thomas Moulard's avatar
Thomas Moulard committed
309

310
311
312
    /*! \brief Parse a command (to be used out of a plugin) 
      and call all objects which registered the method. */
    virtual int ParseCmd(std::istringstream &strm)=0;
Thomas Moulard's avatar
Thomas Moulard committed
313
314
    

315
    /*! @} */
Thomas Moulard's avatar
Thomas Moulard committed
316
317
318



319
320
321
322
323
324
325
326
327
328
329
330
331
332
333
334
335
336
337
338
339
340
341
342
343
344
345
    /*! \brief Returns the ZMP, CoM, left foot absolute position, and 
      right foot absolute position
      for the initiale pose.*/
    virtual void EvaluateStartingState
    (COMState  & lStartingCOMState,
     Eigen::Vector3d & lStartingZMPPosition,
     Eigen::Matrix<double, 6, 1> & lStartingWaistPose,
     FootAbsolutePosition & InitLeftFootAbsPos,
     FootAbsolutePosition & InitRightFootAbsPos)=0;
    
    /*! @} */

    /*! \brief Set velocity reference
      This method is only supported by Herdt's algorithm.
      Currently only a 3D speed is supported:
      \param x: Velocity along the saggital plane.
      \param y: Velocity along the perpendicular plane.
      \param yaw: Angular velocity in the x-y plane.
    */
    virtual void setVelocityReference(double x,
                                      double y,
                                      double yaw)=0;
    /*! \brief Set velocity reference
      \param x: Additive acceleration along the saggital plane.
      \param y: Additive acceleration along the lateral plane.
    */
    virtual void setCoMPerturbationForce(double x, double y)=0;
Thomas Moulard's avatar
Thomas Moulard committed
346
347
348
349

    };

  /*! Factory of Pattern generator interface. */
350
351
  WALK_GEN_JRL_EXPORT PatternGeneratorInterface *
  patternGeneratorInterfaceFactory(PinocchioRobot *);
olivier-stasse's avatar
olivier-stasse committed
352
}
Thomas Moulard's avatar
Thomas Moulard committed
353
354
355
356


#endif /* _PATTERN_GENERATOR_INTERFACE_H_ */