From 63e36f63bc1f1d5168c66d32b4466ecf36d76714 Mon Sep 17 00:00:00 2001
From: olivier-stasse <olivier.stasse@aist.go.jp>
Date: Fri, 11 Mar 2011 05:07:24 +0900
Subject: [PATCH] Revert "Simplifications due to the new structure"
This reverts commit 12a761218e714d3c88ace2b366af6cf9e6f31c28.
---
...FootConstraintsAsLinearSystemForVelRef.cpp | 231 +++++++++++++-----
.../FootConstraintsAsLinearSystemForVelRef.h | 91 +++++--
.../generator-vel-ref.cpp | 17 +-
3 files changed, 249 insertions(+), 90 deletions(-)
diff --git a/src/Mathematics/FootConstraintsAsLinearSystemForVelRef.cpp b/src/Mathematics/FootConstraintsAsLinearSystemForVelRef.cpp
index d114b762..29d4e030 100644
--- a/src/Mathematics/FootConstraintsAsLinearSystemForVelRef.cpp
+++ b/src/Mathematics/FootConstraintsAsLinearSystemForVelRef.cpp
@@ -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,34 +83,30 @@ 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_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_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.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_ZMPPosCstr.leftSS.X[j] = m_lxcoefsLeft[j]*m_LeftFootSize.getHalfWidth();
- m_ZMPPosCstr.leftSS.Y[j] = m_lycoefsLeft[j]*m_LeftFootSize.getHalfHeight();
+ m_LeftZMPConstrVerticesX[j] = m_lxcoefsLeft[j]*m_LeftFootSize.getHalfWidth();
+ m_LeftZMPConstrVerticesY[j] = m_lycoefsLeft[j]*m_LeftFootSize.getHalfHeight();
//Right single support phase
- m_ZMPPosCstr.rightSS.X[j] = m_lxcoefsRight[j]*m_RightFootSize.getHalfWidth();
- m_ZMPPosCstr.rightSS.Y[j] = m_lycoefsRight[j]*m_RightFootSize.getHalfHeight();
+ m_RightZMPConstrVerticesX[j] = m_lxcoefsRight[j]*m_RightFootSize.getHalfWidth();
+ m_RightZMPConstrVerticesY[j] = m_lycoefsRight[j]*m_RightFootSize.getHalfHeight();
//Left DS phase
- 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;
+ m_LeftDSZMPConstrVerticesX[j] = m_lxcoefsLeft[j]*m_LeftFootSize.getHalfWidth();
+ m_LeftDSZMPConstrVerticesY[j] = m_lycoefsLeft[j]*m_LeftFootSize.getHalfHeightDS()-m_DSFeetDistance/2.0;
//Right DS phase
- 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;
+ m_RightDSZMPConstrVerticesX[j] = m_lxcoefsRight[j]*m_RightFootSize.getHalfWidth();
+ m_RightDSZMPConstrVerticesY[j] = m_lycoefsRight[j]*m_RightFootSize.getHalfHeightDS()+m_DSFeetDistance/2.0;
}
return 0;
@@ -136,7 +132,7 @@ FootConstraintsAsLinearSystemForVelRef::setFeetDimensions( CjrlHumanoidDynamicRo
m_LeftFoot = aHS->leftFoot();
if (m_RightFoot==0)
{
- cerr << "Problem while reading of the left foot"<< endl;
+ cerr << "Problem with the reading of the left foot"<< endl;
return 0;
}
m_LeftFoot->getSoleSize( lHalfWidthInit,lHalfHeightInit );
@@ -153,33 +149,64 @@ FootConstraintsAsLinearSystemForVelRef::setFeetDimensions( CjrlHumanoidDynamicRo
int
-FootConstraintsAsLinearSystemForVelRef::setVertices( convex_hull_t & ZMPConstrVertices,
- convex_hull_t & FeetPosConstrVertices,
- double & ZMPConvHullAngle,
- double & FeetPosConvHullAngle,
- support_state_t & PrwSupport)
+FootConstraintsAsLinearSystemForVelRef::setVertices( std::vector<CH_Point> & ZMPConstrVertices,
+ std::vector<CH_Point> & 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 )
{
- FeetPosConstrVertices = m_FootPosCstr.left;
+ m_FPosConstrVerticesX = m_LeftFPosConstrVerticesX;
+ m_FPosConstrVerticesY = m_LeftFPosConstrVerticesY;
if( PrwSupport.Phase == 0 )
- ZMPConstrVertices = m_ZMPPosCstr.leftDS;
+ {
+ m_ZMPConstrVerticesX = m_LeftDSZMPConstrVerticesX;
+ m_ZMPConstrVerticesY = m_LeftDSZMPConstrVerticesY;
+ }
else
- ZMPConstrVertices = m_ZMPPosCstr.leftSS;
+ {
+ m_ZMPConstrVerticesX = m_LeftZMPConstrVerticesX;
+ m_ZMPConstrVerticesY = m_LeftZMPConstrVerticesY;
+ }
}
else
{
- FeetPosConstrVertices = m_FootPosCstr.right;
+ m_FPosConstrVerticesX = m_RightFPosConstrVerticesX;
+ m_FPosConstrVerticesY = m_RightFPosConstrVerticesY;
if( PrwSupport.Phase == 0 )
- ZMPConstrVertices = m_ZMPPosCstr.rightDS;
+ {
+ m_ZMPConstrVerticesX = m_RightDSZMPConstrVerticesX;
+ m_ZMPConstrVerticesY = m_RightDSZMPConstrVerticesY;
+ }
else
- ZMPConstrVertices = m_ZMPPosCstr.rightSS;
+ {
+ m_ZMPConstrVerticesX = m_RightZMPConstrVerticesX;
+ m_ZMPConstrVerticesY = m_RightZMPConstrVerticesY;
+ }
}
- ZMPConstrVertices.rotate(ZMPConvHullAngle);
- FeetPosConstrVertices.rotate(FeetPosConvHullAngle);
+ //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 );
+ }
return 0;
@@ -188,24 +215,24 @@ FootConstraintsAsLinearSystemForVelRef::setVertices( convex_hull_t & ZMPConstrVe
int
-FootConstraintsAsLinearSystemForVelRef::computeLinearSystem( convex_hull_t & ConvexHull,
+FootConstraintsAsLinearSystemForVelRef::computeLinearSystem( vector<CH_Point> & aVecOfPoints,
MAL_MATRIX(&D,double),
MAL_MATRIX(&Dc,double),
support_state_t & PrwSupport )
{
double dx,dy,dc,x1,y1,x2,y2;
- unsigned int n = ConvexHull.X.size();
- MAL_MATRIX_RESIZE( D,ConvexHull.X.size(),2 );
- MAL_MATRIX_RESIZE( Dc,ConvexHull.X.size(),1 );
+ unsigned int n = aVecOfPoints.size();
+ MAL_MATRIX_RESIZE( D,aVecOfPoints.size(),2 );
+ MAL_MATRIX_RESIZE( Dc,aVecOfPoints.size(),1 );
for( unsigned int i=0;i<n-1;i++ )//first n-1 inequalities
{
- y1 = ConvexHull.Y[i];
- y2 = ConvexHull.Y[i+1];
- x1 = ConvexHull.X[i];
- x2 = ConvexHull.X[i+1];
+ y1 = aVecOfPoints[i].row;
+ y2 = aVecOfPoints[i+1].row;
+ x1 = aVecOfPoints[i].col;
+ x2 = aVecOfPoints[i+1].col;
dx = y1-y2;
dy = x2-x1;
@@ -224,10 +251,10 @@ FootConstraintsAsLinearSystemForVelRef::computeLinearSystem( convex_hull_t & Con
//Last inequality
unsigned int i = n-1;
- y1 = ConvexHull.Y[i];
- y2 = ConvexHull.Y[0];
- x1 = ConvexHull.X[i];
- x2 = ConvexHull.X[0];
+ y1 = aVecOfPoints[i].row;
+ y2 = aVecOfPoints[0].row;
+ x1 = aVecOfPoints[i].col;
+ x2 = aVecOfPoints[0].col;
dx = y1-y2;
dy = x2-x1;
@@ -247,6 +274,98 @@ FootConstraintsAsLinearSystemForVelRef::computeLinearSystem( convex_hull_t & Con
}
+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 )
{
diff --git a/src/Mathematics/FootConstraintsAsLinearSystemForVelRef.h b/src/Mathematics/FootConstraintsAsLinearSystemForVelRef.h
index 431f40d1..51890dc2 100644
--- a/src/Mathematics/FootConstraintsAsLinearSystemForVelRef.h
+++ b/src/Mathematics/FootConstraintsAsLinearSystemForVelRef.h
@@ -68,6 +68,38 @@ 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
///
@@ -77,11 +109,11 @@ namespace PatternGeneratorJRL
/// \param FeetPosConvHullOrientation
/// \param PrwSupport previewed support state
/// \return 0
- int setVertices( convex_hull_t & ZMPConstrVertices,
- convex_hull_t & FeetPosConstrVertices,
- double & ZMPConvHullAngle,
- double & FeetPosConvHullAngle,
- support_state_t & PrwSupport);
+ int setVertices (std::vector < CH_Point > & ZMPConstrVertices,
+ std::vector < CH_Point > & FeetPosConstrVertices,
+ double & ZMPConvHullOrientation,
+ double & FeetPosConvHullOrientation,
+ 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.
@@ -91,7 +123,7 @@ namespace PatternGeneratorJRL
/// \param Dc right hand side of the inequalities
/// \param PrwSupport previewed support state
/// \return 0
- int computeLinearSystem (convex_hull_t & ConvexHull,
+ int computeLinearSystem (std::vector < CH_Point > & aVecOfPoints,
MAL_MATRIX (&D, double),
MAL_MATRIX (&Dc, double),
support_state_t & PrwSupport);
@@ -131,30 +163,31 @@ namespace PatternGeneratorJRL
//
private:
/// \brief Reference to the Humanoid Specificities.
- CjrlHumanoidDynamicRobot * m_HS;
+ CjrlHumanoidDynamicRobot * m_HS;
+
+ /// \brief convex hull
+ std::vector < CH_Point > m_ConvexHullVertices;
+
/// \brief Vertices defining the constraints on the feet positions
double *m_FPosConstrVerticesX;
double *m_FPosConstrVerticesY;
-
- 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;
-
+ 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];
/// \brief Some coefficients
///
@@ -174,7 +207,11 @@ namespace PatternGeneratorJRL
/// \brief Distance between the feet in the double support phase
double m_DSFeetDistance;
- CjrlFoot * m_RightFoot, * m_LeftFoot;
+ /// \brief Foot specificities.
+ CjrlFoot *m_RightFoot;
+ CjrlFoot *m_LeftFoot;
+
+ unsigned int m_FullDebug;
};
}
diff --git a/src/ZMPRefTrajectoryGeneration/generator-vel-ref.cpp b/src/ZMPRefTrajectoryGeneration/generator-vel-ref.cpp
index 92cb88ab..ba8e7b39 100644
--- a/src/ZMPRefTrajectoryGeneration/generator-vel-ref.cpp
+++ b/src/ZMPRefTrajectoryGeneration/generator-vel-ref.cpp
@@ -263,8 +263,10 @@ GeneratorVelRef::buildConstraintInequalities( std::deque< FootAbsolutePosition>
FootConstraintsAsLinearSystemForVelRef * FCALS)
{
- convex_hull_t ZMPFeasibilityEdges;
- convex_hull_t FeetFeasibilityEdges;
+ std::vector<CH_Point> ZMPConstrVertices;
+ ZMPConstrVertices.resize(4);
+ std::vector<CH_Point> FeetPosConstrVertices;
+ FeetPosConstrVertices.resize(5);
//determine the current support angle
std::deque<FootAbsolutePosition>::iterator FAP_it;
@@ -288,17 +290,18 @@ GeneratorVelRef::buildConstraintInequalities( std::deque< FootAbsolutePosition>
double FPConvHullOrientation = CurrentSupportAngle;
//set current constraints
- FCALS->setVertices( ZMPFeasibilityEdges, FeetFeasibilityEdges,
+ FCALS->setVertices( ZMPConstrVertices, FeetPosConstrVertices,
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( ZMPFeasibilityEdges, FeetFeasibilityEdges,
+ FCALS->setVertices( ZMPConstrVertices, FeetPosConstrVertices,
ZMPConvHullOrientation, FPConvHullOrientation, PrwSupport );
if( PrwSupport.StateChanged && PrwSupport.StepNumber>0 )
@@ -314,18 +317,18 @@ GeneratorVelRef::buildConstraintInequalities( std::deque< FootAbsolutePosition>
else
FPConvHullOrientation = PreviewedSupportAngles[PrwSupport.StepNumber-2];
- FCALS->setVertices( ZMPFeasibilityEdges, FeetFeasibilityEdges,
+ FCALS->setVertices( ZMPConstrVertices, FeetPosConstrVertices,
ZMPConvHullOrientation, FPConvHullOrientation, PrwSupport );
linear_inequality_ff_t aLCIFP;
- FCALS->computeLinearSystem( FeetFeasibilityEdges, aLCIFP.D, aLCIFP.Dc, PrwSupport );
+ FCALS->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;
- FCALS->computeLinearSystem( ZMPFeasibilityEdges, aLCI.D, aLCI.Dc, PrwSupport );
+ FCALS->computeLinearSystem( ZMPConstrVertices, aLCI.D, aLCI.Dc, PrwSupport );
aLCI.StepNumber = PrwSupport.StepNumber;
ZMPInequalitiesDeque.push_back( aLCI );
NbConstraints += MAL_MATRIX_NB_ROWS( aLCI.D );
--
GitLab