Tesseract Collision Package

Background

This package is used for performing both discrete and continuous collision checking. It understands nothing about connectivity of the object within. It purely allows for the user to add objects to the checker, set object transforms, enable/disable objects, set contact distance per object, and perform collision checks.

Note

The contact managers are load as plugins through a yaml config file which is added to the SRDF file.

Contact Manager Plugin Config

The contact manager config file has four sections:

Section	Description
search_paths	A list of locations to search for libraries. It searches in the order provided.
search_libraries	A list of libraries to search for classes
discrete_plugins	The discrete contact manater plugins plugins.
continuous_plugins	The continuous contact manater plugins plugins.

Example Config file:

contact_manager_plugins:
  search_paths:
    - /usr/local/lib
  search_libraries:
    - tesseract_collision_bullet_factories
    - tesseract_collision_fcl_factories
  discrete_plugins:
    default: BulletDiscreteBVHManager
    plugins:
      BulletDiscreteBVHManager:
        class: BulletDiscreteBVHManagerFactory
      BulletDiscreteSimpleManager:
        class: BulletDiscreteSimpleManagerFactory
      FCLDiscreteBVHManager:
        class: FCLDiscreteBVHManagerFactory
  continuous_plugins:
    default: BulletCastBVHManager
    plugins:
      BulletCastBVHManager:
        class: BulletCastBVHManagerFactory
      BulletCastSimpleManager:
        class: BulletCastSimpleManagerFactory

Features

1. Add Links

2. Set link transforms

3. Enable/Disable links

4. Set default contact threshold

5. Set link pair contact threshold

6. Perform differnt contact test types (FIRST, CLOSEST, ALL)

Contact Test Types

Contact Test Type	Description
ContactTestType::FIRST	Exit on first contact. This is good for planners like Descartes and OMPL.
ContactTestType::CLOSEST	Store only closets for each collision object. This is good for planners like TrajOpt.
ContactTestType::ALL	Store all contacts for each collision object. This is good for planners like TrajOpt.

Available Contact Checkers

Contact Checker	Type	Description
BulletDiscreteBVHManager	Discrete	This leverages the Bullet Physics BVH to perform discrete contact checking
BulletCastBVHManager	Continuous	This leverages the Bullet Physics BVH to perform continuous contact checking by creating a casted convex hull between two link locations.
BulletDiscreteSimpleManager	Discrete	This leverages the Bullet Physics to perform discrete contact checking. It is a naive implementation where it loops over all contact pairs and checks for contact. In relatively small environments this can be faster than leveraging the BVH implementation.
BulletCastSimpleManager	Continuous	This leverages the Bullet Physics to perform continuous contact checking by creating a casted convex hull between two link locations.. It is a naive implementation where it loops over all contact pairs and checks for contact. In relatively small environments this can be faster than leveraging the BVH implementation.
FCLDiscreteBVHManager	Discrete	This leverages the Flexible Collision Library to perform discrete contact checking.
GPUDiscreteBVHManager	Discrete	This leverages an library for performing GPU/CPU contact checking

Discrete Collision Checker Example

#include <tesseract_common/macros.h>
TESSERACT_COMMON_IGNORE_WARNINGS_PUSH
#include <console_bridge/console.h>
TESSERACT_COMMON_IGNORE_WARNINGS_POP

#include <tesseract_collision/bullet/bullet_discrete_bvh_manager.h>
#include <tesseract_collision/bullet/convex_hull_utils.h>

using namespace tesseract_collision;
using namespace tesseract_geometry;

std::string toString(const Eigen::MatrixXd& a)
{
  std::stringstream ss;
  ss << a;
  return ss.str();
}

std::string toString(bool b) { return b ? "true" : "false"; }

