Unverified Commit b1248207 authored by Joseph Mirabel's avatar Joseph Mirabel Committed by GitHub
Browse files

Merge pull request #169 from pFernbach/topic/fix_kino

[kinodynamic] replace test (float==0) with an epsilon
parents 5c58d1f0 4595ad1f
......@@ -126,7 +126,7 @@ double KinodynamicDistance::computeMinTime(double p1, double p2, double v1, doub
double a2 = -a1;
double vLim = (sigma) * vMax_;
//hppDout(info,"Vlim = "<<vLim<<" ; aMax = "<<aMax_);
if((p2-p1) == 0. && (v2-v1)==0. ){
if(fabs(p2-p1) < (std::numeric_limits<double>::epsilon()*100.) && fabs(v2-v1)<(std::numeric_limits<double>::epsilon()*100.) ){
//hppDout(notice,"No movement in this joints, abort.");
return 0.;
}
......
......@@ -219,7 +219,7 @@ namespace hpp {
double a2 = -a1;
double vLim = (sigma) * vMax;
hppDout(info,"Vlim = "<<vLim<<" ; aMax = "<<aMax);
if((p2-p1) == 0. && (v2-v1)==0. ){
if(fabs(p2-p1) < (std::numeric_limits<double>::epsilon()*100.) && fabs(v2-v1)<(std::numeric_limits<double>::epsilon()*100.) ){
hppDout(notice,"No movement in this joints, abort.");
return 0.;
}
......@@ -361,7 +361,7 @@ namespace hpp {
hppDout(info,"sigma Bis= "<<sigma);
}
if(v2_1 == 0 && p2_1 == 0){
if(fabs(v2_1) < (std::numeric_limits<double>::epsilon()*100.) && fabs(p2_1)<(std::numeric_limits<double>::epsilon()*100.) ){
*a1 = 0;
*t0 = 0;
*t1 = 0;
......@@ -468,7 +468,7 @@ namespace hpp {
x2 = c/q;
else{
x2 = sigma*aMax_[index];
hppDout(notice,"q == 0, take x1 = aMax");
hppDout(notice,"q == 0, take x2 = aMax");
}
hppDout(notice,"epsilon = "<<std::numeric_limits<double>::epsilon()*100.);
......
Markdown is supported
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