patterngeneratorinterface.hh 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
/*
 * 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>
mnaveau's avatar
mnaveau committed
38
#include <jrl/walkgen/pinocchiorobot.hh>
39
#include <jrl/walkgen/pgtypes.hh>
Thomas Moulard's avatar
Thomas Moulard committed
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57

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:
58
59
    // overload the new[] eigen operator
    EIGEN_MAKE_ALIGNED_OPERATOR_NEW
Thomas Moulard's avatar
Thomas Moulard committed
60
61
62
63
      /*! 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.
      */
64
      PatternGeneratorInterface(PinocchioRobot *) {};
Thomas Moulard's avatar
Thomas Moulard committed
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

      /*! 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,
Olivier Stasse's avatar
Olivier Stasse committed
92
93
						 Eigen::Vector3d & lStartingZMPPosition,
						 Eigen::VectorXd & BodyAnglesIni,
Thomas Moulard's avatar
Thomas Moulard committed
94
95
96
97
98
99
100
101
102
103
104
105
106
107
108
109
110
111
112
113
114
115
						 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.
      */
Olivier Stasse's avatar
Olivier Stasse committed
116
117
118
119
      virtual bool RunOneStepOfTheControlLoop(Eigen::VectorXd & CurrentConfiguration,
					      Eigen::VectorXd & CurrentVelocity,
					      Eigen::VectorXd & CurrentAcceleration,
					      Eigen::VectorXd & ZMPTarget)=0;
Thomas Moulard's avatar
Thomas Moulard committed
120
121
122
123
124
125
126
127
128
129
130
131
132
133
134

      /*! \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.
      */
Olivier Stasse's avatar
Olivier Stasse committed
135
136
137
138
      virtual bool RunOneStepOfTheControlLoop(Eigen::VectorXd & CurrentConfiguration,
					      Eigen::VectorXd & CurrentVelocity,
					      Eigen::VectorXd & CurrentAcceleration,
					      Eigen::VectorXd &ZMPTarget,
Thomas Moulard's avatar
Thomas Moulard committed
139
140
141
142
143
144
145
146
147
148
149
150
151
152
153
154
155
156
					      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.
      */
Olivier Stasse's avatar
Olivier Stasse committed
157
158
159
160
      virtual bool RunOneStepOfTheControlLoop(Eigen::VectorXd & CurrentConfiguration,
					      Eigen::VectorXd & CurrentVelocity,
					      Eigen::VectorXd & CurrentAcceleration,
					      Eigen::VectorXd &ZMPTarget,
Thomas Moulard's avatar
Thomas Moulard committed
161
162
163
164
165
166
167
168
169
170
171
172
173
174
175
176
177
178
179
180
181
182
183
184
					      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. */
Olivier Stasse's avatar
Olivier Stasse committed
185
      virtual void SetCurrentJointValues(Eigen::VectorXd &lCurrentJointValues)=0;
Thomas Moulard's avatar
Thomas Moulard committed
186
187

      /*! \brief Returns the walking mode. */
188
      virtual int GetWalkMode() const=0;
Thomas Moulard's avatar
Thomas Moulard committed
189
190
    
      /*! \brief Get the leg joint velocity */
Olivier Stasse's avatar
Olivier Stasse committed
191
192
      virtual void GetLegJointVelocity(Eigen::VectorXd &dqr, 
				       Eigen::VectorXd &dql) const=0;
Thomas Moulard's avatar
Thomas Moulard committed
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

      /*! \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. */
244
      virtual void getWaistPositionAndOrientation(double TQ[7],double &Orientation) const=0;
Thomas Moulard's avatar
Thomas Moulard committed
245
246
247
248
249
250
251

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

      /*! \brief Get Waist velocity */
      virtual void getWaistVelocity(double &dx,
				    double &dy,
252
				    double &omega) const=0;
Thomas Moulard's avatar
Thomas Moulard committed
253
254

      /*! \brief An other method to get the waist position using a matrix. */
Olivier Stasse's avatar
Olivier Stasse committed
255
      virtual void getWaistPositionMatrix(Eigen::Matrix4d &lWaistAbsPos) const=0;
Thomas Moulard's avatar
Thomas Moulard committed
256
257
258
259
260
     
      /*!@} */


      /*! \brief Set the initial ZMP reference point. */
Olivier Stasse's avatar
Olivier Stasse committed
261
      virtual void setZMPInitialPoint(Eigen::Vector3d & lZMPInitialPoint)=0;
Thomas Moulard's avatar
Thomas Moulard committed
262
263

      /*! \brief Get the initial ZMP reference point. */
Olivier Stasse's avatar
Olivier Stasse committed
264
      virtual void getZMPInitialPoint(Eigen::Vector3d & lZMPInitialPoint) const=0;
Thomas Moulard's avatar
Thomas Moulard committed
265
266
267
268
269
270
271
272
273
274
275
276
277
278
279
280

      /*! \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,
Olivier Stasse's avatar
Olivier Stasse committed
281
282
					 Eigen::Vector3d & lStartingZMPPosition,
					 Eigen::Matrix<double, 6, 1> & lStartingWaistPose,
Thomas Moulard's avatar
Thomas Moulard committed
283
284
285
286
287
288
289
290
291
292
293
294
295
296
297
298
299
300
301
302
303
304
305
306
					 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. */
307
  WALK_GEN_JRL_EXPORT PatternGeneratorInterface * patternGeneratorInterfaceFactory(PinocchioRobot *);
olivier-stasse's avatar
olivier-stasse committed
308
}
Thomas Moulard's avatar
Thomas Moulard committed
309
310
311
312


#endif /* _PATTERN_GENERATOR_INTERFACE_H_ */