Add ability to request detailed distance information from fcl #662

Levi-Armstrong · 2017-10-18T21:45:19Z

This PR expose the ability to request detailed distance information from FCL. I followed a similar architecture to the collision request and response. I wanted to get this submitted, but there a few remaining items: Update Collision World FCL and correct formatting.

davetcoleman · 2017-10-19T03:52:35Z

great contribution! looks like clang-format is off

Levi-Armstrong · 2017-10-19T22:07:43Z

@davetcoleman As a result of this PR it implements a request and response structure like the collision checking methods which eliminate the need for several methods which I have marked with the deprecated macro. As a result this creates about 260 warning messages throughout MoveIt, how would you like this to be handled?

davetcoleman · 2017-10-20T02:48:37Z

Is there an issue with changing the collision checking calls throughout the codebase to the new API? This shouldn't break any public-facing APIs, correct?

Levi-Armstrong · 2017-10-23T13:05:51Z

I will update the codebase to use the new methods.

davetcoleman · 2017-12-08T03:43:07Z

I have restarted the build

rhaschke

Thanks for this great contribution. However, I have some general remarks:

I think, the collision world functionality should be implemented as well ;-)
You should prepare another commit, removing the now deprecated functions. I guess the use of the old CollisionData structure becomes obsolete then as well?
Having prepared this cleanup commit, will facilitate (or enable at all) the code cleanup in a later release.

rhaschke · 2017-12-08T07:49:34Z

moveit_core/collision_detection/include/moveit/collision_detection/collision_common.h

+  {
+  }
+
+  DistanceRequest(bool detailed, bool global, const std::set<const robot_model::LinkModel*>* active_components_only,


These are a lot of very similar constructors and it's rather hard to figure out their actual differences.
As we are using C++11, could you please change them to use constructor forwarding, e.g.:

DistanceRequest(bool detailed, bool global, const std::set<const robot_model::LinkModel*>& active_components_only, const AllowedCollisionMatrix& acm, double distance_threshold = std::numeric_limits<double>::max()) : DistanceRequest(detailed, global, &active_components_only, &acm, distance_threshold) {}

I think the documentation should point out, that the set of active components as well as the acm should stay in memory as long as the DistanceRequest exists, because you only store pointers to them.

I think, the collision world functionality should be implemented as well ;-)

I believe they have been implemented as part of the PR. Is there something missing?

You should prepare another commit, removing the now deprecated functions.

So you want me to remove the deprecated function as part of this PR, wont that break others code?

I guess the use of the old CollisionData structure becomes obsolete then as well?

This should not have any affect on the Collision checking data structure but they are similar.

You should prepare another commit, removing the now deprecated functions.

So you want me to remove the deprecated function as part of this PR, wont that break others code?

No, the new commit should become another PR, that we can apply e.g. to the Lunar branch (or the next release branch). Just, not to forget about cleaning up...

rhaschke · 2017-12-08T07:51:06Z

moveit_core/collision_detection/include/moveit/collision_detection/collision_common.h

+  }
+
+  /// Update structure data given DistanceResultsData object
+  void update(const DistanceResultsData& results)


Should this read void operator=(const DistanceResultsData& other)?

rhaschke · 2017-12-08T07:53:39Z

moveit_core/collision_detection/include/moveit/collision_detection/collision_robot.h

@@ -163,16 +164,19 @@ class CollisionRobot
                                   const AllowedCollisionMatrix& acm) const = 0;

  /** \brief The distance to self-collision given the robot is at state \e state. */
+  MOVEIT_DEPRECATED


Deprecation should indicate the replacement method. Same applies for the following methods.

Not yet addressed.

rhaschke · 2017-12-08T08:00:49Z

moveit_core/collision_detection/include/moveit/collision_detection/collision_common.h

+  double min_distance;
+
+  /// The nearest points
+  Eigen::Vector3d nearest_points[2];


I guess nearest_points[i] refers to a point on link_name[i]
Rename to nearest_point to be consistent with link_name.

