diff --git a/src/Mathematics/relative-feet-inequalities.cpp b/src/Mathematics/relative-feet-inequalities.cpp
index 4b5d453fa05ed721798a7c7672046de55cd5b9f1..64f4f2312359c0556c31fce8b121a1e8e7882f68 100644
--- a/src/Mathematics/relative-feet-inequalities.cpp
+++ b/src/Mathematics/relative-feet-inequalities.cpp
@@ -103,17 +103,17 @@ RelativeFeetInequalities::init_convex_hulls()
for( unsigned j=0;j<4;j++ )
{
//Left single support phase
- ZMPPosEdges_.LeftSS.X[j] = lxcoefsLeft[j]*LeftFootSize_.getHalfWidth();
- ZMPPosEdges_.LeftSS.Y[j] = lycoefsLeft[j]*LeftFootSize_.getHalfHeight();
+ ZMPPosEdges_.LeftSS.X_vec[j] = lxcoefsLeft[j]*LeftFootSize_.getHalfWidth();
+ ZMPPosEdges_.LeftSS.Y_vec[j] = lycoefsLeft[j]*LeftFootSize_.getHalfHeight();
//Right single support phase
- ZMPPosEdges_.RightSS.X[j] = lxcoefsRight[j]*RightFootSize_.getHalfWidth();
- ZMPPosEdges_.RightSS.Y[j] = lycoefsRight[j]*RightFootSize_.getHalfHeight();
+ ZMPPosEdges_.RightSS.X_vec[j] = lxcoefsRight[j]*RightFootSize_.getHalfWidth();
+ ZMPPosEdges_.RightSS.Y_vec[j] = lycoefsRight[j]*RightFootSize_.getHalfHeight();
//Left DS phase
- ZMPPosEdges_.LeftDS.X[j] = lxcoefsLeft[j]*LeftFootSize_.getHalfWidth();
- ZMPPosEdges_.LeftDS.Y[j] = lycoefsLeft[j]*LeftFootSize_.getHalfHeightDS()-DSFeetDistance_/2.0;
+ ZMPPosEdges_.LeftDS.X_vec[j] = lxcoefsLeft[j]*LeftFootSize_.getHalfWidth();
+ ZMPPosEdges_.LeftDS.Y_vec[j] = lycoefsLeft[j]*LeftFootSize_.getHalfHeightDS()-DSFeetDistance_/2.0;
//Right DS phase
- ZMPPosEdges_.RightDS.X[j] = lxcoefsRight[j]*RightFootSize_.getHalfWidth();
- ZMPPosEdges_.RightDS.Y[j] = lycoefsRight[j]*RightFootSize_.getHalfHeightDS()+DSFeetDistance_/2.0;
+ ZMPPosEdges_.RightDS.X_vec[j] = lxcoefsRight[j]*RightFootSize_.getHalfWidth();
+ ZMPPosEdges_.RightDS.Y_vec[j] = lycoefsRight[j]*RightFootSize_.getHalfHeightDS()+DSFeetDistance_/2.0;
}
return 0;
@@ -171,20 +171,33 @@ RelativeFeetInequalities::set_vertices( convex_hull_t & ConvexHull,
break;
}
- //Prepare the computation of the convex hull
+ //Choose edges
if( Support.Foot == LEFT )
{
if( Support.Phase == DS )
- ConvexHull = ConvexHull_p->LeftDS;
+ {
+ ConvexHull.X_vec = ConvexHull_p->LeftDS.X_vec;
+ ConvexHull.Y_vec = ConvexHull_p->LeftDS.Y_vec;
+ }
else
- ConvexHull = ConvexHull_p->LeftSS;
+ {
+ ConvexHull.Y_vec = ConvexHull_p->LeftSS.X_vec;
+ ConvexHull.Y_vec = ConvexHull_p->LeftSS.Y_vec;
+ }
+
}
else
{
if( Support.Phase == DS )
- ConvexHull = ConvexHull_p->RightDS;
+ {
+ ConvexHull.Y_vec = ConvexHull_p->RightDS.X_vec;
+ ConvexHull.Y_vec = ConvexHull_p->RightDS.Y_vec;
+ }
else
- ConvexHull = ConvexHull_p->RightSS;
+ {
+ ConvexHull.Y_vec = ConvexHull_p->RightSS.X_vec;
+ ConvexHull.Y_vec = ConvexHull_p->RightSS.Y_vec;
+ }
}
ConvexHull.rotate(Support.Yaw);
@@ -194,124 +207,123 @@ RelativeFeetInequalities::set_vertices( convex_hull_t & ConvexHull,
}
-int
-RelativeFeetInequalities::compute_linear_system( const convex_hull_t & ConvexHull,
- MAL_MATRIX(&D,double),
- MAL_MATRIX(&Dc,double),
- const support_state_t & PrwSupport ) const
-{
-
- double dx,dy,dc,x1,y1,x2,y2;
- unsigned n = ConvexHull.X.size();
- MAL_MATRIX_RESIZE( D,ConvexHull.X.size(),2 );
- MAL_MATRIX_RESIZE( Dc,ConvexHull.X.size(),1 );
-
- double Sign;
- if(PrwSupport.Foot == LEFT)
- Sign = 1.0;
- else
- Sign = -1.0;
- for( unsigned 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];
-
- dx = y1-y2;
- dy = x2-x1;
- dc = dx*x1+dy*y1;
-
- //symmetrical constraints
- dx = Sign*dx;
- dy = Sign*dy;
- dc = Sign*dc;
-
- D(i,0) = dx; D(i,1)= dy;
- Dc(i,0) = dc;
- }
-
- {
- //Last inequality
- unsigned i = n-1;
-
- y1 = ConvexHull.Y[i];
- y2 = ConvexHull.Y[0];
- x1 = ConvexHull.X[i];
- x2 = ConvexHull.X[0];
-
- dx = y1-y2;
- dy = x2-x1;
- dc = dx*x1+dy*y1;
-
- //for symmetrical constraints
- dx = Sign*dx;
- dy = Sign*dy;
- dc = Sign*dc;
-
- D(i,0) = dx; D(i,1)= dy;
- Dc(i,0) = dc;
- }
-
- return 0;
-
-}
+//int
+//RelativeFeetInequalities::compute_linear_system( const convex_hull_t & ConvexHull,
+// MAL_MATRIX(&D,double),
+// MAL_MATRIX(&Dc,double),
+// const support_state_t & PrwSupport ) const
+//{
+//
+// double dx,dy,dc,x1,y1,x2,y2;
+// unsigned n = ConvexHull.X.size();
+// MAL_MATRIX_RESIZE( D,ConvexHull.X.size(),2 );
+// MAL_MATRIX_RESIZE( Dc,ConvexHull.X.size(),1 );
+//
+// double Sign;
+// if(PrwSupport.Foot == LEFT)
+// Sign = 1.0;
+// else
+// Sign = -1.0;
+// for( unsigned 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];
+//
+// dx = y1-y2;
+// dy = x2-x1;
+// dc = dx*x1+dy*y1;
+//
+// //symmetrical constraints
+// dx = Sign*dx;
+// dy = Sign*dy;
+// dc = Sign*dc;
+//
+// D(i,0) = dx; D(i,1)= dy;
+// Dc(i,0) = dc;
+// }
+//
+// {
+// //Last inequality
+// unsigned i = n-1;
+//
+// y1 = ConvexHull.Y[i];
+// y2 = ConvexHull.Y[0];
+// x1 = ConvexHull.X[i];
+// x2 = ConvexHull.X[0];
+//
+// dx = y1-y2;
+// dy = x2-x1;
+// dc = dx*x1+dy*y1;
+//
+// //for symmetrical constraints
+// dx = Sign*dx;
+// dy = Sign*dy;
+// dc = Sign*dc;
+//
+// D(i,0) = dx; D(i,1)= dy;
+// Dc(i,0) = dc;
+// }
+//
+// return 0;
+//
+//}
int
-RelativeFeetInequalities::compute_linear_system (const convex_hull_t & ConvexHull,
- double * Dx, double * Dy, double * Dc,
- const support_state_t & PrwSupport) const
+RelativeFeetInequalities::compute_linear_system ( const convex_hull_t & ConvexHull,
+ const support_state_t & PrwSupport ) const
{
double dx,dy,dc,x1,y1,x2,y2;
unsigned nrows = ConvexHull.X.size();
- double Sign;
- if(PrwSupport.Foot == LEFT)
- Sign = 1.0;
+ double sign;
+ if( PrwSupport.Foot == LEFT )
+ sign = 1.0;
else
- Sign = -1.0;
- for( unsigned i=0;i<nrows-1;i++ )//first n-1 inequalities
+ sign = -1.0;
+ for( unsigned i=0; i<nrows-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 = ConvexHull.Y_vec[i];
+ y2 = ConvexHull.Y_vec[i+1];
+ x1 = ConvexHull.X_vec[i];
+ x2 = ConvexHull.X_vec[i+1];
dx = y1-y2;
dy = x2-x1;
dc = dx*x1+dy*y1;
//symmetrical constraints
- dx = Sign*dx;
- dy = Sign*dy;
- dc = Sign*dc;
+ dx = sign*dx;
+ dy = sign*dy;
+ dc = sign*dc;
- Dx[i] = dx; Dy[i]= dy;
- Dc[i] = dc;
+ ConvexHull.A_vec[i] = dx; ConvexHull.B_vec[i]= dy;
+ ConvexHull.D_vec[i] = dc;
}
{
//Last inequality
unsigned i = nrows-1;
- y1 = ConvexHull.Y[i];
- y2 = ConvexHull.Y[0];
- x1 = ConvexHull.X[i];
- x2 = ConvexHull.X[0];
+ y1 = ConvexHull.Y_vec[i];
+ y2 = ConvexHull.Y_vec[0];
+ x1 = ConvexHull.X_vec[i];
+ x2 = ConvexHull.X_vec[0];
dx = y1-y2;
dy = x2-x1;
dc = dx*x1+dy*y1;
//for symmetrical constraints
- dx = Sign*dx;
- dy = Sign*dy;
- dc = Sign*dc;
+ dx = sign*dx;
+ dy = sign*dy;
+ dc = sign*dc;
- Dx[i] = dx; Dy[i]= dy;
- Dc[i] = dc;
+ ConvexHull.A_vec[i] = dx; ConvexHull.B_vec[i]= dy;
+ ConvexHull.D_vec[i] = dc;
}
return 0;
diff --git a/src/Mathematics/relative-feet-inequalities.hh b/src/Mathematics/relative-feet-inequalities.hh
index 2634c2c761601057d698b09fa142886b706af15c..3eca045f754195436ff5bea0fc864f11d079042a 100644
--- a/src/Mathematics/relative-feet-inequalities.hh
+++ b/src/Mathematics/relative-feet-inequalities.hh
@@ -80,18 +80,18 @@ namespace PatternGeneratorJRL
const support_state_t & SupportState,
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.
- ///
- /// \param[in] aVecOfPoints a vector of vertices
- /// \param[out] D left hand side of the inequalities
- /// \param[out] Dc right hand side of the inequalities
- /// \param[in] PrwSupport previewed support state
- /// \return 0
- int compute_linear_system (const convex_hull_t & ConvexHull,
- MAL_MATRIX (&D, double),
- MAL_MATRIX (&Dc, double),
- const support_state_t & PrwSupport) const;
+// /// \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.
+// ///
+// /// \param[in] aVecOfPoints a vector of vertices
+// /// \param[out] D left hand side of the inequalities
+// /// \param[out] Dc right hand side of the inequalities
+// /// \param[in] PrwSupport previewed support state
+// /// \return 0
+// int compute_linear_system (const convex_hull_t & ConvexHull,
+// MAL_MATRIX (&D, double),
+// MAL_MATRIX (&Dc, double),
+// const support_state_t & PrwSupport) const;
/// \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 2812fa8e8be11ac7f99a41c4524fcdd135d7d3b7..5f1519e2125e264a1b2beff4d7bfbb7d0e66bd8e 100755
--- a/src/ZMPRefTrajectoryGeneration/generator-vel-ref.cpp
+++ b/src/ZMPRefTrajectoryGeneration/generator-vel-ref.cpp
@@ -230,10 +230,8 @@ GeneratorVelRef::build_inequalities_cop(linear_inequality_t & Inequalities,
deque<support_state_t>::const_iterator prwSS_it = SupportStates_deq.begin();
- convex_hull_t ZMPFeasibilityEdges;
- RFI_->set_vertices( ZMPFeasibilityEdges,
- *prwSS_it,
- INEQ_COP );
+ convex_hull_t ZMPFeasibilityEdges(4);
+ RFI_->set_vertices( ZMPFeasibilityEdges, *prwSS_it, INEQ_COP );
const unsigned nbEdges = 4;
double D_x[nbEdges] = {0.0, 0.0, 0.0, 0.0};
@@ -244,16 +242,15 @@ GeneratorVelRef::build_inequalities_cop(linear_inequality_t & Inequalities,
for( unsigned i=0; i<N_; i++ )
{
if( prwSS_it->StateChanged )
- RFI_->set_vertices( ZMPFeasibilityEdges,*prwSS_it,
- INEQ_COP );
+ RFI_->set_vertices( ZMPFeasibilityEdges, *prwSS_it, INEQ_COP );
RFI_->compute_linear_system( ZMPFeasibilityEdges, D_x, D_y, dc, *prwSS_it );
for( unsigned j = 0; j < nbEdges; j++ )
{
- Inequalities.D.x.push_back(i*nbEdges+j,i,D_x[j]);
- Inequalities.D.y.push_back(i*nbEdges+j,i,D_y[j]);
- Inequalities.dc(i*nbEdges+j) = dc[j];
+ Inequalities.D.x.push_back( i*nbEdges+j, i, D_x[j] );
+ Inequalities.D.y.push_back( i*nbEdges+j, i, D_y[j] );
+ Inequalities.dc( i*nbEdges+j ) = dc[j];
}
prwSS_it++;
@@ -286,8 +283,7 @@ GeneratorVelRef::build_inequalities_feet( linear_inequality_t & Inequalities,
if( prwSS_it->StateChanged && prwSS_it->StepNumber>0 && prwSS_it->Phase != DS)
{
prwSS_it--;//Take the support state before
- RFI_->set_vertices( FootFeasibilityEdges, *prwSS_it,
- INEQ_FEET );
+ RFI_->set_vertices( FootFeasibilityEdges, *prwSS_it, INEQ_FEET );
prwSS_it++;
RFI_->compute_linear_system( FootFeasibilityEdges, Dx, Dy, dc, *prwSS_it );
diff --git a/src/privatepgtypes.cpp b/src/privatepgtypes.cpp
index 62c81d6235ec111e070b7463c38c4cf39c46b2a9..80602de6cc3fb098882bfe87b713be8e9195e93e 100644
--- a/src/privatepgtypes.cpp
+++ b/src/privatepgtypes.cpp
@@ -155,22 +155,22 @@ namespace PatternGeneratorJRL
void
- convex_hull_t::rotate( double Angle )
+ convex_hull_t::rotate( double angle )
{
- double XOld, YOld;
+ double xOld, yOld;
for( unsigned int j=0; j<X.size(); j++ )
{
- XOld = X[j];
- YOld = Y[j];
- X[j] = ( XOld*cos(Angle) - YOld*sin(Angle) );
- Y[j] = ( XOld*sin(Angle) + YOld*cos(Angle) );
+ xOld = X[j];
+ yOld = Y[j];
+ X[j] = ( xOld*cos(angle) - yOld*sin(angle) );
+ Y[j] = ( xOld*sin(angle) + yOld*cos(angle) );
}
}
- convex_hull_t::convex_hull_t( int Size )
+ convex_hull_t::convex_hull_t( int size )
{
resize(Size);
@@ -196,11 +196,16 @@ namespace PatternGeneratorJRL
void
- convex_hull_t::resize( int Size )
+ convex_hull_t::resize( unsigned size )
{
- X.resize(Size);
- Y.resize(Size);
+ X_vec.resize(size);
+ Y_vec.resize(size);
+ Z_vec.resize(size);
+ A_vec.resize(size);
+ B_vec.resize(size);
+ C_vec.resize(size);
+ D_vec.resize(size);
}
diff --git a/src/privatepgtypes.h b/src/privatepgtypes.h
index b17a0d5220bf7abc8a81891647cf71b7ea3b7145..0d6cedb52db0dce445ba417cf049226fb1156b5f 100644
--- a/src/privatepgtypes.h
+++ b/src/privatepgtypes.h
@@ -38,8 +38,8 @@
namespace PatternGeneratorJRL
{
- // ENUMS:
- // ------
+ /// \name Enum types
+ /// \{
enum foot_type_e
{
LEFT, RIGHT
@@ -65,8 +65,7 @@ namespace PatternGeneratorJRL
POSITION, VELOCITY, ACCELERATION,
JERK, COP_POSITION
};
- // ------
- // :ENUMS
+ /// \}
/// \brief State of the center of mass
struct com_t
@@ -131,48 +130,35 @@ namespace PatternGeneratorJRL
reference_t(const reference_t &);
};
- /// \brief Linear inequality with free foot placement.
- struct linear_inequality_ff_t
- {
- MAL_MATRIX(D,double);
- MAL_MATRIX(Dc,double);
- int StepNumber;
- };
-
- /// \brief Linear constraints
- struct linear_constraint_t
- {
- boost_ublas::compressed_vector<double> A;
- double b;
- };
-
- /// \brief Set of 2-dimensional points
+ /// \brief Convex hull
struct convex_hull_t
{
- boost_ublas::vector<double> X;
- boost_ublas::vector<double> Y;
+ /// \brief Edges
+ std::vector<double> X_vec, Y_vec, Z_vec;
+ /// \brief Inequalities A_vec(i)*x+B_vec(i)y+C_vec(i)z+D_vec(i) > 0
+ std::vector<double> A_vec, B_vec, C_vec, D_vec;
/// \brief Rotate the points around the origin by angle
///
- /// \param[in] Angle
- void rotate( double Angle );
+ /// \param[in] angle
+ void rotate( double angle );
/// \brief Resize members to the desired number of points
///
/// \param[in] size
- void resize( int Size );
+ void resize( int size );
/// \brief Set the vectors from arrays
///
- /// \param[in] X
- /// \param[in] Y
- void set(const double * X_a, const double * Y_a);
+ /// \param[in] X_a
+ /// \param[in] Y_a
+ void set( const double * X_a, const double * Y_a );
/// \brief Set all points to zero
void reset();
- convex_hull_t( int Size );
+ convex_hull_t( unsigned size );
convex_hull_t();
};