Skip to content
Snippets Groups Projects
Commit 12a76121 authored by Andrei Herdt's avatar Andrei Herdt Committed by Olivier Stasse
Browse files

Simplifications due to the new structure 

- Introduce structs for zmp and feet convex hulls
parent 2a298cf6
No related branches found
No related tags found
No related merge requests found
Hide whitespace changes
Inline Side-by-side
src/Mathematics/FootConstraintsAsLinearSystemForVelRef.cpp
+ 56
− 175
View file @ 12a76121
......@@ -55,16 +55,16 @@ FootConstraintsAsLinearSystemForVelRef::FootConstraintsAsLinearSystemForVelRef(
initConvexHulls();
// Register method to handle
string aMethodName[] =
{":setfeetconstraint"};
for(int i=0;i<1;i++)
{
if (!RegisterMethod(aMethodName[i]))
{
std::cerr << "Unable to register " << aMethodName << std::endl;
}
}
string aMethodName[] =
{":setfeetconstraint"};
for(int i=0;i<1;i++)
{
if (!RegisterMethod(aMethodName[i]))
{
std::cerr << "Unable to register " << aMethodName << std::endl;
}
}
}
FootConstraintsAsLinearSystemForVelRef::~FootConstraintsAsLinearSystemForVelRef()
......@@ -83,30 +83,34 @@ FootConstraintsAsLinearSystemForVelRef::initConvexHulls()
double m_lxcoefsLeft[4] = { 1.0, 1.0, -1.0, -1.0};
double m_lycoefsLeft[4] = { 1.0, -1.0, -1.0, 1.0};
m_LeftFPosConstrVerticesX[0] = -0.28;m_LeftFPosConstrVerticesX[1]=-0.2;m_LeftFPosConstrVerticesX[2]=0.0;
m_LeftFPosConstrVerticesX[3]=0.2;m_LeftFPosConstrVerticesX[4]=0.28;
m_LeftFPosConstrVerticesY[0] = 0.2;m_LeftFPosConstrVerticesY[1]=0.3;m_LeftFPosConstrVerticesY[2]=0.4;
m_LeftFPosConstrVerticesY[3]=0.3;m_LeftFPosConstrVerticesY[4]=0.2;
m_RightFPosConstrVerticesX[0]=-0.28;m_RightFPosConstrVerticesX[1]=-0.2;m_RightFPosConstrVerticesX[2]=0.0;
m_RightFPosConstrVerticesX[3]=0.2;m_RightFPosConstrVerticesX[4]=0.28;
m_RightFPosConstrVerticesY[0] = -0.2; m_RightFPosConstrVerticesY[1] = -0.3; m_RightFPosConstrVerticesY[2] = -0.4;
m_RightFPosConstrVerticesY[3] = -0.3; m_RightFPosConstrVerticesY[4] = -0.2;
m_FootPosCstr.left.resize(5);
double LeftFPosEdgesX[5] = {-0.28, -0.2, 0.0, 0.2, 0.28};
double LeftFPosEdgesY[5] = {0.2, 0.3, 0.4, 0.3, 0.2};
m_FootPosCstr.left.set(LeftFPosEdgesX,LeftFPosEdgesY);
m_FootPosCstr.right.resize(5);
double RightFPosEdgesX[5] = {-0.28, -0.2, 0.0, 0.2, 0.28};
double RightFPosEdgesY[5] = {-0.2, -0.3, -0.4, -0.3, -0.2};
m_FootPosCstr.right.set(RightFPosEdgesX,RightFPosEdgesY);
m_ZMPPosCstr.leftDS.resize(4);
m_ZMPPosCstr.leftSS.resize(4);
m_ZMPPosCstr.rightDS.resize(4);
m_ZMPPosCstr.rightSS.resize(4);
for( unsigned j=0;j<4;j++ )
{
//Left single support phase
m_LeftZMPConstrVerticesX[j] = m_lxcoefsLeft[j]*m_LeftFootSize.getHalfWidth();
m_LeftZMPConstrVerticesY[j] = m_lycoefsLeft[j]*m_LeftFootSize.getHalfHeight();
m_ZMPPosCstr.leftSS.X[j] = m_lxcoefsLeft[j]*m_LeftFootSize.getHalfWidth();
m_ZMPPosCstr.leftSS.Y[j] = m_lycoefsLeft[j]*m_LeftFootSize.getHalfHeight();
//Right single support phase
m_RightZMPConstrVerticesX[j] = m_lxcoefsRight[j]*m_RightFootSize.getHalfWidth();
m_RightZMPConstrVerticesY[j] = m_lycoefsRight[j]*m_RightFootSize.getHalfHeight();
m_ZMPPosCstr.rightSS.X[j] = m_lxcoefsRight[j]*m_RightFootSize.getHalfWidth();
m_ZMPPosCstr.rightSS.Y[j] = m_lycoefsRight[j]*m_RightFootSize.getHalfHeight();
//Left DS phase
m_LeftDSZMPConstrVerticesX[j] = m_lxcoefsLeft[j]*m_LeftFootSize.getHalfWidth();
m_LeftDSZMPConstrVerticesY[j] = m_lycoefsLeft[j]*m_LeftFootSize.getHalfHeightDS()-m_DSFeetDistance/2.0;
m_ZMPPosCstr.leftDS.X[j] = m_lxcoefsLeft[j]*m_LeftFootSize.getHalfWidth();
m_ZMPPosCstr.leftDS.Y[j] = m_lycoefsLeft[j]*m_LeftFootSize.getHalfHeightDS()-m_DSFeetDistance/2.0;
//Right DS phase
m_RightDSZMPConstrVerticesX[j] = m_lxcoefsRight[j]*m_RightFootSize.getHalfWidth();
m_RightDSZMPConstrVerticesY[j] = m_lycoefsRight[j]*m_RightFootSize.getHalfHeightDS()+m_DSFeetDistance/2.0;
m_ZMPPosCstr.rightDS.X[j] = m_lxcoefsRight[j]*m_RightFootSize.getHalfWidth();
m_ZMPPosCstr.rightDS.Y[j] = m_lycoefsRight[j]*m_RightFootSize.getHalfHeightDS()+m_DSFeetDistance/2.0;
}
return 0;
......@@ -132,7 +136,7 @@ FootConstraintsAsLinearSystemForVelRef::setFeetDimensions( CjrlHumanoidDynamicRo
m_LeftFoot = aHS->leftFoot();
if (m_RightFoot==0)
{
cerr << "Problem with the reading of the left foot"<< endl;
cerr << "Problem while reading of the left foot"<< endl;
return 0;
}
m_LeftFoot->getSoleSize( lHalfWidthInit,lHalfHeightInit );
......@@ -149,64 +153,33 @@ FootConstraintsAsLinearSystemForVelRef::setFeetDimensions( CjrlHumanoidDynamicRo
int
FootConstraintsAsLinearSystemForVelRef::setVertices( std::vector<CH_Point> & ZMPConstrVertices,
std::vector<CH_Point> & FeetPosConstrVertices,
double & ZMPConvHullAngle,
double & FeetPosConvHullAngle,
support_state_t & PrwSupport)
FootConstraintsAsLinearSystemForVelRef::setVertices( convex_hull_t & ZMPConstrVertices,
convex_hull_t & FeetPosConstrVertices,
double & ZMPConvHullAngle,
double & FeetPosConvHullAngle,
support_state_t & PrwSupport)
{
//compute the cos and sin of the angle
double c_ZMP = cos(ZMPConvHullAngle);
double s_ZMP = sin(ZMPConvHullAngle);
double c_FP = cos(FeetPosConvHullAngle);
double s_FP = sin(FeetPosConvHullAngle);
//Prepare the computation of the convex hull
if( PrwSupport.Foot == 1 )
{
m_FPosConstrVerticesX = m_LeftFPosConstrVerticesX;
m_FPosConstrVerticesY = m_LeftFPosConstrVerticesY;
FeetPosConstrVertices = m_FootPosCstr.left;
if( PrwSupport.Phase == 0 )
{
m_ZMPConstrVerticesX = m_LeftDSZMPConstrVerticesX;
m_ZMPConstrVerticesY = m_LeftDSZMPConstrVerticesY;
}
ZMPConstrVertices = m_ZMPPosCstr.leftDS;
else
{
m_ZMPConstrVerticesX = m_LeftZMPConstrVerticesX;
m_ZMPConstrVerticesY = m_LeftZMPConstrVerticesY;
}
ZMPConstrVertices = m_ZMPPosCstr.leftSS;
}
else
{
m_FPosConstrVerticesX = m_RightFPosConstrVerticesX;
m_FPosConstrVerticesY = m_RightFPosConstrVerticesY;
FeetPosConstrVertices = m_FootPosCstr.right;
if( PrwSupport.Phase == 0 )
{
m_ZMPConstrVerticesX = m_RightDSZMPConstrVerticesX;
m_ZMPConstrVerticesY = m_RightDSZMPConstrVerticesY;
}
ZMPConstrVertices = m_ZMPPosCstr.rightDS;
else
{
m_ZMPConstrVerticesX = m_RightZMPConstrVerticesX;
m_ZMPConstrVerticesY = m_RightZMPConstrVerticesY;
}
ZMPConstrVertices = m_ZMPPosCstr.rightSS;
}
//Set the convex hull of the ZMP constraints
for( int j=0;j<4;j++ )
{
ZMPConstrVertices[j].col = ( m_ZMPConstrVerticesX[j] * c_ZMP - m_ZMPConstrVerticesY[j] * s_ZMP );
ZMPConstrVertices[j].row = ( m_ZMPConstrVerticesX[j] * s_ZMP + m_ZMPConstrVerticesY[j] * c_ZMP );
}
//Set the convex hull of the feet constraints
//TODO: The interior border does not yet depend on the angle
for( int j=0;j<5;j++ )
{
FeetPosConstrVertices[j].col = ( m_FPosConstrVerticesX[j] * c_FP - m_FPosConstrVerticesY[j] * s_FP );
FeetPosConstrVertices[j].row = ( m_FPosConstrVerticesX[j] * s_FP + m_FPosConstrVerticesY[j] * c_FP );
}
ZMPConstrVertices.rotate(ZMPConvHullAngle);
FeetPosConstrVertices.rotate(FeetPosConvHullAngle);
return 0;
......@@ -215,24 +188,24 @@ FootConstraintsAsLinearSystemForVelRef::setVertices( std::vector<CH_Point> & ZMP
int
FootConstraintsAsLinearSystemForVelRef::computeLinearSystem( vector<CH_Point> & aVecOfPoints,
FootConstraintsAsLinearSystemForVelRef::computeLinearSystem( convex_hull_t & ConvexHull,
MAL_MATRIX(&D,double),
MAL_MATRIX(&Dc,double),
support_state_t & PrwSupport )
{
double dx,dy,dc,x1,y1,x2,y2;
unsigned int n = aVecOfPoints.size();
MAL_MATRIX_RESIZE( D,aVecOfPoints.size(),2 );
MAL_MATRIX_RESIZE( Dc,aVecOfPoints.size(),1 );
unsigned int n = ConvexHull.X.size();
MAL_MATRIX_RESIZE( D,ConvexHull.X.size(),2 );
MAL_MATRIX_RESIZE( Dc,ConvexHull.X.size(),1 );
for( unsigned int i=0;i<n-1;i++ )//first n-1 inequalities
{
y1 = aVecOfPoints[i].row;
y2 = aVecOfPoints[i+1].row;
x1 = aVecOfPoints[i].col;
x2 = aVecOfPoints[i+1].col;
y1 = ConvexHull.Y[i];
y2 = ConvexHull.Y[i+1];
x1 = ConvexHull.X[i];
x2 = ConvexHull.X[i+1];
dx = y1-y2;
dy = x2-x1;
......@@ -251,10 +224,10 @@ FootConstraintsAsLinearSystemForVelRef::computeLinearSystem( vector<CH_Point> &
//Last inequality
unsigned int i = n-1;
y1 = aVecOfPoints[i].row;
y2 = aVecOfPoints[0].row;
x1 = aVecOfPoints[i].col;
x2 = aVecOfPoints[0].col;
y1 = ConvexHull.Y[i];
y2 = ConvexHull.Y[0];
x1 = ConvexHull.X[i];
x2 = ConvexHull.X[0];
dx = y1-y2;
dy = x2-x1;
......@@ -274,98 +247,6 @@ FootConstraintsAsLinearSystemForVelRef::computeLinearSystem( vector<CH_Point> &
}
int
FootConstraintsAsLinearSystemForVelRef::buildConstraintInequalities( deque< FootAbsolutePosition> &LeftFootAbsolutePositions,
deque<FootAbsolutePosition> &RightFootAbsolutePositions,
deque<linear_inequality_ff_t> & ZMPInequalitiesDeque,
deque<linear_inequality_ff_t> & FeetPosInequalitiesDeque,
reference_t & RefVel, double StartingTime, double QP_N,
SupportFSM * SupportFSM, support_state_t & CurrentSupport, support_state_t & PrwSupport,
deque<double> &PreviewedSupportAngles, int & NbConstraints )
{
vector<CH_Point> ZMPConstrVertices;
ZMPConstrVertices.resize(4);
vector<CH_Point> FeetPosConstrVertices;
FeetPosConstrVertices.resize(5);
double s_t=0,c_t=0;
double lx = 0.0,ly =0.0;
//determine the current support angle
deque<FootAbsolutePosition>::iterator FAP_it;
//define the current support angle
if( CurrentSupport.Foot==1 )
{
FAP_it = LeftFootAbsolutePositions.end();
FAP_it--;
}
else
{
FAP_it = RightFootAbsolutePositions.end();
FAP_it--;
}
double CurrentSupportAngle = FAP_it->theta*M_PI/180.0;
//initialize the previewed support state before previewing
PrwSupport.Phase = CurrentSupport.Phase;
PrwSupport.Foot = CurrentSupport.Foot;
PrwSupport.StepsLeft = CurrentSupport.StepsLeft;
PrwSupport.TimeLimit = CurrentSupport.TimeLimit;
PrwSupport.StepNumber = 0;
double ZMPConvHullOrientation = CurrentSupportAngle;
double FPConvHullOrientation = CurrentSupportAngle;
//set current constraints
setVertices( ZMPConstrVertices, FeetPosConstrVertices,
ZMPConvHullOrientation, FPConvHullOrientation,
PrwSupport );
//set constraints for the whole preview window
for( int i=1;i<=QP_N;i++ )
{
SupportFSM->setSupportState( StartingTime, i, PrwSupport, RefVel );
if( PrwSupport.StateChanged )
setVertices( ZMPConstrVertices, FeetPosConstrVertices,
ZMPConvHullOrientation, FPConvHullOrientation, PrwSupport );
if( PrwSupport.StateChanged && PrwSupport.StepNumber>0 )
FPConvHullOrientation = PreviewedSupportAngles[PrwSupport.StepNumber-1];
//foot positioning constraints
if( PrwSupport.StateChanged && PrwSupport.StepNumber>0 && PrwSupport.Phase != 0)
{
ZMPConvHullOrientation = PreviewedSupportAngles[PrwSupport.StepNumber-1];
if( PrwSupport.StepNumber==1 )
FPConvHullOrientation = CurrentSupportAngle;
else
FPConvHullOrientation = PreviewedSupportAngles[PrwSupport.StepNumber-2];
setVertices( ZMPConstrVertices, FeetPosConstrVertices,
ZMPConvHullOrientation, FPConvHullOrientation, PrwSupport );
linear_inequality_ff_t aLCIFP;
computeLinearSystem( FeetPosConstrVertices, aLCIFP.D, aLCIFP.Dc, PrwSupport );
aLCIFP.StepNumber = PrwSupport.StepNumber;
FeetPosInequalitiesDeque.push_back( aLCIFP );
NbConstraints += MAL_MATRIX_NB_ROWS( aLCIFP.D );
}
linear_inequality_ff_t aLCI;
computeLinearSystem( ZMPConstrVertices, aLCI.D, aLCI.Dc, PrwSupport );
aLCI.StepNumber = PrwSupport.StepNumber;
ZMPInequalitiesDeque.push_back( aLCI );
NbConstraints += MAL_MATRIX_NB_ROWS( aLCI.D );
}
return 0;
}
void
FootConstraintsAsLinearSystemForVelRef::CallMethod( std::string &Method, std::istringstream &Args )
{
......
src/Mathematics/FootConstraintsAsLinearSystemForVelRef.h
+ 27
− 64
View file @ 12a76121
......@@ -68,38 +68,6 @@ namespace PatternGeneratorJRL
~FootConstraintsAsLinearSystemForVelRef ();
/// \}
/// \brief Build a queue of inequalities
///
/// \param LeftFootAbsolutePositions used to get the current position of the left foot
/// \param RightFootAbsolutePositions used to get the current position of the right foot
/// \param ZMPInequalitiesDeque constraints on the ZMP relative to the foot centers
/// \param FeetPosInequalitiesDeque constraints on the feet placements relative to previous foot placements
/// \param RefVel the velocity reference
/// \param CurrentTime
/// \param QP_N number of instants in the preview horizon
/// \param SupportFSM the finite state machine for the preview of support states
/// \param CurrentSupport
/// \param PrwSupport output of SupportFSM
/// \param PreviewedSupportAngles deque of support orientations previewed previously
/// \param NbOfConstraints total number of previewed constraints
int buildConstraintInequalities (std::deque < FootAbsolutePosition >
&LeftFootAbsolutePositions,
std::deque < FootAbsolutePosition >
&RightFootAbsolutePositions,
std::deque <
linear_inequality_ff_t >
&ZMPInequalitiesDeque,
std::deque <
linear_inequality_ff_t >
&FeetPosInequalitiesDeque,
reference_t & RefVel,
double CurrentTime, double QP_N,
SupportFSM * SupportFSM,
support_state_t & CurrentSupport,
support_state_t & PrwSupport,
std::deque <
double >&PreviewedSupportAngles,
int &NbOfConstraints);
/// \brief Adapt vertices to the support foot and its orientation
///
......@@ -109,11 +77,11 @@ namespace PatternGeneratorJRL
/// \param FeetPosConvHullOrientation
/// \param PrwSupport previewed support state
/// \return 0
int setVertices (std::vector < CH_Point > & ZMPConstrVertices,
std::vector < CH_Point > & FeetPosConstrVertices,
double & ZMPConvHullOrientation,
double & FeetPosConvHullOrientation,
support_state_t & PrwSupport);
int setVertices( convex_hull_t & ZMPConstrVertices,
convex_hull_t & FeetPosConstrVertices,
double & ZMPConvHullAngle,
double & FeetPosConvHullAngle,
support_state_t & PrwSupport);
/// \brief Compute the linear inequalities \f${\bf A}{\bf x} \geq {\bf b}\f$ associated with the
/// convex hull specified by a vector of points.
......@@ -123,7 +91,7 @@ namespace PatternGeneratorJRL
/// \param Dc right hand side of the inequalities
/// \param PrwSupport previewed support state
/// \return 0
int computeLinearSystem (std::vector < CH_Point > & aVecOfPoints,
int computeLinearSystem (convex_hull_t & ConvexHull,
MAL_MATRIX (&D, double),
MAL_MATRIX (&Dc, double),
support_state_t & PrwSupport);
......@@ -163,31 +131,30 @@ namespace PatternGeneratorJRL
//
private:
/// \brief Reference to the Humanoid Specificities.
CjrlHumanoidDynamicRobot * m_HS;
/// \brief convex hull
std::vector < CH_Point > m_ConvexHullVertices;
CjrlHumanoidDynamicRobot * m_HS;
/// \brief Vertices defining the constraints on the feet positions
double *m_FPosConstrVerticesX;
double *m_FPosConstrVerticesY;
double m_LeftFPosConstrVerticesX[5];
double m_RightFPosConstrVerticesX[5];
double m_LeftFPosConstrVerticesY[5];
double m_RightFPosConstrVerticesY[5];
/// \brief Vertices defining the constraints on the zmp positions
double *m_ZMPConstrVerticesX;
double *m_ZMPConstrVerticesY;
double m_LeftZMPConstrVerticesX[4];
double m_RightZMPConstrVerticesX[4];
double m_LeftZMPConstrVerticesY[4];
double m_RightZMPConstrVerticesY[4];
double m_LeftDSZMPConstrVerticesX[4];
double m_RightDSZMPConstrVerticesX[4];
double m_LeftDSZMPConstrVerticesY[4];
double m_RightDSZMPConstrVerticesY[4];
struct foot_pos_edges_s
{
convex_hull_t left, right;
};
struct foot_pos_edges_s m_FootPosCstr;
struct zmp_pos_edges_s
{
convex_hull_t
leftSS,
rightSS,
rightDS,
leftDS;
};
struct zmp_pos_edges_s m_ZMPPosCstr;
/// \brief Some coefficients
///
......@@ -207,11 +174,7 @@ namespace PatternGeneratorJRL
/// \brief Distance between the feet in the double support phase
double m_DSFeetDistance;
/// \brief Foot specificities.
CjrlFoot *m_RightFoot;
CjrlFoot *m_LeftFoot;
unsigned int m_FullDebug;
CjrlFoot * m_RightFoot, * m_LeftFoot;
};
}
......
src/ZMPRefTrajectoryGeneration/generator-vel-ref.cpp
+ 7
− 10
View file @ 12a76121
......@@ -263,10 +263,8 @@ GeneratorVelRef::buildConstraintInequalities( std::deque< FootAbsolutePosition>
FootConstraintsAsLinearSystemForVelRef * FCALS)
{
std::vector<CH_Point> ZMPConstrVertices;
ZMPConstrVertices.resize(4);
std::vector<CH_Point> FeetPosConstrVertices;
FeetPosConstrVertices.resize(5);
convex_hull_t ZMPFeasibilityEdges;
convex_hull_t FeetFeasibilityEdges;
//determine the current support angle
std::deque<FootAbsolutePosition>::iterator FAP_it;
......@@ -290,18 +288,17 @@ GeneratorVelRef::buildConstraintInequalities( std::deque< FootAbsolutePosition>
double FPConvHullOrientation = CurrentSupportAngle;
//set current constraints
FCALS->setVertices( ZMPConstrVertices, FeetPosConstrVertices,
FCALS->setVertices( ZMPFeasibilityEdges, FeetFeasibilityEdges,
ZMPConvHullOrientation, FPConvHullOrientation,
CurrentSupport );
//set constraints for the whole preview window
for( int i=1;i<=m_N;i++ )
{
support_state_t & PrwSupport = deqSupportStates[i];
if( PrwSupport.StateChanged )
FCALS->setVertices( ZMPConstrVertices, FeetPosConstrVertices,
FCALS->setVertices( ZMPFeasibilityEdges, FeetFeasibilityEdges,
ZMPConvHullOrientation, FPConvHullOrientation, PrwSupport );
if( PrwSupport.StateChanged && PrwSupport.StepNumber>0 )
......@@ -317,18 +314,18 @@ GeneratorVelRef::buildConstraintInequalities( std::deque< FootAbsolutePosition>
else
FPConvHullOrientation = PreviewedSupportAngles[PrwSupport.StepNumber-2];
FCALS->setVertices( ZMPConstrVertices, FeetPosConstrVertices,
FCALS->setVertices( ZMPFeasibilityEdges, FeetFeasibilityEdges,
ZMPConvHullOrientation, FPConvHullOrientation, PrwSupport );
linear_inequality_ff_t aLCIFP;
FCALS->computeLinearSystem( FeetPosConstrVertices, aLCIFP.D, aLCIFP.Dc, PrwSupport );
FCALS->computeLinearSystem( FeetFeasibilityEdges, aLCIFP.D, aLCIFP.Dc, PrwSupport );
aLCIFP.StepNumber = PrwSupport.StepNumber;
FeetPosInequalitiesDeque.push_back( aLCIFP );
NbConstraints += MAL_MATRIX_NB_ROWS( aLCIFP.D );
}
linear_inequality_ff_t aLCI;
FCALS->computeLinearSystem( ZMPConstrVertices, aLCI.D, aLCI.Dc, PrwSupport );
FCALS->computeLinearSystem( ZMPFeasibilityEdges, aLCI.D, aLCI.Dc, PrwSupport );
aLCI.StepNumber = PrwSupport.StepNumber;
ZMPInequalitiesDeque.push_back( aLCI );
NbConstraints += MAL_MATRIX_NB_ROWS( aLCI.D );
......
0% or .
You are about to add 0 people to the discussion. Proceed with caution.
Finish editing this message first!
Please register or to comment