include/jrl/walkgen/pgtypes.hh

@@ -28,245 +28,257 @@
   \brief Defines basic types for the Humanoid Walking Pattern Generator.
 */
 #ifndef _PATTERN_GENERATOR_TYPES_H_
-#define  _PATTERN_GENERATOR_TYPES_H_
+#define _PATTERN_GENERATOR_TYPES_H_
 
 // For Windows compatibility.
-#if defined (WIN32)
-#  ifdef jrl_walkgen_EXPORTS
-#    define WALK_GEN_JRL_EXPORT __declspec(dllexport)
-#  else
-#    define WALK_GEN_JRL_EXPORT __declspec(dllimport)
-#  endif
+#if defined(WIN32)
+#ifdef jrl_walkgen_EXPORTS
+#define WALK_GEN_JRL_EXPORT __declspec(dllexport)
 #else
-#  define WALK_GEN_JRL_EXPORT
+#define WALK_GEN_JRL_EXPORT __declspec(dllimport)
 #endif
-#include <iostream>
+#else
+#define WALK_GEN_JRL_EXPORT
+#endif
+#include <Eigen/Dense>
 #include <fstream>
-#include <jrl/mal/matrixabstractlayer.hh>
-
-namespace PatternGeneratorJRL
-{
-  struct COMState_s;
-
-  /// Structure to store the COM position computed by the preview control.
-  struct WALK_GEN_JRL_EXPORT COMPosition_s
-  {
-    double x[3],y[3];
-    double z[3];
-    double yaw; // aka theta
-    double pitch; // aka omega
-    double roll; // aka hip
-
-    struct COMPosition_s & operator=(const COMState_s &aCS);
-
-  };
-
-  inline std::ostream & operator<<(std::ostream & os, const COMPosition_s & aCp)
-  {
-    for(size_t i = 0; i < 3; ++i)
-    {
-      os << "x[" << i << "] " << aCp.x[i] << " y[" << i << "] " << aCp.y[i] << " z[" << i << "] " << aCp.z[i] << std::endl;
-    }
-    os << "yaw " << aCp.yaw << " pitch " << aCp.pitch << " roll " << aCp.roll;
-    return os;
-  }
-
-  typedef struct COMPosition_s COMPosition;
-  typedef struct COMPosition_s WaistState;
-
-  /// Structure to store the COM state computed by the preview control.
-  struct WALK_GEN_JRL_EXPORT COMState_s
-  {
-    double x[3],y[3],z[3];
-    double yaw[3]; // aka theta
-    double pitch[3]; // aka omega
-    double roll[3]; // aka hip
-
-    struct COMState_s & operator=(const COMPosition_s &aCS);
-
-    void reset();
-
-    COMState_s();
-
-    friend std::ostream & operator<<(std::ostream &os, const struct COMState_s & acs);
-  };
-
-
-
-  typedef struct COMState_s COMState;
-
-  /** Structure to store each foot position when the user is specifying
-      a sequence of relative positions. */
-  struct RelativeFootPosition_s
-  {
-    double sx,sy,sz,theta;
-    double SStime;
-    double DStime;
-    int stepType;     //1:normal walking 2:one step before obstacle
-                      //3:first leg over obstacle 4:second leg over obstacle 5:one step after obstacle 6 :stepping stair
-    double DeviationHipHeight;
-  };
-  typedef struct RelativeFootPosition_s RelativeFootPosition;
-
-  inline std::ostream & operator<<(std::ostream & os, const RelativeFootPosition_s & rfp)
-  {
-    os << "sx " << rfp.sx << " sy " << rfp.sy << " theta " << rfp.theta << std::endl;
-    os << "SStime " << rfp.SStime << " DStime " << rfp.DStime << " stepType " << rfp.stepType << " DeviationHipHeight " << rfp.DeviationHipHeight;
-    return os;
-  }
-
-  /** Structure to store each of the ZMP value, with a given
-      direction at a certain time. */
-  struct ZMPPosition_s
-  {
-    double px,py,pz;
-    double theta;//For COM
-    double time;
-    int stepType;     //1:normal walking 2:one step before obstacle
-                      //3:first leg over obstacle 4:second leg over obstacle 5:one step after obstacle
-    //+10 if double support phase
-    //*(-1) if right foot stance else left foot stance
-  };
-  typedef struct ZMPPosition_s ZMPPosition;
-
-  inline std::ostream & operator<<(std::ostream & os, const ZMPPosition_s & zmp)
-  {
-    os << "px " << zmp.px << " py " << zmp.pz << " pz " << zmp.pz << " theta " << zmp.theta << std::endl;
-    os << "time " << zmp.time << " stepType " << zmp.stepType;
-    return os;
+#include <iostream>
+#include <vector>
+
+namespace PatternGeneratorJRL {
+struct COMState_s;
+
+/// Structure to store the COM position computed by the preview control.
+struct WALK_GEN_JRL_EXPORT COMPosition_s {
+  double x[3], y[3];
+  double z[3];
+  double yaw;    // aka theta
+  double pitch;  // aka omega
+  double roll;   // aka hip
+
+  struct COMPosition_s &operator=(const COMState_s &aCS);
+};
+
+inline std::ostream &operator<<(std::ostream &os, const COMPosition_s &aCp) {
+  for (size_t i = 0; i < 3; ++i) {
+    os << "x[" << i << "] " << aCp.x[i] << " y[" << i << "] " << aCp.y[i]
+       << " z[" << i << "] " << aCp.z[i] << std::endl;
   }
+  os << "yaw " << aCp.yaw << " pitch " << aCp.pitch << " roll " << aCp.roll;
+  return os;
+}
 
-  /// Structure to store the absolute foot position.
-  struct FootAbsolutePosition_t
-  {
-    /*! px, py in meters, theta in DEGREES. */
-    double x,y,z, theta, omega, omega2;
-    /*! Speed of the foot. */
-    double dx,dy,dz, dtheta, domega, domega2;
-    /*! Acceleration of the foot. */
-    double ddx,ddy,ddz, ddtheta, ddomega, ddomega2;
-    /*! Jerk of the foot. */
-    double dddx,dddy,dddz, dddtheta, dddomega, dddomega2;
-    /*! Time at which this position should be reached. */
-    double time;
-    /*! 1:normal walking 2:one step before obstacle
-      3:first leg over obstacle 4:second leg over obstacle 5:one step after obstacle
-      +10 if double support phase
-      (-1) if support foot  */
-    int stepType;
-  };
-  typedef struct FootAbsolutePosition_t FootAbsolutePosition;
-
-  inline std::ostream & operator<<(std::ostream & os, const FootAbsolutePosition & fap)
-  {
-    os << "x " << fap.x << " y " << fap.y << " z " << fap.z << " theta " << fap.theta << " omega " << fap.omega << " omega2 " << fap.omega2 << std::endl;
-    os << "dx " << fap.dx << " dy " << fap.dy << " dz " << fap.dz << " dtheta " << fap.dtheta << " domega " << fap.domega << " domega2 " << fap.domega2 << std::endl;
-    os << "ddx " << fap.ddx << " ddy " << fap.ddy << " ddz " << fap.ddz << " ddtheta " << fap.ddtheta << " ddomega " << fap.ddomega << " ddomega2 " << fap.ddomega2 << std::endl;
-    os << "time " << fap.time << " stepType " << fap.stepType;
-    return os;
-  }
+typedef struct COMPosition_s COMPosition;
+typedef struct COMPosition_s WaistState;
+
+/// Structure to store the COM state computed by the preview control.
+struct WALK_GEN_JRL_EXPORT COMState_s {
+  double x[3], y[3], z[3];
+  double yaw[3];    // aka theta
+  double pitch[3];  // aka omega
+  double roll[3];   // aka hip
+
+  struct COMState_s &operator=(const COMPosition_s &aCS);
+
+  void reset();
+
+  COMState_s();
+
+  friend std::ostream &operator<<(std::ostream &os,
+                                  const struct COMState_s &acs);
+};
+
+typedef struct COMState_s COMState;
+
+/** Structure to store each foot position when the user is specifying
+    a sequence of relative positions. */
+struct RelativeFootPosition_s {
+  double sx, sy, sz, theta;
+  double SStime;
+  double DStime;
+  int stepType;  // 1:normal walking 2:one step before obstacle
+                 // 3:first leg over obstacle 4:second leg over obstacle
+  // 5:one step after obstacle 6 :stepping stair
+  double DeviationHipHeight;
+  RelativeFootPosition_s();
+};
+typedef struct RelativeFootPosition_s RelativeFootPosition;
+
+inline std::ostream &operator<<(std::ostream &os,
+                                const RelativeFootPosition_s &rfp) {
+  os << "sx " << rfp.sx << " sy " << rfp.sy << " sz " << rfp.sz << " theta "
+     << rfp.theta << std::endl;
+  os << "SStime " << rfp.SStime << " DStime " << rfp.DStime << " stepType "
+     << rfp.stepType << " DeviationHipHeight " << rfp.DeviationHipHeight;
+  return os;
+}
 
-  /// Structure to store the absolute foot position.
-  struct HandAbsolutePosition_t
-  {
-    /*! x, y, z in meters, theta in DEGREES. */
-    double x,y,z, theta, omega, omega2;
-    /*! Speed of the foot. */
-    double dx,dy,dz, dtheta, domega, domega2;
-    /*! Acceleration of the foot. */
-    double ddx,ddy,ddz, ddtheta, ddomega, ddomega2;
-    /*! Jerk of the hand. */
-    double dddx,dddy,dddz, dddtheta, dddomega, dddomega2;
-    /*! Time at which this position should be reached. */
-    double time;
-    /*! -1 : contact
-     *   1 : no contact
-     */
-    int stepType;
-  };
-  typedef struct HandAbsolutePosition_t HandAbsolutePosition;
+/** Structure to store each of the ZMP value, with a given
+    direction at a certain time. */
+struct ZMPPosition_s {
+  double px, py, pz;
+  double theta;  // For COM
+  double time;
+  int stepType;  // 1:normal walking 2:one step before obstacle
+                 // 3:first leg over obstacle 4:second leg over
+  // obstacle 5:one step after obstacle
+  // +10 if double support phase
+  // *(-1) if right foot stance else left foot stance
+};
+typedef struct ZMPPosition_s ZMPPosition;
+
+inline std::ostream &operator<<(std::ostream &os, const ZMPPosition_s &zmp) {
+  os << "px " << zmp.px << " py " << zmp.pz << " pz " << zmp.pz << " theta "
+     << zmp.theta << std::endl;
+  os << "time " << zmp.time << " stepType " << zmp.stepType;
+  return os;
+}
 
-  inline std::ostream & operator<<(std::ostream & os, const HandAbsolutePosition& hap)
-  {
-    os << "x " << hap.x << " y " << hap.y << " z " << hap.z << " theta " << hap.theta << " omega " << hap.omega << " omega2 " << hap.omega2 << std::endl;
-    os << "dx " << hap.dx << " dy " << hap.dy << " dz " << hap.dz << " dtheta " << hap.dtheta << " domega " << hap.domega << " domega2 " << hap.domega2 << std::endl;
-    os << "ddx " << hap.ddx << " ddy " << hap.ddy << " ddz " << hap.ddz << " ddtheta " << hap.ddtheta << " ddomega " << hap.ddomega << " ddomega2 " << hap.ddomega2 << std::endl;
-    os << "time " << hap.time << " stepType " << hap.stepType;
-    return os;
-  }
+/// Structure to store the absolute foot position.
+struct FootAbsolutePosition_t {
+  /*! px, py in meters, theta in DEGREES. */
+  double x, y, z, theta, omega, omega2;
+  /*! Speed of the foot. */
+  double dx, dy, dz, dtheta, domega, domega2;
+  /*! Acceleration of the foot. */
+  double ddx, ddy, ddz, ddtheta, ddomega, ddomega2;
+  /*! Jerk of the foot. */
+  double dddx, dddy, dddz, dddtheta, dddomega, dddomega2;
+  /*! Time at which this position should be reached. */
+  double time;
+  /*! 1:normal walking 2:one step before obstacle
+    3:first leg over obstacle 4:second leg over obstacle
+    5:one step after  obstacle
+    +10 if double support phase
+    (-1) if support foot  */
+  int stepType;
+};
+typedef struct FootAbsolutePosition_t FootAbsolutePosition;
+
+inline std::ostream &operator<<(std::ostream &os,
+                                const FootAbsolutePosition &fap) {
+  os << "x " << fap.x << " y " << fap.y << " z " << fap.z << " theta "
+     << fap.theta << " omega " << fap.omega << " omega2 " << fap.omega2
+     << std::endl;
+  os << "dx " << fap.dx << " dy " << fap.dy << " dz " << fap.dz << " dtheta "
+     << fap.dtheta << " domega " << fap.domega << " domega2 " << fap.domega2
+     << std::endl;
+  os << "ddx " << fap.ddx << " ddy " << fap.ddy << " ddz " << fap.ddz
+     << " ddtheta " << fap.ddtheta << " ddomega " << fap.ddomega << " ddomega2 "
+     << fap.ddomega2 << std::endl;
+  os << "time " << fap.time << " stepType " << fap.stepType;
+  return os;
+}
 
-  // Linear constraint.
-  struct LinearConstraintInequality_s
-  {
-    MAL_MATRIX(A,double);
-    MAL_MATRIX(B,double);
-    MAL_VECTOR(Center,double);
-    std::vector<int> SimilarConstraints;
-    double StartingTime, EndingTime;
-  };
-  typedef struct LinearConstraintInequality_s
-    LinearConstraintInequality_t;
+/// Structure to store the absolute foot position.
+struct HandAbsolutePosition_t {
+  /*! x, y, z in meters, theta in DEGREES. */
+  double x, y, z, theta, omega, omega2;
+  /*! Speed of the foot. */
+  double dx, dy, dz, dtheta, domega, domega2;
+  /*! Acceleration of the foot. */
+  double ddx, ddy, ddz, ddtheta, ddomega, ddomega2;
+  /*! Jerk of the hand. */
+  double dddx, dddy, dddz, dddtheta, dddomega, dddomega2;
+  /*! Time at which this position should be reached. */
+  double time;
+  /*! -1 : contact
+   *   1 : no contact
+   */
+  int stepType;
+};
+typedef struct HandAbsolutePosition_t HandAbsolutePosition;
+
+inline std::ostream &operator<<(std::ostream &os,
+                                const HandAbsolutePosition &hap) {
+  os << "x " << hap.x << " y " << hap.y << " z " << hap.z << " theta "
+     << hap.theta << " omega " << hap.omega << " omega2 " << hap.omega2
+     << std::endl;
+  os << "dx " << hap.dx << " dy " << hap.dy << " dz " << hap.dz << " dtheta "
+     << hap.dtheta << " domega " << hap.domega << " domega2 " << hap.domega2
+     << std::endl;
+  os << "ddx " << hap.ddx << " ddy " << hap.ddy << " ddz " << hap.ddz
+     << " ddtheta " << hap.ddtheta << " ddomega " << hap.ddomega << " ddomega2 "
+     << hap.ddomega2 << std::endl;
+  os << "time " << hap.time << " stepType " << hap.stepType;
+  return os;
+}
 
-  /// Linear constraints with variable feet placement.
-  struct LinearConstraintInequalityFreeFeet_s
-  {
-    MAL_MATRIX(D,double);
-    MAL_MATRIX(Dc,double);
-    int StepNumber;
-  };
-  typedef struct LinearConstraintInequalityFreeFeet_s
+// Linear constraint.
+struct LinearConstraintInequality_s {
+  Eigen::MatrixXd A;
+  Eigen::MatrixXd B;
+  Eigen::VectorXd Center;
+  std::vector<int> SimilarConstraints;
+  double StartingTime, EndingTime;
+};
+typedef struct LinearConstraintInequality_s LinearConstraintInequality_t;
+
+/// Linear constraints with variable feet placement.
+struct LinearConstraintInequalityFreeFeet_s {
+  Eigen::MatrixXd D;
+  Eigen::MatrixXd Dc;
+  int StepNumber;
+};
+typedef struct LinearConstraintInequalityFreeFeet_s
     LinearConstraintInequalityFreeFeet_t;
 
-  //State of the feet on the ground
-  struct SupportFeet_s
-  {
-    double x,y,theta,StartTime;
-    int SupportFoot;
-  };
-  typedef struct SupportFeet_s
-    SupportFeet_t;
-
-  inline std::ostream & operator<<(std::ostream & os, const SupportFeet_s & sf)
-  {
-    os << "x " << sf.x << " y " << sf.y << " theta " << sf.theta << std::endl;
-    os << " StartTime " << sf.StartTime << " SupportFoot " << sf.SupportFoot;
-    return os;
-  }
-
-  /// Structure to store the absolute reference.
-  struct ReferenceAbsoluteVelocity_t
-  {
-    /*! m/sec or degrees/sec */
-    double x,y,z, dYaw;
-
-    /*! reference values for the whole preview window */
-    MAL_VECTOR(RefVectorX,double);
-    MAL_VECTOR(RefVectorY,double);
-    MAL_VECTOR(RefVectorTheta,double);
-  };
-  typedef struct ReferenceAbsoluteVelocity_t ReferenceAbsoluteVelocity;
-
-  inline std::ostream & operator<<(std::ostream & os, const ReferenceAbsoluteVelocity_t & rav)
-  {
-    os << "x " << rav.x << " y " << rav.y << " z " << rav.z << " dYaw " << rav.dYaw;
-    return os;
-  }
-
-  /// Structure to model a circle (e.g : a stricly convex obstable)
-  struct Circle_t
-  {
-    double x_0 ;
-    double y_0 ;
-    double r ;
-    double margin ;
-  };
-  typedef struct Circle_t Circle ;
+// State of the feet on the ground
+struct SupportFeet_s {
+  double x, y, theta, StartTime;
+  int SupportFoot;
+};
+typedef struct SupportFeet_s SupportFeet_t;
+
+inline std::ostream &operator<<(std::ostream &os, const SupportFeet_s &sf) {
+  os << "x " << sf.x << " y " << sf.y << " theta " << sf.theta << std::endl;
+  os << " StartTime " << sf.StartTime << " SupportFoot " << sf.SupportFoot;
+  return os;
+}
 
-  inline std::ostream & operator<<(std::ostream & os, const Circle_t & circle)
-  {
-    os << "x_0 " << circle.x_0 << " y_0 " << circle.y_0 << " R " << circle.r ;
-    return os;
-  }
+/// Structure to store the absolute reference.
+struct ReferenceAbsoluteVelocity_t {
+  /*! m/sec or degrees/sec */
+  double x, y, z, dYaw;
+
+  /*! reference values for the whole preview window */
+  Eigen::VectorXd RefVectorX;
+  Eigen::VectorXd RefVectorY;
+  Eigen::VectorXd RefVectorTheta;
+};
+typedef struct ReferenceAbsoluteVelocity_t ReferenceAbsoluteVelocity;
+
+inline std::ostream &operator<<(std::ostream &os,
+                                const ReferenceAbsoluteVelocity_t &rav) {
+  os << "x " << rav.x << " y " << rav.y << " z " << rav.z << " dYaw "
+     << rav.dYaw;
+  return os;
+}
 
+/// Structure to model a circle (e.g : a stricly convex obstable)
+struct Circle_t {
+  double x_0;
+  double y_0;
+  double r;
+  double margin;
+};
+typedef struct Circle_t Circle;
+
+inline std::ostream &operator<<(std::ostream &os, const Circle_t &circle) {
+  os << "x_0 " << circle.x_0 << " y_0 " << circle.y_0 << " R " << circle.r;
+  return os;
 }
+
+struct ControlLoopOneStepArgs {
+  Eigen::VectorXd CurrentConfiguration;
+  Eigen::VectorXd CurrentVelocity;
+  Eigen::VectorXd CurrentAcceleration;
+  Eigen::VectorXd ZMPTarget;
+  COMState finalCOMState;
+  FootAbsolutePosition LeftFootPosition;
+  FootAbsolutePosition RightFootPosition;
+  Eigen::VectorXd Momentum;
+};
+
+}  // namespace PatternGeneratorJRL
 #endif

include/jrl/walkgen/pinocchiorobot.hh

+/*
+ * Copyright 2016,
+ *
+ * Maximilien Naveau
+ * Olivier Stasse
+ *
+ * JRL, CNRS/AIST
+ *
+ * This file is part of jrl-walkgen.
+ * jrl-walkgen 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.
+ *
+ * jrl-walkgen 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 jrl-walkgen.  If not, see <http://www.gnu.org/licenses/>.
+ *
+ *  Research carried out within the scope of the
+ *  Joint Japanese-French Robotics Laboratory (JRL)
+ */
+/*! \file PinocchioRobot.hh
+  \brief This object defines a humanoid robot model based on the PinocchioRobot
+frame work */
+
+#ifndef PinocchioRobot_HH
+#define PinocchioRobot_HH
+
+#include "pinocchio/multibody/data.hpp"
+#include "pinocchio/multibody/model.hpp"
+#include "pinocchio/parsers/urdf.hpp"
+#include "pinocchio/spatial/se3.hpp"
+
+namespace PatternGeneratorJRL {
+struct PinocchioRobotFoot_t {
+  pinocchio::JointIndex associatedAnkle;
+  double soleDepth;   // z axis
+  double soleWidth;   // y axis
+  double soleHeight;  // x axis
+  Eigen::Vector3d anklePosition;
+  EIGEN_MAKE_ALIGNED_OPERATOR_NEW
+
+  inline PinocchioRobotFoot_t()
+      : associatedAnkle(0),
+        soleDepth(0.0),
+        soleWidth(0.0),
+        soleHeight(0.0),
+        anklePosition(0.0, 0.0, 0.0) {}
+};
+typedef PinocchioRobotFoot_t PRFoot;
+
+namespace pinocchio_robot {
+const int RPY_SIZE = 6;
+const int QUATERNION_SIZE = 7;
+}  // namespace pinocchio_robot
+
+class PinocchioRobot {
+ public:
+  // overload the new[] eigen operator
+  EIGEN_MAKE_ALIGNED_OPERATOR_NEW
+
+  /// Constructor and Destructor
+  PinocchioRobot();
+  virtual ~PinocchioRobot();
+
+  /// Compute RNEA algorithm
+  /// This is a front end for computeInverseDynamics(q,v,a).
+  void computeInverseDynamics();
+
+  /// Compute RNEA algorithm from
+  /// \param[in] q: \f$q=[r,\theta,\hat{q}]\f$ with \f$r\f$ the position
+  /// of the free floating base (usually the waist), \f$theta\f$ the
+  /// free floating
+  /// orientation in RPY format, $\hat{q}$ the motor angles position.
+  void computeInverseDynamics(Eigen::VectorXd &q, Eigen::VectorXd &v,
+                              Eigen::VectorXd &a);
+
+  /// Compute the geometry of the robot.
+  void computeForwardKinematics();
+
+  void RPYToSpatialFreeFlyer(Eigen::Vector3d &rpy, Eigen::Vector3d &drpy,
+                             Eigen::Vector3d &ddrpy, Eigen::Quaterniond &quat,
+                             Eigen::Vector3d &omega, Eigen::Vector3d &domega);
+
+  /// \brief ComputeSpecializedInverseKinematics :
+  /// compute POSITION (not velocity) of the joints from end effector pose
+  /// This is the implementation of the analitycal inverse kinematic extracted
+  /// from the book of Kajita
+  /// Authors Shuuji Kajita ; Hirohisa Hirukawa ; Kensuke Harada ;
+  /// Kazuhito Yokoi
+  /// ISBN 9782287877162 ; 9782287877155
+  /// It needs a specific distribution of the joint to work.
+  /// a test at the initialization of the class is [should be] done to check
+  /// if everything is correct
+  /// param root joint index, i.e. the waist or the shoulder
+  /// param end joint index, i.e, the wrist or ankle indexes
+  /// param root joint homogenous matrix position,
+  /// param root joint homogenous matrix index,
+  /// param 6D vector output, filled with zeros if the robot is not compatible
+  ///
+  virtual bool ComputeSpecializedInverseKinematics(
+      const pinocchio::JointIndex &jointRoot,
+      const pinocchio::JointIndex &jointEnd,
+      const Eigen::Matrix4d &jointRootPosition,
+      const Eigen::Matrix4d &jointEndPosition, Eigen::VectorXd &q);
+
+  ///
+  /// \brief testArmsInverseKinematics :
+  /// test if the robot arms has the good joint
+  /// configuration to use the analitical inverse geometry
+  /// to be overloaded if the user wants another inverse kinematics
+  /// RY-RX-RZ-RY-RZ-RY
+  /// \return
+  ///
+  virtual bool testArmsInverseKinematics();
+  ///
+  /// \brief testLegsInverseKinematics :
+  /// test if the robot legs has the good joint
+  /// configuration to use the analitical inverse geometry
+  /// to be overloaded if the user wants another inverse kinematics
+  /// Two modes are available:
+  /// FF-RZ-RX-RY-RY-RY-RX
+  /// FF-RX-RZ-RY-RY-RY-RX
+  /// \return
+  ///
+  virtual bool testLegsInverseKinematics();
+  ///
+  /// \brief testOneModefOfLegInverseKinematics :
+  /// test if the robot legs has one good joint
+  /// configuration to use the analitical inverse geometry
+  /// to be overloaded if the user wants another inverse kinematics
+  /// \return
+  ///
+  virtual bool testOneModeOfLegsInverseKinematics(
+      std::vector<std::string> &leftLegJointNames,
+      std::vector<std::string> &rightLegJointNames);
+
+  ///
+  /// \brief initializeInverseKinematics :
+  /// initialize the internal data for the inverse kinematic e.g. the femur
+  /// length
+  /// \return
+  ///
+  virtual void initializeLegsInverseKinematics();
+
+ public:
+  /// tools :
+  std::vector<pinocchio::JointIndex> jointsBetween(
+      pinocchio::JointIndex first, pinocchio::JointIndex second);
+  std::vector<pinocchio::JointIndex> fromRootToIt(pinocchio::JointIndex it);
+
+ private:
+  // needed for the inverse geometry (ComputeSpecializedInverseKinematics)
+  void getWaistFootKinematics(const Eigen::Matrix4d &jointRootPosition,
+                              const Eigen::Matrix4d &jointEndPosition,
+                              Eigen::VectorXd &q, Eigen::Vector3d &Dt) const;
+  double ComputeXmax(double &Z);
+  void getShoulderWristKinematics(const Eigen::Matrix4d &jointRootPosition,
+                                  const Eigen::Matrix4d &jointEndPosition,
+                                  Eigen::VectorXd &q, int side);
+  void DetectAutomaticallyShoulders();
+  void DetectAutomaticallyOneShoulder(pinocchio::JointIndex aWrist,
+                                      pinocchio::JointIndex &aShoulder);
+
+  /*! \brief Computes the size of the free flyer/root robot
+    loads by the urdf.
+    Set the value of m_PinoFreeFlyerSize.
+  */
+  void ComputeRootSize();
+
+ public:
+  /// Getters
+  /// ///////
+  inline pinocchio::Data *Data() { return m_robotData; }
+  inline pinocchio::Data *DataInInitialePose() {
+    return m_robotDataInInitialePose;
+  }
+  inline pinocchio::Model *Model() { return m_robotModel; }
+
+  inline PRFoot *leftFoot() { return &m_leftFoot; }
+  inline PRFoot *rightFoot() { return &m_rightFoot; }
+
+  inline pinocchio::JointIndex leftWrist() { return m_leftWrist; }
+  inline pinocchio::JointIndex rightWrist() { return m_rightWrist; }
+
+  inline pinocchio::JointIndex chest() { return m_chest; }
+  inline pinocchio::JointIndex waist() { return m_waist; }
+
+  inline double mass() { return m_mass; }
+
+  inline pinocchio::JointModelVector &getActuatedJoints() {
+    return m_robotModel->joints;
+  }
+
+  inline Eigen::VectorXd &currentPinoConfiguration() { return m_qpino; }
+  inline Eigen::VectorXd &currentRPYConfiguration() { return m_qrpy; }
+  inline Eigen::VectorXd &currentPinoVelocity() { return m_vpino; }
+  inline Eigen::VectorXd &currentPinoAcceleration() { return m_apino; }
+  inline Eigen::VectorXd &currentRPYVelocity() { return m_vrpy; }
+  inline Eigen::VectorXd &currentRPYAcceleration() { return m_arpy; }
+  inline Eigen::VectorXd &currentTau() { return m_tau; }
+
+  inline unsigned numberDof() { return (unsigned)m_robotModel->nq; }
+
+  inline unsigned numberVelDof() { return (unsigned)m_robotModel->nv; }
+
+  inline void zeroMomentumPoint(Eigen::Vector3d &zmp) {
+    m_externalForces = m_robotData->liMi[1].act(m_robotData->f[1]);
+    m_f = m_externalForces.linear();
+    m_n = m_externalForces.angular();
+    zmp(0) = -m_n(1) / m_f(2);
+    zmp(1) = m_n(0) / m_f(2);
+    zmp(2) = 0.0;  // by default
+  }
+
+  inline void positionCenterOfMass(Eigen::Vector3d &com) {
+    m_com = m_robotData->com[0];
+    com(0) = m_com(0);
+    com(1) = m_com(1);
+    com(2) = m_com(2);
+  }
+  inline void CenterOfMass(Eigen::Vector3d &com, Eigen::Vector3d &dcom,
+                           Eigen::Vector3d &ddcom) {
+    m_com = m_robotData->acom[0];
+    ddcom(0) = m_com(0);
+    ddcom(1) = m_com(1);
+    ddcom(2) = m_com(2);
+
+    m_com = m_robotData->vcom[0];
+    dcom(0) = m_com(0);
+    dcom(1) = m_com(1);
+    dcom(2) = m_com(2);
+
+    m_com = m_robotData->com[0];
+    com(0) = m_com(0);
+    com(1) = m_com(1);
+    com(2) = m_com(2);
+  }
+
+  /// SETTERS
+  /// ///////
+  void currentPinoConfiguration(Eigen::VectorXd &conf);
+  void currentRPYConfiguration(Eigen::VectorXd &);
+  inline void currentPinoVelocity(Eigen::VectorXd &vel) { m_vpino = vel; }
+  inline void currentPinoAcceleration(Eigen::VectorXd &acc) { m_apino = acc; }
+  inline void currentRPYVelocity(Eigen::VectorXd &vel) { m_vrpy = vel; }
+  inline void currentRPYAcceleration(Eigen::VectorXd &acc) { m_arpy = acc; }
+
+  inline pinocchio::JointIndex getFreeFlyerSize() const {
+    return m_PinoFreeFlyerSize;
+  }
+
+  inline pinocchio::JointIndex getFreeFlyerVelSize() const {
+    return m_PinoFreeFlyerVelSize;
+  }
+
+  /// Initialization functions
+  /// ////////////////////////
+  inline bool isInitialized() {
+    return m_boolModel && m_boolData && m_boolLeftFoot && m_boolRightFoot;
+  }
+  bool checkModel(pinocchio::Model *robotModel);
+  bool initializeRobotModelAndData(pinocchio::Model *robotModel,
+                                   pinocchio::Data *robotData);
+  bool initializeLeftFoot(PRFoot leftFoot);
+  bool initializeRightFoot(PRFoot rightFoot);
+
+  const std::string &getName() const;
+  /// Attributes
+  /// //////////
+ private:
+  pinocchio::Model *m_robotModel;
+  pinocchio::Data *m_robotDataInInitialePose;  // internal variable
+  pinocchio::Data *m_robotData;
+  PRFoot m_leftFoot, m_rightFoot;
+  double m_mass;
+  pinocchio::JointIndex m_chest, m_waist, m_leftShoulder, m_rightShoulder;
+  pinocchio::JointIndex m_leftWrist, m_rightWrist;
+
+  /// Fields member to store  configurations, velocity and acceleration
+  /// @{
+  /// Configuration SE(3) position + quaternion + NbDofs
+  Eigen::VectorXd m_qpino;
+  /// Configuration SE(3) position + RPY + NbDofs
+  Eigen::VectorXd m_qrpy;
+  /// Velocity se(3) + NbDofs
+  Eigen::VectorXd m_vpino;
+  /// Velocity se(3) + NbDofs
+  Eigen::VectorXd m_vrpy;
+  /// Acceleration acc + NbDofs
+  Eigen::VectorXd m_apino;
+  Eigen::VectorXd m_arpy;
+  // @}
+
+  // tmp variables
+  Eigen::Quaterniond m_quat;
+  Eigen::Matrix3d m_rot;
+  pinocchio::Force m_externalForces;  // external forces and torques
+  Eigen::VectorXd m_tau;              // external forces and torques
+  Eigen::Vector3d m_f, m_n;           // external forces and torques
+  Eigen::Vector3d m_com;              // multibody CoM
+  Eigen::Matrix3d m_S;
+  Eigen::Vector3d m_rpy, m_drpy, m_ddrpy, m_omega, m_domega;
+
+  // Variables extracted form the urdf used for the analitycal inverse
+  // kinematic
+  bool m_isLegInverseKinematic;
+  unsigned int m_modeLegInverseKinematic;
+  bool m_isArmInverseKinematic;
+
+  // length between the waist and the hip
+  Eigen::Vector3d m_leftDt, m_rightDt;
+  double m_femurLength;
+  double m_tibiaLengthZ;
+  double m_tibiaLengthY;
+
+  bool m_boolModel;
+  bool m_boolData;
+  bool m_boolLeftFoot;
+  bool m_boolRightFoot;
+
+  /// \brief Size of the free flyer configuration space.
+  pinocchio::JointIndex m_PinoFreeFlyerSize;
+
+  /// \brief Size of the free flyer velocity space.
+  pinocchio::JointIndex m_PinoFreeFlyerVelSize;
+
+};  // PinocchioRobot
+}  // namespace PatternGeneratorJRL
+#endif  // PinocchioRobot_HH

package.xml

+<package format="2">
+  <name>jrl-walkgen</name>
+  <version>4.2.8</version>
+  <description>jrl-walkgen library</description>
+  <maintainer email="guilhem.saurel@laas.fr">Guilhem Saurel</maintainer>
+  <license>BSD</license>
+
+  <url>http://github.com/stack-of-tasks/sot-talos</url>
+  <author>Olivier Stasse</author>
+
+  <build_depend>roscpp</build_depend>
+
+  <exec_depend>roscpp</exec_depend>
+
+  <buildtool_depend>catkin</buildtool_depend>
+
+  <doc_depend>doxygen</doc_depend>
+
+</package>

private_cmake/FindqpOASES.cmake

-#.rst:
-# FindqpOASES
-# -----------
-#
-# Try to find the qpOASES library.
-# Once done this will define the following variables::
-#
-#  qpOASES_FOUND         - System has qpOASES
-#  qpOASES_INCLUDE_DIRS  - qpOASES include directory
-#  qpOASES_LIBRARIES     - qpOASES libraries
-#
-# qpOASES does not have an "install" step, and the includes are in the source
-# tree, while the libraries are in the build tree.
-# Therefore the environment and cmake variables `qpOASES_SOURCE_DIR` and
-# `qpOASES_BINARY_DIR` will be used to locate the includes and libraries.
-
-#=============================================================================
-# Copyright 2014 iCub Facility, Istituto Italiano di Tecnologia
-#   Authors: Daniele E. Domenichelli <daniele.domenichelli@iit.it>
-#
-# Distributed under the OSI-approved BSD License (the "License");
-# see accompanying file Copyright.txt for details.
-#
-# This software is distributed WITHOUT ANY WARRANTY; without even the
-# implied warranty of MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.
-# See the License for more information.
-#=============================================================================
-# (To distribute this file outside of YCM, substitute the full
-#  License text for the above reference.)
-
-
-include(FindPackageHandleStandardArgs)
-
-find_path(qpOASES_INCLUDEDIR
-          NAMES qpOASES.hpp
-          HINTS "${qpOASES_SOURCE_DIR}"
-                ENV qpOASES_SOURCE_DIR
-          PATH_SUFFIXES include)
-find_library(qpOASES_LIB
-             NAMES qpOASES
-             HINTS "${qpOASES_BINARY_DIR}"
-                   ENV qpOASES_BINARY_DIR
-             PATH_SUFFIXES lib
-                           libs)
-
-set(qpOASES_INCLUDE_DIRS ${qpOASES_INCLUDEDIR})
-set(qpOASES_LIBRARIES ${qpOASES_LIB})
-
-find_package_handle_standard_args(qpOASES DEFAULT_MSG qpOASES_LIBRARIES
-                                                      qpOASES_INCLUDE_DIRS)
-set(qpOASES_FOUND ${QPOASES_FOUND})

private_cmake/cmake_metapod_configure.cmake

-# Copyright 2012,
-#
-# Sébastien Barthélémy (Aldebaran Robotics)
-#
-# This file is part of metapod.
-# metapod 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.
-#
-# metapod 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 metapod.  If not, see <http://www.gnu.org/licenses/>.
-# Copyright 2010, 2012, 2013
-
-
-# GENERATE_CONFIG_HEADER
-#
-# Generates a configuration header for DLL API import/export
-#
-# OUTPUT: path (including filename) of the generated file. Usually the filename
-#         is config.hh
-# LIBRARY_NAME: the name of the library. It will be normalized.
-FUNCTION(GENERATE_CONFIG_HEADER OUTPUT LIBRARY_NAME)
-  STRING(REGEX REPLACE "[^a-zA-Z0-9]" "_" LIBRARY_NAME "${LIBRARY_NAME}")
-  STRING(TOLOWER "${LIBRARY_NAME}" "LIBRARY_NAME_LOWER")
-  SET(EXPORT_SYMBOL "${LIBRARY_NAME_LOWER}_EXPORTS")
-  STRING(TOUPPER "${LIBRARY_NAME}" "LIBRARY_NAME")
-  # create the directory (and its parents)
-  GET_FILENAME_COMPONENT(OUTPUT_DIR "${OUTPUT}" PATH)
-  FILE(MAKE_DIRECTORY "${OUTPUT_DIR}")
-  # Generate the header. The following variables are used in the template
-  # LIBRARY_NAME: CPP symbol prefix, should match the compiled library name,
-  #               usually in upper case
-  # EXPORT_SYMBOL: what symbol controls the switch between symbol
-  #                import/export, usually libname_EXPORTS, with libname in
-  #                lower case.
-  # PROJECT_VERSION: the project version
-  CONFIGURE_FILE(
-    ${PROJECT_SOURCE_DIR}/cmake/config.hh.cmake
-    ${OUTPUT}
-    @ONLY)
-ENDFUNCTION(GENERATE_CONFIG_HEADER)
-
-FUNCTION(FIND_GENERATOR GENERATOR_NAME)
-  SET(WITH_${GENERATOR_NAME} FALSE)
-  # maybe the user passed the location
-  IF(${GENERATOR_NAME}_EXECUTABLE)
-    SET(WITH_${GENERATOR_NAME} TRUE)
-  ELSE()
-    # last resort: search for it
-    FIND_PACKAGE(${GENERATOR_NAME} QUIET)
-    SET(WITH_${GENERATOR_NAME} ${${GENERATOR_NAME}_FOUND})
-  ENDIF()
-  SET(WITH_${GENERATOR_NAME} ${WITH_${GENERATOR_NAME}} PARENT_SCOPE)
-ENDFUNCTION()
-
-# ADD_SAMPLEURDFMODEL
-#
-# Call metapodfromurdf to create one of the sample models
-#
-# NAME: the name of the model. Either simple_arm or simple_humanoid.
-FUNCTION(ADD_SAMPLEURDFMODEL name)
-  IF(NOT WITH_METAPODFROMURDF)
-    ERROR("Could not find metapodfromurdf")
-  ENDIF()
-  SET(_libname "metapod_${name}")
-  SET(_data_path    "$ENV{ROS_WORKSPACE}/install/share/metapod/data/${name}")
-  SET(_urdf_file    "${_data_path}/${name}.urdf")
-  SET(_config_file  "${_data_path}/${name}.config")
-  SET(_license_file "${_data_path}/${name}_license_file.txt")
-  SET(_model_dir    "$ENV{ROS_WORKSPACE}/install/include/metapod/models/${name}")
-  SET(METAPODFROMURDF_EXECUTABLE "$ENV{ROS_WORKSPACE}/install/bin/metapodfromurdf")
-
-  INCLUDE_DIRECTORIES("$ENV{ROS_WORKSPACE}/install")
-  INCLUDE_DIRECTORIES("$ENV{ROS_WORKSPACE}/install/include")
-  SET(_sources
-    ${_model_dir}/config.hh
-    ${_model_dir}/${name}.hh
-    ${_model_dir}/${name}.cc
-  )
-  message (STATUS ${METAPODFROMURDF_EXECUTABLE} " --name " ${name} " --libname " ${_libname} " --directory " ${_model_dir} " --config-file " ${_config_file} " --license-file " ${_license_file} " " ${_urdf_file})
-  ADD_CUSTOM_COMMAND(PRE_BUILD
-    OUTPUT ${_sources}
-    COMMAND ${METAPODFROMURDF_EXECUTABLE}
-    --name ${name}
-    --libname ${_libname}
-    --directory ${_model_dir}
-    --config-file ${_config_file}
-    --license-file ${_license_file}
-    ${_urdf_file}
-    DEPENDS ${METAPODFROMURDF_EXECUTABLE} ${_urdf_file} ${_config_file}
-    ${_license_file}
-    MAIN_DEPENDENCY ${_urdf_file}
-  )
-  ADD_LIBRARY(${_libname} SHARED ${_sources})
-  
-  INSTALL(TARGETS ${_libname} DESTINATION ${CMAKE_INSTALL_LIBDIR})
-  
-ENDFUNCTION()

private_cmake/config_private.hh.cmake

-// Copyright (C) 2008-2015 LAAS-CNRS, JRL AIST-CNRS.
-//
-// This file is part of jrl-walkgen.
-// jrl-walkgen 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.
-//
-// jrl-walkgen 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 jrl-walkgen.  If not, see <http://www.gnu.org/licenses/>.
-
-#ifndef JRL_WALKGEN_CONFIG_PRIVATE_HH
-# define JRL_WALKGEN_CONFIG__PRIVATE_HH
-
-// Package version (header).
-# define JRL_WALKGEN_CONFIG_VERSION "@PROJECT_VERSION@"
-
-#if @WITH_HRP2_DYNAMICS_BOOL@ //! WITH_HRP2_DYNAMICS?
-# define WITH_HRP2DYNAMICS
-#endif //! WITH_HRP2_DYNAMICS?
-#endif //! JRL_WALKGEN_CONFIG_PRIVATE_HH

src/CMakeLists.txt

-# Copyright 2010, Olivier Stasse, JRL, CNRS/AIST
-#
-# This file is part of jrl-walkgen.
-# jrl-walkgen 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.
-#
-# jrl-walkgen 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
-# jrl-walkgen. If not, see <http://www.gnu.org/licenses/>.
-
-# Make sure to find Debug.h
-INCLUDE_DIRECTORIES(BEFORE ${PROJECT_SOURCE_DIR}/src)
-INCLUDE_DIRECTORIES(BEFORE ${HOME}/devel/metapod/_bin/include/)
-INCLUDE_DIRECTORIES(BEFORE ${HOME}/devel/metapod/_bin/include/metapod/)
-
-# Add Boost path to include directories.
-INCLUDE_DIRECTORIES(${Boost_INCLUDE_DIRS})
-message (STATUS "-- Boost Dir : " ${Boost_INCLUDE_DIRS} )
-
-# add flag to compile qld.cc
-IF(WIN32)
-ADD_DEFINITIONS("/D __STDC__")
-ENDIF(WIN32)
-
-IF(USE_LSSOL)
-ADD_DEFINITIONS("/DLSSOL_FOUND")
-ENDIF(USE_LSSOL)
-
-SET(INCLUDES
-  PreviewControl/rigid-body.hh
-  PreviewControl/OptimalControllerSolver.hh
-  PreviewControl/rigid-body-system.hh
-  PreviewControl/ZMPPreviewControlWithMultiBodyZMP.hh
-  PreviewControl/SupportFSM.hh
-  PreviewControl/LinearizedInvertedPendulum2D.hh
-  PreviewControl/PreviewControl.hh
-  PreviewControl/SupportFSM_backup.hh
-  FootTrajectoryGeneration/FootTrajectoryGenerationMultiple.hh
-  FootTrajectoryGeneration/FootTrajectoryGenerationAbstract.hh
-  FootTrajectoryGeneration/FootTrajectoryGenerationStandard.hh
-  FootTrajectoryGeneration/LeftAndRightFootTrajectoryGenerationMultiple.hh
-  Debug.hh
-  SimplePluginManager.hh
-  privatepgtypes.hh
-#  MultiContactRefTrajectoryGeneration/MultiContactHirukawa.hh
-  patterngeneratorinterfaceprivate.hh
-  Mathematics/FootConstraintsAsLinearSystem.hh
-  Mathematics/Polynome.hh
-  Mathematics/ConvexHull.hh
-  Mathematics/Bsplines.hh
-  Mathematics/StepOverPolynome.hh
-  Mathematics/AnalyticalZMPCOGTrajectory.hh
-  Mathematics/qld.hh
-  Mathematics/PLDPSolver.hh
-  Mathematics/FootHalfSize.hh
-  Mathematics/relative-feet-inequalities.hh
-  Mathematics/intermediate-qp-matrices.hh
-  Mathematics/PolynomeFoot.hh
-  Mathematics/PLDPSolverHerdt.hh
-  Mathematics/OptCholesky.hh
-  StepStackHandler.hh
-  configJRLWPG.hh
-  Clock.hh
-  GlobalStrategyManagers/CoMAndFootOnlyStrategy.hh
-  GlobalStrategyManagers/GlobalStrategyManager.hh
-  GlobalStrategyManagers/DoubleStagePreviewControlStrategy.hh
-  ZMPRefTrajectoryGeneration/AnalyticalMorisawaAbstract.hh
-  ZMPRefTrajectoryGeneration/DynamicFilter.hh
-  ZMPRefTrajectoryGeneration/ZMPConstrainedQPFastFormulation.hh
-  ZMPRefTrajectoryGeneration/qp-problem.hh
-  ZMPRefTrajectoryGeneration/ZMPDiscretization.hh
-  ZMPRefTrajectoryGeneration/OrientationsPreview.hh
-  ZMPRefTrajectoryGeneration/mpc-trajectory-generation.hh
-  ZMPRefTrajectoryGeneration/ZMPRefTrajectoryGeneration.hh
-  ZMPRefTrajectoryGeneration/ZMPVelocityReferencedQP.hh
-  ZMPRefTrajectoryGeneration/AnalyticalMorisawaCompact.hh
-  ZMPRefTrajectoryGeneration/generator-vel-ref.hh
-  ZMPRefTrajectoryGeneration/nmpc_generator.hh
-  ZMPRefTrajectoryGeneration/FilteringAnalyticalTrajectoryByPreviewControl.hh
-  ZMPRefTrajectoryGeneration/problem-vel-ref.hh
-  ZMPRefTrajectoryGeneration/ZMPQPWithConstraint.hh
-  SimplePlugin.hh
-  portability/gettimeofday.hh
-  portability/bzero.hh
-  MotionGeneration/ComAndFootRealizationByGeometry.hh
-  MotionGeneration/StepOverPlanner.hh
-  MotionGeneration/WaistHeightVariation.hh
-  MotionGeneration/ComAndFootRealization.hh
-  MotionGeneration/GenerateMotionFromKineoWorks.hh
-  MotionGeneration/UpperBodyMotion.hh
-  MotionGeneration/CollisionDetector.hh
-  ../tests/CommonTools.hh
-  ../tests/ClockCPUTime.hh
-  ../tests/TestObject.hh
-  ../doc/additionalHeader/modules.hh
-  ../include/jrl/walkgen/pgtypes.hh
-  ../include/jrl/walkgen/patterngeneratorinterface.hh
-)
-
-IF(${qpOASES_FOUND} STREQUAL "TRUE")
-SET(INCLUDES
-  ${INCLUDES}
-  ZMPRefTrajectoryGeneration/ZMPVelocityReferencedSQP.hh
-  ZMPRefTrajectoryGeneration/nmpc_generator.hh
-)
-ENDIF(${qpOASES_FOUND} STREQUAL "TRUE")
-
-SET(SOURCES
-  ${INCLUDES}
-  FootTrajectoryGeneration/FootTrajectoryGenerationAbstract.cpp
-  FootTrajectoryGeneration/FootTrajectoryGenerationStandard.cpp
-  FootTrajectoryGeneration/FootTrajectoryGenerationMultiple.cpp
-  FootTrajectoryGeneration/LeftAndRightFootTrajectoryGenerationMultiple.cpp
-  FootTrajectoryGeneration/OnLineFootTrajectoryGeneration.cpp
-  GlobalStrategyManagers/CoMAndFootOnlyStrategy.cpp
-  GlobalStrategyManagers/GlobalStrategyManager.cpp
-  GlobalStrategyManagers/DoubleStagePreviewControlStrategy.cpp
-  Mathematics/AnalyticalZMPCOGTrajectory.cpp
-  Mathematics/ConvexHull.cpp
-  Mathematics/FootConstraintsAsLinearSystem.cpp
-#  Mathematics/FootConstraintsAsLinearSystemForVelRef.cpp
-  Mathematics/FootHalfSize.cpp
-  Mathematics/OptCholesky.cpp
-  Mathematics/Bsplines.cpp
-  Mathematics/Polynome.cpp
-  Mathematics/PolynomeFoot.cpp
-  Mathematics/PLDPSolver.cpp
-  Mathematics/qld.cpp
-  Mathematics/StepOverPolynome.cpp
-  Mathematics/relative-feet-inequalities.cpp
-  Mathematics/intermediate-qp-matrices.cpp
-  PreviewControl/PreviewControl.cpp
-  PreviewControl/OptimalControllerSolver.cpp
-  PreviewControl/ZMPPreviewControlWithMultiBodyZMP.cpp
-  PreviewControl/LinearizedInvertedPendulum2D.cpp
-  PreviewControl/rigid-body.cpp
-  PreviewControl/rigid-body-system.cpp
-  PreviewControl/SupportFSM.cpp
-  ZMPRefTrajectoryGeneration/ZMPRefTrajectoryGeneration.cpp
-  ZMPRefTrajectoryGeneration/ZMPDiscretization.cpp
-  ZMPRefTrajectoryGeneration/ZMPQPWithConstraint.cpp
-  ZMPRefTrajectoryGeneration/ZMPConstrainedQPFastFormulation.cpp
-  ZMPRefTrajectoryGeneration/ZMPVelocityReferencedQP.cpp
-  ZMPRefTrajectoryGeneration/OrientationsPreview.cpp
-  ZMPRefTrajectoryGeneration/AnalyticalMorisawaAbstract.cpp
-  ZMPRefTrajectoryGeneration/AnalyticalMorisawaCompact.cpp
-  ZMPRefTrajectoryGeneration/FilteringAnalyticalTrajectoryByPreviewControl.cpp
-  ZMPRefTrajectoryGeneration/problem-vel-ref.cpp
-  ZMPRefTrajectoryGeneration/qp-problem.cpp
-  ZMPRefTrajectoryGeneration/generator-vel-ref.cpp
-  ZMPRefTrajectoryGeneration/mpc-trajectory-generation.cpp
-  ZMPRefTrajectoryGeneration/DynamicFilter.cpp
-#  MultiContactRefTrajectoryGeneration/MultiContactHirukawa.cc
-  MotionGeneration/StepOverPlanner.cpp
-  MotionGeneration/CollisionDetector.cpp
-  MotionGeneration/WaistHeightVariation.cpp
-  MotionGeneration/UpperBodyMotion.cpp
-  MotionGeneration/GenerateMotionFromKineoWorks.cpp
-  MotionGeneration/ComAndFootRealizationByGeometry.cpp
-  StepStackHandler.cpp
-  PatternGeneratorInterfacePrivate.cpp
-  SimplePlugin.cpp
-  SimplePluginManager.cpp
-  pgtypes.cpp
-  Clock.cpp
-  portability/gettimeofday.cc
-  privatepgtypes.cpp
-)
-
-IF(${qpOASES_FOUND} STREQUAL "TRUE")
-SET(SOURCES
-  ${SOURCES}
-  ZMPRefTrajectoryGeneration/ZMPVelocityReferencedSQP.cpp
-  ZMPRefTrajectoryGeneration/nmpc_generator.cpp
-)
-ENDIF(${qpOASES_FOUND} STREQUAL "TRUE")
-
-
-# prefix and suffix each element of list by ${prefix}elemnt${suffix}
-macro(ADDPREFIX newlist prefix list_name)
-    # create empty list - necessary?
-    SET(${newlist})
-
-     # prefix and suffix elements
-    foreach(l ${${list_name}})
-      list(APPEND ${newlist} ${prefix}${l} )
-    endforeach()
-
-endmacro(ADDPREFIX)
-
-IF ( '${CMAKE_EXTRA_GENERATOR}' STREQUAL 'CodeBlocks' )
-    ADDPREFIX(${PROJECT_NAME}_ABSOLUTE_HEADERS  "${CMAKE_SOURCE_DIR}/"  ${PROJECT_NAME}_HEADERS)
-ENDIF ( '${CMAKE_EXTRA_GENERATOR}' STREQUAL 'CodeBlocks' )
-
-# define and search for metapod library
-SET(WITH_METAPODFROMURDF TRUE)
-SET(model_name "hrp2_14")
-# Define the simple humanoid model for the metapod algorithm
-ADD_SAMPLEURDFMODEL(${model_name})
-
-ADD_LIBRARY(jrl-walkgen SHARED ${SOURCES} ${${PROJECT_NAME}_ABSOLUTE_HEADERS})
-SET_TARGET_PROPERTIES(jrl-walkgen PROPERTIES SOVERSION ${PROJECT_VERSION})
-# Define dependencies
-
-TARGET_LINK_LIBRARIES(jrl-walkgen metapod_${model_name} ${Boost_LIBRARIES})
-SET_TARGET_PROPERTIES(jrl-walkgen PROPERTIES COMPILE_FLAGS "-msse -msse2 -msse3 -march=core2 -mfpmath=sse -fivopts -ftree-loop-im -fipa-pta ")
-
-INSTALL(TARGETS jrl-walkgen DESTINATION ${CMAKE_INSTALL_LIBDIR})
-
-PKG_CONFIG_USE_DEPENDENCY(jrl-walkgen abstract-robot-dynamics)
-PKG_CONFIG_USE_DEPENDENCY(jrl-walkgen jrl-mal)
-PKG_CONFIG_USE_DEPENDENCY(jrl-walkgen jrl-dynamics)
-#PKG_CONFIG_USE_DEPENDENCY(jrl-walkgen pinocchio)
-
-IF(USE_LSSOL)
- PKG_CONFIG_USE_DEPENDENCY(jrl-walkgen lssol)
-ENDIF(USE_LSSOL)
-
-IF(${qpOASES_FOUND} STREQUAL "TRUE")
-  TARGET_LINK_LIBRARIES(jrl-walkgen ${qpOASES_LIBRARIES})
-ENDIF(${qpOASES_FOUND} STREQUAL "TRUE")

src/Clock.cpp

 /*
- * Copyright 2005, 2006, 2007, 2008, 2009, 2010, 
+ * Copyright 2005, 2006, 2007, 2008, 2009, 2010,
  *
  * Olivier Stasse
  *
@@ -18,113 +18,90 @@
  * 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 
+ *  Research carried out within the scope of the
  *  Joint Japanese-French Robotics Laboratory (JRL)
  */
 /*! \file Clock.cpp
   \brief This object allows to make time measurement on the code.
 
-   Copyright (c) 2008, 
-   Olivier Stasse,
-   
-   JRL-Japan, CNRS/AIST
+  Copyright (c) 2008,
+  Olivier Stasse,
 
-   All rights reserved.
+  JRL-Japan, CNRS/AIST
 
-   Please see license.txt for more information on license.
+  All rights reserved.
+
+  Please see license.txt for more information on license.
 */
 
-#include <iostream>
-#include <fstream>
 #include <Clock.hh>
+#include <fstream>
+#include <iostream>
 
 using namespace PatternGeneratorJRL;
 
-Clock::Clock()
-{
+Clock::Clock() {
   Reset();
   m_DataBuffer.resize(300000);
 
   struct timeval startingtime;
-  gettimeofday(&startingtime,0);
-  m_StartingTime = startingtime.tv_sec + 0.000001 * startingtime.tv_usec;
+  gettimeofday(&startingtime, 0);
+  m_StartingTime =
+      (double)startingtime.tv_sec + 0.000001 * (double)startingtime.tv_usec;
 }
 
+Clock::~Clock() {}
 
-Clock::~Clock()
-{
-}
-
-void Clock::Reset()
-{
+void Clock::Reset() {
   m_NbOfIterations = 0;
   m_MaximumTime = 0.0;
-  m_TotalTime=0.0;
+  m_TotalTime = 0.0;
 
   struct timeval startingtime;
-  gettimeofday(&startingtime,0);
-  m_StartingTime = startingtime.tv_sec + 0.000001 * startingtime.tv_usec;
+  gettimeofday(&startingtime, 0);
+  m_StartingTime =
+      (double)startingtime.tv_sec + 0.000001 * (double)startingtime.tv_usec;
 }
 
-void Clock::StartTiming()
-{
-  gettimeofday(&m_BeginTimeStamp,0);
+void Clock::StartTiming() { gettimeofday(&m_BeginTimeStamp, 0); }
 
-}
-
-void Clock::StopTiming()
-{
-  gettimeofday(&m_EndTimeStamp,0);
-  double ltime = m_EndTimeStamp.tv_sec - m_BeginTimeStamp.tv_sec +
-    0.000001 * (m_EndTimeStamp.tv_usec - m_BeginTimeStamp.tv_usec);
+void Clock::StopTiming() {
+  gettimeofday(&m_EndTimeStamp, 0);
+  double ltime =
+      (double)m_EndTimeStamp.tv_sec - (double)m_BeginTimeStamp.tv_sec +
+      0.000001 * (double)(m_EndTimeStamp.tv_usec - m_BeginTimeStamp.tv_usec);
 
   m_MaximumTime = m_MaximumTime < ltime ? ltime : m_MaximumTime;
   m_TotalTime += ltime;
 
-  m_DataBuffer[(m_NbOfIterations*2)%3000000]=m_BeginTimeStamp.tv_sec +
-    0.000001 * m_BeginTimeStamp.tv_usec - m_StartingTime;
-  m_DataBuffer[(m_NbOfIterations*2+1)%3000000]=ltime;
+  m_DataBuffer[(m_NbOfIterations * 2) % 3000000] =
+      (double)m_BeginTimeStamp.tv_sec +
+      0.000001 * (double)m_BeginTimeStamp.tv_usec - m_StartingTime;
+  m_DataBuffer[(m_NbOfIterations * 2 + 1) % 3000000] = ltime;
 }
 
-void Clock::IncIteration(int lNbOfIts)
-{
-  m_NbOfIterations += lNbOfIts;
-}
+void Clock::IncIteration(int lNbOfIts) { m_NbOfIterations += lNbOfIts; }
 
-void Clock::RecordDataBuffer(std::string filename)
-{
+void Clock::RecordDataBuffer(std::string filename) {
   std::ofstream aof(filename.c_str());
-  for(unsigned int i=0;i<2*m_NbOfIterations%300000;i+=2)
-    aof << m_DataBuffer[i]<< " " << m_DataBuffer[i+1] << std::endl;
+  for (unsigned int i = 0; i < 2 * m_NbOfIterations % 300000; i += 2)
+    aof << m_DataBuffer[i] << " " << m_DataBuffer[i + 1] << std::endl;
   aof.close();
 }
-unsigned long int Clock::NbOfIterations()
-{
-  return m_NbOfIterations;
-}
+unsigned long int Clock::NbOfIterations() { return m_NbOfIterations; }
 
-double Clock::MaxTime()
-{
-  return m_MaximumTime;
-}
+double Clock::MaxTime() { return m_MaximumTime; }
 
-double Clock::TotalTime()
-{
-  return m_TotalTime;
-}
+double Clock::TotalTime() { return m_TotalTime; }
 
-double Clock::AverageTime()
-{
-  if (m_NbOfIterations!=0)
-    return m_TotalTime/(double)m_NbOfIterations;
+double Clock::AverageTime() {
+  if (m_NbOfIterations != 0) return m_TotalTime / (double)m_NbOfIterations;
   return 0.0;
 }
 
-void Clock::Display()
-{
+void Clock::Display() {
   std::cout << "Average Time : " << AverageTime() << std::endl;
   std::cout << "Total Time : " << TotalTime() << std::endl;
   std::cout << "Max Time : " << MaxTime() << std::endl;
   std::cout << "Nb of iterations: " << NbOfIterations() << std::endl;
-  
 }

src/Clock.hh

@@ -24,93 +24,89 @@
  */
 
 /*! \file Clock.h
-    \brief Defines an object for measuring the time spend in some code part.
+  \brief Defines an object for measuring the time spend in some code part.
 */
 #ifndef _HWPG_CLOCK_H_
-# define _HWPG_CLOCK_H_
-# include <time.h>
-# include <vector>
-# include <string>
-
-# include "portability/gettimeofday.hh"
-
-# ifdef WIN32
-#  include <Windows.h>
-# endif
-
-
-namespace PatternGeneratorJRL
-{
-  /*! \brief Measure time spend in some code.
-    The object measure the time between StartTiming() and StopTiming() slots.
-    The number of iteration is incremented by IncIteration().
-    MaxTime() and AverageTime() returns the maximum time spend in one iteration,
-    and the average time spends in one iteration respectively.
-    TotalTime() returns the time spend in total in the code measured.
-    The precision is expected to be in micro-second but is OS dependent.
-  */
-  class  Clock
-  {
-  public:
-
-    /*! \brief Default constructor */
-    Clock();
-
-    /*! \brief Default destructor */
-    ~Clock();
+#define _HWPG_CLOCK_H_
+#include <time.h>
+
+#include <string>
+#include <vector>
+
+#include "portability/gettimeofday.hh"
+
+#ifdef WIN32
+#include <Windows.h>
+#endif
+
+namespace PatternGeneratorJRL {
+/*! \brief Measure time spend in some code.
+  The object measure the time between StartTiming() and StopTiming() slots.
+  The number of iteration is incremented by IncIteration().
+  MaxTime() and AverageTime() returns the maximum time spend in one iteration,
+  and the average time spends in one iteration respectively.
+  TotalTime() returns the time spend in total in the code measured.
+  The precision is expected to be in micro-second but is OS dependent.
+*/
+class Clock {
+ public:
+  /*! \brief Default constructor */
+  Clock();
 
-    /*! \brief Start Timing. */
-    void StartTiming();
+  /*! \brief Default destructor */
+  ~Clock();
 
-    /*! \brief End Timing. */
-    void StopTiming();
+  /*! \brief Start Timing. */
+  void StartTiming();
 
-    /*! \brief Increment Iteration by specifying the number of iterations*/
-    void IncIteration(int lNbOfIts=1);
+  /*! \brief End Timing. */
+  void StopTiming();
 
-    /*! \brief Returns number of iteration. */
-    unsigned long int NbOfIterations();
+  /*! \brief Increment Iteration by specifying the number of iterations*/
+  void IncIteration(int lNbOfIts = 1);
 
-    /*! \brief Returns maximum time interval measured
-      between two increment of iteration. */
-    double MaxTime();
+  /*! \brief Returns number of iteration. */
+  unsigned long int NbOfIterations();
 
-    /*! \brief Returns average time interval measured
-      between two increment of iteration. */
-    double AverageTime();
+  /*! \brief Returns maximum time interval measured
+    between two increment of iteration. */
+  double MaxTime();
 
-    /*! \brief Returns the total time measured. */
-    double TotalTime();
+  /*! \brief Returns average time interval measured
+    between two increment of iteration. */
+  double AverageTime();
 
-    /*! \brief Reset the clock to restart a campaign
-      of measures */
-    void Reset();
+  /*! \brief Returns the total time measured. */
+  double TotalTime();
 
-    /*! \brief Display a brief description of the current status. */
-    void Display();
+  /*! \brief Reset the clock to restart a campaign
+    of measures */
+  void Reset();
 
-    /*! \brief Record buffer of time consumption. */
-    void RecordDataBuffer(std::string filename);
+  /*! \brief Display a brief description of the current status. */
+  void Display();
 
-  private:
+  /*! \brief Record buffer of time consumption. */
+  void RecordDataBuffer(std::string filename);
 
-    /*! Storing begin and end timestamps. */
-    struct timeval m_BeginTimeStamp, m_EndTimeStamp;
+ private:
+  /*! Storing begin and end timestamps. */
+  struct timeval m_BeginTimeStamp, m_EndTimeStamp;
 
-    /*! Starting time of the clock. */
-    double m_StartingTime;
+  /*! Starting time of the clock. */
+  double m_StartingTime;
 
-    /*! Number of iterations. */
-    unsigned long int m_NbOfIterations;
+  /*! Number of iterations. */
+  unsigned long int m_NbOfIterations;
 
-    /*! Maximum time. */
-    double m_MaximumTime;
+  /*! Maximum time. */
+  double m_MaximumTime;
 
-    /*! Total time. */
-    double m_TotalTime;
+  /*! Total time. */
+  double m_TotalTime;
 
-    /*! Buffer */
-    std::vector<double> m_DataBuffer;
-  };
-}
+  /*! Buffer */
+  std::vector<double> m_DataBuffer;
+};
+}  // namespace PatternGeneratorJRL
 #endif /* _HWPG_CLOCK_H_ */

src/Debug.hh

 /*
- * Copyright 2005, 2006, 2007, 2008, 2009, 2010, 
+ * Copyright 2005, 2006, 2007, 2008, 2009, 2010,
  *
  * Olivier Stasse
  *
@@ -18,81 +18,99 @@
  * 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 
+ *  Research carried out within the scope of the
  *  Joint Japanese-French Robotics Laboratory (JRL)
  */
 /* @doc File for debugging messages. */
-#include <iostream>
-#include <fstream>
 #include <exception>
+#include <fstream>
+#include <iostream>
 
-#define LTHROW(x) \
-  { \
-    class Exception: public std::exception \
-    { \
-      virtual const char * what() const throw() \
-      {  \
-        return x; \
-      } \
-    }; \
-  \
-  Exception almsg; \
-  throw almsg; }
+#define LTHROW(x)                                            \
+  {                                                          \
+    class Exception : public std::exception {                \
+      virtual const char *what() const throw() { return x; } \
+    };                                                       \
+                                                             \
+    Exception almsg;                                         \
+    throw almsg;                                             \
+  }
 
 #define ODEBUG2(x)
-#define ODEBUG3(x) std::cerr << __FILE__ << ":" \
-                             << __FUNCTION__ << "(#" \
-                             << __LINE__ << "):" << x << std::endl;
-#define RESETDEBUG5(y) { std::ofstream DebugFile; \
-DebugFile.open(y,ofstream::out); \
-DebugFile.close();}
-#define ODEBUG5(x,y) { std::ofstream DebugFile; \
-DebugFile.open(y,ofstream::app); \
-DebugFile << __FILE__ << ":" \
-          << __FUNCTION__ << "(#" \
-          << __LINE__ << "):" << x << std::endl; \
-          DebugFile.close();}
-#define ODEBUG5SIMPLE(x,y) { std::ofstream DebugFile; \
-DebugFile.open(y,ofstream::app); \
-DebugFile << x << std::endl; \
-          DebugFile.close();}
+#define ODEBUG3(x)                                                 \
+  std::cerr << __FILE__ << ":" << __FUNCTION__ << "(#" << __LINE__ \
+            << "):" << x << std::endl;
+#define RESETDEBUG5(y)                \
+  {                                   \
+    std::ofstream DebugFile;          \
+    DebugFile.open(y, ofstream::out); \
+    DebugFile.close();                \
+  }
+#define ODEBUG5(x, y)                                                \
+  {                                                                  \
+    std::ofstream DebugFile;                                         \
+    DebugFile.open(y, ofstream::app);                                \
+    DebugFile.precision(8);                                          \
+    DebugFile.setf(ios::scientific, ios::floatfield);                \
+    DebugFile << __FILE__ << ":" << __FUNCTION__ << "(#" << __LINE__ \
+              << "):" << x << std::endl;                             \
+    DebugFile.close();                                               \
+  }
+#define ODEBUG5SIMPLE(x, y)           \
+  {                                   \
+    std::ofstream DebugFile;          \
+    DebugFile.open(y, ofstream::app); \
+    DebugFile << x << std::endl;      \
+    DebugFile.close();                \
+  }
 
 #define ODEBUG3_NENDL(x) std::cerr << x
 
 #ifdef _DEBUG_MODE_ON_
-#define ODEBUG(x) std::cerr << __FILE__ << ":" \
-                            << __FUNCTION__ << "(#" \
-                            << __LINE__ << "):" << x << std::endl;
+#define ODEBUG(x)                                                  \
+  std::cerr << __FILE__ << ":" << __FUNCTION__ << "(#" << __LINE__ \
+            << "):" << x << std::endl;
 
 #define ODEBUG_NENDL(x) std::cerr << x
 
 #define ODEBUG_CODE(x) x
 #else
 #define ODEBUG(x)
-#define ODEBUG_NENDL(x) 
+#define ODEBUG_NENDL(x)
 #define ODEBUG_CODE(x)
 #endif
 
 #ifdef _DEBUG_MODE_ON_
-#define RESETDEBUG4(y) { std::ofstream DebugFile; DebugFile.open(y,ofstream::out); DebugFile.close();}
-#define ODEBUG4(x,y) { std::ofstream DebugFile; DebugFile.open(y,ofstream::app); \
-    DebugFile << __FILE__ << ":" \
-              << __FUNCTION__ << "(#" \
-              << __LINE__ << "):" << x << std::endl; \
-    DebugFile.close();}
-#define ODEBUG4SIMPLE(x,y) { std::ofstream DebugFile; \
-DebugFile.open(y,ofstream::app); \
-DebugFile << x << std::endl; \
-          DebugFile.close();}
+#define RESETDEBUG4(y)                \
+  {                                   \
+    std::ofstream DebugFile;          \
+    DebugFile.open(y, ofstream::out); \
+    DebugFile.close();                \
+  }
+#define ODEBUG4(x, y)                                                \
+  {                                                                  \
+    std::ofstream DebugFile;                                         \
+    DebugFile.open(y, ofstream::app);                                \
+    DebugFile << __FILE__ << ":" << __FUNCTION__ << "(#" << __LINE__ \
+              << "):" << x << std::endl;                             \
+    DebugFile.close();                                               \
+  }
+#define ODEBUG4SIMPLE(x, y)           \
+  {                                   \
+    std::ofstream DebugFile;          \
+    DebugFile.open(y, ofstream::app); \
+    DebugFile << x << std::endl;      \
+    DebugFile.close();                \
+  }
 
 #define _DEBUG_4_ACTIVATED_ 1
 #else
 #define RESETDEBUG4(y)
-#define ODEBUG4(x,y)
-#define ODEBUG4SIMPLE(x,y)
+#define ODEBUG4(x, y)
+#define ODEBUG4SIMPLE(x, y)
 #endif
 
 #define RESETDEBUG6(x)
-#define ODEBUG6(x,y)
-#define ODEBUG6SIMPLE(x,y)
+#define ODEBUG6(x, y)
+#define ODEBUG6SIMPLE(x, y)
 #define CODEDEBUG6(x)

src/FootTrajectoryGeneration/FootTrajectoryGenerationAbstract.cpp

@@ -24,21 +24,21 @@
  */
 
 /*!\file FootTrajectoryGenerationAbstract.h
-   \brief This class determinate how it s generate all the values for the foot trajectories.
+  \brief This class determinate how it s generate all the values
+  for the foot trajectories.
 
-   @ingroup foottrajectorygeneration
+  @ingroup foottrajectorygeneration
 */
-#include <Debug.hh>
 #include "FootTrajectoryGeneration/FootTrajectoryGenerationAbstract.hh"
 
+#include <Debug.hh>
+
 using namespace PatternGeneratorJRL;
 
-FootTrajectoryGenerationAbstract::FootTrajectoryGenerationAbstract(SimplePluginManager *lSPM,
-								   CjrlFoot *aFoot)
-  : SimplePlugin(lSPM)
-{
+FootTrajectoryGenerationAbstract::FootTrajectoryGenerationAbstract(
+    SimplePluginManager *lSPM, PRFoot *aFoot)
+    : SimplePlugin(lSPM), m_Foot(aFoot) {
   m_Omega = 0.0;
-  m_Foot= aFoot;
   m_SamplingPeriod = 0.005;
   m_isStepStairOn = 1;
   m_StepHeight = 0.07;
@@ -46,76 +46,62 @@ FootTrajectoryGenerationAbstract::FootTrajectoryGenerationAbstract(SimplePluginM
   // Define here the curving during the Step (ray defined along Y axis)
   m_StepCurving = 0.2;
 
+  std::string aMethodName[5] = {":omega", ":stepheight", ":singlesupporttime",
+                                ":doublesupporttime",
+                                ":samplingperiod"
+                                ":stepstairon"};
 
-  std::string aMethodName[5] =
-    {":omega",
-     ":stepheight",
-     ":singlesupporttime",
-     ":doublesupporttime",
-     ":samplingperiod"
-     ":stepstairon"};
-
-  for (int i=0;i<5;i++)
-    {
-      if (!RegisterMethod(aMethodName[i]))
-	{
-	  std::cerr << "Unable to register " << aMethodName << std::endl;
-	}
+  for (int i = 0; i < 5; i++) {
+    if (!RegisterMethod(aMethodName[i])) {
+      std::cerr << "Unable to register " << aMethodName << std::endl;
     }
-
+  }
 }
 
 void FootTrajectoryGenerationAbstract::CallMethod(std::string &Method,
-					     std::istringstream &strm)
-{
-  if (Method==":omega")
-    {
-      strm >> m_Omega;
-    }
-  else if (Method==":omega2")
-    {
-      strm >> m_Omega2;
-    }
-  else if (Method==":singlesupporttime")
-    {
-      strm >> m_TSingle;
-    }
-  else if (Method==":doublesupporttime")
-    {
-      strm >> m_TDouble;
-    }
-  else if (Method==":samplingperiod")
-    {
-      strm >> m_SamplingPeriod;
-      ODEBUG("Sampling period: " << m_SamplingPeriod);
-    }
-  else if (Method==":stepheight")
-    {
-      strm >> m_StepHeight;
-      ODEBUG(":stepheight " << m_StepHeight);
-    }
+                                                  std::istringstream &strm) {
+  if (Method == ":omega") {
+    strm >> m_Omega;
+  } else if (Method == ":omega2") {
+    strm >> m_Omega2;
+  } else if (Method == ":singlesupporttime") {
+    strm >> m_TSingle;
+  } else if (Method == ":doublesupporttime") {
+    strm >> m_TDouble;
+  } else if (Method == ":samplingperiod") {
+    strm >> m_SamplingPeriod;
+    ODEBUG("Sampling period: " << m_SamplingPeriod);
+  } else if (Method == ":stepheight") {
+    strm >> m_StepHeight;
+    ODEBUG(":stepheight " << m_StepHeight);
+  }
 }
 
-void FootTrajectoryGenerationAbstract::UpdateFootPosition(std::deque<FootAbsolutePosition> & , //SupportFootAbsolutePositions,
-							  std::deque<FootAbsolutePosition> &, //NoneSupportFootAbsolutePositions,
-							  int , //CurrentAbsoluteIndex,
-							  int , //IndexInitial,
-							  double , //ModulatedSingleSupportTime,
-							  int , //StepType,
-                              int ) //LeftOrRight)
+void FootTrajectoryGenerationAbstract::UpdateFootPosition(
+    std::deque<FootAbsolutePosition> &,  // SupportFootAbsolutePositions,
+    std::deque<FootAbsolutePosition> &,  // NoneSupportFootAbsolutePositions,
+    int,                                 // CurrentAbsoluteIndex,
+    int,                                 // IndexInitial,
+    double,                              // ModulatedSingleSupportTime,
+    int,                                 // StepType,
+    int)                                 // LeftOrRight)
 {
-  LTHROW("FootTrajectoryGenerationAbstract::UpdateFootPosition-1: To be implemented ");
+  LTHROW(
+      "FootTrajectoryGenerationAbstract::UpdateFootPosition-1: \
+          To be implemented ");
 }
 
-void FootTrajectoryGenerationAbstract::UpdateFootPosition(std::deque<FootAbsolutePosition> & ,//SupportFootAbsolutePositions,
-							  std::deque<FootAbsolutePosition> & ,//NoneSupportFootAbsolutePositions,
-							  int , // StartIndex,
-							  int , //k,
-							  double , //LocalInterpolationStartTime,
-							  double , //ModulatedSingleSupportTime,
-							  int , //StepType,
-							  int ) //LeftOrRight)
+void FootTrajectoryGenerationAbstract::UpdateFootPosition(
+    std::deque<FootAbsolutePosition> &,  // SupportFootAbsolutePositions,
+    std::deque<FootAbsolutePosition> &,  // NoneSupportFootAbsolutePositions,
+    int,                                 // StartIndex,
+    int,                                 // k,
+    double,                              // LocalInterpolationStartTime,
+    double,                              // ModulatedSingleSupportTime,
+    int,                                 // StepType,
+    int)                                 // LeftOrRight)
 {
-  LTHROW("FootTrajectoryGenerationAbstract::UpdateFootPosition-2: To be implemented ");
+  LTHROW(
+      "FootTrajectoryGenerationAbstract::UpdateFootPosition-2: \
+          To be implemented ");
 }
-

src/FootTrajectoryGeneration/FootTrajectoryGenerationAbstract.hh

@@ -23,230 +23,211 @@
  */
 
 /*!\file FootTrajectoryGenerationAbstract.h
-   \brief This class determinate how it s generate all the values for the foot trajectories.
+  \brief This class determinate how it s generate all the values for the foot
+  trajectories.
 
-   @ingroup foottrajectorygeneration
+  @ingroup foottrajectorygeneration
 */
 
-
 #ifndef _FOOT_TRAJECTORY_GENERATION_ABSTRACT_H_
 #define _FOOT_TRAJECTORY_GENERATION_ABSTRACT_H_
 
 /* System related inclusions */
 #include <deque>
 
-/*! MatrixAbstractLayer */
-#include <jrl/mal/matrixabstractlayer.hh>
-
-/* dynamics JRL Japan related inclusions */
-#include <abstract-robot-dynamics/foot.hh>
+/* dynamics pinocchio related inclusions */
+#include <jrl/walkgen/pinocchiorobot.hh>
 
 /* Walking pattern generation related inclusions */
 
-#include <jrl/walkgen/pgtypes.hh>
 #include <SimplePlugin.hh>
+#include <jrl/walkgen/pgtypes.hh>
+#include <privatepgtypes.hh>
 
-namespace PatternGeneratorJRL
-{
+namespace PatternGeneratorJRL {
 
-  /** @ingroup foottrajectorygeneration
-      This class defines the abstract interface to interact with foot generation object.
+/** @ingroup foottrajectorygeneration
+    This class defines the abstract interface to interact with
+    foot generation object.
 
-      Two parameters \f$ T_{DS} \f$ and \f$ T_{SS} \f$ defines respectively
-      the double support time and the single support time.
+    Two parameters \f$ T_{DS} \f$ and \f$ T_{SS} \f$ defines respectively
+    the double support time and the single support time.
 
-      \f$ \omega \f$ defines the angle of the feet for landing and taking off.
-      The foot rotates around the toe from \f$ 0 \f$ to \f$ \omega \f$
-      for taking off. Whereas for the landing the foot rotates around the
-      heel from \f$ \omega \f$ to \f$ 0 \f$.
+    \f$ \omega \f$ defines the angle of the feet for landing and taking off.
+    The foot rotates around the toe from \f$ 0 \f$ to \f$ \omega \f$
+    for taking off. Whereas for the landing the foot rotates around the
+    heel from \f$ \omega \f$ to \f$ 0 \f$.
 
-      The sampling control time indicates the amount of time between two
-      iteration of the algorithm. This parameter is used in the method
-      UpdateFootPosition to compute the time from the iteration number
-      given in parameters.
+    The sampling control time indicates the amount of time between two
+    iteration of the algorithm. This parameter is used in the method
+    UpdateFootPosition to compute the time from the iteration number
+    given in parameters.
 
-      An instance of a class derived from FootTrajectoryGenerationAbstract,
-      should call InitializeInternalDataStructures() once all the internal
-      parameters of the object are set.
+    An instance of a class derived from FootTrajectoryGenerationAbstract,
+    should call InitializeInternalDataStructures() once all the internal
+    parameters of the object are set.
 
-      The virtual function FreeInternalDataStructures() is used when changing some
-      parameters and by the destructor.
+    The virtual function FreeInternalDataStructures() is used when changing
+    some parameters and by the destructor.
 
-      The most important function is UpdateFootPosition() which populates a
-      queue of foot absolute positions data structure.
+    The most important function is UpdateFootPosition() which populates a
+    queue of foot absolute positions data structure.
 
-      See a derived class such as FootTrajectoryGenerationStandard
-      for more precise information on the usage and sample codes.
+    See a derived class such as FootTrajectoryGenerationStandard
+    for more precise information on the usage and sample codes.
 
+*/
+class FootTrajectoryGenerationAbstract : public SimplePlugin {
+ public:
+  /*! Constructor: In order to compute some appropriate strategies,
+    this class needs to extract specific details from the humanoid model. */
+  FootTrajectoryGenerationAbstract(SimplePluginManager *lSPM, PRFoot *inFoot);
+
+  /*! Default destructor. */
+  virtual ~FootTrajectoryGenerationAbstract(){};
+
+  /*! This method computes the position of the swinging foot during
+    single support phase, and maintian a constant position
+    for the support foot.
+    @param SupportFootAbsolutePositions: Queue of absolute position for the
+    support foot.
+    This method will set the foot position at index CurrentAbsoluteIndex
+    of the queue.
+    This position is supposed to be constant.
+    @param NoneSupportFootAbsolutePositions: Queue of absolute position
+    for the swinging
+    foot. This method will set the foot position at index
+    NoneSupportFootAbsolutePositions
+    of the queue.
+    @param CurrentAbsoluteIndex: Index in the queues of the foot position to
+    be set.
+    @param IndexInitial: Index in the queues which correspond to the
+    starting point of the current single support phase.
+    @param ModulatedSingleSupportTime: Amount of time where the foot is flat.
+    @param StepType: Type of steps (for book-keeping).
+    @param LeftOrRight: Specify if it is left (1) or right (-1).
+  */
+  virtual void UpdateFootPosition(
+      std::deque<FootAbsolutePosition> &SupportFootAbsolutePositions,
+      std::deque<FootAbsolutePosition> &NoneSupportFootAbsolutePositions,
+      int CurrentAbsoluteIndex, int IndexInitial,
+      double ModulatedSingleSupportTime, int StepType, int LeftOrRight);
+
+  virtual void UpdateFootPosition(
+      std::deque<FootAbsolutePosition> &SupportFootAbsolutePositions,
+      std::deque<FootAbsolutePosition> &NoneSupportFootAbsolutePositions,
+      int StartIndex, int k, double LocalInterpolationStartTime,
+      double ModulatedSingleSupportTime, int StepType, int LeftOrRight);
+
+  /*! Initialize internal data structures. */
+  virtual void InitializeInternalDataStructures() = 0;
+
+  /*! Free internal data structures */
+  virtual void FreeInternalDataStructures() = 0;
+
+  /*! \name Setter and getter for parameters
+    @{
   */
-  class  FootTrajectoryGenerationAbstract :public SimplePlugin
-  {
-  public:
-
-    /*! Constructor: In order to compute some appropriate strategies,
-      this class needs to extract specific details from the humanoid model. */
-    FootTrajectoryGenerationAbstract(SimplePluginManager *lSPM,
-				     CjrlFoot *inFoot) ;
-
-
-    /*! Default destructor. */
-    virtual ~FootTrajectoryGenerationAbstract(){};
-
-    /*! This method computes the position of the swinging foot during single support phase,
-      and maintian a constant position for the support foot.
-      @param SupportFootAbsolutePositions: Queue of absolute position for the support foot.
-      This method will set the foot position at index CurrentAbsoluteIndex of the queue.
-      This position is supposed to be constant.
-      @param NoneSupportFootAbsolutePositions: Queue of absolute position for the swinging
-      foot. This method will set the foot position at index NoneSupportFootAbsolutePositions
-      of the queue.
-      @param CurrentAbsoluteIndex: Index in the queues of the foot position to be set.
-      @param IndexInitial: Index in the queues which correspond to the starting point
-      of the current single support phase.
-      @param ModulatedSingleSupportTime: Amount of time where the foot is flat.
-      @param StepType: Type of steps (for book-keeping).
-      @param LeftOrRight: Specify if it is left (1) or right (-1).
-    */
-    virtual void UpdateFootPosition(std::deque<FootAbsolutePosition> &SupportFootAbsolutePositions,
-				    std::deque<FootAbsolutePosition> &NoneSupportFootAbsolutePositions,
-				    int CurrentAbsoluteIndex,
-				    int IndexInitial,
-				    double ModulatedSingleSupportTime,
-				    int StepType, int LeftOrRight);
-
-    virtual void UpdateFootPosition(std::deque<FootAbsolutePosition> &SupportFootAbsolutePositions,
-				    std::deque<FootAbsolutePosition> &NoneSupportFootAbsolutePositions,
-				    int StartIndex, int k,
-				    double LocalInterpolationStartTime,
-				    double ModulatedSingleSupportTime,
-				    int StepType, int LeftOrRight);
-
-    /*! Initialize internal data structures. */
-    virtual void InitializeInternalDataStructures()=0;
-
-    /*! Free internal data structures */
-    virtual void FreeInternalDataStructures()=0;
-
-    /*! \name Setter and getter for parameters
-      @{
-     */
-
-    /*! \name Single Support Time
-      @{
-    */
-
-    /*! \brief Set single support time */
-    void SetSingleSupportTime(double aTSingle)
-    { m_TSingle =aTSingle;};
-
-    /*! \brief Get single support time */
-    double GetSingleSupportTime() const
-    { return m_TSingle;};
-    /*! @} */
-
-    /*! \name Double Support Time
-      @{
-    */
-
-    /*! \brief Set double support time */
-    void SetDoubleSupportTime(double aTDouble)
-    { m_TDouble =aTDouble;};
-
-    /*! \brief Get single support time */
-    double GetDoubleSupportTime() const
-    { return m_TDouble;};
 
-    /*! @}*/
+  /*! \name Single Support Time
+    @{
+  */
 
-    /*! \name Sampling control Time
-      @{
-    */
+  /*! \brief Set single support time */
+  void SetSingleSupportTime(double aTSingle) { m_TSingle = aTSingle; };
 
-    /*! \brief Set single support time */
-    void SetSamplingPeriod(double aSamplingPeriod)
-    { m_SamplingPeriod = aSamplingPeriod;};
+  /*! \brief Get single support time */
+  double GetSingleSupportTime() const { return m_TSingle; };
+  /*! @} */
 
-    /*! \brief Get single support time */
-    double GetSamplingPeriod() const
-    { return m_SamplingPeriod;};
+  /*! \name Double Support Time
+    @{
+  */
 
-    /*!@}*/
+  /*! \brief Set double support time */
+  void SetDoubleSupportTime(double aTDouble) { m_TDouble = aTDouble; };
 
-    /*! \name Omega.
-      @{*/
+  /*! \brief Get single support time */
+  double GetDoubleSupportTime() const { return m_TDouble; };
 
-    /*! Get Omega */
-    double GetOmega() const
-    { return m_Omega; };
+  /*! @}*/
 
-    /*! Set Omega */
-    void SetOmega(double anOmega)
-    { m_Omega = anOmega; };
+  /*! \name Sampling control Time
+    @{
+  */
 
+  /*! \brief Set single support time */
+  void SetSamplingPeriod(double aSamplingPeriod) {
+    m_SamplingPeriod = aSamplingPeriod;
+  };
 
-    /*! \name StepHeight.
-      @{*/
+  /*! \brief Get single support time */
+  double GetSamplingPeriod() const { return m_SamplingPeriod; };
 
-    /*! Get StepHeight */
-    double GetStepHeight() const
-    { return m_StepHeight; };
+  /*!@}*/
 
-    /*! Set Omega */
-    void SetStepHeight(double aStepHeight)
-    { m_StepHeight = aStepHeight; };
+  /*! \name Omega.
+    @{*/
 
+  /*! Get Omega */
+  double GetOmega() const { return m_Omega; };
 
-    /*! \name isStepStairOn.
-      @{*/
+  /*! Set Omega */
+  void SetOmega(double anOmega) { m_Omega = anOmega; };
 
-    /*! Get isStepStairOn */
-    int GetSetStepStairOn() const
-    { return  m_isStepStairOn; };
+  /*! \name StepHeight.
+    @{*/
 
-    /*! Set isStepStairOn */
-    void SetStepStairOn(int aSSO)
-    {  m_isStepStairOn= aSSO;};
+  /*! Get StepHeight */
+  double GetStepHeight() const { return m_StepHeight; };
 
-    /*! @} */
-    /*! @} */
+  /*! Set Omega */
+  void SetStepHeight(double aStepHeight) { m_StepHeight = aStepHeight; };
 
-    /*! \brief Reimplementation of the call method for the plugin manager.
-      More explicitly this object will deal with the call which initialize
-      the feet behaviors (\f$omega\f$, \f$ stepheight \f$) .
-    */
-    virtual void CallMethod(std::string &Method, std::istringstream &strm);
+  /*! \name isStepStairOn.
+    @{*/
 
+  /*! Get isStepStairOn */
+  int GetSetStepStairOn() const { return m_isStepStairOn; };
 
-  protected:
+  /*! Set isStepStairOn */
+  void SetStepStairOn(int aSSO) { m_isStepStairOn = aSSO; };
 
-    /*! Sampling period of the control. */
-    double m_SamplingPeriod;
+  /*! @} */
+  /*! @} */
 
-    /*! Duration time for single support. */
-    double m_TSingle;
+  /*! \brief Reimplementation of the call method for the plugin manager.
+    More explicitly this object will deal with the call which initialize
+    the feet behaviors (\f$omega\f$, \f$ stepheight \f$) .
+  */
+  virtual void CallMethod(std::string &Method, std::istringstream &strm);
 
-    /*! Duration time for double support. */
-    double m_TDouble;
+ protected:
+  /*! Sampling period of the control. */
+  double m_SamplingPeriod;
 
-    /*! Store a pointer to Foot information. */
-    CjrlFoot * m_Foot;
+  /*! Duration time for single support. */
+  double m_TSingle;
 
-    /*! Omega the angle for taking off and landing. */
-    double m_Omega;
+  /*! Duration time for double support. */
+  double m_TDouble;
 
-    /*! Omega the angle for slope walking. */
-    double m_Omega2 ;
+  /*! Store a pointer to Foot information. */
+  PRFoot *m_Foot;
 
-    int m_isStepStairOn;
+  /*! Omega the angle for taking off and landing. */
+  double m_Omega;
 
-    /// \brief Height of the flying foot in the middle of the SS phase
-    double m_StepHeight;
+  /*! Omega the angle for slope walking. */
+  double m_Omega2;
 
-    double m_StepCurving;
+  int m_isStepStairOn;
 
-  };
+  /// \brief Height of the flying foot in the middle of the SS phase
+  double m_StepHeight;
 
+  double m_StepCurving;
+};
 
-}
+}  // namespace PatternGeneratorJRL
 #endif /* _FOOT_TRAJECTORY_GENERATION_ABSTRACT_H_ */
-