patterngeneratorinterfaceprivate.hh 26.4 KB
Newer Older
Thomas Moulard's avatar
Thomas Moulard committed
1
/*
2
 * Copyright 2005, 2006, 2007, 2008, 2009, 2010,
Thomas Moulard's avatar
Thomas Moulard committed
3
4
5
6
 *
 * Andrei     Herdt
 * Benallegue Mehdi
 * Olivier    Stasse
7
 * Eiichi     Yoshida
Thomas Moulard's avatar
Thomas Moulard committed
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
 *
 * 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/>.
 *
24
 *  Research carried out within the scope of the
Thomas Moulard's avatar
Thomas Moulard committed
25
26
27
 *  Joint Japanese-French Robotics Laboratory (JRL)
 */
/*! \file PatternGeneratorInterface.h
28
  \brief This object provides a unified interface to access
Olivier Stasse's avatar
Olivier Stasse committed
29
  the pattern generator.
30
  It allows to hide all the computation and hacking to the user.
Thomas Moulard's avatar
Thomas Moulard committed
31
32
33
34
35
36
37
*/

#ifndef _PATTERN_GENERATOR_INTERFACE_PRIVATE_H_
#define _PATTERN_GENERATOR_INTERFACE_PRIVATE_H_

#include <sstream>

38
#include <jrl/walkgen/patterngeneratorinterface.hh>
Thomas Moulard's avatar
Thomas Moulard committed
39

40
#include <PreviewControl/PreviewControl.hh>
41
#include <PreviewControl/ZMPPreviewControlWithMultiBodyZMP.hh>
Thomas Moulard's avatar
Thomas Moulard committed
42

43
44
#include <ZMPRefTrajectoryGeneration/AnalyticalMorisawaCompact.hh>
#include <ZMPRefTrajectoryGeneration/ZMPConstrainedQPFastFormulation.hh>
45
46
47
#include <ZMPRefTrajectoryGeneration/ZMPDiscretization.hh>
#include <ZMPRefTrajectoryGeneration/ZMPQPWithConstraint.hh>
#include <ZMPRefTrajectoryGeneration/ZMPVelocityReferencedQP.hh>
48
#include <ZMPRefTrajectoryGeneration/ZMPVelocityReferencedSQP.hh>
Thomas Moulard's avatar
Thomas Moulard committed
49

50
51
52
#include <MotionGeneration/ComAndFootRealizationByGeometry.hh>
#include <MotionGeneration/GenerateMotionFromKineoWorks.hh>
#include <MotionGeneration/StepOverPlanner.hh>
Thomas Moulard's avatar
Thomas Moulard committed
53

54
#include <FootTrajectoryGeneration/LeftAndRightFootTrajectoryGenerationMultiple.hh>
Thomas Moulard's avatar
Thomas Moulard committed
55

56
#include <StepStackHandler.hh>
Thomas Moulard's avatar
Thomas Moulard committed
57

58
#include <SimplePlugin.hh>
59
#include <SimplePluginManager.hh>
Thomas Moulard's avatar
Thomas Moulard committed
60

61
#include <GlobalStrategyManagers/CoMAndFootOnlyStrategy.hh>
62
#include <GlobalStrategyManagers/DoubleStagePreviewControlStrategy.hh>
Thomas Moulard's avatar
Thomas Moulard committed
63

