feat(simple_controller_manager): add max_effort parameter to Grippe…

…rCommand action This commit adds the `max_effort` parameter to the GripperCommand declaration in the `controller_list` (see issue moveit#2956). This value is only used when effort is set in the requested gripper trajectory. Co-authored-by: Jafar Abdi <[email protected]>
rickstaa · Dec 8, 2021 · c65a194 · c65a194
1 parent cb52064
commit c65a194
Show file tree

Hide file tree

Showing 2 changed files with 18 additions and 3 deletions.
diff --git a/...e_controller_manager/include/moveit_simple_controller_manager/gripper_controller_handle.h b/...e_controller_manager/include/moveit_simple_controller_manager/gripper_controller_handle.h
@@ -51,10 +51,11 @@ class GripperControllerHandle : public ActionBasedControllerHandle<control_msgs:
 {
 public:
   /* Topics will map to name/ns/goal, name/ns/result, etc */
-  GripperControllerHandle(const std::string& name, const std::string& ns)
+  GripperControllerHandle(const std::string& name, const std::string& ns, const double max_effort = 0.0)
     : ActionBasedControllerHandle<control_msgs::GripperCommandAction>(name, ns)
     , allow_failure_(false)
     , parallel_jaw_gripper_(false)
+    , max_effort_(max_effort)
   {
   }
 
@@ -110,7 +111,6 @@ class GripperControllerHandle : public ActionBasedControllerHandle<control_msgs:
     // goal to be sent
     control_msgs::GripperCommandGoal goal;
     goal.command.position = 0.0;
-    goal.command.max_effort = 0.0;
 
     // send last point
     int tpoint = trajectory.joint_trajectory.points.size() - 1;
@@ -130,7 +130,13 @@ class GripperControllerHandle : public ActionBasedControllerHandle<control_msgs:
       goal.command.position += trajectory.joint_trajectory.points[tpoint].positions[idx];
 
       if (trajectory.joint_trajectory.points[tpoint].effort.size() > 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<control_msgs:
    */
   bool parallel_jaw_gripper_;
 
+  /*
+   * The ``max_effort`` used in the GripperCommand message when no ``max_effort`` was
+   * specified in the requested trajectory. Defaults to ``0.0``.
+   */
+  double max_effort_;
+
   /*
    * A GripperCommand message has only a single float64 for the
    * "command", thus only a single joint angle can be sent -- however,

diff --git a/moveit_plugins/moveit_simple_controller_manager/src/moveit_simple_controller_manager.cpp b/moveit_plugins/moveit_simple_controller_manager/src/moveit_simple_controller_manager.cpp
@@ -94,7 +94,10 @@ class MoveItSimpleControllerManager : public moveit_controller_manager::MoveItCo
         ActionBasedControllerHandleBasePtr new_handle;
         if (type == "GripperCommand")
         {
-          new_handle = std::make_shared<GripperControllerHandle>(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<GripperControllerHandle>(name, action_ns, max_effort);
           if (static_cast<GripperControllerHandle*>(new_handle.get())->isConnected())
           {
             if (controller_list[i].hasMember("parallel"))