diff --git a/moveit_plugins/moveit_simple_controller_manager/include/moveit_simple_controller_manager/gripper_controller_handle.h b/moveit_plugins/moveit_simple_controller_manager/include/moveit_simple_controller_manager/gripper_controller_handle.h index 212992de5bc..ecdce0fdeb5 100644 --- a/moveit_plugins/moveit_simple_controller_manager/include/moveit_simple_controller_manager/gripper_controller_handle.h +++ b/moveit_plugins/moveit_simple_controller_manager/include/moveit_simple_controller_manager/gripper_controller_handle.h @@ -51,10 +51,11 @@ class GripperControllerHandle : public ActionBasedControllerHandle(name, ns) , allow_failure_(false) , parallel_jaw_gripper_(false) + , max_effort_(max_effort) { } @@ -110,7 +111,6 @@ class GripperControllerHandle : public ActionBasedControllerHandle idx) + { goal.command.max_effort = trajectory.joint_trajectory.points[tpoint].effort[idx]; + } + else + { + goal.command.max_effort = max_effort_; + } } controller_action_client_->sendGoal( @@ -202,6 +208,12 @@ class GripperControllerHandle : public ActionBasedControllerHandle(name, action_ns); + const double max_effort = + controller_list[i].hasMember("max_effort") ? double(controller_list[i]["max_effort"]) : 0.0; + + new_handle = std::make_shared(name, action_ns, max_effort); if (static_cast(new_handle.get())->isConnected()) { if (controller_list[i].hasMember("parallel"))