add velocity interface for gripper

Signed-off-by: Paul Gesel <[email protected]>

gripper_controllers/include/gripper_controllers/gripper_action_controller.hpp

@@ -65,8 +65,9 @@ class GripperActionController : public controller_interface::ControllerInterface
    */
   struct Commands
   {
-    double position_;    // Last commanded position
-    double max_effort_;  // Max allowed effort
+    double position_;         // Last commanded position
+    double target_velocity_;  // Desired gripper velocity
+    double max_effort_;       // Max allowed effort
   };
 
   GRIPPER_ACTION_CONTROLLER_PUBLIC GripperActionController();
@@ -123,9 +124,15 @@ class GripperActionController : public controller_interface::ControllerInterface
 
   bool verbose_ = false;  ///< Hard coded verbose flag to help in debugging
   std::string name_;      ///< Controller name.
+
   std::experimental::optional<std::reference_wrapper<hardware_interface::LoanedCommandInterface>>
     joint_position_command_interface_;
-  std::experimental::optional<std::reference_wrapper<hardware_interface::LoanedStateInterface>>
+  std::optional<std::reference_wrapper<hardware_interface::LoanedCommandInterface>>
+    force_multiplier_interface_;
+  std::optional<std::reference_wrapper<hardware_interface::LoanedCommandInterface>>
+    speed_multiplier_interface_;
+  std::optional<std::reference_wrapper<hardware_interface::LoanedStateInterface>>
+
     joint_position_state_interface_;
   std::experimental::optional<std::reference_wrapper<hardware_interface::LoanedStateInterface>>
     joint_velocity_state_interface_;

gripper_controllers/include/gripper_controllers/gripper_action_controller_impl.hpp

@@ -68,14 +68,14 @@ controller_interface::return_type GripperActionController<HardwareInterface>::up
   const double current_velocity = joint_velocity_state_interface_->get().get_value();
 
   const double error_position = command_struct_rt_.position_ - current_position;
-  const double error_velocity = -current_velocity;
+  const double error_velocity = command_struct_rt_.target_velocity_ - current_velocity;
 
   check_for_success(get_node()->now(), error_position, current_position, current_velocity);
 
   // Hardware interface adapter: Generate and send commands
   computed_command_ = hw_iface_adapter_.updateCommand(
-    command_struct_rt_.position_, 0.0, error_position, error_velocity,
-    command_struct_rt_.max_effort_);
+    command_struct_rt_.position_, command_struct_rt_.target_velocity_, error_position,
+    error_velocity, command_struct_rt_.max_effort_);
   return controller_interface::return_type::OK;
 }
 
