Merge pull request #1 from efernandez/accel_limits

Acceleration limits
pal-robotics-forks · Nov 12, 2013 · 35fbc27 · 35fbc27
2 parents 63ab728 + e9e3bde
commit 35fbc27
Showing 1 changed file with 12 additions and 5 deletions.
diff --git a/joint_limits_interface/include/joint_limits_interface/joint_limits_interface.h b/joint_limits_interface/include/joint_limits_interface/joint_limits_interface.h
@@ -237,11 +237,14 @@ class EffortJointSoftLimitsHandle
 class VelocityJointSaturationHandle
 {
 public:
-  VelocityJointSaturationHandle () {}
+  VelocityJointSaturationHandle ()
+    : prev_cmd_(0.0)
+  {}
 
   VelocityJointSaturationHandle(const hardware_interface::JointHandle& jh, const JointLimits& limits)
     : jh_(jh),
-      limits_(limits)
+      limits_(limits),
+      prev_cmd_(0.0)
   {
     if (!limits.has_velocity_limits)
     {
@@ -268,11 +271,10 @@ class VelocityJointSaturationHandle
     if (limits_.has_acceleration_limits)
     {
       assert(period.toSec() > 0.0);
-      const double vel = jh_.getVelocity();
       const double dt  = period.toSec();
 
-      vel_low  = std::max(vel - limits_.max_acceleration * dt, -limits_.max_velocity);
-      vel_high = std::min(vel + limits_.max_acceleration * dt,  limits_.max_velocity);
+      vel_low  = std::max(prev_cmd_ - limits_.max_acceleration * dt, -limits_.max_velocity);
+      vel_high = std::min(prev_cmd_ + limits_.max_acceleration * dt,  limits_.max_velocity);
     }
     else
     {
@@ -285,11 +287,16 @@ class VelocityJointSaturationHandle
                                     vel_low,
                                     vel_high);
     jh_.setCommand(vel_cmd);
+
+    // Cache variables
+    prev_cmd_ = jh_.getCommand();
   }
 
 private:
   hardware_interface::JointHandle jh_;
   JointLimits limits_;
+
+  double prev_cmd_;
 };
 
 /**