int main(int /*argc*/, char** /*argv*/)
{
  // documentation:start:1: Create Collision Manager
  tesseract_collision_bullet::BulletDiscreteBVHManager checker;
  // documentation:end:1: Create Collision Manager

  // documentation:start:2: Add box to checker
  // Create a box
  CollisionShapePtr box = std::make_shared<Box>(1, 1, 1);
  Eigen::Isometry3d box_pose;
  box_pose.setIdentity();

  CollisionShapesConst obj1_shapes;
  tesseract_common::VectorIsometry3d obj1_poses;
  obj1_shapes.push_back(box);
  obj1_poses.push_back(box_pose);

  // Add box to checker in enabled state
  checker.addCollisionObject("box_link", 0, obj1_shapes, obj1_poses);
  // documentation:end:2: Add box to checker

  // documentation:start:3: Add thin box
  // Create a thin box
  CollisionShapePtr thin_box = std::make_shared<Box>(0.1, 1, 1);
  Eigen::Isometry3d thin_box_pose;
  thin_box_pose.setIdentity();

  CollisionShapesConst obj2_shapes;
  tesseract_common::VectorIsometry3d obj2_poses;
  obj2_shapes.push_back(thin_box);
  obj2_poses.push_back(thin_box_pose);

  // Add thin box to checker in disabled state
  checker.addCollisionObject("thin_box_link", 0, obj2_shapes, obj2_poses, false);
  // documentation:end:3: Add thin box

  // documentation:start:4: Add convex hull
  // Add second box to checker, but convert to convex hull mesh
  CollisionShapePtr second_box;

  auto mesh_vertices = std::make_shared<tesseract_common::VectorVector3d>();
  auto mesh_faces = std::make_shared<Eigen::VectorXi>();
  loadSimplePlyFile(std::string(TESSERACT_SUPPORT_DIR) + "/meshes/box_2m.ply", *mesh_vertices, *mesh_faces, true);

  auto mesh = std::make_shared<tesseract_geometry::Mesh>(mesh_vertices, mesh_faces);
  second_box = makeConvexMesh(*mesh);
  // documentation:end:4: Add convex hull

  // documentation:start:5: Add convex hull collision
  Eigen::Isometry3d second_box_pose;
  second_box_pose.setIdentity();

  CollisionShapesConst obj3_shapes;
  tesseract_common::VectorIsometry3d obj3_poses;
  obj3_shapes.push_back(second_box);
  obj3_poses.push_back(second_box_pose);

  checker.addCollisionObject("second_box_link", 0, obj3_shapes, obj3_poses);
  // documentation:end:5: Add convex hull collision

  CONSOLE_BRIDGE_logInform("Test when object is inside another");
  // documentation:start:6: Set active collision object
  checker.setActiveCollisionObjects({ "box_link", "second_box_link" });
  // documentation:end:6: Set active collision object

  // documentation:start:7: Set contact distance threshold
  checker.setCollisionMarginData(CollisionMarginData(0.1));
  // documentation:end:7: Set contact distance threshold

  // documentation:start:8: Set collision object transform
  // Set the collision object transforms
  tesseract_common::TransformMap location;
  location["box_link"] = Eigen::Isometry3d::Identity();
  location["box_link"].translation()(0) = 0.2;
  location["box_link"].translation()(1) = 0.1;
  location["second_box_link"] = Eigen::Isometry3d::Identity();

  checker.setCollisionObjectsTransform(location);
  // documentation:end:8: Set collision object transform

  // documentation:start:9: Perform collision check
  ContactResultMap result;
  ContactRequest request(ContactTestType::CLOSEST);
  checker.contactTest(result, request);

  ContactResultVector result_vector;
  flattenMoveResults(std::move(result), result_vector);

  CONSOLE_BRIDGE_logInform("Has collision: %s", toString(result_vector.empty()).c_str());
  CONSOLE_BRIDGE_logInform("Distance: %f", result_vector[0].distance);
  CONSOLE_BRIDGE_logInform("Link %s nearest point: %s",
                           result_vector[0].link_names[0].c_str(),
                           toString(result_vector[0].nearest_points[0]).c_str());
  CONSOLE_BRIDGE_logInform("Link %s nearest point: %s",
                           result_vector[0].link_names[1].c_str(),
                           toString(result_vector[0].nearest_points[1]).c_str());
  CONSOLE_BRIDGE_logInform("Direction to move Link %s out of collision with Link %s: %s",
                           result_vector[0].link_names[0].c_str(),
                           result_vector[0].link_names[1].c_str(),
                           toString(result_vector[0].normal).c_str());
  // documentation:end:9: Perform collision check

  // documentation:start:10: Set collision object transform
  CONSOLE_BRIDGE_logInform("Test object is out side the contact distance");
  location["box_link"].translation() = Eigen::Vector3d(1.60, 0, 0);
  checker.setCollisionObjectsTransform(location);
  // documentation:end:10: Set collision object transform

  // documentation:start:11: Perform collision check
  result = ContactResultMap();
  result.clear();
  result_vector.clear();

  // Check for collision after moving object
  checker.contactTest(result, request);
  flattenMoveResults(std::move(result), result_vector);
  CONSOLE_BRIDGE_logInform("Has collision: %s", toString(result_vector.empty()).c_str());
  // documentation:end:11: Perform collision check

  // documentation:start:12: Change contact distance threshold
  // Set higher contact distance threshold
  checker.setDefaultCollisionMarginData(0.25);
  // documentation:end:12: Change contact distance threshold

  // documentation:start:13: Perform collision check
  CONSOLE_BRIDGE_logInform("Test object inside the contact distance");
  result = ContactResultMap();
  result.clear();
  result_vector.clear();

  // Check for contact with new threshold
  checker.contactTest(result, request);
  flattenMoveResults(std::move(result), result_vector);

  CONSOLE_BRIDGE_logInform("Has collision: %s", toString(result_vector.empty()).c_str());
  CONSOLE_BRIDGE_logInform("Distance: %f", result_vector[0].distance);
  CONSOLE_BRIDGE_logInform("Link %s nearest point: %s",
                           result_vector[0].link_names[0].c_str(),
                           toString(result_vector[0].nearest_points[0]).c_str());
  CONSOLE_BRIDGE_logInform("Link %s nearest point: %s",
                           result_vector[0].link_names[1].c_str(),
                           toString(result_vector[0].nearest_points[1]).c_str());
  CONSOLE_BRIDGE_logInform("Direction to move Link %s further from Link %s: %s",
                           result_vector[0].link_names[0].c_str(),
                           result_vector[0].link_names[1].c_str(),
                           toString(result_vector[0].normal).c_str());
  // documentation:end:13: Perform collision check
}

