Replaced collision checking from MoveIt by a direct interface to FCL #125

VladimirIvan · 2017-09-26T15:55:21Z

Moved CollisionScene to a separate header and turned it into an abstract class.
Moved KinematicElement into a separate header.
Moved all FCL dependent code into a plugin extending the CollisionScene class.
Implemented a direct interface to FCL using collision objects and pointers to KinematicElements.
Added SceneInitializer property to set the collision scene (selecting a plugin to load).
Added fcl_catkin package compiling the FCL 0.6.0 from source (submodue).
Added CollisionSceneFCLLatest correctly computing correct distances between shapes using FCL 0.6.0 (insteas of the outdated moveit version).
Added CollisionProxy class capturing distances between shapes and publishProxies method to publish them as markers to Rviz.

When computing distances/proxies:

Collision normal is the direction from one contact point to the other.
When the shapes are colliding, centres of shapes are used due to insufficient contact points provided by the FCL.
Distance computation only works with FCL 0.6.0.

…ica.git into vi-collisionscene

- Workaround for travis-ci/apt-source-safelist#229

wxmerkt · 2017-09-27T10:55:00Z

exotations/collision_scene_fcl/include/collision_scene_fcl/CollisionSceneFCL.h

+       * @return     The collision world links.
+       */
+    virtual std::vector<std::string> getCollisionWorldLinks()
+    {


Why are e.g. these two methods fully defined in the header while the others are in the implementation cpp?

wxmerkt · 2017-09-27T10:55:11Z

exotations/collision_scene_fcl/include/collision_scene_fcl/CollisionSceneFCL.h

+       * @return     The collision robot links.
+       */
+    virtual std::vector<std::string> getCollisionRobotLinks()
+    {


wxmerkt · 2017-09-27T11:30:59Z

exotations/collision_scene_fcl_latest/src/CollisionSceneFCLLatest.cpp

+
+namespace fcl_convert
+{
+void fcl2Eigen(const fcl::Vector3d & fcl, Eigen::Vector3d & eigen)


All of the fcl2Eigen are unused in the project as of now - remnant?

wxmerkt · 2017-09-27T11:34:51Z

exotations/collision_scene_fcl_latest/src/CollisionSceneFCLLatest.cpp

+        }
+        else
+        {
+            new_object = cache_entry->second;


Not sure but this may be a copy operation, so perhaps move new_object into the if(cache_entry==end() and the L99 fixed in both the if and else block with the corresponding right-hand-side, i.e. the else-block would be fcl_objects_[i++] = cache_entry->second.get(); and the if-block fcl_objects_[i++] = constructFclCollisionObject(object.second).get();

Nope, just copying the pointer, not the data.

wxmerkt · 2017-09-27T11:35:56Z

exotations/collision_scene_fcl_latest/src/CollisionSceneFCLLatest.cpp

+    }
+}
+
+// This function was copied from 'moveit_core/collision_detection_fcl/src/collision_common.cpp'


Provide link to the file in the ros-planning/moveit GitHub repository so people can check for license, changes, etc.

wxmerkt · 2017-09-28T11:15:51Z

exotica/resources/configs/example_manipulate_ompl.xml

@@ -10,7 +10,6 @@

    <PlanningScene>
      <Scene>
-        <!-- Computing distances is enabled by default, disable for a Sampling problem by setting the PlanningMode to speed up Scene updates -->


I think this comment was helpful for people using the example configs to learn about how the system works.

That is a deprecated parameter. I removed it from the XML.

wxmerkt · 2017-09-28T11:17:52Z

exotica/src/Scene.cpp

-                   << joint_index_.size() << " but " << x.rows()
-                   << " is provided");
-
-    if (BaseType == BASE_TYPE::FIXED)


Since this was such a major pain to fix, just to make sure: does the new CollisionScene work correctly for a) fixed base, b) floating base, c) mixed base, i.e. model is floating base but planning group is fixed base?

There is no base handling in the collision scene. All of that is in the KinematicTree.

wxmerkt · 2017-09-28T11:19:26Z

exotica_python/src/Exotica.cpp

