From 7f10292ab349da7f1c611d31ca38f4bf8dd16112 Mon Sep 17 00:00:00 2001 From: Robert Haschke Date: Thu, 4 May 2023 15:29:53 +0200 Subject: [PATCH] JointInterpolationPlanner: pass optional max_effort property along to GripperCommand (#458) MoveIt passes the effort field of the last trajectory point as the max_effort for a GripperCommand. Thus we pass the max_effort property to the effort field of the trajectory's last waypoint. --- core/src/solvers/joint_interpolation.cpp | 16 ++++++++++++++++ 1 file changed, 16 insertions(+) diff --git a/core/src/solvers/joint_interpolation.cpp b/core/src/solvers/joint_interpolation.cpp index e9727529..705510cd 100644 --- a/core/src/solvers/joint_interpolation.cpp +++ b/core/src/solvers/joint_interpolation.cpp @@ -51,6 +51,8 @@ using namespace trajectory_processing; JointInterpolationPlanner::JointInterpolationPlanner() { auto& p = properties(); p.declare("max_step", 0.1, "max joint step"); + // allow passing max_effort to GripperCommand actions via + p.declare("max_effort", "max_effort for GripperCommand actions"); } void JointInterpolationPlanner::init(const core::RobotModelConstPtr& /*robot_model*/) {} @@ -95,6 +97,20 @@ bool JointInterpolationPlanner::plan(const planning_scene::PlanningSceneConstPtr timing->computeTimeStamps(*result, props.get("max_velocity_scaling_factor"), props.get("max_acceleration_scaling_factor")); + // set max_effort on first and last waypoint (first, because we might reverse the trajectory) + const auto& max_effort = properties().get("max_effort"); + if (!max_effort.empty()) { + double effort = boost::any_cast(max_effort); + for (const auto* jm : jmg->getActiveJointModels()) { + if (jm->getVariableCount() != 1) + continue; + result->getFirstWayPointPtr()->dropAccelerations(); + result->getFirstWayPointPtr()->setJointEfforts(jm, &effort); + result->getLastWayPointPtr()->dropAccelerations(); + result->getLastWayPointPtr()->setJointEfforts(jm, &effort); + } + } + return true; }