diff --git a/moveit_experimental/moveit_jog_arm/config/ur_simulated_config.yaml b/moveit_experimental/moveit_jog_arm/config/ur_simulated_config.yaml index f978ea58174..8a42b955dd6 100644 --- a/moveit_experimental/moveit_jog_arm/config/ur_simulated_config.yaml +++ b/moveit_experimental/moveit_jog_arm/config/ur_simulated_config.yaml @@ -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] diff --git a/moveit_experimental/moveit_jog_arm/include/moveit_jog_arm/jog_arm_data.h b/moveit_experimental/moveit_jog_arm/include/moveit_jog_arm/jog_arm_data.h index 25b3fef8163..ec2f69794e8 100644 --- a/moveit_experimental/moveit_jog_arm/include/moveit_jog_arm/jog_arm_data.h +++ b/moveit_experimental/moveit_jog_arm/include/moveit_jog_arm/jog_arm_data.h @@ -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; diff --git a/moveit_experimental/moveit_jog_arm/src/collision_check_thread.cpp b/moveit_experimental/moveit_jog_arm/src/collision_check_thread.cpp index 0657f13229c..e2425be5d6e 100644 --- a/moveit_experimental/moveit_jog_arm/src/collision_check_thread.cpp +++ b/moveit_experimental/moveit_jog_arm/src/collision_check_thread.cpp @@ -70,14 +70,20 @@ 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; + bool collision_detected; + // 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 ///////////////////////////////////////////////// @@ -92,29 +98,48 @@ 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(); + collision_detected = false; // 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_detected |= collision_result.collision; + + collision_result.clear(); + getLockedPlanningSceneRO()->getCollisionEnvUnpadded()->checkSelfCollision(collision_request, collision_result, + current_state, acm); + self_collision_distance = collision_result.distance; + collision_detected |= collision_result.collision; + velocity_scale = 1; // If we're definitely in collision, stop immediately - if (collision_result.collision) + if (collision_detected) { velocity_scale = 0; } // 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(); diff --git a/moveit_experimental/moveit_jog_arm/src/jog_interface_base.cpp b/moveit_experimental/moveit_jog_arm/src/jog_interface_base.cpp index d07b10981a1..9b318d7b987 100644 --- a/moveit_experimental/moveit_jog_arm/src/jog_interface_base.cpp +++ b/moveit_experimental/moveit_jog_arm/src/jog_interface_base.cpp @@ -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", - 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); @@ -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 "