@@ -552,6 +553,19 @@ PYBIND11_MODULE(_pyexotica, module)
    samplingProblem.def("getSpaceDim", &SamplingProblem::getSpaceDim);
    samplingProblem.def("getBounds", &SamplingProblem::getBounds);

+    py::class_<CollisionProxy, std::shared_ptr<CollisionProxy>> proxy(module, "Proxy");
+    proxy.def(py::init());
+    proxy.def_readonly("Contact1", &CollisionProxy::contact1);


A bit at unease with the inconsistency of using Uppercase at the start of variables - sometimes it's lower case (most common), then in other places suddenly uppercase...

wxmerkt · 2017-09-28T11:20:09Z

exotica_python/src/Exotica.cpp

-      }
-      return py::make_tuple(d, p1, p2);
-    });
+    scene.def("isStateValid", [](Scene* instance, bool self) {return instance->getCollisionScene()->isStateValid(self);}, py::arg("self")=true);


I'd like the margin/safe distance back - we used it heavily in the past (both in C++ and in Python).

wxmerkt · 2017-09-28T11:28:02Z

exotica_python/src/Exotica.cpp

-        instance->getCollisionScene()->getRobotDistance(link, self, d, p1, p2, norm, c1, c2, safeDist);
-        return py::make_tuple(d, norm);
-    });
-    scene.def("getDistance", [](Scene* instance, std::string o1, std::string o2,


I think we should bring this one back.

Done. The return is a vector of proxies.

wxmerkt · 2017-09-29T13:18:21Z

PlanningMode is now deprecated

- Removed CollisionData.Done - Separated narrow phace routine - Added collision/distance check overloads taking object names - Fixed typose, comments, and removed depricated XML tags

…ica.git into vi-collisionscene

wxmerkt · 2017-09-29T15:56:29Z

exotations/collision_scene_fcl_latest/src/CollisionSceneFCLLatest.cpp

+    data->Proxies.push_back(p);
+}
+
+bool CollisionSceneFCLLatest::collisionCallbackDistance(fcl::CollisionObjectd* o1, fcl::CollisionObjectd* o2, void* data, double& dist)


dist is now being passed in as a reference but not assigned a value/used as part of the function - computeDistance changes the values in data_ but dist does not get assigned a value.

- Addresses #149 - Broken in #125 Conflicts: exotica_python/src/Exotica.cpp

- Addresses and resolves #149 - Broken in #125

- Addresses #149 - Broken in #125

- Addresses and resolves #149 - Broken in #125

wxmerkt · 2017-10-16T19:35:39Z

exotica/src/Scene.cpp

-          world_joint_ + "/trans_y", x(1));
-      ps_->getCurrentStateNonConst().setVariablePosition(
-          world_joint_ + "/trans_z", x(2));
-      KDL::Rotation rot = KDL::Rotation::EulerZYX(x(3), x(4), x(5));


This is why never noticed the ZYX <> RPY bug in Kinematica in practice - Kinematica was never used for floating base and the MoveIt planning scene for sampling based planning was correct due to this.

Cf. #138

- Broken in #125 - Adds method that updates the model state from Kinematica just before publishing - Does not handle attached objects (addObject/attachObject/detachObject) - Addresses #150

Vladimir Ivan and others added 22 commits September 24, 2017 20:32

Added CollisionTreeMap, Shape storage and closes robot link

c975722

Added color tool and removed update counter from ompl solver

43758a3

Added simplified allowed collision matrix

310baa1

Reimplemented collision checking by directly updating FCL

e952fd0

Added collision proxy publishing

4b34c2e

Moved moveit planning scene

336781a

Cleaned up collision scene

743d3c1

Moved KinematicElement to its own header

692b425

Moved proxyToMarker to scene

086469e

Minor fixes

dbc0f92

Moved collision checking into a plugin

c30e0ea

Fixed normal orientation

f5950cc

Added FCL 0.6.0 collision scene

49cd444

Switched to using double precision

c9b020f

switched to using fcl_catkin

b42ae76

Changed proxy publish frame transformation

fcfadec

Add fcl_catkin submodule

01a1214

