Pick cartesian solver access

core/python/bindings/src/stages.cpp

@@ -37,6 +37,7 @@
 #include <moveit/task_constructor/stages.h>
 #include <moveit/task_constructor/stages/pick.h>
 #include <moveit/task_constructor/stages/simple_grasp.h>
+#include <moveit/task_constructor/solvers/cartesian_path.h>
 #include <moveit/planning_scene/planning_scene.h>
 #include <moveit_msgs/PlanningScene.h>
 #include <pybind11/stl.h>
@@ -452,6 +453,7 @@ void export_stages(pybind11::module& m) {
 	    .property<std::string>("eef_parent_group", "str: Joint model group of the eef's parent")
 	    .def(py::init<Stage::pointer&&, const std::string&>(), "grasp_generator"_a,
 	         "name"_a = std::string("pick"))
+	    .def_property_readonly("cartesian_solver", &Pick::cartesianSolver)
 	    .def("setApproachMotion", &Pick::setApproachMotion, R"(
 			The approaching motion towards the grasping state is represented
 			by a twist message.

core/python/test/rostest_trampoline.py

@@ -16,19 +16,6 @@ def setUpModule():
     roscpp_init("test_mtc")
 
 
-def pybind11_versions():
-    try:
-        keys = __builtins__.keys()  # for use with pytest
-    except AttributeError:
-        keys = __builtins__.__dict__.keys()  # use from cmdline
-    return [k for k in keys if k.startswith("__pybind11_internals_v")]
-
-
-incompatible_pybind11_msg = "MoveIt and MTC use incompatible pybind11 versions: " + "\n- ".join(
-    pybind11_versions()
-)
-
-
 class PyGenerator(core.Generator):
     """Implements a custom 'Generator' stage."""
 
@@ -109,18 +96,20 @@ def create(self, *stages):
         return task
 
     def plan(self, task, expected_solutions=None, wait=False):
-        task.plan()
+        try:
+            task.plan()
+        except TypeError as e:
+            self.fail(f"{e}\nDo MoveIt and MTC use ABI-compatible pybind11 versions?")
+
         if expected_solutions is not None:
             self.assertEqual(len(task.solutions), expected_solutions)
         if wait:
             input("Waiting for any key (allows inspection in rviz)")
 
-    @unittest.skipIf(len(pybind11_versions()) > 1, incompatible_pybind11_msg)
     def test_generator(self):
         task = self.create(PyGenerator())
         self.plan(task, expected_solutions=PyGenerator.max_calls)
 
-    @unittest.skipIf(len(pybind11_versions()) > 1, incompatible_pybind11_msg)
     def test_monitoring_generator(self):
         task = self.create(
             stages.CurrentState("current"),

core/python/test/test_mtc.py

@@ -228,6 +228,7 @@ def test_Pick(self):
         self._check(stage, "eef_frame", "eef_frame")
         self._check(stage, "eef_group", "eef_group")
         self._check(stage, "eef_parent_group", "eef_parent_group")
+        self._check(stage.cartesian_solver, "max_velocity_scaling_factor", 0.1)
 
     def test_Place(self):
         generator_stage = stages.GeneratePose("generator")

demo/scripts/pickplace.py

@@ -98,6 +98,8 @@
 approach.twist.linear.z = -1.0
 pick.setApproachMotion(approach, 0.03, 0.1)
 
+pick.cartesian_solver.max_velocity_scaling_factor = 0.1
+
 # Twist to lift the object
 lift = TwistStamped()
 lift.header.frame_id = "panda_hand"