You can find this example here.

Example Explanation

Create Contact Checker

  tesseract_collision_bullet::BulletDiscreteBVHManager checker;

Add Collision Objects to Contact Checker

Add a collision object in a enabled state

  // Create a box
  CollisionShapePtr box = std::make_shared<Box>(1, 1, 1);
  Eigen::Isometry3d box_pose;
  box_pose.setIdentity();

  CollisionShapesConst obj1_shapes;
  tesseract_common::VectorIsometry3d obj1_poses;
  obj1_shapes.push_back(box);
  obj1_poses.push_back(box_pose);

  // Add box to checker in enabled state
  checker.addCollisionObject("box_link", 0, obj1_shapes, obj1_poses);

Note

A collision object can consist of multiple collision shapes.

Add collision object in a disabled state

  // Create a thin box
  CollisionShapePtr thin_box = std::make_shared<Box>(0.1, 1, 1);
  Eigen::Isometry3d thin_box_pose;
  thin_box_pose.setIdentity();

  CollisionShapesConst obj2_shapes;
  tesseract_common::VectorIsometry3d obj2_poses;
  obj2_shapes.push_back(thin_box);
  obj2_poses.push_back(thin_box_pose);

  // Add thin box to checker in disabled state
  checker.addCollisionObject("thin_box_link", 0, obj2_shapes, obj2_poses, false);

Create convex hull from mesh file

  // Add second box to checker, but convert to convex hull mesh
  CollisionShapePtr second_box;

  auto mesh_vertices = std::make_shared<tesseract_common::VectorVector3d>();
  auto mesh_faces = std::make_shared<Eigen::VectorXi>();
  loadSimplePlyFile(std::string(TESSERACT_SUPPORT_DIR) + "/meshes/box_2m.ply", *mesh_vertices, *mesh_faces, true);

  auto mesh = std::make_shared<tesseract_geometry::Mesh>(mesh_vertices, mesh_faces);
  second_box = makeConvexMesh(*mesh);

