From e9e3bde19d29fb1f781345d44ec1fb9b0b7fd945 Mon Sep 17 00:00:00 2001 From: enriquefernandez Date: Thu, 7 Nov 2013 12:09:13 +0100 Subject: [PATCH] added accel limits --- .../joint_limits_interface.h | 17 ++++++++++++----- 1 file changed, 12 insertions(+), 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 index 49374c483..14629978e 100644 --- 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_; }; /**