@@ -97,11 +97,13 @@ void GripperActionController<HardwareInterface>::accepted_callback(
     // Accept new goal
     preempt_active_goal();
 
-    // This is the non-realtime command_struct
-    // We use command_ for sharing
-    command_struct_.position_ = goal_handle->get_goal()->command.position;
-    command_struct_.max_effort_ = goal_handle->get_goal()->command.max_effort;
-    command_.writeFromNonRT(command_struct_);
+  // This is the non-realtime command_struct
+  // We use command_ for sharing
+  command_struct_.position_ = goal_handle->get_goal()->command.position;
+  command_struct_.target_velocity_ = goal_handle->get_goal()->command.target_velocity;
+  command_struct_.max_effort_ = goal_handle->get_goal()->command.max_effort;
+  command_.writeFromNonRT(command_struct_);
+
 
     pre_alloc_result_->reached_goal = false;
     pre_alloc_result_->stalled = false;
@@ -145,6 +147,7 @@ void GripperActionController<HardwareInterface>::set_hold_position()
 {
   command_struct_.position_ = joint_position_state_interface_->get().get_value();
   command_struct_.max_effort_ = default_max_effort_;
+  command_struct_.target_velocity_ = 0.0;
   command_.writeFromNonRT(command_struct_);
 }
 
@@ -281,12 +284,29 @@ controller_interface::CallbackReturn GripperActionController<HardwareInterface>:
   joint_position_state_interface_ = *position_state_interface_it;
   joint_velocity_state_interface_ = *velocity_state_interface_it;
 
+  for (auto & interface : command_interfaces_)
+  {
+    if (interface.get_name() == "gripper_speed_multiplier")
+    {
+      speed_multiplier_interface_ = interface;
+    }
+    else if (interface.get_name() == "gripper_force_multiplier")
+    {
+      force_multiplier_interface_ = interface;
+    }
+  }
+  force_multiplier_interface_.value().get().set_value(params_.force_multiplier);
+  speed_multiplier_interface_.value().get().set_value(params_.speed_multiplier);
+
   // Hardware interface adapter
-  hw_iface_adapter_.init(joint_position_command_interface_, get_node());
+  hw_iface_adapter_.init(
+    joint_position_command_interface_, force_multiplier_interface_, speed_multiplier_interface_, get_node());
 
   // Command - non RT version
   command_struct_.position_ = joint_position_state_interface_->get().get_value();
   command_struct_.max_effort_ = default_max_effort_;
+  command_struct_.target_velocity_ = 0.0;
+  command_.initRT(command_struct_);
 
   // Result
   pre_alloc_result_ = std::make_shared<control_msgs::action::GripperCommand::Result>();

gripper_controllers/include/gripper_controllers/hardware_interface_adapter.hpp

@@ -71,29 +71,43 @@ class HardwareInterfaceAdapter<hardware_interface::HW_IF_POSITION>
 {
 public:
   bool init(
-    std::experimental::optional<std::reference_wrapper<hardware_interface::LoanedCommandInterface>>
-      joint_handle,
+    std::optional<std::reference_wrapper<hardware_interface::LoanedCommandInterface>> joint_handle,
+    std::optional<std::reference_wrapper<hardware_interface::LoanedCommandInterface>> force_handle,
+    std::optional<std::reference_wrapper<hardware_interface::LoanedCommandInterface>>
+      target_velocity_handle,
     const rclcpp_lifecycle::LifecycleNode::SharedPtr & /* node */)
   {
     joint_handle_ = joint_handle;
+    force_handle_ = force_handle;
+    target_velocity_handle_ = target_velocity_handle;
     return true;
   }
 
   void starting(const rclcpp::Time & /* time */) {}
   void stopping(const rclcpp::Time & /* time */) {}
 
   double updateCommand(
-    double desired_position, double /* desired_velocity */, double /* error_position */,
+    double desired_position, double desired_velocity, double /* error_position */,
     double /* error_velocity */, double max_allowed_effort)
   {
     // Forward desired position to command
     joint_handle_->get().set_value(desired_position);
+    if (target_velocity_handle_.has_value())
+    {
+      target_velocity_handle_->get().set_value(desired_velocity);
+    }
+    if (force_handle_.has_value())
+    {
+      force_handle_->get().set_value(max_allowed_effort);
+    }
     return max_allowed_effort;
   }
 
 private:
-  std::experimental::optional<std::reference_wrapper<hardware_interface::LoanedCommandInterface>>
-    joint_handle_;
+  std::optional<std::reference_wrapper<hardware_interface::LoanedCommandInterface>> joint_handle_;
+  std::optional<std::reference_wrapper<hardware_interface::LoanedCommandInterface>> force_handle_;
+  std::optional<std::reference_wrapper<hardware_interface::LoanedCommandInterface>>
+    target_velocity_handle_;
 };
 
 /**
@@ -130,8 +144,11 @@ class HardwareInterfaceAdapter<hardware_interface::HW_IF_EFFORT>
   }
 
   bool init(
-    std::experimental::optional<std::reference_wrapper<hardware_interface::LoanedCommandInterface>>
-      joint_handle,
+    std::optional<std::reference_wrapper<hardware_interface::LoanedCommandInterface>> joint_handle,
+    std::optional<
+      std::reference_wrapper<hardware_interface::LoanedCommandInterface>> /* force_handle */,
+    std::optional<std::reference_wrapper<
+      hardware_interface::LoanedCommandInterface>> /* target_velocity_handle */,
     const std::shared_ptr<rclcpp_lifecycle::LifecycleNode> & node)
   {
     joint_handle_ = joint_handle;

gripper_controllers/src/gripper_action_controller_parameters.yaml

@@ -0,0 +1,59 @@
+gripper_action_controller:
+  action_monitor_rate: {
+    type: double,
+    default_value: 20.0,
+    description: "Hz",
+    validation: {
+      gt_eq: [ 0.1 ]
+    },
+  }
+  joint: {
+    type: string,
+    default_value: "",
+  }
+  goal_tolerance: {
+    type: double,
+    default_value: 0.01,
+    validation: {
+      gt_eq: [ 0.0 ]
+    },
+  }
+  max_effort: {
+    type: double,
+    default_value: 0.0,
+    description: "Max allowable effort",
+    validation: {
+      gt_eq: [ 0.0 ]
+    },
+  }
+  allow_stalling: {
+    type: bool,
+    description: "Allow stalling will make the action server return success if the gripper stalls when moving to the goal",
+    default_value: false,
+  }
+  stall_velocity_threshold: {
+    type: double,
+    description: "stall velocity threshold",
+    default_value: 0.001,
+  }
+  stall_timeout: {
+    type: double,
+    description: "stall timeout",
+    default_value: 1.0,
+  }
+  force_multiplier: {
+    type: double,
+    description: "Scales the maximum effort of the gripper defined in the URDF",
+    default_value: 1.0,
+    validation: {
+      bounds<>: [ 0.0, 1.0 ]
+    },
+  }
+  speed_multiplier: {
+    type: double,
+    description: "Scales the default velocity of the gripper defined in the URDF",
+    default_value: 1.0,
+    validation: {
+      bounds<>: [ 0.0, 1.0 ]
+    },
+  }

ros2_controllers/package.xml

@@ -15,6 +15,7 @@
   <exec_depend>effort_controllers</exec_depend>
   <exec_depend>force_torque_sensor_broadcaster</exec_depend>
   <exec_depend>forward_command_controller</exec_depend>
+  <exec_depend>gripper_controllers</exec_depend>
   <exec_depend>imu_sensor_broadcaster</exec_depend>
   <exec_depend>joint_state_broadcaster</exec_depend>
   <exec_depend>joint_trajectory_controller</exec_depend>