rhaschke · 2017-12-08T08:01:59Z

moveit_core/collision_detection/include/moveit/collision_detection/collision_common.h

+  std::string link_name[2];
+
+  /// The gradient
+  Eigen::Vector3d gradient;


Specify the direction of the gradient: nearest_point[1] - nearest_point[0] or the other way around?
Is the vector normalized?

rhaschke · 2017-12-08T08:03:43Z

moveit_core/collision_detection/include/moveit/collision_detection/collision_common.h

+  /// The gradient
+  Eigen::Vector3d gradient;
+
+  /// Indicates if gradient was calculated.


Aren't nearest points and gradient coupled? What do I miss?

Yes they are coupled

Actually my statement is not correct. In the case where there is not a collision the statement is true but in the case of a collision the nearest points are the same for the fcl case so and the normal comes from contact structure. I recently ran into this and I am pushing changes along with your other requested changes.

rhaschke · 2017-12-08T08:05:21Z

moveit_core/collision_detection/include/moveit/collision_detection/collision_common.h

+  /// Indicates if two objects were in collision
+  bool collision;
+
+  /// The results for the two objects with the minimum distance


I think this needs more documentation. What's the difference between minimum_distance and distance?
What exactly is stored in the DistanceMap?

The DistanceMap stores distance results information for every link in active components list.

rhaschke · 2017-12-08T08:12:47Z

...sion_distance_field/include/moveit/collision_distance_field/collision_world_distance_field.h

+  virtual void distanceRobot(const DistanceRequest& req, DistanceResult& res,
+                             const CollisionRobot& robot, const robot_state::RobotState& state) const
+  {
+    logWarn("Not implemented");


I think this should be logError.

I agree, this is the way it was originally, but I am happy to update them.

Not yet addressed.

This reverts commit b5d0e32.

This reverts commit 58240d4, reversing changes made to 7f90a85.

rhaschke

Thanks for the cleanups. One major question remains: Why don't you get correct distance from fcl, but need to compute that manually?

rhaschke · 2018-01-03T11:02:07Z

moveit_core/collision_detection/include/moveit/collision_detection/collision_common.h

+  /// Indicate if a signed distance should be calculated in a collision.
+  bool enable_signed_distance;
+
+  /// Indicate if the global minimum should only be found be found. If this is true


typo: "be found be found"

rhaschke · 2018-01-03T11:02:51Z

moveit_core/collision_detection/include/moveit/collision_detection/collision_common.h

+
+  /// Indicate if the global minimum should only be found be found. If this is true
+  /// it will only try to find the global minimum distance and not store information 
+  /// on a link by link bases. If this is set to false it will store distance information 


typo: bases -> basis

rhaschke · 2018-01-03T11:10:28Z

moveit_core/collision_detection/include/moveit/collision_detection/collision_common.h


-  /// Indicates if gradient was calculated.
-  bool hasGradient;
+  /// A normalized vector pointing from link_names[0] to link_names[1].


Shouldn't that read:

A normalized vector pointing from nearest_points[0] to nearest_points[1]

Sorry, I believe it should be link_names[0] to link_names[1] mainly because in the event of a collision the nearest_points are the same but the normal is always calculate such that it points from link_names[0] to link_names[1].

rhaschke · 2018-01-03T11:13:30Z

moveit_core/collision_detection/include/moveit/collision_detection/collision_common.h

-  {
-  }
-
-  DistanceRequest(bool detailed, bool global, const std::set<const robot_model::LinkModel*>* active_components_only,


That's a nice cleanup! +1

rhaschke · 2018-01-03T11:16:35Z

moveit_core/collision_detection_fcl/src/collision_common.cpp

+      dist_result.normal = (dist_result.nearest_points[1] - dist_result.nearest_points[0]).normalized();
+    }
+
+    if (d <= 0 && cdata->req->enable_signed_distance)


Why do you need this extra code block? Shouldn't fcl::distance compute the correct distance?

That is what I had hoped, but in fcl 0.5 if two objects are in collision it returns 0 or -1 depending on the object type. Now fcl has been making a lot of improvements over the past year and in version 0.6 which has not been release will return this information directly. This interim fix to get this information until the 0.6 release. I also have another branch which I will push this week that make the necessary change for 0.6 since they reorganized everything and switch to using Eigen instead of custom matrix types.

rhaschke · 2018-01-05T09:00:03Z

Could you, please, also rebase your branch onto the latest kinetic-devel head?

…anceDetailedCallback

Levi-Armstrong · 2018-01-06T00:40:11Z

@rhaschke I believe I have addressed the requested changes.

Levi-Armstrong · 2018-02-28T03:41:53Z

@davetcoleman and @rhaschke, I am currently making one more change to improve the storage of distance information similar to collision contacts. In some cases (Optimization Motion Planning) it would be useful to have all objects within a distance to a given link instead of just the closest one.

Levi-Armstrong · 2018-03-05T05:27:07Z

@davetcoleman and @rhaschke I have made the change let me know if you would like anything changed.

davetcoleman · 2018-03-08T21:18:56Z

moveit_core/collision_detection/include/moveit/collision_detection/collision_common.h

@@ -221,12 +221,21 @@ struct CollisionRequest
  bool verbose;
 };

