Fix broken industrial collision detection

constrained_ik/CMakeLists.txt

@@ -21,8 +21,7 @@ find_package(catkin REQUIRED COMPONENTS
               moveit_core
               dynamic_reconfigure
               tf_conversions
-              cmake_modules
-              industrial_collision_detection)
+              cmake_modules)
 
 ## System dependencies are found with CMake's conventions
 find_package(orocos_kdl REQUIRED)
@@ -66,7 +65,7 @@ generate_dynamic_reconfigure_options(
 catkin_package(
  INCLUDE_DIRS include ${catkin_INCLUDE_DIRS} ${EIGEN3_INCLUDE_DIRS}
  LIBRARIES constrained_ik constrained_ik_constraints constrained_ik_moveit
- CATKIN_DEPENDS roscpp urdf eigen_conversions tf_conversions urdf moveit_ros_planning moveit_core dynamic_reconfigure kdl_parser cmake_modules industrial_collision_detection
+ CATKIN_DEPENDS roscpp urdf eigen_conversions tf_conversions urdf moveit_ros_planning moveit_core dynamic_reconfigure kdl_parser cmake_modules
  DEPENDS Boost EIGEN3 orocos_kdl
 )
 

constrained_ik/include/constrained_ik/constraints/avoid_obstacles.h

@@ -29,7 +29,7 @@
 
 #include "constrained_ik/constraint.h"
 #include "constrained_ik/constrained_ik.h"
-#include <industrial_collision_detection/collision_detection/collision_common.h>
+#include <moveit/collision_detection/collision_common.h>
 #include <vector>
 #include <algorithm>
 #include <kdl/chain.hpp>
@@ -40,6 +40,27 @@ namespace constrained_ik
 namespace constraints
 {
 
+/** @brief Contains distance information in the planning frame queried from getDistanceInfo() */
+struct DistanceInfo
+{
+  EIGEN_MAKE_ALIGNED_OPERATOR_NEW
+  std::string nearest_obsticle;     /**< The link name for nearest obsticle/link to request link. */
+  Eigen::Vector3d link_point;       /**< Point on request link */
+  Eigen::Vector3d obsticle_point;   /**< Point on nearest link to requested link */
+  Eigen::Vector3d avoidance_vector; /**< Normilized Vector created by nearest points */
+  double distance;                  /**< Distance between nearest points */
+};
+typedef std::map<std::string, DistanceInfo> DistanceInfoMap;
+
+/**
+ * @brief getDistanceInfo
+ * @param distance_detailed Detailed Distance Map
+ * @param distance_info_map Stores the distance information for each link in DistanceDetailedMap
+ * @param tf This allows for a transformation to be applied the distance data since it is always returned in the world frame from fcl.
+ * @return bool, true if succesfully converted DistanceDetailedMap to DistanceInfoMap
+ */
+bool getDistanceInfo(const collision_detection::DistanceMap &distance_detailed, DistanceInfoMap &distance_info_map, const Eigen::Affine3d &tf);
+
 /**
  * @class constrained_ik::constraints::AvoidObstacles
  * @brief Constraint to avoid obstacles
@@ -124,6 +145,7 @@ class AvoidObstacles: public Constraint
   }
 
 public:
+
   EIGEN_MAKE_ALIGNED_OPERATOR_NEW
   /** @brief This structure stores constraint data */
   struct AvoidObstaclesData: public ConstraintData
@@ -132,7 +154,7 @@ class AvoidObstacles: public Constraint
     const constraints::AvoidObstacles* parent_; /**< pointer to parent class AvoidObstacles */
     collision_detection::DistanceResult distance_res_; /**< stores the minimum distance results */
     collision_detection::DistanceMap distance_map_; /**< map of link names to its distance results */
-    collision_detection::DistanceInfoMap distance_info_map_; /**< map of link names to distance information */
+    DistanceInfoMap distance_info_map_; /**< map of link names to distance information */
 
     /** @brief See base class for documentation */
     AvoidObstaclesData(const constrained_ik::SolverState &state, const constraints::AvoidObstacles* parent);

constrained_ik/include/constrained_ik/solver_state.h

@@ -31,8 +31,8 @@
 #include <Eigen/Geometry>
 #include <constrained_ik/enum_types.h>
 #include <moveit/planning_scene/planning_scene.h>
-#include <industrial_collision_detection/collision_detection/collision_robot_industrial.h>
-#include <industrial_collision_detection/collision_detection/collision_world_industrial.h>
+#include <moveit/collision_detection/collision_robot.h>
+#include <moveit/collision_detection/collision_world.h>
 
 namespace constrained_ik
 {
@@ -54,8 +54,8 @@ struct SolverState
   bool auxiliary_at_limit;                                               /**< This is set if auxiliary reached motion or iteration limit. */
   initialization_state::InitializationState condition;                   /**< State of the IK Solver */
   planning_scene::PlanningSceneConstPtr planning_scene;                  /**< Pointer to the planning scene, some constraints require it */
-  collision_detection::CollisionRobotIndustrialConstPtr collision_robot; /**< Pointer to the collision robot, some constraints require it */
-  collision_detection::CollisionWorldIndustrialConstPtr collision_world; /**< Pointer to the collision world, some constraints require it */
+  collision_detection::CollisionRobotConstPtr collision_robot;           /**< Pointer to the collision robot, some constraints require it */
+  collision_detection::CollisionWorldConstPtr collision_world;            /**< Pointer to the collision world, some constraints require it */
   moveit::core::RobotStatePtr robot_state;                               /**< Pointer to the current robot state */
   std::string group_name;                                                /**< Move group name */
 

constrained_ik/package.xml

@@ -27,7 +27,6 @@
   <build_depend>eigen_conversions</build_depend>
   <build_depend>cmake_modules</build_depend>
   <build_depend>dynamic_reconfigure</build_depend>
-  <build_depend>industrial_collision_detection</build_depend>
 
   <run_depend>boost</run_depend>
   <run_depend>kdl_parser</run_depend>
@@ -41,7 +40,6 @@
   <run_depend>eigen_conversions</run_depend>
   <run_depend>cmake_modules</run_depend>
   <run_depend>dynamic_reconfigure</run_depend>
-  <run_depend>industrial_collision_detection</run_depend>
 
   <!-- The export tag contains other, unspecified, tags -->
   <export>

constrained_ik/src/constrained_ik.cpp

@@ -240,8 +240,8 @@ bool Constrained_IK::calcInvKin(const Eigen::Affine3d &goal,
       throw std::runtime_error(error_message.str());
     }
 
-    state.collision_robot = std::static_pointer_cast<const collision_detection::CollisionRobotIndustrial>(planning_scene->getCollisionRobot());
-    state.collision_world = std::static_pointer_cast<const collision_detection::CollisionWorldIndustrial>(planning_scene->getCollisionWorld());
+    state.collision_robot = planning_scene->getCollisionRobot();
+    state.collision_world = planning_scene->getCollisionWorld();
   }
 
   if (state.condition == initialization_state::NothingInitialized || state.condition == initialization_state::AuxiliaryOnly)

constrained_ik/src/constraints/avoid_obstacles.cpp

@@ -53,7 +53,56 @@ using Eigen::MatrixXd;
 using std::string;
 using std::vector;
 
-AvoidObstacles::LinkAvoidance::LinkAvoidance(): weight_(DEFAULT_WEIGHT), min_distance_(DEFAULT_MIN_DISTANCE), avoidance_distance_(DEFAULT_AVOIDANCE_DISTANCE), amplitude_(DEFAULT_AMPLITUDE), jac_solver_(NULL) {}
+bool getDistanceInfo(const collision_detection::DistanceMap &distance_detailed, DistanceInfoMap &distance_info_map, const Eigen::Affine3d &tf)
+{
+  bool status = true;
+  for (DistanceMap::const_iterator it = distance_detailed.begin(); it != distance_detailed.end(); ++it)
+  {
+    DistanceInfo dist_info;
+    DistanceResultsData dist = static_cast<const DistanceResultsData>(it->second.front());
+    if (dist.link_names[0] == it->first.first)
+    {
+      dist_info.nearest_obsticle = dist.link_names[1];
+      dist_info.link_point = tf * dist.nearest_points[0];
+      dist_info.obsticle_point = tf * dist.nearest_points[1];
+      dist_info.avoidance_vector = dist_info.link_point - dist_info.obsticle_point;
+      dist_info.avoidance_vector.normalize();
+      dist_info.distance = dist.distance;
+    }
+    else if (dist.link_names[1] == it->first.first)
+    {
+      dist_info.nearest_obsticle = dist.link_names[0];
+      dist_info.link_point = tf * dist.nearest_points[1];
+      dist_info.obsticle_point = tf * dist.nearest_points[0];
+      dist_info.avoidance_vector = dist_info.link_point - dist_info.obsticle_point;
+      dist_info.avoidance_vector.normalize();
+      dist_info.distance = dist.distance;
+    }
+    else
+    {
+      ROS_ERROR("getDistanceInfo was unable to find link after match!");
+      status &= false;
+    }
+
+    distance_info_map.insert(std::make_pair(it->first.first, dist_info));
+  }
+
+  return status;
+}
+
+AvoidObstacles::LinkAvoidance::LinkAvoidance():
+    weight_(DEFAULT_WEIGHT),
+    min_distance_(DEFAULT_MIN_DISTANCE),
+    avoidance_distance_(DEFAULT_AVOIDANCE_DISTANCE),
+    amplitude_(DEFAULT_AMPLITUDE),
+    jac_solver_(NULL),
+    num_inboard_joints_(0),
+    num_robot_joints_(0),
+    num_obstacle_joints_(0)
+{
+
+}
+
 AvoidObstacles::LinkAvoidance::LinkAvoidance(std::string link_name): LinkAvoidance() {link_name_ = link_name;}
 
 void AvoidObstacles::init(const Constrained_IK * ik)
@@ -187,7 +236,8 @@ ConstraintResults AvoidObstacles::evalConstraint(const SolverState &state) const
     DistanceInfoMap::const_iterator dit = cdata.distance_info_map_.find(it->second.link_name_);
     if (dit != cdata.distance_info_map_.end() && dit->second.distance > 0)
     {
-      dynamic_weight = std::exp(DYNAMIC_WEIGHT_FUNCTION_CONSTANT * (std::abs(dit->second.distance-cdata.distance_res_.minimum_distance.min_distance)/distance_threshold_));
+      dynamic_weight = std::exp(DYNAMIC_WEIGHT_FUNCTION_CONSTANT * (std::abs(
+          dit->second.distance-cdata.distance_res_.minimum_distance.distance)/distance_threshold_));
       constrained_ik::ConstraintResults tmp;
       tmp.error = calcError(cdata, it->second) * it->second.weight_ * dynamic_weight;
       tmp.jacobian = calcJacobian(cdata, it->second)  * it->second.weight_ * dynamic_weight;
@@ -274,7 +324,12 @@ bool AvoidObstacles::checkStatus(const AvoidObstacles::AvoidObstaclesData &cdata
 
 AvoidObstacles::AvoidObstaclesData::AvoidObstaclesData(const SolverState &state, const AvoidObstacles *parent): ConstraintData(state), parent_(parent)
 {
-  DistanceRequest distance_req(true, false, parent_->link_models_, state_.planning_scene->getAllowedCollisionMatrix(), parent_->distance_threshold_);
+  DistanceRequest distance_req;
+  distance_req.enable_nearest_points = true;
+  distance_req.enable_signed_distance = true;
+  distance_req.active_components_only = &parent_->link_models_;
+  distance_req.acm = &state_.planning_scene->getAllowedCollisionMatrix();
+  distance_req.distance_threshold = parent_->distance_threshold_;
   distance_req.group_name = state.group_name;
   distance_res_.clear();
 
@@ -292,7 +347,7 @@ AvoidObstacles::AvoidObstaclesData::AvoidObstaclesData(const SolverState &state,
     state.collision_world->checkRobotCollision(collision_req, collision_res, *state.collision_robot, *state_.robot_state, state_.planning_scene->getAllowedCollisionMatrix());
   }
   Eigen::Affine3d tf = state_.robot_state->getGlobalLinkTransform(parent_->ik_->getKin().getRobotBaseLinkName()).inverse();
-  getDistanceInfo(distance_res_.distance, distance_info_map_, tf);
+  getDistanceInfo(distance_res_.distances, distance_info_map_, tf);
 }
 
 } // end namespace constraints

industrial_collision_detection/README.md

@@ -0,0 +1,7 @@
+Industrial Collision Detection
+=====
+
+This package started as a copy of the MoveIt! **collision_detection** library and was modified to include distance query  
+ features.  Eventually these features were [added to MoveIt!](https://github.com/ros-planning/moveit/pull/662/files) so this package is now deprecated and developers are encouraged to use  
+ the MoveIt! **collision_detection** library.  Nevertheless, this package will be kept around as there may be functionality that is relevant  
+ to the **constrained_ik** package that was not ported over.