Add force and velocity hardware interfaces (Kinovarobotics#204)

Signed-off-by: Paul Gesel <[email protected]>
Co-authored-by: Ashton Larkin <[email protected]>

Author: pac48

kortex_driver/README.md

@@ -0,0 +1,13 @@
+# ROS 2 Kortex Driver
+The ROS 2 Kortex driver implements the `ros2_control` hardware interface for a `SystemInterface`.
+
+### Command interfaces
+This driver exports commands interfaces for position, velocity, and effort interfaces for each joint defined in the URDF.
+Additionally, twist interfaces are exported for the end effector for operational space control.
+Several additional interfaces are exported, including `set_gripper_max_velocity`, `set_gripper_max_effort` for the gripper joint,
+`reset_fault/command`, and `reset_fault/async_success` for fault management.
+
+### State interfaces
+This driver exports position and velocity state interfaces for joint defined in the URDF.
+
+Additionally, one state interface `reset_fault/internal_fault` is used for determining the robot's fault state.

kortex_driver/include/kortex_driver/hardware_interface.hpp

@@ -144,11 +144,13 @@ class KortexMultiInterfaceHardware : public hardware_interface::SystemInterface
 
   // Gripper
   k_api::GripperCyclic::MotorCommand * gripper_motor_command_;
-  double gripper_command_position_;
-  double gripper_command_max_velocity_;
-  double gripper_command_max_force_;
-  double gripper_position_;
-  double gripper_velocity_;
+  double gripper_command_position_ = 0.0;
+  double gripper_command_max_velocity_ = 0.0;
+  double gripper_command_max_force_ = 0.0;
+  double gripper_position_ = 0.0;
+  double gripper_velocity_ = 0.0;
+  double gripper_force_command_ = 0.0;
+  double gripper_speed_command_ = 0.0;
 
   rclcpp::Time controller_switch_time_;
   std::atomic<bool> block_write = false;

kortex_driver/src/hardware_interface.cpp

@@ -341,6 +341,13 @@ KortexMultiInterfaceHardware::export_command_interfaces()
     {
       command_interfaces.emplace_back(hardware_interface::CommandInterface(
         info_.joints[i].name, hardware_interface::HW_IF_POSITION, &gripper_command_position_));
+
+      command_interfaces.emplace_back(hardware_interface::CommandInterface(
+        info_.joints[i].name, "set_gripper_max_velocity", &gripper_speed_command_));
+      gripper_speed_command_ = gripper_command_max_velocity_;
+      command_interfaces.emplace_back(hardware_interface::CommandInterface(
+        info_.joints[i].name, "set_gripper_max_effort", &gripper_force_command_));
+      gripper_force_command_ = gripper_command_max_force_;
     }
     else
     {
@@ -671,9 +678,9 @@ CallbackReturn KortexMultiInterfaceHardware::on_activate(
   base_command_.mutable_interconnect()->mutable_command_id()->set_identifier(0);
   gripper_motor_command_ =
     base_command_.mutable_interconnect()->mutable_gripper_command()->add_motor_cmd();
-  gripper_motor_command_->set_position(gripper_initial_position);       // % position
-  gripper_motor_command_->set_velocity(gripper_command_max_velocity_);  // % speed
-  gripper_motor_command_->set_force(gripper_command_max_force_);        // % torque
+  gripper_motor_command_->set_position(gripper_initial_position);  // % position
+  gripper_motor_command_->set_velocity(gripper_speed_command_);    // % speed
+  gripper_motor_command_->set_force(gripper_force_command_);       // % force
 
   // Send a first frame
   base_feedback = base_cyclic_.Refresh(base_command_);
@@ -864,8 +871,7 @@ return_type KortexMultiInterfaceHardware::write(
 
       // gripper control
       sendGripperCommand(
-        arm_mode_, gripper_command_position_, gripper_command_max_velocity_,
-        gripper_command_max_force_);
+        arm_mode_, gripper_command_position_, gripper_speed_command_, gripper_force_command_);
       // read after write in twist mode
       feedback_ = base_cyclic_.RefreshFeedback();
     }
@@ -877,8 +883,7 @@ return_type KortexMultiInterfaceHardware::write(
 
       // gripper control
       sendGripperCommand(
-        arm_mode_, gripper_command_position_, gripper_command_max_velocity_,
-        gripper_command_max_force_);
+        arm_mode_, gripper_command_position_, gripper_speed_command_, gripper_force_command_);
 
       if (joint_based_controller_running_)
       {