Add convex hull collision object

  Eigen::Isometry3d second_box_pose;
  second_box_pose.setIdentity();

  CollisionShapesConst obj3_shapes;
  tesseract_common::VectorIsometry3d obj3_poses;
  obj3_shapes.push_back(second_box);
  obj3_poses.push_back(second_box_pose);

  checker.addCollisionObject("second_box_link", 0, obj3_shapes, obj3_poses);

Set the active collision objects

  checker.setActiveCollisionObjects({ "box_link", "second_box_link" });

Set the contact distance threshold

  checker.setCollisionMarginData(CollisionMarginData(0.1));

Set the collision object’s transform

  // Set the collision object transforms
  tesseract_common::TransformMap location;
  location["box_link"] = Eigen::Isometry3d::Identity();
  location["box_link"].translation()(0) = 0.2;
  location["box_link"].translation()(1) = 0.1;
  location["second_box_link"] = Eigen::Isometry3d::Identity();

  checker.setCollisionObjectsTransform(location);

Perform collision check

Note

One object is inside another object

  ContactResultMap result;
  ContactRequest request(ContactTestType::CLOSEST);
  checker.contactTest(result, request);

  ContactResultVector result_vector;
  flattenMoveResults(std::move(result), result_vector);

  CONSOLE_BRIDGE_logInform("Has collision: %s", toString(result_vector.empty()).c_str());
  CONSOLE_BRIDGE_logInform("Distance: %f", result_vector[0].distance);
  CONSOLE_BRIDGE_logInform("Link %s nearest point: %s",
                           result_vector[0].link_names[0].c_str(),
                           toString(result_vector[0].nearest_points[0]).c_str());
  CONSOLE_BRIDGE_logInform("Link %s nearest point: %s",
                           result_vector[0].link_names[1].c_str(),
                           toString(result_vector[0].nearest_points[1]).c_str());
  CONSOLE_BRIDGE_logInform("Direction to move Link %s out of collision with Link %s: %s",
                           result_vector[0].link_names[0].c_str(),
                           result_vector[0].link_names[1].c_str(),
                           toString(result_vector[0].normal).c_str());

Set the collision object’s transform

  CONSOLE_BRIDGE_logInform("Test object is out side the contact distance");
  location["box_link"].translation() = Eigen::Vector3d(1.60, 0, 0);
  checker.setCollisionObjectsTransform(location);

Perform collision check

Note

The objects are outside the contact threshold

  result = ContactResultMap();
  result.clear();
  result_vector.clear();

  // Check for collision after moving object
  checker.contactTest(result, request);
  flattenMoveResults(std::move(result), result_vector);
  CONSOLE_BRIDGE_logInform("Has collision: %s", toString(result_vector.empty()).c_str());

Change contact distance threshold

  // Set higher contact distance threshold
  checker.setDefaultCollisionMarginData(0.25);

Perform collision check

Note

The objects are inside the contact threshold

  CONSOLE_BRIDGE_logInform("Test object inside the contact distance");
  result = ContactResultMap();
  result.clear();
  result_vector.clear();

  // Check for contact with new threshold
  checker.contactTest(result, request);
  flattenMoveResults(std::move(result), result_vector);

  CONSOLE_BRIDGE_logInform("Has collision: %s", toString(result_vector.empty()).c_str());
  CONSOLE_BRIDGE_logInform("Distance: %f", result_vector[0].distance);
  CONSOLE_BRIDGE_logInform("Link %s nearest point: %s",
                           result_vector[0].link_names[0].c_str(),
                           toString(result_vector[0].nearest_points[0]).c_str());
  CONSOLE_BRIDGE_logInform("Link %s nearest point: %s",
                           result_vector[0].link_names[1].c_str(),
                           toString(result_vector[0].nearest_points[1]).c_str());
  CONSOLE_BRIDGE_logInform("Direction to move Link %s further from Link %s: %s",
                           result_vector[0].link_names[0].c_str(),
                           result_vector[0].link_names[1].c_str(),
                           toString(result_vector[0].normal).c_str());

Running the Example

Build the Tesseract Workspace:

catkin build

Navigate to the build folder containing the executable:

cd build/tesseract_collision/examples

Run the executable:

./tesseract_collision_box_box_example