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

Split collision proximity threshold #2008

Merged
merged 7 commits into from
Apr 13, 2020
Merged

Split collision proximity threshold #2008

Show file tree
Hide file tree
Changes from 6 commits
Commits
Show all changes
7 commits
Select commit Hold shift + click to select a range
b819c45
added separate self-collision proximity threshold
Apr 2, 2020
080e0d4
larger scene collision proximity threshold
Apr 3, 2020
dea0d6c
trying Henning's suggestion
Apr 8, 2020
80ecf97
Henning's comments
Apr 8, 2020
43d4385
self-collision checking needs the ACM
Apr 8, 2020
91b989e
better param deprecation
Apr 8, 2020
14af9a5
capture collision result
Apr 9, 2020
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
19 changes: 8 additions & 11 deletions moveit_core/collision_detection_fcl/src/collision_common.cpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -465,19 +465,16 @@ bool distanceCallback(fcl::CollisionObjectd* o1, fcl::CollisionObjectd* o2, void
cd1->getID().c_str(), cd2->getID().c_str());
}
}
else
else if (cd2->type == BodyTypes::ROBOT_LINK && cd1->type == BodyTypes::ROBOT_ATTACHED)
JStech marked this conversation as resolved.
Show resolved Hide resolved
{
if (cd2->type == BodyTypes::ROBOT_LINK && cd1->type == BodyTypes::ROBOT_ATTACHED)
const std::set<std::string>& tl = cd1->ptr.ab->getTouchLinks();
if (tl.find(cd2->getID()) != tl.end())
{
const std::set<std::string>& tl = cd1->ptr.ab->getTouchLinks();
if (tl.find(cd2->getID()) != tl.end())
{
always_allow_collision = true;
if (cdata->req->verbose)
ROS_DEBUG_NAMED("collision_detection.fcl",
"Robot link '%s' is allowed to touch attached object '%s'. No distances are computed.",
cd2->getID().c_str(), cd1->getID().c_str());
}
always_allow_collision = true;
if (cdata->req->verbose)
ROS_DEBUG_NAMED("collision_detection.fcl",
"Robot link '%s' is allowed to touch attached object '%s'. No distances are computed.",
cd2->getID().c_str(), cd1->getID().c_str());
}
}

Expand Down
3 changes: 2 additions & 1 deletion moveit_experimental/moveit_jog_arm/config/ur_simulated_config.yaml
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -53,4 +53,5 @@ command_out_topic: joint_group_position_controller/command # Publish outgoing co
## Collision checking for the entire robot body
check_collisions: true # Check collisions?
collision_check_rate: 10 # [Hz] Collision-checking can easily bog down a CPU if done too often.
collision_proximity_threshold: 0.01 # Start decelerating when a collision is this far [m]
self_collision_proximity_threshold: 0.01 # Start decelerating when a self-collision is this far [m]
scene_collision_proximity_threshold: 0.05 # Start decelerating when a scene collision is this far [m]
3 changes: 2 additions & 1 deletion moveit_experimental/moveit_jog_arm/include/moveit_jog_arm/jog_arm_data.h
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -121,7 +121,8 @@ struct JogArmParameters
double joint_scale;
double lower_singularity_threshold;
double hard_stop_singularity_threshold;
double collision_proximity_threshold;
double scene_collision_proximity_threshold;
double self_collision_proximity_threshold;
double low_pass_filter_coeff;
double publish_period;
double incoming_command_timeout;
Expand Down
39 changes: 30 additions & 9 deletions moveit_experimental/moveit_jog_arm/src/collision_check_thread.cpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -70,14 +70,19 @@ void CollisionCheckThread::startMainLoop(JogArmShared& shared_variables)
// Copy the planning scene's version of current state into new memory
moveit::core::RobotState current_state(getLockedPlanningSceneRO()->getCurrentState());

double velocity_scale_coefficient = -log(0.001) / parameters_.collision_proximity_threshold;
const double self_velocity_scale_coefficient = -log(0.001) / parameters_.self_collision_proximity_threshold;
const double scene_velocity_scale_coefficient = -log(0.001) / parameters_.scene_collision_proximity_threshold;
ros::Rate collision_rate(parameters_.collision_check_rate);

double self_collision_distance = 0;
double scene_collision_distance = 0;

// Scale robot velocity according to collision proximity and user-defined thresholds.
// I scaled exponentially (cubic power) so velocity drops off quickly after the threshold.
// Proximity decreasing --> decelerate
double velocity_scale = 1;

collision_detection::AllowedCollisionMatrix acm = getLockedPlanningSceneRO()->getAllowedCollisionMatrix();
/////////////////////////////////////////////////
// Spin while checking collisions
/////////////////////////////////////////////////
Expand All @@ -92,12 +97,20 @@ void CollisionCheckThread::startMainLoop(JogArmShared& shared_variables)
for (std::size_t i = 0; i < jts.position.size(); ++i)
current_state.setJointPositions(jts.name[i], &jts.position[i]);

collision_result.clear();
current_state.updateCollisionBodyTransforms();

// Do a thread-safe distance-based collision detection
getLockedPlanningSceneRO()->checkCollision(collision_request, collision_result, current_state);
collision_result.clear();
getLockedPlanningSceneRO()->getCollisionEnv()->checkRobotCollision(collision_request, collision_result,
current_state);
scene_collision_distance = collision_result.distance;