Changed to using shape centres when in collision

323003c

Changed proxy markers

113c858

Added collision scene parameter into the initializer

b893942

Added collision distance example, updated attach example

8ef1a5e

Merge branch 'vi-collisionscene' of git@github.com:openhumanoids/exot…

4dbfa55

…ica.git into vi-collisionscene

VladimirIvan requested a review from wxmerkt September 26, 2017 15:55

Travis: add lccd-debs ppa

1a3bfdb

- Workaround for travis-ci/apt-source-safelist#229

VladimirIvan mentioned this pull request Sep 27, 2017

Simple FK and Jacobian methods #126

Merged

wxmerkt requested changes Sep 28, 2017

View reviewed changes

wxmerkt mentioned this pull request Sep 29, 2017

Re-add getDistance(link_name, link_name) and map between collision objects and links #130

Closed

3 tasks

Vladimir Ivan added 2 commits September 29, 2017 16:27

PR fixes:

ac20417

- Removed CollisionData.Done - Separated narrow phace routine - Added collision/distance check overloads taking object names - Fixed typose, comments, and removed depricated XML tags

Merge branch 'vi-collisionscene' of git@github.com:openhumanoids/exot…

0b20dce

…ica.git into vi-collisionscene

wxmerkt reviewed Sep 29, 2017

View reviewed changes

wxmerkt approved these changes Sep 29, 2017

View reviewed changes

wxmerkt merged commit b9cea2c into master Sep 29, 2017

wxmerkt deleted the vi-collisionscene branch September 29, 2017 16:16

wxmerkt mentioned this pull request Oct 1, 2017

trans_x floating joint does not get created anymore after #125 #136

Closed

This was referenced Oct 16, 2017

Scene updates from MoveIt messages broken #149

Closed

MoveIt Planning Scene publishing broken #150

Closed

wxmerkt added a commit that referenced this pull request Oct 16, 2017

Scene: Fix updateWorld, including in Python

4e9e29e

- Addresses #149 - Broken in #125 Conflicts: exotica_python/src/Exotica.cpp

wxmerkt added a commit that referenced this pull request Oct 16, 2017

Scene: Fix setCollisionScene

5ff431a

- Addresses and resolves #149 - Broken in #125

wxmerkt added a commit that referenced this pull request Oct 16, 2017

Scene: Fix updateWorld, including in Python

28b0f2d

- Addresses #149 - Broken in #125

wxmerkt added a commit that referenced this pull request Oct 16, 2017

Scene: Fix setCollisionScene

a266da0

- Addresses and resolves #149 - Broken in #125

wxmerkt reviewed Oct 16, 2017

View reviewed changes

wxmerkt mentioned this pull request Oct 16, 2017

Scene: Add way to update MoveIt PlanningScene to fix scene publishing #154

Merged

Provide feedback

Saved searches

Use saved searches to filter your results more quickly

Replaced collision checking from MoveIt by a direct interface to FCL #125

Replaced collision checking from MoveIt by a direct interface to FCL #125

VladimirIvan commented Sep 26, 2017

wxmerkt Sep 27, 2017

wxmerkt Sep 27, 2017

wxmerkt Sep 27, 2017

wxmerkt Sep 27, 2017

VladimirIvan Sep 29, 2017

wxmerkt Sep 27, 2017

wxmerkt Sep 28, 2017

VladimirIvan Sep 29, 2017

wxmerkt Sep 28, 2017

VladimirIvan Sep 29, 2017

wxmerkt Sep 28, 2017

wxmerkt Sep 28, 2017

VladimirIvan Sep 29, 2017

wxmerkt Sep 28, 2017

VladimirIvan Sep 29, 2017

wxmerkt commented Sep 29, 2017

wxmerkt Sep 29, 2017

wxmerkt Oct 16, 2017

Replaced collision checking from MoveIt by a direct interface to FCL #125

Replaced collision checking from MoveIt by a direct interface to FCL #125

Conversation

VladimirIvan commented Sep 26, 2017

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

wxmerkt commented Sep 29, 2017

Choose a reason for hiding this comment

Choose a reason for hiding this comment