Skip to content

Commit

Permalink
remove acceleration smoothing
Browse files Browse the repository at this point in the history 
Signed-off-by: Paul Gesel <paulgesel@gmail.com>
  • Loading branch information
@pac48
pac48 committed Dec 28, 2023
1 parent 72d4f92 commit 459a5b0
Show file tree
Hide file tree
Showing 4 changed files with 7 additions and 104 deletions.
11 changes: 0 additions & 11 deletions moveit_ros/moveit_servo/include/moveit_servo/utils/common.hpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -162,17 +162,6 @@ std::pair<double, StatusCode> velocityScalingFactorForSingularity(const moveit::
double jointLimitVelocityScalingFactor(const Eigen::VectorXd& velocities,
const moveit::core::JointBoundsVector& joint_bounds, double scaling_override);

/**
* \brief Apply acceleration scaling based on joint limits. If the robot model does not have acceleration limits
* defined, then a scale factor of 1.0 will be returned.
* @param accelerations The commanded accelerations.
* @param joint_bounds The bounding information for the robot joints.
* @param scaling_override The user defined acceleration scaling override.
* @return The acceleration scaling factor.
*/
double jointLimitAccelerationScalingFactor(const Eigen::VectorXd& accelerations,
const moveit::core::JointBoundsVector& joint_bounds,
double scaling_override);
/**
* \brief Finds the joints that are exceeding allowable position limits.
* @param positions The joint positions.
Expand Down
30 changes: 7 additions & 23 deletions moveit_ros/moveit_servo/src/servo.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -521,6 +521,9 @@ KinematicState Servo::getNextJointState(const moveit::core::RobotStatePtr& robot
// Compute the next joint positions based on the joint position deltas
target_state.positions = current_state.positions + joint_position_delta;

// Compute the joint velocities required to reach positions
target_state.velocities = joint_position_delta / servo_params_.publish_period;

// Apply smoothing to the positions if a smoother was provided.
doSmoothing(target_state);

Expand All @@ -540,19 +543,6 @@ KinematicState Servo::getNextJointState(const moveit::core::RobotStatePtr& robot
}
target_state.velocities *= joint_velocity_limit_scale;

// Scale down the acceleration based on joint acceleration limit or user defined scaling if applicable.
Eigen::VectorXd joint_accelerations =
(target_state.velocities - current_state.velocities) / (servo_params_.publish_period);
const double joint_acceleration_limit_scale =
jointLimitAccelerationScalingFactor(joint_accelerations, joint_bounds, 1.0);
if (joint_acceleration_limit_scale < 1.0) // 1.0 means no scaling.
{
RCLCPP_DEBUG_STREAM(logger_,
"Joint acceleration limit scaling applied by a factor of " << joint_acceleration_limit_scale);
}
target_state.velocities =
current_state.velocities + joint_acceleration_limit_scale * joint_accelerations * servo_params_.publish_period;

// Adjust joint position based on scaled down velocity
target_state.positions = current_state.positions + (target_state.velocities * servo_params_.publish_period);

Expand Down Expand Up @@ -676,15 +666,11 @@ std::pair<bool, KinematicState> Servo::smoothHalt(const moveit::core::RobotState
bool stopped = false;
auto target_state = halt_state;

// Get joint bounds from robot state for current move group.
const moveit::core::JointModelGroup* joint_model_group =
robot_state->getJointModelGroup(servo_params_.move_group_name);
const moveit::core::JointBoundsVector joint_bounds = joint_model_group->getActiveJointModelsBounds();
// set target velocity
target_state.velocities *= 0.0;

// apply scaling to target velocity based on robot's limits
Eigen::VectorXd acceleration = -target_state.velocities / servo_params_.publish_period;
const double joint_acceleration_limit_scale = jointLimitAccelerationScalingFactor(acceleration, joint_bounds, 1.0);
target_state.velocities += joint_acceleration_limit_scale * acceleration * servo_params_.publish_period;
// apply smoothing: this will change target position/velocity to make slow down gradual
doSmoothing(target_state);

// scale velocity in case of obstacle
target_state.velocities *= collision_velocity_scale_;
Expand All @@ -698,8 +684,6 @@ std::pair<bool, KinematicState> Servo::smoothHalt(const moveit::core::RobotState
(target_state.velocities[i] - halt_state.velocities[i]) / servo_params_.publish_period;
}

doSmoothing(target_state);

// If all velocities are near zero, robot has decelerated to a stop.
stopped = (target_state.velocities.cwiseAbs().array() < STOPPED_VELOCITY_EPS).all();

Expand Down
16 changes: 0 additions & 16 deletions moveit_ros/moveit_servo/src/utils/common.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -418,22 +418,6 @@ double jointLimitVelocityScalingFactor(const Eigen::VectorXd& velocities,
return jointLimitScalingFactorCommon(velocities, joint_bounds, scaling_override, calculate_joint_scaling_factor);
}

double jointLimitAccelerationScalingFactor(const Eigen::VectorXd& accelerations,
const moveit::core::JointBoundsVector& joint_bounds, double scaling_override)
{
auto calculate_joint_scaling_factor = [](moveit::core::VariableBounds variable_bound, double target_accel) {
if (variable_bound.acceleration_bounded_ && target_accel != 0.0)
{
// Find the ratio of clamped acceleration to original acceleration
const auto bounded_vel =
std::clamp(target_accel, variable_bound.min_acceleration_, variable_bound.max_acceleration_);
return bounded_vel / target_accel;
}
return 1.0;
};
return jointLimitScalingFactorCommon(accelerations, joint_bounds, scaling_override, calculate_joint_scaling_factor);
}

std::vector<int> jointsToHalt(const Eigen::VectorXd& positions, const Eigen::VectorXd& velocities,
const moveit::core::JointBoundsVector& joint_bounds, const std::vector<double>& margins)
{
Expand Down
54 changes: 0 additions & 54 deletions moveit_ros/moveit_servo/tests/test_utils.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -95,60 +95,6 @@ TEST(ServoUtilsUnitTests, JointLimitVelocityScaling)
ASSERT_NEAR(scaling_factor, 1.0 / 1.1, tol);
}

TEST(ServoUtilsUnitTests, JointLimitAccelerationScaling)
{
using moveit::core::loadTestingRobotModel;
moveit::core::RobotModelPtr robot_model = loadTestingRobotModel("panda");
const auto joint_model_group = robot_model->getJointModelGroup("panda_arm");
auto const_joint_bounds = joint_model_group->getActiveJointModelsBounds();

// Get the upper bound for the accelerations of each joint.
Eigen::VectorXd incoming_accelerations(const_joint_bounds.size());

// The URDF does not have accelerations, so add values manually
incoming_accelerations << 3.75, 1.875, 2.5, 3.125, 3.75, 5.0, 5.0;

// Build joint_bounds with acceleration data
std::vector<const moveit::core::JointModel::Bounds*> joint_bounds;
std::vector<moveit::core::JointModel::Bounds> bounds_vector;
bounds_vector.resize(const_joint_bounds.size());
for (size_t i = 0; i < const_joint_bounds.size(); i++)
{
moveit::core::JointModel::Bounds* b = &bounds_vector[i];
*b = *const_joint_bounds[i];
(*b)[0].acceleration_bounded_ = true;
(*b)[0].max_acceleration_ = incoming_accelerations(i);
joint_bounds.emplace_back(b);
}

// Create incoming accelerations with only joint 1 and joint 2 over limit by a factor of 0.1 and 0.05
// Scale down all other joint accelerations by 0.3 to keep it within limits.
incoming_accelerations(0) *= 1.1;
incoming_accelerations(1) *= 1.05;
incoming_accelerations.tail<5>() *= 0.7;

constexpr double tol = 0.001;

// The resulting scaling factor from joints should be 1 / 1.1 = 0.90909
double user_acceleration_override = 0.0;
double scaling_factor = moveit_servo::jointLimitAccelerationScalingFactor(incoming_accelerations, joint_bounds,
user_acceleration_override);
ASSERT_NEAR(scaling_factor, 1.0 / 1.1, tol);

// With a scaling override lower than the joint limit scaling, it should use the override value.
user_acceleration_override = 0.5;
scaling_factor = moveit_servo::jointLimitAccelerationScalingFactor(incoming_accelerations, joint_bounds,
user_acceleration_override);
ASSERT_NEAR(scaling_factor, 0.5, tol);

// With a scaling override higher than the joint limit scaling, it should still use the joint limits.
// Safety always first!
user_acceleration_override = 1.0;
scaling_factor = moveit_servo::jointLimitAccelerationScalingFactor(incoming_accelerations, joint_bounds,
user_acceleration_override);
ASSERT_NEAR(scaling_factor, 1.0 / 1.1, tol);
}

TEST(ServoUtilsUnitTests, validVector)
{
Eigen::VectorXd valid_vector(7);
Expand Down

0 comments on commit 459a5b0

Please sign in to comment.