Search code examples
c++collision-detectionrosmoveit

MoveIt: How to use collision_detection::CollisionEnv::distanceRobot method?

I would like to find the minimum distance and normal vector between a robot (franka panda) and an obstacle via MoveIt and ROS. The collision_detection::CollisionEnv::distanceRobot method from MoveIt (documentation) provides exactly the info I want but unfortunately is not available in the Python API. So, I would like some help on how to call this function since I am not familiar with C++.

This is my attempt so far:

#include <ros/ros.h>

// MoveIt
#include <moveit/robot_model_loader/robot_model_loader.h>
#include <moveit/planning_scene/planning_scene.h>
#include <moveit/collision_detection/collision_env.h>
#include <moveit_msgs/CollisionObject.h>
#include <moveit/move_group_interface/move_group_interface.h>
#include <moveit/planning_scene_interface/planning_scene_interface.h>


int main(int argc, char** argv)
{
  ros::init(argc, argv, "panda_coll_detection");
  ros::AsyncSpinner spinner(1);
  spinner.start();

  // ---------------------------------------------------------------------------
  // Define interfaces
  // ---------------------------------------------------------------------------

  // Define move_group_interface & planning_scene_interface
  static const std::string PLANNING_GROUP = "panda_arm";
  moveit::planning_interface::MoveGroupInterface move_group_interface(PLANNING_GROUP);
  moveit::planning_interface::PlanningSceneInterface planning_scene_interface; 

  // Define planning_scene
  robot_model_loader::RobotModelLoader robot_model_loader("robot_description");
  const moveit::core::RobotModelPtr& kinematic_model = robot_model_loader.getModel();
  planning_scene::PlanningScene planning_scene(kinematic_model);

  // ---------------------------------------------------------------------------
  // Add a collision object
  // ---------------------------------------------------------------------------

  // Define a collision object ROS message for the robot to avoid
  moveit_msgs::CollisionObject collision_object;
  collision_object.header.frame_id = move_group_interface.getPlanningFrame();
  collision_object.id = "box1";

  // Define a box to add to the world
  shape_msgs::SolidPrimitive primitive;
  primitive.type = primitive.BOX;
  primitive.dimensions.resize(3);
  primitive.dimensions[primitive.BOX_X] = 0.4;
  primitive.dimensions[primitive.BOX_Y] = 0.02;
  primitive.dimensions[primitive.BOX_Z] = 0.02;

  // Define a pose for the box (specified relative to frame_id)
  geometry_msgs::Pose box_pose;
  box_pose.orientation.w = 1.0;
  box_pose.position.x = 0.5;
  box_pose.position.y = 0.0;
  box_pose.position.z = 0.35;

  // Add box to collision_object
  collision_object.primitives.push_back(primitive);
  collision_object.primitive_poses.push_back(box_pose);
  collision_object.operation = collision_object.ADD;

  // Define a vector of collision objects that could contain additional objects
  std::vector<moveit_msgs::CollisionObject> collision_objects;
  collision_objects.push_back(collision_object);

  // Add the collision object into the world
  planning_scene_interface.applyCollisionObjects(collision_objects);

  // ---------------------------------------------------------------------------
  // Min distance & Collision Checking 
  // ---------------------------------------------------------------------------

  // Define collision request & result
  auto distance_request = collision_detection::DistanceRequest();
  auto distance_result = collision_detection::DistanceResult();
  
  // Get robot state & model
  moveit::core::RobotState copied_state = planning_scene.getCurrentState();
  const moveit::core::RobotModelConstPtr kinematic_model_cnstPtr = robot_model_loader.getModel();
  
  // Call distanceRobot()
  collision_detection::CollisionEnv collision_env(kinematic_model_cnstPtr, 0.0, 1.0);
  collision_env.distanceRobot(distance_request, distance_result, copied_state);

  // Show result
  ROS_INFO_STREAM("Collision="<<distance_result.collision);  // ToDo: Get all results
  

  ros::shutdown();
  return 0;
}

To be precise, my question is:

  1. How to properly declare collision_env and call distanceRobot()?

    Currently, I am getting this error: "cannot declare variable ‘collision_env’ to be of abstract type ‘collision_detection::CollisionEnv’".

Additionally,

  1. The CollisionEnv constructor has multiple versions (doc). One requires model,padding,scale as inputs and another requires model,world,padding,scale. What is the difference and which one should I use?

  2. In the MoveIt tutorials is stated that the PlanningSceneMonitor is the recommended method to create and maintain the current planning scene and not instantiating a PlanningScene class directly. However, they do not provide any examples using the PlanningSceneMonitor. If my current approach is not the recommended one, where can I find examples on how to use the PlanningSceneMonitor?

UPDATE:

Based on andersonjwan's answer, I included this header file and made the following change:

#include <moveit/collision_detection_fcl/collision_env_fcl.h>

collision_detection::CollisionEnvFCL collision_env(kinematic_model_cnstPtr, 0.0, 1.0);
Solution

  • By just looking through your sample code, I will answer your explicit questions first and then provide some additional tips that you may find useful to you.

    1. How to properly declare collision_env and call distanceRobot()?

    The namespace collision_detection provides several class type definitions. For your case, the error you are receiving derives from trying to instantiate an abstract class (i.e., an interface).

    Instead, you should be instantiating a concrete class (i.e., a class) that implements the interface (i.e., derived class). From briefly looking at the documentation, it seems you have several options:

    From your application, it seems that the CollisionEnvDistanceField, CollisionEnvFCL, or CollisionEnvHybrid where FCL stands for the Flexible Collision Library would be most appropriate to use.

    1. The CollisionEnv constructor has multiple versions (doc). One requires model,padding,scale as inputs and another requires model,world,padding,scale. What is the difference and which one should I use?

    With respect to the first answer, I would review the constructors for the class you require based on your planned application. In most cases, it is better to start with less and add use the more verbose ones as needed.

    1. In the MoveIt tutorials is stated that the PlanningSceneMonitor is the recommended method to create and maintain the current planning scene and not instantiating a PlanningScene class directly. However, they do not provide any examples using the PlanningSceneMonitor. If my current approach is not the recommended one, where can I find examples on how to use the PlanningSceneMonitor?

    It seems more information of differentiating the PlanningScene from the PlanningSceneMonitor can be found here.

    However, after briefly reading through, it seems that the PlanningScene is the wrapper to capture ROS-related scene events (e.g., objects in the world, robot state, etc), and the PlanningSceneMonitor encompasses the PlanningScene and more; so, it should be used in order to effectively retrieve more information from the ROS environment where the PlanningScene monitor provides minimum information.

    Some Tips:

    • To reduce the length of the line, it may be useful to use the using directive to qualify namespaces, so you do not have to type out the entire namespace path.

    For example, it is natural to qualify class types that have no conflicts with your own:

    using moveit::planning_interface::MoveGroupInterface;

int main(int argc, char** argv) {
    MoveGroupInterface move_group_interface(...);
}