Skip to content
New issue

Have a question about this project? Sign up for a free GitHub account to open an issue and contact its maintainers and the community.

Sign up for GitHub

By clicking “Sign up for GitHub”, you agree to our terms of service and privacy statement. We’ll occasionally send you account related emails.

Already on GitHub? Sign in to your account

Jump to bottom

Enforcing position limits in the VelocityJointSaturationHandle. #264

Open
wants to merge 1 commit into
base: kinetic-devel
Choose a base branch
from
Open

Enforcing position limits in the VelocityJointSaturationHandle. #264

Changes from all commits
Commits
Show all changes
1 commit
Select commit
File filter

Filter by extension

Filter by extension
Conversations
Failed to load comments.
Loading
Jump to
Jump to file
Failed to load files.
Loading
Diff view
Diff view
21 changes: 12 additions & 9 deletions joint_limits_interface/include/joint_limits_interface/joint_limits_interface.h
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -433,22 +433,25 @@ class VelocityJointSaturationHandle
using internal::saturate;

// Velocity bounds
double vel_low;
double vel_high;
double vel_low = -limits_.max_velocity;
double vel_high = limits_.max_velocity;

if (limits_.has_position_limits)
{
// Velocity bounds depend on the velocity limit and the proximity to the position limit.
const double pos = jh_.getPosition();
vel_low = saturate(limits_.min_position - pos, -limits_.max_velocity, limits_.max_velocity);
vel_high = saturate(limits_.max_position - pos, -limits_.max_velocity, limits_.max_velocity);
Copy link
Member

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

these two lines seem like they have incorrect behavior... I think you copied these from the PositionJointSoftLimitsHandle but flipped the subtraction of the position and position limit, resulting in incorrect units being used / arbitrary values.

I would assume you want a hard-stop position limit like how the effort is implemented, here

}

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);
}
else
{
vel_low = -limits_.max_velocity;
vel_high = limits_.max_velocity;
vel_low = std::max(vel - limits_.max_acceleration * dt, vel_low);
vel_high = std::min(vel + limits_.max_acceleration * dt, vel_high);
}

// Saturate velocity command according to limits
Expand Down