diff --git a/core/python/bindings/src/stages.cpp b/core/python/bindings/src/stages.cpp index 7e819a60d..a62d625c6 100644 --- a/core/python/bindings/src/stages.cpp +++ b/core/python/bindings/src/stages.cpp @@ -37,6 +37,7 @@ #include #include #include +#include #include #include #include @@ -452,6 +453,7 @@ void export_stages(pybind11::module& m) { .property("eef_parent_group", "str: Joint model group of the eef's parent") .def(py::init(), "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. diff --git a/core/python/pybind11 b/core/python/pybind11 index f4bc71f98..664876eeb 160000 --- a/core/python/pybind11 +++ b/core/python/pybind11 @@ -1 +1 @@ -Subproject commit f4bc71f981d4eb2dd780215fd3c5a7420f1f03aa +Subproject commit 664876eebf23171fc4ac42853c40cef8b93a57c9 diff --git a/core/python/test/rostest_trampoline.py b/core/python/test/rostest_trampoline.py index 8d33a31d3..98cf4cc89 100755 --- a/core/python/test/rostest_trampoline.py +++ b/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"), diff --git a/core/python/test/test_mtc.py b/core/python/test/test_mtc.py index ff1d7076c..b584ab1ea 100644 --- a/core/python/test/test_mtc.py +++ b/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") diff --git a/demo/scripts/pickplace.py b/demo/scripts/pickplace.py index 709b786c9..89ea0ab20 100755 --- a/demo/scripts/pickplace.py +++ b/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"