[jog_arm] Split collision proximity threshold (moveit#2008)

* separate proximity threshold values for self-collisions and scene collisions
* increase default value of scene collision proximity threshold
* deprecate old parameters

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]

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;

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();

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 "