Merge pull request orocos#39 from rethink-forrest/master

Check that the maxvel is obtainable in a half trapezoidal velocity profile

Author: Ruben Smits

orocos_kdl/src/path_composite.cpp

@@ -53,8 +53,8 @@ namespace KDL {
 // you propably want to use the cached_index variable
 double Path_Composite::Lookup(double s) const
 {
-	assert(s>=0);
-	assert(s<=pathlength);
+	assert(s>=-1e-12);
+	assert(s<=pathlength+1e-12);
 	if ( (cached_starts <=s) && ( s <= cached_ends) ) {
 		return s - cached_starts;
 	}

orocos_kdl/src/velocityprofile_traphalf.cpp

@@ -58,9 +58,9 @@ void VelocityProfile_TrapHalf::PlanProfile1(double v,double a) {
 	a3 = 0;
 	a2 = 0;
 	a1 = startpos;
-	b3 = a/2;
+	b3 = a/2.0;
 	b2 = -a*t1;
-	b1 = startpos + a*t1*t1/2;
+	b1 = startpos + a*t1*t1/2.0;
 	c3 = 0;
 	c2 = v;
 	c1 = endpos - v*duration;
@@ -70,9 +70,9 @@ void VelocityProfile_TrapHalf::PlanProfile2(double v,double a) {
 	a3 = 0;
 	a2 = v;
 	a1 = startpos;
-	b3 = -a/2;
+	b3 = -a/2.0;
 	b2 = a*t2;
-	b1 = endpos - a*t2*t2/2;
+	b1 = endpos - a*t2*t2/2.0;
 	c3 = 0;
 	c2 = 0;
 	c1 = endpos;
@@ -82,15 +82,17 @@ void VelocityProfile_TrapHalf::SetProfile(double pos1,double pos2) {
 	startpos        = pos1;
 	endpos          = pos2;
 	double s        = sign(endpos-startpos);
-	duration		= s*(endpos-startpos)/maxvel+maxvel/maxacc/2.0;
+	// check that the maxvel is obtainable
+	double vel = std::min(maxvel, sqrt(2.0*s*(endpos-startpos)*maxacc));
+	duration		= s*(endpos-startpos)/vel+vel/maxacc/2.0;
 	if (starting) {
 		t1 = 0;
-		t2 = maxvel/maxacc;
-		PlanProfile1(maxvel*s,maxacc*s);
+		t2 = vel/maxacc;
+		PlanProfile1(vel*s,maxacc*s);
 	} else {
-		t1 = duration-maxvel/maxacc;
+		t1 = duration-vel/maxacc;
 		t2 = duration;
-		PlanProfile2(s*maxvel,s*maxacc);
+		PlanProfile2(s*vel,s*maxacc);
 	}
 }
 

orocos_kdl/src/velocityprofile_traphalf.hpp

@@ -89,9 +89,16 @@ class VelocityProfile_TrapHalf : public VelocityProfile
 		 */
 		VelocityProfile_TrapHalf(double _maxvel=0,double _maxacc=0,bool _starting=true);
 
-        void SetMax(double _maxvel,double _maxacc, bool _starting );
+		void SetMax(double _maxvel,double _maxacc,bool _starting);
 
 		/**
+		 * Plans a 'Half' Trapezoidal VelocityProfile between pos1 and pos2.
+		 * If the distance is too short betweeen pos1 and pos2,
+		 * only the acceleration phase is set and the max velocity is not reached.
+		 *
+		 * \param pos1 Starting position
+		 * \param pos2 Ending position
+		 *
 		 * Can throw a Error_MotionPlanning_Not_Feasible
 		 */
 		virtual void SetProfile(double pos1,double pos2);