From ffa31f82dd639ea9476731a92c6f00dff7b85e31 Mon Sep 17 00:00:00 2001 From: Stephen Roderick Date: Thu, 13 Jan 2011 08:37:03 -0500 Subject: [PATCH 2/3] Support trap. velocity profile with velocity specification --- src/velocityprofile_trap.cpp | 19 +++++++++++++++++++ src/velocityprofile_trap.hpp | 12 ++++++++++++ 2 files changed, 31 insertions(+), 0 deletions(-) diff --git a/src/velocityprofile_trap.cpp b/src/velocityprofile_trap.cpp index 679b3ad..798caa9 100644 --- a/src/velocityprofile_trap.cpp +++ b/src/velocityprofile_trap.cpp @@ -108,6 +108,25 @@ void VelocityProfile_Trap::SetProfileDuration( t2 /= factor; } +void VelocityProfile_Trap::SetProfileVelocity( + double pos1,double pos2,double newvelocity) { + // Max velocity + SetProfile(pos1,pos2); + // Must be Slower : + double factor = newvelocity; // valid = [KDL::epsilon, 1.0] + if (1.0 < factor) factor = 1.0; + if (KDL::epsilon > factor) factor = KDL::epsilon; + a2*=factor; + a3*=factor*factor; + b2*=factor; + b3*=factor*factor; + c2*=factor; + c3*=factor*factor; + duration = duration / factor; + t1 /= factor; + t2 /= factor; +} + void VelocityProfile_Trap::SetMax(double _maxvel,double _maxacc) { maxvel = _maxvel; maxacc = _maxacc; diff --git a/src/velocityprofile_trap.hpp b/src/velocityprofile_trap.hpp index a8e1ba0..80efeb4 100644 --- a/src/velocityprofile_trap.hpp +++ b/src/velocityprofile_trap.hpp @@ -83,6 +83,18 @@ class VelocityProfile_Trap : public VelocityProfile double pos1,double pos2,double newduration ); + /** Compute trapezoidal profile at a given fraction of max velocity + @param pos1 Position to start from + @param pos2 Position to end at + @param newvelocity Fraction of max velocity to use during the + non-ramp, flat-velocity part of the profile. + @param KDL::epsilon <= newvelocity <= 1.0 (forcibly clamped to + this range internally) + */ + virtual void SetProfileVelocity( + double pos1,double pos2,double newvelocity + ); + virtual void SetMax(double _maxvel,double _maxacc); virtual double Duration() const; virtual double Pos(double time) const; -- 1.7.0.2