diff --git a/grippers/robotiq_description/urdf/2f_140.ros2_control.xacro b/grippers/robotiq_description/urdf/2f_140.ros2_control.xacro index 5c97fe4..5298184 100644 --- a/grippers/robotiq_description/urdf/2f_140.ros2_control.xacro +++ b/grippers/robotiq_description/urdf/2f_140.ros2_control.xacro @@ -57,6 +57,13 @@ ${gripper_closed_position} + + + diff --git a/grippers/robotiq_description/urdf/2f_85.ros2_control.xacro b/grippers/robotiq_description/urdf/2f_85.ros2_control.xacro index 8f404a7..d78cc52 100644 --- a/grippers/robotiq_description/urdf/2f_85.ros2_control.xacro +++ b/grippers/robotiq_description/urdf/2f_85.ros2_control.xacro @@ -69,6 +69,13 @@ ${gripper_closed_position} + + + diff --git a/grippers/robotiq_driver/include/robotiq_driver/gripper_scaling.hpp b/grippers/robotiq_driver/include/robotiq_driver/gripper_scaling.hpp index 3dde196..6029073 100644 --- a/grippers/robotiq_driver/include/robotiq_driver/gripper_scaling.hpp +++ b/grippers/robotiq_driver/include/robotiq_driver/gripper_scaling.hpp @@ -92,4 +92,12 @@ inline constexpr uint8_t kGripperRange = kGripperMaxPos - kGripperMinPos; } return static_cast(std::min(value / maximum, 1.0) * 0xFF); } + +//! The inverse: a 0x00..0xFF register read back as the fraction of \p maximum +//! it stands for. Total where registerFromFractionOf is not — every byte names +//! a fraction, so there is no reading the arithmetic cannot map. +[[nodiscard]] inline double fractionOfFromRegister(uint8_t register_value, double maximum) +{ + return maximum * register_value / 0xFF; +} } // namespace robotiq_driver diff --git a/grippers/robotiq_driver/include/robotiq_driver/hardware_interface.hpp b/grippers/robotiq_driver/include/robotiq_driver/hardware_interface.hpp index c75bfc6..fe57f21 100644 --- a/grippers/robotiq_driver/include/robotiq_driver/hardware_interface.hpp +++ b/grippers/robotiq_driver/include/robotiq_driver/hardware_interface.hpp @@ -58,6 +58,11 @@ #include namespace robotiq_driver { +//! Name of the state interface carrying gOBJ. ros2_control defines HW_IF_ +//! constants for position, velocity and effort but has nothing for object +//! detection, so the URDF and every consumer spell this one out. +inline constexpr const char* kObjectStatusInterface = "object_status"; + //! ros2_control hardware interface for a Robotiq 2F gripper, driven through the //! gripper SDK. //! Member order carries an invariant: recovery_ is declared after gripper_ so @@ -114,7 +119,8 @@ class RobotiqGripperHardwareInterface : public hardware_interface::SystemInterfa CallbackReturn on_error(const rclcpp_lifecycle::State& previous_state) override; /** - * This method exposes position and velocity of joints for reading. + * This method exposes position, velocity, effort and object status of joints + * for reading. */ ROBOTIQ_DRIVER_PUBLIC std::vector export_state_interfaces() override; @@ -179,6 +185,17 @@ class RobotiqGripperHardwareInterface : public hardware_interface::SystemInterfa double gripper_position_ = 0.0; double gripper_velocity_ = 0.0; + + // gCU read back through the same full-scale force the rFR command uses, so + // the effort reported and the effort requested are on one scale. It is a + // motor-current proxy either way, not a measurement of the grip. + double gripper_effort_ = 0.0; + + // gOBJ as it comes off the wire: 0 moving, 1 held while opening, 2 held + // while closing, 3 at the requested position. A double because that is what + // a state interface carries; consumers compare it against those four values. + double gripper_object_status_ = 0.0; + double gripper_position_command_ = 0.0; // The last command write() sent. A register the arithmetic cannot produce diff --git a/grippers/robotiq_driver/src/hardware_interface.cpp b/grippers/robotiq_driver/src/hardware_interface.cpp index d5ae43a..8401daa 100644 --- a/grippers/robotiq_driver/src/hardware_interface.cpp +++ b/grippers/robotiq_driver/src/hardware_interface.cpp @@ -161,27 +161,24 @@ hardware_interface::CallbackReturn RobotiqGripperHardwareInterface::on_init(cons return CallbackReturn::ERROR; } - // There are two state interfaces: position and velocity. - if(joint.state_interfaces.size() != 2) + // export_state_interfaces() exports the four below whatever the description + // says, so this checks that the description names no fifth one the hardware + // cannot answer for. A description is free to declare fewer. + for(const hardware_interface::InterfaceInfo& state_interface : joint.state_interfaces) { - RCLCPP_FATAL(kLogger, - "Joint '%s' has %zu state interface. 2 expected.", - joint.name.c_str(), - joint.state_interfaces.size()); - return CallbackReturn::ERROR; - } - - for(int i = 0; i < 2; ++i) - { - if(!(joint.state_interfaces[i].name == hardware_interface::HW_IF_POSITION - || joint.state_interfaces[i].name == hardware_interface::HW_IF_VELOCITY)) + if(!(state_interface.name == hardware_interface::HW_IF_POSITION + || state_interface.name == hardware_interface::HW_IF_VELOCITY + || state_interface.name == hardware_interface::HW_IF_EFFORT + || state_interface.name == kObjectStatusInterface)) { RCLCPP_FATAL(kLogger, - "Joint '%s' has %s state interface. Expected %s or %s.", + "Joint '%s' has %s state interface. Expected %s, %s, %s or %s.", joint.name.c_str(), - joint.state_interfaces.at(i).name.c_str(), + state_interface.name.c_str(), hardware_interface::HW_IF_POSITION, - hardware_interface::HW_IF_VELOCITY); + hardware_interface::HW_IF_VELOCITY, + hardware_interface::HW_IF_EFFORT, + kObjectStatusInterface); return CallbackReturn::ERROR; } } @@ -258,6 +255,10 @@ std::vector RobotiqGripperHardwareInterface: hardware_interface::StateInterface(info_.joints[0].name, hardware_interface::HW_IF_POSITION, &gripper_position_)); state_interfaces.emplace_back( hardware_interface::StateInterface(info_.joints[0].name, hardware_interface::HW_IF_VELOCITY, &gripper_velocity_)); + state_interfaces.emplace_back( + hardware_interface::StateInterface(info_.joints[0].name, hardware_interface::HW_IF_EFFORT, &gripper_effort_)); + state_interfaces.emplace_back( + hardware_interface::StateInterface(info_.joints[0].name, kObjectStatusInterface, &gripper_object_status_)); return state_interfaces; } @@ -418,6 +419,8 @@ hardware_interface::return_type RobotiqGripperHardwareInterface::read(const rclc // The status block carries no velocity — the gripper reports position and // motor current only. gripper_velocity_ = 0.0; + gripper_effort_ = fractionOfFromRegister(status.current, parameters_.max_force); + gripper_object_status_ = static_cast(status.gripperStatus.objectDetection()); // A faulted link recovers by itself on the next successful exchange, so // this warns rather than errors; the position above is the last good diff --git a/grippers/robotiq_driver/tests/test_gripper_scaling.cpp b/grippers/robotiq_driver/tests/test_gripper_scaling.cpp index 1a8ecd6..2b3ea1c 100644 --- a/grippers/robotiq_driver/tests/test_gripper_scaling.cpp +++ b/grippers/robotiq_driver/tests/test_gripper_scaling.cpp @@ -133,4 +133,27 @@ TEST(GripperScaling, whenTheMaximumIsNearZero_theRegisterIsTheMaxAllowedValue) { EXPECT_EQ(255, registerFromFractionOf(0.15, std::numeric_limits::denorm_min())); } + +TEST(GripperScaling, ReadingAFractionBackSpansTheWholeScale) +{ + constexpr double kMaxForce = 235.0; + EXPECT_DOUBLE_EQ(0.0, fractionOfFromRegister(0, kMaxForce)); + EXPECT_DOUBLE_EQ(kMaxForce, fractionOfFromRegister(255, kMaxForce)); + EXPECT_DOUBLE_EQ(kMaxForce * 127 / 255, fractionOfFromRegister(127, kMaxForce)); +} + +TEST(GripperScaling, ReadingBackTheRegisterACommandProducedRecoversTheCommand) +{ + constexpr double kMaxForce = 235.0; + constexpr double kRequested = 100.0; + + const auto register_value = registerFromFractionOf(kRequested, kMaxForce); + ASSERT_TRUE(register_value.has_value()); + + // registerFromFractionOf truncates, so the value read back sits at most one + // count low — the same tolerance the position round trip carries. + const double recovered = fractionOfFromRegister(*register_value, kMaxForce); + EXPECT_LE(recovered, kRequested); + EXPECT_GT(recovered, kRequested - kMaxForce / 0xFF); +} } // namespace robotiq_driver::test diff --git a/grippers/robotiq_driver/tests/test_robotiq_gripper_hardware_interface.cpp b/grippers/robotiq_driver/tests/test_robotiq_gripper_hardware_interface.cpp index eb830f5..ecc5606 100644 --- a/grippers/robotiq_driver/tests/test_robotiq_gripper_hardware_interface.cpp +++ b/grippers/robotiq_driver/tests/test_robotiq_gripper_hardware_interface.cpp @@ -92,6 +92,8 @@ std::string minimalRobotUrdf(const std::string& extra_hardware_params = "") 0.7929 + + @@ -151,6 +153,31 @@ TEST(TestRobotiqGripperHardwareInterface, ExportsExpectedCommandInterfaces) "reactivate_gripper/reactivate_gripper_response"})); } +/** + * A grasping client reads object_status to tell a caught part from a missed one + * and effort to judge the grip, both by name. object_status in particular is not + * a ros2_control standard interface, so nothing but this test pins its spelling. + */ +TEST(TestRobotiqGripperHardwareInterface, ExportsExpectedStateInterfaces) +{ + const std::string urdf = minimalRobotUrdf(); + + rclcpp::Node node{"test_robotiq_gripper_hardware_interface"}; + +#if HARDWARE_INTERFACE_VERSION_GTE(4, 13, 0) + hardware_interface::ResourceManager rm(urdf, node.get_node_clock_interface(), node.get_node_logging_interface()); +#else + hardware_interface::ResourceManager rm(urdf); +#endif + + const auto keys = rm.state_interface_keys(); + EXPECT_THAT(keys, + testing::IsSupersetOf({"robotiq_85_left_knuckle_joint/position", + "robotiq_85_left_knuckle_joint/velocity", + "robotiq_85_left_knuckle_joint/effort", + "robotiq_85_left_knuckle_joint/object_status"})); +} + /** * use_dummy brings the component all the way up with no gripper and no serial * port, and the joint position then follows the commanded position. It is the