Commit c4f486d3 authored by Olivier Stasse's avatar Olivier Stasse
Browse files

[clang-format] Homogeneize format.

parent 8c78ed7c
/*
* Copyright 2006, 2007, 2008, 2009, 2010,
* Copyright 2006, 2007, 2008, 2009, 2010,
*
* Florent Lamiraux
* Olivier Stasse
......@@ -19,39 +19,39 @@
* 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)
*/
/**
\mainpage
/**
\mainpage
\section sec_intro Introduction
This library implements a series of algorithms generating CoM, ZMP and feet
trajectories when given a set of foot-prints. In some cases, when a robot
model provides a specialized inverse kinematics some algorithms can generate
model provides a specialized inverse kinematics some algorithms can generate
articular values.
\section walkGenJrl_sec_approach Approach implemented
This code assumes that the mass distribution of your humanoid robot is
This code assumes that the mass distribution of your humanoid robot is
centered around the waist. This is very important as all the algorithms
implemented uses the single point mass model. If the masses of your robot
legs and arms are too important to be ignored it is very likely that
none of the algorithms will give a feasible pair of CoM-ZMP trajectories.
The first available algorithm is the preview algorithm \ref Kajita2003 and \ref Kajita2005.
This algorithm is real-time if you do not change the foot-prints inside the preview
window.
If you do want to perform this modification you should see \ref Morisawa2007.
This come at the expense of a possible delay in the step execution, if the modifcations
are too important. On the other hand this is one of the fastest solution to modify
the futur in the stack of foot-steps.
The first available algorithm is the preview algorithm \ref Kajita2003 and \ref
Kajita2005. This algorithm is real-time if you do not change the foot-prints
inside the preview window. If you do want to perform this modification you
should see \ref Morisawa2007. This come at the expense of a possible delay in
the step execution, if the modifcations are too important. On the other hand
this is one of the fastest solution to modify the futur in the stack of
foot-steps.
Two variants similar to the preview control but including inequalities have been introduced
by \ref Wieber2006 and \ref Dimitrov2009. The former takes 28 ms to be computed whereas the
second introduce a new solver able to solve the problem in less than 2 ms.
The solver named PLDP is included inside the library.
Two variants similar to the preview control but including inequalities have been
introduced by \ref Wieber2006 and \ref Dimitrov2009. The former takes 28 ms to
be computed whereas the second introduce a new solver able to solve the problem
in less than 2 ms. The solver named PLDP is included inside the library.
A new algorithm able to take a reference velocity of the CoM is also provided.
Its description can be found in \ref Herdt2010. This problem is currently solved
......@@ -60,13 +60,14 @@ letting us distribute his code in our LGPL code.
\section References
\anchor Kajita2003
S. Kajita and F. Kanehiro and K. Kaneko and K. Fujiwara and K. Harada and K. Yokoi and H. Hirukawa,
"Biped Walking Pattern Generation by using Preview Control of Zero-Moment Point",
International Conference on Robotics And Automation, Taipei Taiwan, 2003
S. Kajita and F. Kanehiro and K. Kaneko and K. Fujiwara and K. Harada and K.
Yokoi and H. Hirukawa, "Biped Walking Pattern Generation by using Preview
Control of Zero-Moment Point", International Conference on Robotics And
Automation, Taipei Taiwan, 2003
\anchor Kajita2005
S. Kajita,
Omsha,
Omsha,
"Humanoid Robot",
2005,(In Japanese) ISBN4-274-20058-2, 2005
......@@ -76,22 +77,23 @@ B. Verrelst and K. Yokoi and O. Stasse and H. Arisumi and B. Vanderborght,
International Conference on Mechatronics and Automation
\anchor Morisawa2007
M. Morisawa and K. Harada and S. Kajita and S. Nakaoka and K. Fujiwara and F. Kanehiro and K. Kaneko and H. Hirukawa,
"Experimentation of Humanoid Walking Allowing Immediate Modification of Foot Place Based on Analytical Solution",
M. Morisawa and K. Harada and S. Kajita and S. Nakaoka and K. Fujiwara and F.
Kanehiro and K. Kaneko and H. Hirukawa, "Experimentation of Humanoid Walking
Allowing Immediate Modification of Foot Place Based on Analytical Solution",
IEEE Int. Conf. on Robotics and Automation,
3989--3994, 2007
\anchor Wieber2006
P.-B. Wieber,
"Trajectory Free Linear Model Predictive Control for Stable Walking in the Presence of Strong Perturbations"
IEEE/RAS Int. Conf. on Humanoid Robots
"Trajectory Free Linear Model Predictive Control for Stable Walking in the
Presence of Strong Perturbations" IEEE/RAS Int. Conf. on Humanoid Robots
137-142, 2006
\anchor Dimitrov2009
D. Dimitrov, P.-B. Wieber, O. Stasse, H.J. Ferreau and H. Diedam,
"An Optimized Linear Model Predictive Control Solver for Online Walking Motion Generation",
IEEE International Conference on Robotics and Automation (ICRA)
pp. 1171--1176, 2009
"An Optimized Linear Model Predictive Control Solver for Online Walking Motion
Generation", IEEE International Conference on Robotics and Automation (ICRA) pp.
1171--1176, 2009
\anchor Herdt2010
A. Herdt, D. Holger, P.B. Wieber, D. Dimitrov, K. Mombaur and D. Moritz
......@@ -99,7 +101,7 @@ A. Herdt, D. Holger, P.B. Wieber, D. Dimitrov, K. Mombaur and D. Moritz
Advanced Robotics
Vol. 24 Issue 5-6, pp. 719--737
\anchor Schittkowski2007
\anchor Schittkowski2007
K. Schittkowski (2007): QL: A Fortran code for convex
quadratic programming - user's guide, Report, Department
of Computer Science, University of Bayreuth
......@@ -107,7 +109,8 @@ of Computer Science, University of Bayreuth
\defgroup pgjrl JRL Walking Pattern Generator Library (WalkGenJRL)
@{
This library is intended to implement walking pattern generator algorithms for humanoid robots.
This library is intended to implement walking pattern generator algorithms
for humanoid robots.
*/
......@@ -128,12 +131,11 @@ of Computer Science, University of Bayreuth
/**
\defgroup walkGenJrl_geometry Geometry
This group implements some basic geometrical tools for the Pattern Generator.
@{
*/
/**
@}
*/
......@@ -153,9 +155,9 @@ of Computer Science, University of Bayreuth
/**
\defgroup pginterfaces Interfaces to WalkGenJRL
This group shows how to interface the WalkGenJRL library to three kinds of applications:
\li An OpenHRP plugin, to run in real-time inside an HRP-2 humanoid robot,
\li A console program, for quick but yet complete testing of the pattern generator,
\li An OpenGL interface.
This group shows how to interface the WalkGenJRL library to three kinds of
applications: \li An OpenHRP plugin, to run in real-time inside an HRP-2
humanoid robot, \li A console program, for quick but yet complete testing of
the pattern generator, \li An OpenGL interface.
*/
......@@ -28,306 +28,247 @@
\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
#else
#define WALK_GEN_JRL_EXPORT
#endif
#include <iostream>
#include <fstream>
#include <iostream>
#include <Eigen/Dense>
#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;
}
typedef struct COMPosition_s COMPosition;
typedef struct COMPosition_s WaistState;
namespace PatternGeneratorJRL {
struct COMState_s;
/// 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 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
/** 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;
struct COMPosition_s &operator=(const COMState_s &aCS);
};
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 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
{
Eigen::MatrixXd A;
Eigen::MatrixXd B;
Eigen::VectorXd Center;
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
{
Eigen::MatrixXd D;
Eigen::MatrixXd Dc;
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 */
Eigen::VectorXd RefVectorX;
Eigen::VectorXd RefVectorY;
Eigen::VectorXd RefVectorTheta;
};
typedef struct ReferenceAbsoluteVelocity_t ReferenceAbsoluteVelocity;