64
namespace PatternGeneratorJRL {
Thomas Moulard's avatar
Thomas Moulard committed
65

66
67
68
69
70
71
/** @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().
Thomas Moulard's avatar
Thomas Moulard committed
72

73
    There is a set of functionalities directly supported by the API:
74

Thomas Moulard's avatar
Thomas Moulard committed
75

76

77
78
79
80
81
*/
class PatternGeneratorInterfacePrivate
    : public virtual PatternGeneratorInterface,
      SimplePluginManager,
      SimplePlugin
Thomas Moulard's avatar
Thomas Moulard committed
82

83
84
85
86
87
88
89
90
91
{
public:
  EIGEN_MAKE_ALIGNED_OPERATOR_NEW
  /*! 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.
  */
  PatternGeneratorInterfacePrivate(PinocchioRobot *aPinocchioRobotRobot);
Thomas Moulard's avatar
Thomas Moulard committed
92

93
94
  /*! Destructor */
  ~PatternGeneratorInterfacePrivate();
Thomas Moulard's avatar
Thomas Moulard committed
95

96
97
98
99
100
101
  /*! \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.
  */
  void AddStepInStack(double dx, double dy, double theta);
Thomas Moulard's avatar
Thomas Moulard committed
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
  /*! \name High levels function to create automatically stack of steps
    following specific motions.
    @{
  */
  /*! \brief This methods generate a stack of steps which make the robot
    follows an arc.
    The direction of the robot is tangential to the arc.

    @param[in] x: Position of the center of the circle along the X-axis.
    @param[in] y: Position of the center of the circle along the Y-axis.
    @param[in] R: Ray of the circle.
    @param[in] arc_deg: Arc in degrees along which the robot walks.
    @param[in] SupportFoot: Indicates which is the first support foot (1)
    Left or (-1) Right.
  */
  void CreateArcInStepStack(double x, double y, double R, double arc_deg,
                            int SupportFoot);

  /*! \brief This methods generate a stack of steps which make the robot
    follows an arc.
    The direction of the robot is towards the center of the arc.
    The robot is therefore expected to move sideways.

    @param[in] R: Ray of the circle.
    @param[in] arc_deg: Arc in degrees along which the robot walks.
    @param[in] SupportFoot: Indicates which is the first support foot (1)
    Left or (-1) Right.
  */
  void CreateArcCenteredInStepStack(double R, double arc_deg, int SupportFoot);

  /*! \brief This specifies which foot will be used as the first support
    of the motion. */
  void PrepareForSupportFoot(int SupportFoot);

  /*! \brief This method precomputes all the buffers necessary for walking
    according to the chosen strategy. */
  void FinishAndRealizeStepSequence();
  /*! @} */

  /*! 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.
  */
  void CommonInitializationOfWalking(
      COMState &lStartingCOMState, Eigen::Vector3d &lStartingZMPPosition,
      Eigen::VectorXd &BodyAnglesIni, FootAbsolutePosition &InitLeftFootAbsPos,
      FootAbsolutePosition &InitRightFootAbsPos,
      deque<RelativeFootPosition> &lRelativeFootPositions,
      std::vector<double> &lCurrentJointValues, bool ClearStepStackHandler);

  /*! \name Methods for the control part.
    @{
  */
Thomas Moulard's avatar
Thomas Moulard committed
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
  /*! \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 Pinocchio.
    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 Pinocchio.
    @param[out]  CurrentAcceleration  The current acceleration of the robot
    according to the
    the implementation of Pinocchio.
    @param[out]  ZMPTarget  The target ZMP in the waist reference frame.
    @return True is there is still some data to send, false otherwise.
  */
  bool RunOneStepOfTheControlLoop(Eigen::VectorXd &CurrentConfiguration,
                                  Eigen::VectorXd &CurrentVelocity,
                                  Eigen::VectorXd &CurrentAcceleration,
                                  Eigen::VectorXd &ZMPTarget);

  /*! \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 Pinocchio. 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 Pinocchio.
    @param[out]  CurrentAcceleration  The current acceleration of the robot
    according to the
    the implementation of Pinocchio.
    @param[out]  ZMPTarget  The target ZMP in the waist reference frame.
    @param[out] COMState The CoM state 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.
  */
  bool RunOneStepOfTheControlLoop(Eigen::VectorXd &CurrentConfiguration,
                                  Eigen::VectorXd &CurrentVelocity,
                                  Eigen::VectorXd &CurrentAcceleration,
                                  Eigen::VectorXd &ZMPTarget,
                                  COMState &COMState,
                                  FootAbsolutePosition &LeftFootPosition,
                                  FootAbsolutePosition &RightFootPosition);

  /*! \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 Pinocchio. 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 Pinocchio.
    @param[out]  CurrentAcceleration  The current acceleration of the robot
    according to the
    the implementation of Pinocchio.
    @param[out]  ZMPTarget  The target ZMP in the waist reference frame.
    @param[out] aCOMPosition 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.
  */
  bool RunOneStepOfTheControlLoop(Eigen::VectorXd &CurrentConfiguration,
                                  Eigen::VectorXd &CurrentVelocity,
                                  Eigen::VectorXd &CurrentAcceleration,
                                  Eigen::VectorXd &ZMPTarget,
                                  COMPosition &aCOMPosition,
                                  FootAbsolutePosition &LeftFootPosition,
                                  FootAbsolutePosition &RightFootPosition);

  /*! \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.
  */
  bool RunOneStepOfTheControlLoop(FootAbsolutePosition &LeftFootPosition,
                                  FootAbsolutePosition &RightFootPosition,
                                  ZMPPosition &ZMPRefPos,
                                  COMPosition &COMRefPos);
261
262
263


  bool RunOneStepOfTheControlLoop(ControlLoopOneStepArgs &aControlLoopStepArgs);
264
  /*! @} */
Thomas Moulard's avatar
Thomas Moulard committed
265

266
267
268
269
270
271
  /*! 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. */
  void SetCurrentJointValues(Eigen::VectorXd &lCurrentJointValues);
Thomas Moulard's avatar
Thomas Moulard committed
272

273
274
  /*! \brief Returns the walking mode. */
  int GetWalkMode() const;
Thomas Moulard's avatar
Thomas Moulard committed
275

276
277
  /*! \brief Get the leg joint velocity */
  void GetLegJointVelocity(Eigen::VectorXd &dqr, Eigen::VectorXd &dql) const;
Thomas Moulard's avatar
Thomas Moulard committed
278

279
280
  /*! \brief Read a velocity reference. */
  void setVelReference(istringstream &strm);
Thomas Moulard's avatar
Thomas Moulard committed
281

282
283
  /*! \brief Read a perturbation force on the com. */
  void setCoMPerturbationForce(istringstream &strm);
Thomas Moulard's avatar
Thomas Moulard committed
284

285
286
  /*! \brief Initialize online mode of Herdt. */
  void initOnlineHerdt();
Thomas Moulard's avatar
Thomas Moulard committed
287

288
289
  /*! \brief Initialize online mode of Naveau. */
  void initOnlineNaveau();
Thomas Moulard's avatar
Thomas Moulard committed
290

291
292
  /*! \brief Read a sequence of steps. */
  void ReadSequenceOfSteps(istringstream &strm);
293

294
295
296
297
298
299
300
301
302
303
304
305
306
307
308
309
310
311
312
313
314
315
316
317
318
319
320
321
322
323
324
325
326
  /*! \name On-line steps related methods
    @{
  */
  /*! \brief Start the creation of steps on line. */
  void StartOnLineStepSequencing();

  /*! \brief Stop the creation of steps on line. */
  void StopOnLineStepSequencing();

  /*! \brief Add an online step */
  void AddOnLineStep(double X, double Y, double Theta);

  /*! \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.
  */
  int ChangeOnLineStep(double Time, FootAbsolutePosition &aFootAbsolutePosition,
                       double &newtime);
Thomas Moulard's avatar
Thomas Moulard committed
327

328
329
330
331
332
333
  /*! \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 */
  void ChangeOnLineStep(istringstream &strm, double &newtime);
Thomas Moulard's avatar
Thomas Moulard committed
334

335
  /*! @} */
Thomas Moulard's avatar
Thomas Moulard committed
336

337
338
339
340
  /*! \name For SLAM
    @{ */
  /*! \brief Update the current waist absolute position */
  void UpdateAbsolutePosition(bool UpdateAbsMotionOrNot);
Thomas Moulard's avatar
Thomas Moulard committed
341

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

346
347
  /*! \brief Set Waist position and Orientation  */
  void setWaistPositionAndOrientation(double TQ[7]);
348

349
350
  /*! \brief Get Waist velocity */
  void getWaistVelocity(double &dx, double &dy, double &omega) const;
Thomas Moulard's avatar
Thomas Moulard committed
351

352
353
  /*! \brief An other method to get the waist position using a matrix. */
  void getWaistPositionMatrix(Eigen::Matrix4d &lWaistAbsPos) const;
Thomas Moulard's avatar
Thomas Moulard committed
354

355
  /*!@} */
Thomas Moulard's avatar
Thomas Moulard committed
356

357
358
359
  /*! \name Handling of the inter-objects relationships.
    @{
  */
Thomas Moulard's avatar
Thomas Moulard committed
360

361
362
  /*! \brief Instanciate the necessary objects. */
  void ObjectsInstanciation();
Thomas Moulard's avatar
Thomas Moulard committed
363

364
365
  /*! \brief Set the inter object relationship. */
  void InterObjectRelationInitialization();
Thomas Moulard's avatar
Thomas Moulard committed
366

367
  /*! @}*/
368

369
370
  /*! \brief Set the initial ZMP reference point. */
  void setZMPInitialPoint(Eigen::Vector3d &lZMPInitialPoint);
Thomas Moulard's avatar
Thomas Moulard committed
371

372
373
  /*! \brief Get the initial ZMP reference point. */
  void getZMPInitialPoint(Eigen::Vector3d &lZMPInitialPoint) const;
student's avatar
update    
student committed
374

375
376
377
  /*! \name System to call a given method based on registration of a method.
    @{
  */
Thomas Moulard's avatar
Thomas Moulard committed
378

379
380
381
382
383
384
385
386
387
388
389
390
391
392
393
394
395
396
397
398
399
400
401
402
403
404
405
406
407
  /*! \brief Parse a command (to be used out of a plugin) and call all
    objects which registered the method. */
  int ParseCmd(std::istringstream &strm);

  /*! \brief This method register a method to a specific object which
    derivates from SimplePlugin class. */
  bool RegisterMethod(string &MethodName, SimplePlugin *aSP);

  /*! @} */

  /*! \brief Returns the ZMP, CoM, left foot absolute position, and
    right foot absolute position
    for the initiale pose.*/
  void EvaluateStartingState(COMState &lStartingCOMState,
                             Eigen::Vector3d &lStartingZMPPosition,
                             Eigen::Matrix<double, 6, 1> &lStartingWaistPose,
                             FootAbsolutePosition &InitLeftFootAbsPos,
                             FootAbsolutePosition &InitRightFootAbsPos);

  /*! @} */

  /*! \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.
  */
  void setVelocityReference(double x, double y, double yaw);
Thomas Moulard's avatar
Thomas Moulard committed
408

409
  /*! @} */
Thomas Moulard's avatar
Thomas Moulard committed
410

411
412
413
414
415
  /*! \brief Set additive acceleration to the state
    \param x: Additive acceleration therm along the saggital plane.
    \param y: Additive acceleration therm along the perpendicular plane.
  */
  void setCoMPerturbationForce(double x, double y);
416

417
418
419
420
protected:
  /*! \name Methods for interpreter.
    @{
  */
Thomas Moulard's avatar
Thomas Moulard committed
421

422
423
  /*! \brief Set the shift of the ZMP height for stepping over. */
  virtual void m_SetZMPShiftParameters(std::istringstream &strm);
Thomas Moulard's avatar
Thomas Moulard committed
424

425
426
  /*! \brief Set time distribution parameters. */
  virtual void m_SetTimeDistrParameters(std::istringstream &strm);
Thomas Moulard's avatar
Thomas Moulard committed
427

428
429
  /*! \brief Set upper body motion parameters. */
  virtual void m_SetUpperBodyMotionParameters(std::istringstream &strm);
Thomas Moulard's avatar
Thomas Moulard committed
430

431
432
  /*! \brief Set the limits of the feasibility (stepping over parameters) */
  virtual void m_SetLimitsFeasibility(std::istringstream &strm);
433

434
435
  /*! \brief Read file from Kineoworks. */
  virtual void m_ReadFileFromKineoWorks(std::istringstream &strm);
Thomas Moulard's avatar
Thomas Moulard committed
436

437
438
439
  /*! \brief Specify a sequence of step without asking for
    immediate execution and end sequence. */
  virtual void m_PartialStepSequence(istringstream &strm);
440

441
442
  /*! \brief This method set PBW's algorithm for ZMP trajectory planning. */
  virtual void m_SetAlgoForZMPTraj(istringstream &strm);
Thomas Moulard's avatar
Thomas Moulard committed
443

444
445
446
  /*! \brief Interface to hrpsys to start the realization of
    the stacked of step sequences. */
  virtual void m_FinishAndRealizeStepSequence(std::istringstream &strm);
Thomas Moulard's avatar
Thomas Moulard committed
447

448
449
  /*! \brief Realize a sequence of steps. */
  virtual void m_StepSequence(std::istringstream &strm);
Thomas Moulard's avatar
Thomas Moulard committed
450

451
  virtual void m_StepStairSequence(std::istringstream &strm);
Thomas Moulard's avatar
Thomas Moulard committed
452

453
454
455
private:
  /*! Object to handle the stack of relative steps. */
  StepStackHandler *m_StepStackHandler;
456

457
458
459
  /*! Buffer needed to perform the stepping over
    obstacle. */
  std::vector<double> m_ZMPShift;
Thomas Moulard's avatar
Thomas Moulard committed
460

461
462
  /*! Gain factor for the default arm motion while walking. */
  double m_GainFactor;
463

464
465
466
467
  /*! Objects to generate a ZMP profile from
    the step of stacks. They provide a buffer for
    the ZMP position to be used every dt
    in the control loop. @{ */
Thomas Moulard's avatar
Thomas Moulard committed
468

469
470
  /*! Kajita's heuristic: the center of the convex hull. */
  ZMPDiscretization *m_ZMPD;
471

472
473
  /*! QP formulation with constraints. */
  ZMPQPWithConstraint *m_ZMPQP;
Thomas Moulard's avatar
Thomas Moulard committed
474

475
476
  /*! QP formulation with constraints. */
  ZMPConstrainedQPFastFormulation *m_ZMPCQPFF;
477

478
479
  /*! QP formulation with a velocity reference. */
  ZMPVelocityReferencedQP *m_ZMPVRQP;
480

481
482
483
484
#if USE_QUADPROG == 1
  /*! SQP formulation with a velocity reference. */
  ZMPVelocityReferencedSQP *m_ZMPVRSQP;
#endif
485

486
487
  /*! ZMP and CoM trajectories generation from an analytical formulation */
  AnalyticalMorisawaCompact *m_ZMPM;
488

489
490
  /*! Specified ZMP starting point. */
  Eigen::Vector3d m_ZMPInitialPoint;
Thomas Moulard's avatar
Thomas Moulard committed
491

492
493
494
495
  /*! Boolean stating if the user has specified or not the ZMP initial point.
    This boolean is set to true when the user is setting the previous value.
    Otherwise it is set to false.*/
  bool m_ZMPInitialPointSet;
496

497
  /*@} */
498

499
500
  /*! The Preview Control object. */
  PreviewControl *m_PC;
Thomas Moulard's avatar
Thomas Moulard committed
501

502
503
504
  /*! The object to be used to perform one step of
    control, and generates the corrected CoM trajectory. */
  ZMPPreviewControlWithMultiBodyZMP *m_ZMPpcwmbz;
Thomas Moulard's avatar
Thomas Moulard committed
505

506
507
  /*! Object needed to perform a path provided by Kineo */
  GenerateMotionFromKineoWorks *m_GMFKW;
Thomas Moulard's avatar
Thomas Moulard committed
508

509
510
  /*! Conversion between the index of the plan and the robot DOFs. */
  std::vector<int> m_ConversionForUpperBodyFromLocalIndexToRobotDOFs;
511

512
513
  /*! Current Actuated Joint values of the robot. */
  std::vector<double> m_CurrentActuatedJointValues;
Thomas Moulard's avatar
Thomas Moulard committed
514

515
516
517
  /*! Position of the waist:
    Relative.*/
  Eigen::Matrix4d m_WaistRelativePos;
Thomas Moulard's avatar
Thomas Moulard committed
518

519
520
  /*! Absolute: */
  Eigen::Matrix4d m_WaistAbsPos;
521

522
523
  /*! Orientation: */
  double m_AbsTheta, m_AbsMotionTheta;
Thomas Moulard's avatar
Thomas Moulard committed
524

525
526
527
  /*! Position of the motion: */
  Eigen::Matrix4d m_MotionAbsPos;
  Eigen::Matrix4d m_MotionAbsOrientation;
Thomas Moulard's avatar
Thomas Moulard committed
528

529
530
531
  /*! Absolute speed:*/
  Eigen::Vector4d m_AbsLinearVelocity;
  Eigen::Vector4d m_AbsAngularVelocity;
532

533
534
  /*! Aboluste acceleration */
  Eigen::Vector4d m_AbsLinearAcc;
Thomas Moulard's avatar
Thomas Moulard committed
535

536
537
  /*! Keeps track of the last correct support foot. */
  int m_KeepLastCorrectSupportFoot;
Thomas Moulard's avatar
Thomas Moulard committed
538

539
540
541
  /*! Boolean to ensure a correct initialization of the
    step's stack. */
  bool m_IncorrectInitialization;
Thomas Moulard's avatar
Thomas Moulard committed
542

543
544
545
546
547
  /*! \name Global strategy handlers
    @{
  */
  /*! \brief Double stage preview control strategy */
  DoubleStagePreviewControlStrategy *m_DoubleStagePCStrategy;
Thomas Moulard's avatar
Thomas Moulard committed
548

549
550
  /*! \brief Simple strategy just output CoM and Foot position. */
  CoMAndFootOnlyStrategy *m_CoMAndFootOnlyStrategy;
551

552
553
  /*! \brief General handler. */
  GlobalStrategyManager *m_GlobalStrategyManager;
554

555
  /*! @} */
Thomas Moulard's avatar
Thomas Moulard committed
556

557
558
  /*! Store the debug mode. */
  int m_DebugMode;
Thomas Moulard's avatar
Thomas Moulard committed
559

560
561
  /*! Store the number of degree of freedoms */
  int m_DOF;
Thomas Moulard's avatar
Thomas Moulard committed
562

563
564
  /*! Store the height of the arm. */
  double m_ZARM;
Thomas Moulard's avatar
Thomas Moulard committed
565

566
567
568
569
570
  /**! \name Time related parameters.
     @{
  */
  /*! \brief Sampling period of the control loop. */
  double m_SamplingPeriod;
Thomas Moulard's avatar
Thomas Moulard committed
571

572
573
  /*! \brief Window of the preview control */
  double m_PreviewControlTime;
Thomas Moulard's avatar
Thomas Moulard committed
574

575
576
577
578
579
  /*! \brief Internal clock.
    This field is updated every call to RunOneStepOfControl.
    It is assumed that this is done every m_SamplingPeriod.
  */
  double m_InternalClock;
Thomas Moulard's avatar
Thomas Moulard committed
580

581
  /*! @} */
582

583
584
585
  /*! Store the local Single support time,
    and the Double support time. */
  double m_TSsupport, m_TDsupport;
586

587
588
  /*! Height of the CoM. */
  double m_Zc;
589

590
591
  /*! Discrete size of the preview control window */
  unsigned int m_NL;
592

593
594
  /*! Local time while performing the control loop. */
  unsigned long int m_count;
595

596
597
  /*! Maximal value for the arms in front of the robot */
  double m_Xmax;
598

599
600
601
602
603
  /*! Variables used to compute speed for other purposes. */
  Eigen::VectorXd m_prev_qr;
  Eigen::VectorXd m_prev_ql;
  Eigen::VectorXd m_prev_dqr;
  Eigen::VectorXd m_prev_dql;
Thomas Moulard's avatar
Thomas Moulard committed
604

605
606
607
608
609
610
611
612
  /* Debug variables. */
  Eigen::VectorXd m_Debug_prev_qr;
  Eigen::VectorXd m_Debug_prev_ql;
  Eigen::VectorXd m_Debug_prev_dqr;
  Eigen::VectorXd m_Debug_prev_dql;
  Eigen::VectorXd m_Debug_prev_UpperBodyAngles;
  Eigen::VectorXd m_Debug_prev_qr_RefState;
  Eigen::VectorXd m_Debug_prev_ql_RefState;
Thomas Moulard's avatar
Thomas Moulard committed
613

614
615
616
  double m_Debug_prev_qWaistYaw, m_Debug_prev_dqWaistYaw;
  Eigen::Vector3d m_Debug_prev_P, m_Debug_prev_L;
  bool m_FirstPrint, m_FirstRead;
617

618
  bool m_ShouldBeRunning;
Thomas Moulard's avatar
Thomas Moulard committed
619

620
  bool m_Running;
Thomas Moulard's avatar
Thomas Moulard committed
621

622
  bool m_feedBackControl;
623

624
625
626
  /*! \name To handle a new step.
    @{
  */
Thomas Moulard's avatar
Thomas Moulard committed
627

628
629
  /*! \brief There is a new user specified step. */
  bool m_NewStep;
Thomas Moulard's avatar
Thomas Moulard committed
630

631
632
633
  /*! \brief X, Y, Theta coordinates of the new step.*/
  double m_NewStepX, m_NewStepY, m_NewStepZ, m_NewTheta;
  /*! @} */
634

635
636
  /*! \brief Add automatically the first new step */
  bool m_AutoFirstStep;
637

638
639
  /* ! Store the current relative state of the waist */
  COMState m_CurrentWaistState;
Thomas Moulard's avatar
Thomas Moulard committed
640

641
642
  /* ! current time period for the control */
  double m_dt;
Thomas Moulard's avatar
Thomas Moulard committed
643

644
645
646
647
  /*! \name Internals to deal with several ZMP CoM generation algorithms
    @{ */
  /*! Algorithm to compute ZMP and CoM trajectory */
  int m_AlgorithmforZMPCOM;
Thomas Moulard's avatar
Thomas Moulard committed
648

649
650
651
652
653
  /*! Constants
    @{ */
  /*! Using Preview Control with 2 stages proposed by Shuuji Kajita in 2003.
   */
  static const int ZMPCOM_KAJITA_2003 = 1;
Thomas Moulard's avatar
Thomas Moulard committed
654

655
656
657
  /*! Using the preview control with 2 stages proposed by Pierre-Brice
    Wieber in 2006. */
  static const int ZMPCOM_WIEBER_2006 = 2;
658

659
660
  /*! Using the analytical solution proposed by Morisawa in 2007. */
  static const int ZMPCOM_MORISAWA_2007 = 3;
Thomas Moulard's avatar
Thomas Moulard committed
661

662
663
664
  /*! Using the QP constrained problem resolution proposed by Dimitrov
    in 2008. */
  static const int ZMPCOM_DIMITROV_2008 = 4;
665

666
667
  /*! Using the velocity referenced QP proposed by Herdt in 2010. */
  static const int ZMPCOM_HERDT_2010 = 5;
Thomas Moulard's avatar
Thomas Moulard committed
668

669
670
671
672
  /*! Using the velocity referenced QP proposed by Herdt in 2010. */
  static const int ZMPCOM_NAVEAU_2015 = 6;
  /*! @} */
  /*! @} */
Thomas Moulard's avatar
Thomas Moulard committed
673

674
675
676
677
  /*! Humanoid Dynamic robot */
  PinocchioRobot *m_PinocchioRobot;
  // CjrlHumanoidDynamicRobot * m_HumanoidDynamicRobot, *
  // m_2HumanoidDynamicRobot;
Thomas Moulard's avatar
Thomas Moulard committed
678

679
680
  /*! Speed of the leg. */
  Eigen::VectorXd m_dqr, m_dql;
681

682
683
684
  /*! Objet to realize the generate the posture for given CoM
    and feet positions. */
  std::vector<ComAndFootRealization *> m_ComAndFootRealization;
Thomas Moulard's avatar
Thomas Moulard committed
685

686
687
688
  /* \name Object related to stepping over.
     @{
  */
Thomas Moulard's avatar
Thomas Moulard committed
689

690
691
  /*! Planner for stepping over an obstacle. */
  StepOverPlanner *m_StOvPl;
692

693
694
  /*! Position and parameters related to the obstacle. */
  ObstaclePar m_ObstaclePars;
695

696
697
  /*! Boolean on the obstacle's detection */
  bool m_ObstacleDetected;
Thomas Moulard's avatar
Thomas Moulard committed
698

699
700
  /*! Time Distribution factor */
  std::vector<double> m_TimeDistrFactor;
Thomas Moulard's avatar
Thomas Moulard committed
701

702
703
  /*! Variable for delta feasibility limit */
  double m_DeltaFeasibilityLimit;
704

705
706
  /*! New time for step interval  using Changing On Line Step. */
  double m_NewNextStepInterval;
707

708
  /* @} */
Thomas Moulard's avatar
Thomas Moulard committed
709

710
711
  /*! \brief Foot Trajectory Generator */
  LeftAndRightFootTrajectoryGenerationMultiple *m_FeetTrajectoryGenerator;
Thomas Moulard's avatar
Thomas Moulard committed
712

713
714
715
  /*! \name Buffers of Positions.
    @{
  */
Thomas Moulard's avatar
Thomas Moulard committed
716

717
718
  /*! Buffer of ZMP positions */
  deque<ZMPPosition> m_ZMPPositions;
719

720
721
  /*! Buffer of Absolute foot position (World frame) */
  deque<FootAbsolutePosition> m_FootAbsolutePositions;
Thomas Moulard's avatar
Thomas Moulard committed
722

723
724
  /*! Buffer of absolute foot position. */
  deque<FootAbsolutePosition> m_LeftFootPositions, m_RightFootPositions;
725

726
727
  /*! Buffer for the COM position. */
  deque<COMState> m_COMBuffer;
Thomas Moulard's avatar
Thomas Moulard committed
728

729
  /*! @} */
Thomas Moulard's avatar
Thomas Moulard committed
730

731
732
  /*! \brief Reimplement the SimplePlugin interface. */
  virtual void CallMethod(string &MethodName, istringstream &istrm);
Thomas Moulard's avatar
Thomas Moulard committed
733

734
735
736
  /*! \brief Register the methods handled by the SimplePlugin part of
    this object. */
  void RegisterPluginMethods();
Thomas Moulard's avatar
Thomas Moulard committed
737

738
739
  /*! \brief Start FPE trapping. */
  void AllowFPE();
Thomas Moulard's avatar
Thomas Moulard committed
740

741
742
743
744
745
protected:
  /*! \name Internal methods which are not to be exposed.
    They are therefore subject to change.
    @{
  */
746

747
748
749
  /*! \brief Expansion of the buffers handling Center of Masse positions,
    as well as Upper Body Positions. */
  void ExpandCOMPositionsQueues(int aNumber);
Thomas Moulard's avatar
Thomas Moulard committed
750

751
752
753
754
  /*! \brief Compute the COM, left and right foot position for a
    given BodyAngle position */
  void EvaluateStartingCOM(Eigen::VectorXd &Configuration,
                           Eigen::Vector3d &lStartingCOMPosition);
Thomas Moulard's avatar
Thomas Moulard committed
755

756
757
758
759
760
761
762
763
764
765
766
767
768
769
770
771
772
773
774
775
776
  /*! \brief Fill the internal buffer with the appropriate information
    depending on the strategy.
    The behavior of this method depends on \a m_AlgorithmforZMPCOM.
  */
  int CreateZMPReferences(deque<RelativeFootPosition> &lRelativeFootPositions,
                          COMState &lStartingCOMState,
                          Eigen::Vector3d &lStartingZMPPosition,
                          FootAbsolutePosition &InitLeftFootAbsPos,
                          FootAbsolutePosition &InitRightFootAbsPos);

  /*! \brief Create automatically a new step for a ZMP based stability
    criteria */
  void
  AutomaticallyAddFirstStep(deque<RelativeFootPosition> &lRelativeFootPositions,
                            FootAbsolutePosition &InitLeftFootAbsPos,
                            FootAbsolutePosition &InitRightFootAbsPos,
                            COMState &lStartingCOMState);
  /* @} */
};

} // namespace PatternGeneratorJRL
Thomas Moulard's avatar
Thomas Moulard committed
777
778

#endif