Pick with custom max_velocity_scaling_factor during approach+lift

Author: rhaschke

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>
@@ -434,6 +435,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/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"