PatternGeneratorInterface.h 12.8 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
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66
67
68
69
70
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
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
224
225
226
227
228
229
230
231
232
233
234
235
236
237
238
239
240
241
242
243
244
245
246
247
248
249
250
251
252
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
296
297
298
299
300
301
302
303
304
305
306
307
308
309
310
311
/*
 * 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
  \brief  This object provides a unified interface to access the pattern generator.
  It allows to hide all the computation and hacking to the user.
*/


#ifndef _PATTERN_GENERATOR_INTERFACE_H_
#define _PATTERN_GENERATOR_INTERFACE_H_

#include <deque>
#include <robotDynamics/jrlHumanoidDynamicRobot.h>
#include <walkGenJrl/PGTypes.h>

namespace PatternGeneratorJRL
{

  /** @ingroup Interface
      This class is the interface between the Pattern Generator and the 
      external world. In addition to the classical setter and getter for various parameters
      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:
    
      /*! 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(CjrlHumanoidDynamicRobot *aHDR) {};

      /*! Destructor */
      virtual ~PatternGeneratorInterface() {};
    
      /*! \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,
						 MAL_S3_VECTOR(,double) & lStartingZMPPosition,
						 MAL_VECTOR(  & ,double) 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(MAL_VECTOR(,double) & CurrentConfiguration,
					      MAL_VECTOR(,double) & CurrentVelocity,
					      MAL_VECTOR(,double) & CurrentAcceleration,
					      MAL_VECTOR(,double) & ZMPTarget)=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] 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(MAL_VECTOR(,double) & CurrentConfiguration,
					      MAL_VECTOR(,double) & CurrentVelocity,
					      MAL_VECTOR(,double) & CurrentAcceleration,
					      MAL_VECTOR(,double) &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(MAL_VECTOR(,double) & CurrentConfiguration,
					      MAL_VECTOR(,double) & CurrentVelocity,
					      MAL_VECTOR(,double) & CurrentAcceleration,
					      MAL_VECTOR(,double) &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(MAL_VECTOR( &lCurrentJointValues,double))=0;

      /*! \brief Returns the walking mode. */
      virtual int GetWalkMode()=0;
    
      /*! \brief Get the leg joint velocity */
      virtual void GetLegJointVelocity(MAL_VECTOR( &dqr,double), 
				       MAL_VECTOR( &dql,double))=0;

      /*! \brief Read a sequence of steps. */
      virtual void ReadSequenceOfSteps(std::istringstream &strm)=0;
    
      /*! \name On-line steps related methods 
	@{
      */
      /*! \brief Start the creation of steps on line. */
      virtual void StartOnLineStepSequencing()=0;
    
      /*! \brief Stop the creation of steps on line. */
      virtual void StopOnLineStepSequencing()=0;

      /*! \brief Add an online step */
      virtual void AddOnLineStep(double X, double Y, double Theta)=0;
    
      /*! \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. 
      
	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)=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)=0;

      /*! \brief An other method to get the waist position using a matrix. */
      virtual void getWaistPositionMatrix(MAL_S4x4_MATRIX( &lWaistAbsPos,double))=0;
     
      /*!@} */


      /*! \brief Set the initial ZMP reference point. */
      virtual void setZMPInitialPoint(MAL_S3_VECTOR(&,double) lZMPInitialPoint)=0;

      /*! \brief Get the initial ZMP reference point. */
      virtual void getZMPInitialPoint(MAL_S3_VECTOR(&,double) lZMPInitialPoint)=0;

      /*! \name System to call a given method based on registration of a method. 
	@{
      */

      /*! \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;
    

      /*! @} */



      /*! \brief Returns the ZMP, CoM, left foot absolute position, and right foot absolute position
	for the initiale pose.*/
      virtual void EvaluateStartingState(COMState  & lStartingCOMState,
					 MAL_S3_VECTOR(,double) & lStartingZMPPosition,
					 MAL_VECTOR(,double) & 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;

    };

  /*! Factory of Pattern generator interface. */
  WALK_GEN_JRL_EXPORT PatternGeneratorInterface * patternGeneratorInterfaceFactory(CjrlHumanoidDynamicRobot *aHDR);
};


#endif /* _PATTERN_GENERATOR_INTERFACE_H_ */