collision_result.clear();
JStech marked this conversation as resolved.
Show resolved Hide resolved
getLockedPlanningSceneRO()->getCollisionEnvUnpadded()->checkSelfCollision(collision_request, collision_result,
current_state, acm);
self_collision_distance = collision_result.distance;

velocity_scale = 1;
// If we're definitely in collision, stop immediately
if (collision_result.collision)
{
Expand All @@ -106,15 +119,23 @@ void CollisionCheckThread::startMainLoop(JogArmShared& shared_variables)

// If we are far from a collision, velocity_scale should be 1.
// If we are very close to a collision, velocity_scale should be ~zero.
// When collision_proximity_threshold is breached, start decelerating exponentially.
else if (collision_result.distance < parameters_.collision_proximity_threshold)
// When scene_collision_proximity_threshold is breached, start decelerating exponentially.
if (scene_collision_distance < parameters_.scene_collision_proximity_threshold)
{
// velocity_scale = e ^ k * (collision_result.distance - threshold)
// velocity_scale = e ^ k * (collision_distance - threshold)
// k = - ln(0.001) / collision_proximity_threshold
// velocity_scale should equal one when collision_result.distance is at collision_proximity_threshold.
// velocity_scale should equal 0.001 when collision_result.distance is at zero.
// velocity_scale should equal one when collision_distance is at collision_proximity_threshold.
// velocity_scale should equal 0.001 when collision_distance is at zero.
velocity_scale =
std::min(velocity_scale, exp(scene_velocity_scale_coefficient *
(scene_collision_distance - parameters_.scene_collision_proximity_threshold)));
}

if (self_collision_distance < parameters_.self_collision_proximity_threshold)
{
velocity_scale =
exp(velocity_scale_coefficient * (collision_result.distance - parameters_.collision_proximity_threshold));
std::min(velocity_scale, exp(self_velocity_scale_coefficient *
(self_collision_distance - parameters_.self_collision_proximity_threshold)));
}

shared_variables.lock();
Expand Down
39 changes: 35 additions & 4 deletions moveit_experimental/moveit_jog_arm/src/jog_interface_base.cpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -88,8 +88,28 @@ bool JogInterfaceBase::readParameters(ros::NodeHandle& n)
ros_parameters_.lower_singularity_threshold);
error += !rosparam_shortcuts::get("", n, parameter_ns + "/hard_stop_singularity_threshold",
ros_parameters_.hard_stop_singularity_threshold);
error += !rosparam_shortcuts::get("", n, parameter_ns + "/collision_proximity_threshold",
JStech marked this conversation as resolved.
Show resolved Hide resolved
ros_parameters_.collision_proximity_threshold);
// parameter was removed, replaced with separate self- and scene-collision proximity thresholds; if old parameter
// exists, print warning and use old parameter for both new parameter values
// TODO(JStech): remove this deprecation warning in ROS Noetic
if (!rosparam_shortcuts::get("", n, parameter_ns + "/self_collision_proximity_threshold",
ros_parameters_.self_collision_proximity_threshold) &&
!rosparam_shortcuts::get("", n, parameter_ns + "/scene_collision_proximity_threshold",
ros_parameters_.scene_collision_proximity_threshold))
{
if (rosparam_shortcuts::get("", n, parameter_ns + "/collision_proximity_threshold",
ros_parameters_.self_collision_proximity_threshold))
{
ROS_WARN_NAMED(LOGNAME,
"'collision_proximity_threshold' parameter is deprecated, and has been replaced by separate"
"'self_collision_proximity_threshold' and 'scene_collision_proximity_threshold' "
"parameters. Please update the jogging yaml file.");
ros_parameters_.scene_collision_proximity_threshold = ros_parameters_.self_collision_proximity_threshold;
}
else
{
error += 1;
}
}
error += !rosparam_shortcuts::get("", n, parameter_ns + "/move_group_name", ros_parameters_.move_group_name);
error += !rosparam_shortcuts::get("", n, parameter_ns + "/planning_frame", ros_parameters_.planning_frame);
error += !rosparam_shortcuts::get("", n, parameter_ns + "/use_gazebo", ros_parameters_.use_gazebo);
Expand Down Expand Up @@ -143,12 +163,23 @@ bool JogInterfaceBase::readParameters(ros::NodeHandle& n)
"greater than zero. Check yaml file.");
return false;
}
if (ros_parameters_.collision_proximity_threshold < 0.)
if (ros_parameters_.self_collision_proximity_threshold < 0.)
{
ROS_WARN_NAMED(LOGNAME, "Parameter 'self_collision_proximity_threshold' should be "
"greater than zero. Check yaml file.");
return false;
}
if (ros_parameters_.scene_collision_proximity_threshold < 0.)
{
ROS_WARN_NAMED(LOGNAME, "Parameter 'collision_proximity_threshold' should be "
ROS_WARN_NAMED(LOGNAME, "Parameter 'scene_collision_proximity_threshold' should be "
"greater than zero. Check yaml file.");
return false;
}
if (ros_parameters_.scene_collision_proximity_threshold < ros_parameters_.self_collision_proximity_threshold)
{
ROS_WARN_NAMED(LOGNAME, "Parameter 'self_collision_proximity_threshold' should probably be less "
"than or equal to 'scene_collision_proximity_threshold'. Check yaml file.");
}
if (ros_parameters_.low_pass_filter_coeff < 0.)
{
ROS_WARN_NAMED(LOGNAME, "Parameter 'low_pass_filter_coeff' should be "
Expand Down