diff --git a/src/Mathematics/relative-feet-inequalities.cpp b/src/Mathematics/relative-feet-inequalities.cpp index eb111858a4778bf6f5ad87aa990612eec9903cb2..4b5d453fa05ed721798a7c7672046de55cd5b9f1 100644 --- a/src/Mathematics/relative-feet-inequalities.cpp +++ b/src/Mathematics/relative-feet-inequalities.cpp @@ -85,14 +85,14 @@ RelativeFeetInequalities::init_convex_hulls() FootPosEdges_.LeftDS.resize(5); FootPosEdges_.LeftSS.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}; + double LeftFPosEdgesY[5] = {-0.2, -0.3, -0.4, -0.3, -0.2}; FootPosEdges_.LeftDS.set(LeftFPosEdgesX,LeftFPosEdgesY); FootPosEdges_.LeftSS.set(LeftFPosEdgesX,LeftFPosEdgesY); FootPosEdges_.RightDS.resize(5); FootPosEdges_.RightSS.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}; + double RightFPosEdgesY[5] = {0.2, 0.3, 0.4, 0.3, 0.2}; FootPosEdges_.RightDS.set(RightFPosEdgesX,RightFPosEdgesY); FootPosEdges_.RightSS.set(RightFPosEdgesX,RightFPosEdgesY); @@ -155,40 +155,39 @@ RelativeFeetInequalities::set_feet_dimensions( CjrlHumanoidDynamicRobot *aHS ) int RelativeFeetInequalities::set_vertices( convex_hull_t & ConvexHull, - double Orientation, - const support_state_t & PrwSupport, - int Type) + const support_state_t & Support, + ineq_e type) { edges_s * ConvexHull_p = 0; - switch(Type) + switch(type) { - case ZMP_CONSTRAINTS: + case INEQ_COP: ConvexHull_p = & ZMPPosEdges_; break; - case FOOT_CONSTRAINTS: + case INEQ_FEET: ConvexHull_p = & FootPosEdges_; break; } //Prepare the computation of the convex hull - if( PrwSupport.Foot == LEFT ) + if( Support.Foot == LEFT ) { - if( PrwSupport.Phase == DS ) + if( Support.Phase == DS ) ConvexHull = ConvexHull_p->LeftDS; else ConvexHull = ConvexHull_p->LeftSS; } else { - if( PrwSupport.Phase == DS ) + if( Support.Phase == DS ) ConvexHull = ConvexHull_p->RightDS; else ConvexHull = ConvexHull_p->RightSS; } - ConvexHull.rotate(Orientation); + ConvexHull.rotate(Support.Yaw); return 0; diff --git a/src/Mathematics/relative-feet-inequalities.hh b/src/Mathematics/relative-feet-inequalities.hh index 21f251c07c78e0670e217c825fc9cd9bd7f6e208..2634c2c761601057d698b09fa142886b706af15c 100644 --- a/src/Mathematics/relative-feet-inequalities.hh +++ b/src/Mathematics/relative-feet-inequalities.hh @@ -55,8 +55,6 @@ namespace PatternGeneratorJRL // Public types // public: - const static int ZMP_CONSTRAINTS = 0; - const static int FOOT_CONSTRAINTS = 1; // // Public member functions @@ -79,9 +77,8 @@ namespace PatternGeneratorJRL /// \param[in] Type Type /// \return 0 int set_vertices( convex_hull_t & ConvexHull, - double Orientation, const support_state_t & SupportState, - int Type); + ineq_e type); /// \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. diff --git a/src/ZMPRefTrajectoryGeneration/generator-vel-ref.cpp b/src/ZMPRefTrajectoryGeneration/generator-vel-ref.cpp index 7d9d94ef93ce8a6f9a690ffd2c25e4c883b814bd..6fc63b1bf47dce5668748dc8ef3a8ca873a090bc 100755 --- a/src/ZMPRefTrajectoryGeneration/generator-vel-ref.cpp +++ b/src/ZMPRefTrajectoryGeneration/generator-vel-ref.cpp @@ -221,7 +221,7 @@ GeneratorVelRef::build_inequalities_cop(linear_inequality_t & Inequalities, convex_hull_t ZMPFeasibilityEdges; RFI_->set_vertices( ZMPFeasibilityEdges, *prwSS_it, - RelativeFeetInequalities::ZMP_CONSTRAINTS ); + INEQ_COP ); const unsigned nbEdges = 4; double D_x[nbEdges] = {0.0, 0.0, 0.0, 0.0}; @@ -233,7 +233,7 @@ GeneratorVelRef::build_inequalities_cop(linear_inequality_t & Inequalities, { if( prwSS_it->StateChanged ) RFI_->set_vertices( ZMPFeasibilityEdges,*prwSS_it, - RelativeFeetInequalities::ZMP_CONSTRAINTS ); + INEQ_COP ); RFI_->compute_linear_system( ZMPFeasibilityEdges, D_x, D_y, dc, *prwSS_it ); @@ -275,7 +275,7 @@ GeneratorVelRef::build_inequalities_feet( linear_inequality_t & Inequalities, { prwSS_it--;//Take the support state before RFI_->set_vertices( FootFeasibilityEdges, *prwSS_it, - RelativeFeetInequalities::FOOT_CONSTRAINTS ); + INEQ_FEET ); prwSS_it++; RFI_->compute_linear_system( FootFeasibilityEdges, Dx, Dy, dc, *prwSS_it );