Pick with custom max_velocity_scaling_factor during approach+lift

This commit is contained in:
Robert Haschke 2025-02-15 19:34:39 +01:00
parent 24f22484ca
commit f3659da82b
3 changed files with 5 additions and 0 deletions

View File

@ -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.

View File

@ -228,6 +228,7 @@ class TestStages(unittest.TestCase):
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")

View File

@ -98,6 +98,8 @@ approach.header.frame_id = "world"
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"