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