From 94a2368b4c6c0e3d6ed15beede5e0b6daee4e76d Mon Sep 17 00:00:00 2001
From: stevet <stevetonneau@gotmail.fr>
Date: Mon, 19 Mar 2018 11:51:36 +0100
Subject: [PATCH] [Cleaning] removed all warning from library, remaining in
tests
---
.../bezier-com-traj/common_solve_methods.hh | 2 +-
include/bezier-com-traj/solve.hh | 2 +-
.../waypoints/waypoints_c0_dc0_c1.hh | 2 +-
.../waypoints/waypoints_c0_dc0_dc1_c1.hh | 2 +-
.../waypoints/waypoints_c0_dc0_ddc0_c1.hh | 2 +-
.../waypoints/waypoints_c0_dc0_ddc0_dc1_c1.hh | 2 +-
.../waypoints_c0_dc0_ddc0_ddc1_dc1_c1.hh | 2 +-
src/common_solve_methods.cpp | 12 ++--
src/solve_transition.cpp | 58 +++++++++----------
tests/test-transition.cc | 14 ++---
tests/test_helper.hh | 8 +--
11 files changed, 51 insertions(+), 55 deletions(-)
diff --git a/include/bezier-com-traj/common_solve_methods.hh b/include/bezier-com-traj/common_solve_methods.hh
index 192f581..ac5270f 100644
--- a/include/bezier-com-traj/common_solve_methods.hh
+++ b/include/bezier-com-traj/common_solve_methods.hh
@@ -67,7 +67,7 @@ BEZIER_COM_TRAJ_DLLAPI ResultData solveIntersection(const std::pair<MatrixXX, Ve
* @param degree required degree
* @return
*/
-std::vector<spline::Bern<double> > ComputeBersteinPolynoms(int degree);
+std::vector<spline::Bern<double> > ComputeBersteinPolynoms(const unsigned int degree);
} // end namespace bezier_com_traj
diff --git a/include/bezier-com-traj/solve.hh b/include/bezier-com-traj/solve.hh
index 5eccb1e..88ace9b 100644
--- a/include/bezier-com-traj/solve.hh
+++ b/include/bezier-com-traj/solve.hh
@@ -33,7 +33,7 @@ namespace bezier_com_traj
* @param timeStep time step used by the discretization
* @return ResultData a struct containing the resulting trajectory, if success is true.
*/
- BEZIER_COM_TRAJ_DLLAPI ResultDataCOMTraj solveOnestep(const ProblemData& pData, const VectorX& Ts, const Vector3& init_guess,const int pointsPerPhase = 3 ,const double feasability_treshold = 0.);
+ BEZIER_COM_TRAJ_DLLAPI ResultDataCOMTraj solveOnestep(const ProblemData& pData, const VectorX& Ts, const Vector3& init_guess,const int pointsPerPhase = 3);
void printQHullFile(const std::pair<MatrixXX, VectorX>& Ab,VectorX intPoint,const std::string& fileName,bool clipZ = false);
diff --git a/include/bezier-com-traj/waypoints/waypoints_c0_dc0_c1.hh b/include/bezier-com-traj/waypoints/waypoints_c0_dc0_c1.hh
index c9b33eb..46119de 100644
--- a/include/bezier-com-traj/waypoints/waypoints_c0_dc0_c1.hh
+++ b/include/bezier-com-traj/waypoints/waypoints_c0_dc0_c1.hh
@@ -57,7 +57,7 @@ std::vector<waypoint6_t> computeWwaypoints(const ProblemData& pData,double T){
std::vector<waypoint6_t> wps;
std::vector<point_t> pi = computeConstantWaypoints(pData,T);
std::vector<Matrix3> Cpi;
- for(int i = 0 ; i < pi.size() ; ++i){
+ for(std::size_t i = 0 ; i < pi.size() ; ++i){
Cpi.push_back(skew(pi[i]));
}
const Vector3 g = pData.contacts_.front().contactPhase_->m_gravity;
diff --git a/include/bezier-com-traj/waypoints/waypoints_c0_dc0_dc1_c1.hh b/include/bezier-com-traj/waypoints/waypoints_c0_dc0_dc1_c1.hh
index 994c999..bb06d23 100644
--- a/include/bezier-com-traj/waypoints/waypoints_c0_dc0_dc1_c1.hh
+++ b/include/bezier-com-traj/waypoints/waypoints_c0_dc0_dc1_c1.hh
@@ -61,7 +61,7 @@ std::vector<waypoint6_t> computeWwaypoints(const ProblemData& pData,double T){
std::vector<waypoint6_t> wps;
std::vector<point_t> pi = computeConstantWaypoints(pData,T);
std::vector<Matrix3> Cpi;
- for(int i = 0 ; i < pi.size() ; ++i){
+ for(std::size_t i = 0 ; i < pi.size() ; ++i){
Cpi.push_back(skew(pi[i]));
}
const Vector3 g = pData.contacts_.front().contactPhase_->m_gravity;
diff --git a/include/bezier-com-traj/waypoints/waypoints_c0_dc0_ddc0_c1.hh b/include/bezier-com-traj/waypoints/waypoints_c0_dc0_ddc0_c1.hh
index f018e35..5c9c264 100644
--- a/include/bezier-com-traj/waypoints/waypoints_c0_dc0_ddc0_c1.hh
+++ b/include/bezier-com-traj/waypoints/waypoints_c0_dc0_ddc0_c1.hh
@@ -64,7 +64,7 @@ std::vector<waypoint6_t> computeWwaypoints(const ProblemData& pData,double T){
std::vector<waypoint6_t> wps;
std::vector<point_t> pi = computeConstantWaypoints(pData,T);
std::vector<Matrix3> Cpi;
- for(int i = 0 ; i < pi.size() ; ++i){
+ for(std::size_t i = 0 ; i < pi.size() ; ++i){
Cpi.push_back(skew(pi[i]));
}
const Vector3 g = pData.contacts_.front().contactPhase_->m_gravity;
diff --git a/include/bezier-com-traj/waypoints/waypoints_c0_dc0_ddc0_dc1_c1.hh b/include/bezier-com-traj/waypoints/waypoints_c0_dc0_ddc0_dc1_c1.hh
index 4878972..a394cdc 100644
--- a/include/bezier-com-traj/waypoints/waypoints_c0_dc0_ddc0_dc1_c1.hh
+++ b/include/bezier-com-traj/waypoints/waypoints_c0_dc0_ddc0_dc1_c1.hh
@@ -68,7 +68,7 @@ std::vector<waypoint6_t> computeWwaypoints(const ProblemData& pData,double T){
std::vector<waypoint6_t> wps;
std::vector<point_t> pi = computeConstantWaypoints(pData,T);
std::vector<Matrix3> Cpi;
- for(int i = 0 ; i < pi.size() ; ++i){
+ for(std::size_t i = 0 ; i < pi.size() ; ++i){
Cpi.push_back(skew(pi[i]));
}
const Vector3 g = pData.contacts_.front().contactPhase_->m_gravity;
diff --git a/include/bezier-com-traj/waypoints/waypoints_c0_dc0_ddc0_ddc1_dc1_c1.hh b/include/bezier-com-traj/waypoints/waypoints_c0_dc0_ddc0_ddc1_dc1_c1.hh
index 30e8093..868bf75 100644
--- a/include/bezier-com-traj/waypoints/waypoints_c0_dc0_ddc0_ddc1_dc1_c1.hh
+++ b/include/bezier-com-traj/waypoints/waypoints_c0_dc0_ddc0_ddc1_dc1_c1.hh
@@ -71,7 +71,7 @@ std::vector<waypoint6_t> computeWwaypoints(const ProblemData& pData,double T){
std::vector<waypoint6_t> wps;
std::vector<point_t> pi = computeConstantWaypoints(pData,T);
std::vector<Matrix3> Cpi;
- for(int i = 0 ; i < pi.size() ; ++i){
+ for(std::size_t i = 0 ; i < pi.size() ; ++i){
Cpi.push_back(skew(pi[i]));
}
const Vector3 g = pData.contacts_.front().contactPhase_->m_gravity;
diff --git a/src/common_solve_methods.cpp b/src/common_solve_methods.cpp
index e86555e..bc8d663 100644
--- a/src/common_solve_methods.cpp
+++ b/src/common_solve_methods.cpp
@@ -51,13 +51,13 @@ std::vector<waypoint6_t> ComputeDiscretizedWaypoints(const std::vector<waypoint6
return res;
}
-MatrixXX initMatrixA(const int dimH, const std::vector<waypoint6_t>& wps, const int extraConstraintSize, const bool useAngMomentum)
+MatrixXX initMatrixA(const int dimH, const std::vector<waypoint6_t>& wps, const long int extraConstraintSize, const bool useAngMomentum)
{
int dimPb = useAngMomentum ? 6 : 3;
return MatrixXX::Zero(dimH * wps.size() + extraConstraintSize, dimPb);
}
-void addKinematic(Ref_matrixXX A, Ref_vectorX b, Cref_matrixX3 Kin, Cref_vectorX kin, const int otherConstraintIndex)
+void addKinematic(Ref_matrixXX A, Ref_vectorX b, Cref_matrixX3 Kin, Cref_vectorX kin, const long int otherConstraintIndex)
{
int dimKin = (int)(kin.rows());
if (dimKin == 0) return;
@@ -97,7 +97,7 @@ int removeZeroRows(Ref_matrixXX& A, Ref_vectorX& b)
else
++i;
}
- return last+1;
+ return (int)last+1;
}
int Normalize(Ref_matrixXX A, Ref_vectorX b)
@@ -128,8 +128,8 @@ std::pair<MatrixXX, VectorX> compute6dControlPointInequalities(const ContactData
int dimH = (int)(H.rows());
MatrixXX mH = cData.contactPhase_->m_mass * H;
// init and fill Ab matrix
- int dimKin = cData.kin_.size();
- int dimAng = useAngMomentum ? (int)(cData.ang_.size()) : 0;
+ long int dimKin = cData.kin_.size();
+ long int dimAng = useAngMomentum ? (long int)(cData.ang_.size()) : 0;
A = initMatrixA(dimH, wps, dimKin + dimAng, useAngMomentum);
b = VectorX::Zero(A.rows());
point6_t bc = point6_t::Zero(); bc.head(3) = g; // constant part of Aub, Aubi = mH * (bc - wsi)
@@ -207,7 +207,7 @@ ResultData solve(Cref_matrixXX A, Cref_vectorX ci0, Cref_matrixXX H, Cref_vector
}
-std::vector<spline::Bern<double> > ComputeBersteinPolynoms(int degree)
+std::vector<spline::Bern<double> > ComputeBersteinPolynoms(const unsigned int degree)
{
std::vector<spline::Bern<double> > res;
for (unsigned int i =0; i <= degree; ++i)
diff --git a/src/solve_transition.cpp b/src/solve_transition.cpp
index e36d04e..c30e0b7 100644
--- a/src/solve_transition.cpp
+++ b/src/solve_transition.cpp
@@ -38,7 +38,7 @@ void printQHullFile(const std::pair<MatrixXX, VectorX>& Ab,VectorX intPoint,cons
file<<"\t "<<intPoint[0]<<"\t"<<intPoint[1]<<"\t"<<intPoint[2]<<endl;
file<<"4"<<endl;
clipZ ? file<<Ab.first.rows()+2<<endl : file<<Ab.first.rows()<<endl;
- for(size_t i = 0 ; i < Ab.first.rows() ; ++i){
+ for(int i = 0 ; i < Ab.first.rows() ; ++i){
file<<"\t"<<Ab.first(i,0)<<"\t"<<Ab.first(i,1)<<"\t"<<Ab.first(i,2)<<"\t"<<-Ab.second[i]-0.001<<endl;
}
if(clipZ){
@@ -64,7 +64,7 @@ std::vector<double> computeDiscretizedTime(const VectorX& phaseTimings,const int
std::vector<double> timeArray;
double t = 0;
double t_total = 0;
- for(size_t i = 0 ; i < phaseTimings.size() ; ++i)
+ for(int i = 0 ; i < phaseTimings.size() ; ++i)
t_total += phaseTimings[i];
for(int i = 0 ; i < phaseTimings.size() ; ++i){
@@ -85,7 +85,7 @@ std::vector<coefs_t> computeDiscretizedWaypoints(const ProblemData& pData,double
std::vector<point_t> pi = computeConstantWaypoints(pData,T);
// evaluate curve work with normalized time !
double t;
- for (int i = 0 ; i<timeArray.size() ; ++i ){
+ for (std::size_t i = 0 ; i<timeArray.size() ; ++i ){
t = timeArray[i] / T;
if(t>1)
t=1.;
@@ -98,12 +98,12 @@ std::vector<coefs_t> computeDiscretizedWaypoints(const ProblemData& pData,double
std::vector<waypoint6_t> computeDiscretizedWwaypoints(const ProblemData& pData,double T,const std::vector<double>& timeArray){
std::vector<waypoint6_t> wps = computeWwaypoints(pData,T);
std::vector<waypoint6_t> res;
- std::vector<spline::Bern<double> > berns = ComputeBersteinPolynoms(wps.size()-1);
+ std::vector<spline::Bern<double> > berns = ComputeBersteinPolynoms((int)wps.size()-1);
double t;
double b;
- for(int i = 0 ; i < timeArray.size() ; ++i){
+ for(std::size_t i = 0 ; i < timeArray.size() ; ++i){
waypoint6_t w = initwp<waypoint6_t>();
- for (int j = 0 ; j < wps.size() ; ++j){
+ for (std::size_t j = 0 ; j < wps.size() ; ++j){
t = timeArray[i]/T;
if(t>1.)
t=1.;
@@ -123,7 +123,7 @@ std::vector<coefs_t> computeDiscretizedAccelerationWaypoints(const ProblemData&
std::vector<coefs_t> wps;
std::vector<point_t> pi = computeConstantWaypoints(pData,T);
double t;
- for (int i = 0 ; i<timeArray.size() ; ++i ){
+ for (std::size_t i = 0 ; i<timeArray.size() ; ++i ){
t = timeArray[i] / T;
if(t>1)
t=1.;
@@ -207,7 +207,7 @@ void compareStabilityMethods(const MatrixXX& mH,const VectorX& h,const Vector3&
}
-std::pair<MatrixXX, VectorX> computeConstraintsOneStep(const ProblemData& pData,const VectorX& Ts,const int pointsPerPhase,VectorX& constraints_equivalence){
+std::pair<MatrixXX, VectorX> computeConstraintsOneStep(const ProblemData& pData,const VectorX& Ts,const int pointsPerPhase,VectorX& /*constraints_equivalence*/){
// compute the list of discretized waypoint :
double t_total = 0.;
for(int i = 0 ; i < Ts.size() ; ++i)
@@ -216,12 +216,6 @@ std::pair<MatrixXX, VectorX> computeConstraintsOneStep(const ProblemData& pData,
//std::cout<<"total time : "<<t_total<<std::endl;
std::vector<double> timeArray = computeDiscretizedTime(Ts,pointsPerPhase);
std::vector<coefs_t> wps_c = computeDiscretizedWaypoints(pData,t_total,timeArray);
- std::vector<coefs_t> wps_ddc;
- Vector3 acc_bounds;
- if(pData.constraints_.constraintAcceleration_){
- wps_ddc = computeDiscretizedAccelerationWaypoints(pData,t_total,timeArray);
- acc_bounds = Vector3::Ones()*pData.constraints_.maxAcceleration_;
- }
std::vector<waypoint6_t> wps_w = computeDiscretizedWwaypoints(pData,t_total,timeArray);
//std::cout<<" number of discretized waypoints c: "<<wps_c.size()<<std::endl;
//std::cout<<" number of discretized waypoints w: "<<wps_w.size()<<std::endl;
@@ -238,9 +232,9 @@ std::pair<MatrixXX, VectorX> computeConstraintsOneStep(const ProblemData& pData,
#else
numCol = 3;
#endif
- int num_ineq = 0;
- int num_stab_ineq = 0;
- int num_kin_ineq = 0;
+ long int num_ineq = 0;
+ long int num_stab_ineq = 0;
+ long int num_kin_ineq = 0;
int numStepForPhase;
centroidal_dynamics::MatrixXX Hrow;
VectorX h;
@@ -266,10 +260,10 @@ std::pair<MatrixXX, VectorX> computeConstraintsOneStep(const ProblemData& pData,
VectorX b(num_ineq);
std::pair<MatrixXX,VectorX> Ab_stab;
- int id_rows = 0;
- int current_size;
+ long int id_rows = 0;
+ long int current_size;
- int id_phase = 0;
+ std::size_t id_phase = 0;
ContactData phase = pData.contacts_[id_phase];
// compute some constant matrice for the current phase :
const Vector3& g = phase.contactPhase_->m_gravity;
@@ -286,7 +280,7 @@ std::pair<MatrixXX, VectorX> computeConstraintsOneStep(const ProblemData& pData,
// assign the Stability constraints for each discretized waypoints :
// we don't consider the first point, because it's independant of x.
- for(int id_step = 0 ; id_step < timeArray.size() ; ++id_step ){
+ for(std::size_t id_step = 0 ; id_step < timeArray.size() ; ++id_step ){
// add stability constraints :
Ab_stab = dynamicStabilityConstraints(mH,h,g,wps_w[id_step]);
@@ -296,8 +290,8 @@ std::pair<MatrixXX, VectorX> computeConstraintsOneStep(const ProblemData& pData,
id_rows += dimH ;
// check if we are going to switch phases :
- for(int i = 0 ; i < (stepIdForPhase.size()-1) ; ++i){
- if(id_step == stepIdForPhase[i]){
+ for(std::size_t i = 0 ; i < (stepIdForPhase.size()-1) ; ++i){
+ if((int)id_step == stepIdForPhase[i]){
// switch to phase i
id_phase=i+1;
phase = pData.contacts_[id_phase];
@@ -324,20 +318,20 @@ std::pair<MatrixXX, VectorX> computeConstraintsOneStep(const ProblemData& pData,
phase = pData.contacts_[id_phase];
// assign the Kinematics constraints for each discretized waypoints :
// we don't consider the first point, because it's independant of x.
- for(int id_step = 0 ; id_step < timeArray.size() ; ++id_step ){
+ for(std::size_t id_step = 0 ; id_step < timeArray.size() ; ++id_step ){
// add constraints for wp id_step, on current phase :
// add kinematics constraints :
// constraint are of the shape A c <= b . But here c(x) = Fx + s so : AFx <= b - As
if(id_step != timeArray.size()-1){ // we don't consider kinematics constraints for the last point (because it's independant of x)
- current_size = phase.kin_.rows();
+ current_size = (int)phase.kin_.rows();
A.block(id_rows,0,current_size,3) = (phase.Kin_ * wps_c[id_step].first);
b.segment(id_rows,current_size) = phase.kin_ - (phase.Kin_*wps_c[id_step].second);
id_rows += current_size;
}
// check if we are going to switch phases :
- for(int i = 0 ; i < (stepIdForPhase.size()-1) ; ++i){
- if(id_step == stepIdForPhase[i]){
+ for(std::size_t i = 0 ; i < (stepIdForPhase.size()-1) ; ++i){
+ if(id_step == (std::size_t)stepIdForPhase[i]){
// switch to phase i
id_phase=i+1;
phase = pData.contacts_[id_phase];
@@ -351,8 +345,10 @@ std::pair<MatrixXX, VectorX> computeConstraintsOneStep(const ProblemData& pData,
}
}
- if(pData.constraints_.constraintAcceleration_) { // assign the acceleration constraints for each discretized waypoints :
- for(int id_step = 0 ; id_step < timeArray.size() ; ++id_step ){
+ if(pData.constraints_.constraintAcceleration_) { // assign the acceleration constraints for each discretized waypoints :
+ std::vector<coefs_t> wps_ddc = computeDiscretizedAccelerationWaypoints(pData,t_total,timeArray);
+ Vector3 acc_bounds = Vector3::Ones()*pData.constraints_.maxAcceleration_;
+ for(std::size_t id_step = 0 ; id_step < timeArray.size() ; ++id_step ){
A.block(id_rows,0,3,3) = Matrix3::Identity() * wps_ddc[id_step].first; // upper
b.segment(id_rows,3) = acc_bounds - wps_ddc[id_step].second;
A.block(id_rows+3,0,3,3) = -Matrix3::Identity() * wps_ddc[id_step].first; // lower
@@ -445,7 +441,7 @@ double analyseSlack(const VectorX& slack,const VectorX& constraint_equivalence )
//std::cout<<"slack : "<<slack<<std::endl;
std::cout<<"list of violated constraints : "<<std::endl;
double previous_id = -1;
- for(size_t i = 0 ; i < slack.size() ; ++i){
+ for(long int i = 0 ; i < slack.size() ; ++i){
if((slack[i]*slack[i]) > std::numeric_limits<double>::epsilon()){
if(constraint_equivalence[i] != previous_id){
std::cout<<"step "<<constraint_equivalence[i]<<std::endl;
@@ -457,7 +453,7 @@ double analyseSlack(const VectorX& slack,const VectorX& constraint_equivalence )
return slack.lpNorm<Eigen::Infinity>();
}
-ResultDataCOMTraj solveOnestep(const ProblemData& pData, const VectorX& Ts,const Vector3& init_guess,const int pointsPerPhase, const double feasability_treshold){
+ResultDataCOMTraj solveOnestep(const ProblemData& pData, const VectorX& Ts,const Vector3& init_guess,const int pointsPerPhase){
assert(pData.contacts_.size() ==2 || pData.contacts_.size() ==3);
assert(Ts.size() == pData.contacts_.size());
double T = 0;
diff --git a/tests/test-transition.cc b/tests/test-transition.cc
index d4ca1c2..342daee 100644
--- a/tests/test-transition.cc
+++ b/tests/test-transition.cc
@@ -32,10 +32,10 @@ std::vector<double> computeDiscretizedTime(const VectorX& phaseTimings,const int
std::vector<double> timeArray;
double t = 0;
double t_total = 0;
- for(size_t i = 0 ; i < phaseTimings.size() ; ++i)
+ for(long int i = 0 ; i < phaseTimings.size() ; ++i)
t_total += phaseTimings[i];
- for(int i = 0 ; i < phaseTimings.size() ; ++i){
+ for(long int i = 0 ; i < phaseTimings.size() ; ++i){
double step = (double) phaseTimings[i] / pointsPerPhase;
for(int j = 0 ; j < pointsPerPhase ; ++j){
t += step;
@@ -52,7 +52,7 @@ bool check_constraints(const bezier_com_traj::ContactData& contactPhase, Vector3
BOOST_CHECK(verifyKinematicConstraints(std::make_pair(contactPhase.Kin_,contactPhase.kin_),c));
BOOST_CHECK(verifyStabilityConstraintsDLP(*contactPhase.contactPhase_,c,dc,ddc));
BOOST_CHECK(verifyStabilityConstraintsPP(*contactPhase.contactPhase_,c,dc,ddc));
-
+ return true;
}
@@ -60,7 +60,7 @@ void check_transition(bezier_com_traj::ProblemData& pData, VectorX Ts,bool shoul
BOOST_CHECK_EQUAL(pData.contacts_.size(),Ts.size());
double t_total = 0;
- for(size_t i = 0 ; i < Ts.size() ; ++i)
+ for(long int i = 0 ; i < Ts.size() ; ++i)
t_total += Ts[i];
Vector3 init = (pData.c1_ - pData.c0_)/2.;
@@ -97,7 +97,7 @@ void check_transition(bezier_com_traj::ProblemData& pData, VectorX Ts,bool shoul
std::vector<int> stepIdForPhase; // stepIdForPhase[i] is the id of the last step of phase i / first step of phase i+1 (overlap)
for(int i = 0 ; i < Ts.size() ; ++i)
stepIdForPhase.push_back(pointsPerPhase*(i+1)-1);
- int id_phase = 0;
+ std::size_t id_phase = 0;
bezier_com_traj::ContactData phase = pData.contacts_[id_phase];
for(size_t id_step = 0 ; id_step < timings.size() ; ++id_step){
@@ -109,8 +109,8 @@ void check_transition(bezier_com_traj::ProblemData& pData, VectorX Ts,bool shoul
check_constraints(phase,c,dc,ddc);
// check if we switch phases
- for(int i = 0 ; i < (stepIdForPhase.size()-1) ; ++i){
- if(id_step == stepIdForPhase[i]){
+ for(std::size_t i = 0 ; i < (stepIdForPhase.size()-1) ; ++i){
+ if(id_step == (std::size_t)stepIdForPhase[i]){
id_phase=i+1;
phase = pData.contacts_[id_phase];
check_constraints(phase,c,dc,ddc);
diff --git a/tests/test_helper.hh b/tests/test_helper.hh
index 1a50dbb..8accc69 100644
--- a/tests/test_helper.hh
+++ b/tests/test_helper.hh
@@ -84,8 +84,8 @@ std::pair<MatrixX3, MatrixX3> computeRectangularContacts(MatrixX3 normals, Matri
-lx, ly, 0;
- for (size_t ic = 0 ; ic < normals.rows() ; ++ic){
- for (size_t i = 0 ; i < 4 ; ++i){
+ for (long int ic = 0 ; ic < normals.rows() ; ++ic){
+ for (long int i = 0 ; i < 4 ; ++i){
rec_normals.block<1,3>(ic*4+i,0) = normals.block<1,3>(ic,0);
rec_positions.block<1,3>(ic*4+i,0) = positions.block<1,3>(ic,0) + p.block<1,3>(i,0);
}
@@ -167,7 +167,7 @@ ConstraintsPair stackConstraints(const ConstraintsPair& Ab,const ConstraintsPair
bool verifyKinematicConstraints(const ConstraintsPair& Ab, const Vector3 &point){
- for(size_t i = 0 ; i < Ab.second.size() ; ++i){
+ for(long int i = 0 ; i < Ab.second.size() ; ++i){
if(Ab.first.block<1,3>(i,0).dot(point) > Ab.second[i] ){
return false;
}
@@ -207,7 +207,7 @@ bool verifyStabilityConstraintsPP(centroidal_dynamics::Equilibrium contactPhase,
VectorX b = h+mH.block(0,0,dimH,3)*(g - acc);
// verify inequalities with c :
- for(size_t i = 0 ; i < b.size() ; ++i){
+ for(long int i = 0 ; i < b.size() ; ++i){
if(A.block<1,3>(i,0).dot(c) > b[i] ){
return false;
}
--
GitLab