+enum DistanceRequestType


Namespaceify your enums, e.g.: namespace DistanceRequestType {

"This prevents enums from polluting the namespace they're inside. Individual items within the enum are referenced by: Choices::Choice1, but the typedef still allows declaration of the Choice enum without the namespace."
http://wiki.ros.org/CppStyleGuide

I will fix it.

davetcoleman · 2018-03-08T21:23:03Z

moveit_core/collision_detection_fcl/src/collision_common.cpp

      {
-        cdata->res->collision = true;
+        std::vector<DistanceResultsData> data;
+        data.reserve(cdata->req->max_contacts_per_body);


i don't understand why you reserve this vector then only push_back one item. i don't see a for loop anywhere else to fill this in.

The loop is hidden within fcl, it calls the callback function multiple times.

please add this as a comment since its not clear why reserve is otherwise needed and it would run fine without it (its a hidden optimization)

davetcoleman · 2018-03-08T21:24:02Z

moveit_core/collision_detection_fcl/src/collision_common.cpp


-    if (cdata->req->active_components_only)
+
+  if (it != cdata->res->distances.end())


all this logic you've modified could really use some explanation comments

Before the latest commit it was only possible to get the global minimum distance or the minimum distance for a given link. This logic change adds the ability to get all of the contacts for a given pair of links. This is useful if a link is composed of multiple geometries and by getting all of the contacts you can accurately define a gradient for those pair of links to move them out of collision.

Levi-Armstrong · 2018-03-14T14:18:45Z

@davetcoleman and @rhaschke I have made the requested changes.

davetcoleman

@rhaschke let's get this great addition merged in!

davetcoleman · 2018-03-26T01:08:41Z

moveit_core/collision_detection_fcl/src/collision_common.cpp

      {
-        cdata->res->collision = true;
+        std::vector<DistanceResultsData> data;
+        data.reserve(cdata->req->max_contacts_per_body);


please add this as a comment since its not clear why reserve is otherwise needed and it would run fine without it (its a hidden optimization)

rhaschke · 2018-03-26T06:45:15Z

This is on my todo list. But there is a more urgent deadline this week. Please be patient some more days.

davetcoleman · 2018-03-29T14:11:15Z

@rhaschke its been 5 months, I think we should really move forward with this so other PRs like #746 don't break this one

rhaschke · 2018-04-01T09:36:54Z

moveit_core/collision_detection_fcl/src/collision_common.cpp

+        return cdata->done;
+      }
+    }
+    else if (cdata->req->type != DistanceRequestType::GLOBAL)


These if-checks do not make sense to me. Shouldn't they equality instead of inequality checks?

Good catch, you are correct those should be equality.

rhaschke · 2018-04-01T09:45:54Z

moveit_core/collision_detection_fcl/src/collision_common.cpp

+        dist_result.distance = -contact.penetration_depth;
+        dist_result.nearest_points[0] = Eigen::Vector3d(contact.pos.data.vs);
+        dist_result.nearest_points[1] = Eigen::Vector3d(contact.pos.data.vs);
+        dist_result.normal = Eigen::Vector3d(contact.normal.data.vs);


Here you deviate from your definition that the normal vector connects nearest points. This should be documented.

This is a deviation. If you are using convex to convex checking and in contact it would be as documented but in the case of non-convex to non-convex fcl provides the contact normal (One of the triangle normals). From what I could tell the way it choose which triangle normal to use it picked the one with the least penetration. Also for non-convex to non-convex if the objects are not in contact it is as documented. I something along what I just described sufficient to add?

In the case of moveit there is not a way to tell fcl to use convex representation so currently everything uses non-convex but it would be nice to support this in the future.

Yes, such an explanation will be fine.

rhaschke

I posed one remaining questions. Otherwise LGTM.

Levi-Armstrong · 2018-04-17T16:46:18Z

@davetcoleman and @rhaschke the remaining two requests should be complete.

davetcoleman · 2018-04-17T23:55:47Z

There's a minor clang-format issue failing Travis @Levi-Armstrong

davetcoleman · 2018-04-18T16:42:39Z

Hurray!! 👏

@Levi-Armstrong want to announce this change on the Discourse channel?

BoltzmannBrain · 2018-04-19T17:25:18Z

Thanks for doing this @Levi-Armstrong! Just caught up here, from this ROS-answers thread where I asked about a collision distance per robot link. How can I now implement this feature? Currently I have something like,

auto req = collision_detection::CollisionRequest();
req.distance = true;  // Compute single proximity distance value

BoltzmannBrain · 2018-05-05T20:18:25Z

I've been using the new functionality in one of my feature projects, working off the tip of kinetic-devel. For our workflow, however, the rest of the system uses the most recent release (i.e. 0.9.11). When can we expect a new release to be cut (that includes this merged PR)? @davetcoleman @rhaschke

davetcoleman · 2018-05-05T21:02:44Z

Good question, please move this release discussion to #886

… same node (moveit#662) Co-authored-by: Dave Coleman <dave@picknik.ai>

Levi-Armstrong force-pushed the addDetailedKinetic branch from 30e6d5a to ab9d3d9 Compare October 19, 2017 22:00

davetcoleman mentioned this pull request Dec 8, 2017

Clang Tidy to collision_detection and collision_detection_fcl #693

Closed

5 tasks

BryceStevenWilley added a commit to BryceStevenWilley/moveit that referenced this pull request Dec 8, 2017

Merged with moveit#662.

58240d4

BryceStevenWilley added a commit to BryceStevenWilley/moveit that referenced this pull request Dec 8, 2017

Clang-formated non-collision packages changed in moveit#662.

b5d0e32

rhaschke requested changes Dec 8, 2017

View reviewed changes

BryceStevenWilley added a commit to BryceStevenWilley/moveit that referenced this pull request Dec 9, 2017

Revert "Clang-formated non-collision packages changed in moveit#662."

bf9f702

This reverts commit b5d0e32.

BryceStevenWilley added a commit to BryceStevenWilley/moveit that referenced this pull request Dec 9, 2017

Revert "Merged with moveit#662."

a6af745

This reverts commit 58240d4, reversing changes made to 7f90a85.

rhaschke requested changes Jan 3, 2018

View reviewed changes

Levi-Armstrong added 7 commits January 5, 2018 18:31

Add ability to request detailed distance information from fcl

0c1ddea

remove use of deprecated methods

ae87932

Cleanup DistanceResult and DistanceRequest Structure and improve dist…

227ac8a

…anceDetailedCallback

fix signed distance calculation

8af1ffa

Switch logWarn to logError for collision_distance_field

d8cd714

fix typos

404023e

Add Replaced by for each deprecated method

9a2ef68

Levi-Armstrong force-pushed the addDetailedKinetic branch from 71304c0 to 9a2ef68 Compare January 6, 2018 00:32

Update NULL to nullptr in DistanceRequest

3781cef

rhaschke self-assigned this Jan 9, 2018

rhaschke added 5 commits January 9, 2018 11:25

remove virtual from DistanceData destructor

b0ca04f

replace distanceCallback with new (detailed) function

46ce1ef

keep old functions as conviencency wrappers

52f886f

minimize differences

48b4d3e

clang formatting

0cb9bb3

davetcoleman reviewed Mar 8, 2018

View reviewed changes

Restructure detailed distance requests

e1ce04a

Levi-Armstrong force-pushed the addDetailedKinetic branch from d4ee595 to e1ce04a Compare March 14, 2018 14:09

davetcoleman reviewed Mar 26, 2018

View reviewed changes

davetcoleman mentioned this pull request Mar 29, 2018

Clang tidy core #746

Closed

5 tasks

rhaschke reviewed Apr 1, 2018

View reviewed changes

rhaschke added 3 commits April 1, 2018 11:47

fixed typos + comments

428b6c2

minor optimizations

89ed343

fix clang format

51249ad

rhaschke requested changes Apr 1, 2018

View reviewed changes

davetcoleman mentioned this pull request Apr 11, 2018

code clean up of moveit_core #829

Closed

5 tasks

Levi-Armstrong added 2 commits April 17, 2018 11:38

Correct check to be equality

6eafcaf

Update DistanceResults normal documentation.

eae2515

clang-format

039a65d

rhaschke approved these changes Apr 18, 2018

View reviewed changes

rhaschke merged commit f235eac into moveit:kinetic-devel Apr 18, 2018

BoltzmannBrain mentioned this pull request Jun 26, 2018

New collision distance functionality gives nonsensical/inconsistent results #963

Open

JafarAbdi added a commit to JafarAbdi/moveit that referenced this pull request Mar 24, 2022

PlanningSceneMonitor: Fix warning about having two publisher with the…

4eeacb7

… same node (moveit#662) Co-authored-by: Dave Coleman <dave@picknik.ai>


		if (cdata->req->active_components_only)

		if (it != cdata->res->distances.end())

Add ability to request detailed distance information from fcl #662

Add ability to request detailed distance information from fcl #662

Conversation

Levi-Armstrong commented Oct 18, 2017

davetcoleman commented Oct 19, 2017

Levi-Armstrong commented Oct 19, 2017

davetcoleman commented Oct 20, 2017

Levi-Armstrong commented Oct 23, 2017 • edited Loading

davetcoleman commented Dec 8, 2017

rhaschke left a comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

rhaschke left a comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

rhaschke commented Jan 5, 2018

Levi-Armstrong commented Jan 6, 2018

Levi-Armstrong commented Feb 28, 2018 • edited Loading

Levi-Armstrong commented Mar 5, 2018

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Levi-Armstrong commented Mar 14, 2018

davetcoleman left a comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

rhaschke commented Mar 26, 2018

davetcoleman commented Mar 29, 2018

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

rhaschke left a comment

Choose a reason for hiding this comment

Levi-Armstrong commented Apr 17, 2018

davetcoleman commented Apr 17, 2018 • edited Loading

davetcoleman commented Apr 18, 2018

BoltzmannBrain commented Apr 19, 2018

BoltzmannBrain commented May 5, 2018

davetcoleman commented May 5, 2018 • edited Loading

Levi-Armstrong commented Oct 23, 2017 •

edited

Loading

Levi-Armstrong commented Feb 28, 2018 •

edited

Loading

davetcoleman commented Apr 17, 2018 •

edited

Loading

davetcoleman commented May 5, 2018 •

edited

Loading