Skip to content
Snippets Groups Projects

Compare revisions

Changes are shown as if the source revision was being merged into the target revision. Learn more about comparing revisions.

Source

Select target project
No results found

Target

Select target project
  • imaroger/jrl-walkgen
  • lscherrer/jrl-walkgen
2 results
Show changes
Expand all Hide whitespace changes
Inline Side-by-side
Showing
with 3562 additions and 4111 deletions
include/jrl/walkgen/pgtypes.hh
View file @ 0ac1d6e6
......@@ -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
View file @ 0ac1d6e6
......@@ -29,253 +29,305 @@ frame work */
#ifndef PinocchioRobot_HH
#define PinocchioRobot_HH
#include "pinocchio/spatial/se3.hpp"
#include "pinocchio/multibody/model.hpp"
#include "pinocchio/multibody/data.hpp"
#include <jrl/mal/matrixabstractlayer.hh>
namespace PatternGeneratorJRL
{
struct PinocchioRobotFoot_t{
se3::JointIndex associatedAnkle ;
double soleDepth ; // z axis
double soleWidth ; // y axis
double soleHeight ;// x axis
vector3d anklePosition ;
EIGEN_MAKE_ALIGNED_OPERATOR_NEW
};
typedef PinocchioRobotFoot_t PRFoot ;
class PinocchioRobot
{
public:
// overload the new[] eigen operator
EIGEN_MAKE_ALIGNED_OPERATOR_NEW
/// Constructor and Destructor
PinocchioRobot();
virtual ~PinocchioRobot();
/// Functions computing kinematics and dynamics
void computeInverseDynamics();
void computeInverseDynamics(MAL_VECTOR_TYPE(double) & q,
MAL_VECTOR_TYPE(double) & v,
MAL_VECTOR_TYPE(double) & a);
void computeForwardKinematics();
void computeForwardKinematics(MAL_VECTOR_TYPE(double) & q);
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 se3::JointIndex &jointRoot,
const se3::JointIndex &jointEnd,
const MAL_S4x4_MATRIX_TYPE(double) & jointRootPosition,
const MAL_S4x4_MATRIX_TYPE(double) & jointEndPosition,
MAL_VECTOR_TYPE(double) &q);
///
/// \brief testInverseKinematics :
/// test if the robot has the good joint
/// configuration to use the analitical inverse geometry
/// to be overloaded if the user wants another inverse kinematics
/// \return
///
virtual bool testInverseKinematics();
///
/// \brief initializeInverseKinematics :
/// initialize the internal data for the inverse kinematic e.g. the femur
/// length
/// \return
///
virtual void initializeInverseKinematics();
public :
/// tools :
std::vector<se3::JointIndex> jointsBetween
( se3::JointIndex first, se3::JointIndex second);
std::vector<se3::JointIndex> fromRootToIt (se3::JointIndex it);
private :
// needed for the inverse geometry (ComputeSpecializedInverseKinematics)
void getWaistFootKinematics(const matrix4d & jointRootPosition,
const matrix4d & jointEndPosition,
vectorN &q,
vector3d Dt);
double ComputeXmax(double & Z);
void getShoulderWristKinematics(const matrix4d & jointRootPosition,
const matrix4d & jointEndPosition,
vectorN &q,
int side);
void DetectAutomaticallyShoulders();
void DetectAutomaticallyOneShoulder(se3::JointIndex aWrist,
se3::JointIndex & aShoulder);
public :
/// Getters
/// ///////
inline se3::Data * Data()
{return m_robotData;}
inline se3::Data * DataInInitialePose()
{return m_robotDataInInitialePose;}
inline se3::Model * Model()
{return m_robotModel;}
inline PRFoot * leftFoot()
{return &m_leftFoot;}
inline PRFoot * rightFoot()
{return &m_rightFoot;}
inline se3::JointIndex leftWrist()
{return m_leftWrist;}
inline se3::JointIndex rightWrist()
{return m_rightWrist;}
inline se3::JointIndex chest()
{return m_chest;}
inline se3::JointIndex waist()
{return m_waist;}
inline double mass()
{return m_mass;}
inline se3::JointModelVector & getActuatedJoints()
{return m_robotModel->joints;}
inline MAL_VECTOR_TYPE(double) currentConfiguration()
{return m_qmal;}
inline MAL_VECTOR_TYPE(double) currentVelocity()
{return m_vmal;}
inline MAL_VECTOR_TYPE(double) currentAcceleration()
{return m_amal;}
inline unsigned numberDof()
{return m_robotModel->nv;}
inline void zeroMomentumPoint(MAL_S3_VECTOR_TYPE(double) & 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(MAL_S3_VECTOR_TYPE(double) & 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(MAL_S3_VECTOR_TYPE(double) & com,
MAL_S3_VECTOR_TYPE(double) & dcom,
MAL_S3_VECTOR_TYPE(double) & 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
/// ///////
inline void currentConfiguration(MAL_VECTOR_TYPE(double) conf)
{m_qmal=conf;}
inline void currentVelocity(MAL_VECTOR_TYPE(double) vel)
{m_vmal=vel;}
inline void currentAcceleration(MAL_VECTOR_TYPE(double) acc)
{m_amal=acc;}
/// Initialization functions
/// ////////////////////////
inline bool isInitialized()
{
return m_boolModel && m_boolData && m_boolLeftFoot && m_boolRightFoot;
}
bool checkModel(se3::Model * robotModel);
bool initializeRobotModelAndData(se3::Model * robotModel,
se3::Data * robotData);
bool initializeLeftFoot(PRFoot leftFoot);
bool initializeRightFoot(PRFoot rightFoot);
/// Attributes
/// //////////
private :
se3::Model * m_robotModel ;
se3::Data * m_robotDataInInitialePose ; // internal variable
se3::Data * m_robotData ;
PRFoot m_leftFoot , m_rightFoot ;
double m_mass ;
se3::JointIndex m_chest, m_waist, m_leftShoulder, m_rightShoulder ;
se3::JointIndex m_leftWrist , m_rightWrist ;
MAL_VECTOR_TYPE(double) m_qmal ;
MAL_VECTOR_TYPE(double) m_vmal ;
MAL_VECTOR_TYPE(double) m_amal ;
Eigen::VectorXd m_q ;
Eigen::VectorXd m_v ;
Eigen::VectorXd m_a ;
// tmp variables
Eigen::Quaterniond m_quat ;
Eigen::Matrix3d m_rot ;
se3::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 ;
bool m_isArmInverseKinematic ;
// length between the waist and the hip
MAL_S3_VECTOR_TYPE(double) 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 ;
}; //PinocchioRobot
}// namespace PatternGeneratorJRL
#endif // PinocchioRobot_HH
#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 0 → 100644
View file @ 0ac1d6e6
<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 deleted 100644 → 0
View file @ 131ca7df
#.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 deleted 100644 → 0
View file @ 131ca7df
# 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})
SET_TARGET_PROPERTIES(${_libname} PROPERTIES COMPILE_FLAGS "-msse -msse2 -msse3 -march=core2 -mfpmath=sse -fivopts -ftree-loop-im -fipa-pta ")
INSTALL(TARGETS ${_libname} DESTINATION ${CMAKE_INSTALL_LIBDIR})
ENDFUNCTION()
private_cmake/config_private.hh.cmake deleted 100644 → 0
View file @ 131ca7df
// 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@"
#endif //! JRL_WALKGEN_CONFIG_PRIVATE_HH
src/CMakeLists.txt deleted 100644 → 0
View file @ 131ca7df
# 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/>.
IF(USE_QUADPROG)
SET(QUADPROG_COMPILE_FLAG 1)
ELSE()
SET(QUADPROG_COMPILE_FLAG 0)
ENDIF()
CONFIGURE_FILE(configJRLWPG.hh.in ${CMAKE_BINARY_DIR}/include/jrl/walkgen/configJRLWPG.hh)
# Make sure to find Debug.h
INCLUDE_DIRECTORIES(BEFORE ${PROJECT_SOURCE_DIR}/src)
# 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
../include/jrl/walkgen/pinocchiorobot.hh
)
IF(USE_QUADPROG)
SET(INCLUDES
${INCLUDES}
ZMPRefTrajectoryGeneration/ZMPVelocityReferencedSQP.hh
ZMPRefTrajectoryGeneration/nmpc_generator.hh
)
ENDIF(USE_QUADPROG)
SET(SOURCES
${INCLUDES}
RobotDynamics/pinocchiorobot.cpp
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(USE_QUADPROG)
SET(SOURCES
${SOURCES}
ZMPRefTrajectoryGeneration/ZMPVelocityReferencedSQP.cpp
ZMPRefTrajectoryGeneration/nmpc_generator.cpp
)
ENDIF(USE_QUADPROG)
# 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' )
ADD_LIBRARY(${PROJECT_NAME} SHARED ${SOURCES} ${${PROJECT_NAME}_ABSOLUTE_HEADERS})
# Define dependencies
SET_TARGET_PROPERTIES(${PROJECT_NAME} PROPERTIES COMPILE_FLAGS "-msse -msse2 -msse3 -march=core2 -mfpmath=sse -fivopts -ftree-loop-im -fipa-pta ")
PKG_CONFIG_USE_DEPENDENCY(${PROJECT_NAME} jrl-mal)
PKG_CONFIG_USE_DEPENDENCY(${PROJECT_NAME} pinocchio)
IF(USE_LSSOL)
PKG_CONFIG_USE_DEPENDENCY(${PROJECT_NAME} lssol)
ENDIF(USE_LSSOL)
IF(USE_QUADPROG)
PKG_CONFIG_USE_DEPENDENCY(${PROJECT_NAME} eigen-quadprog)
ENDIF(USE_QUADPROG)
INSTALL(TARGETS ${PROJECT_NAME} DESTINATION ${CMAKE_INSTALL_PREFIX}/lib)
SET_TARGET_PROPERTIES(${PROJECT_NAME} PROPERTIES SOVERSION ${PROJECT_VERSION})
src/Clock.cpp
View file @ 0ac1d6e6
/*
* 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
View file @ 0ac1d6e6
......@@ -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
View file @ 0ac1d6e6
/*
* 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
View file @ 0ac1d6e6
......@@ -24,19 +24,20 @@
*/
/*!\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,
PRFoot * aFoot)
: SimplePlugin(lSPM),m_Foot(aFoot)
{
FootTrajectoryGenerationAbstract::FootTrajectoryGenerationAbstract(
SimplePluginManager *lSPM, PRFoot *aFoot)
: SimplePlugin(lSPM), m_Foot(aFoot) {
m_Omega = 0.0;
m_SamplingPeriod = 0.005;
m_isStepStairOn = 1;
......@@ -45,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
View file @ 0ac1d6e6
......@@ -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 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,
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
@{
*/
/*! \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. */
PRFoot * 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_ */