From 7700c1a2d0a85487f195f3ab340b5d115f13ce92 Mon Sep 17 00:00:00 2001 From: iory Date: Thu, 18 Jan 2024 02:25:30 +0900 Subject: [PATCH] Support torque vector using difference of reference and actual vectors --- rcb4/armh7interface.py | 9 +++++++++ ros/kxr_controller/node_scripts/rcb4_ros_bridge.py | 3 +++ 2 files changed, 12 insertions(+) diff --git a/rcb4/armh7interface.py b/rcb4/armh7interface.py index db430292..de0cad3d 100644 --- a/rcb4/armh7interface.py +++ b/rcb4/armh7interface.py @@ -404,6 +404,15 @@ def reference_angle_vector(self, servo_ids=None): ServoStruct, slot_name='ref_angle')[servo_ids] return ref_angles + def servo_error(self, servo_ids=None): + if servo_ids is None: + servo_ids = self.search_servo_ids() + if len(servo_ids) == 0: + return np.empty(shape=0) + error_angles = self.read_cstruct_slot_vector( + ServoStruct, slot_name='error_angle')[servo_ids] + return error_angles + def servo_id_to_index(self, servo_ids=None): if servo_ids is None: servo_ids = self.search_servo_ids() diff --git a/ros/kxr_controller/node_scripts/rcb4_ros_bridge.py b/ros/kxr_controller/node_scripts/rcb4_ros_bridge.py index 0c133351..50bd72e0 100644 --- a/ros/kxr_controller/node_scripts/rcb4_ros_bridge.py +++ b/ros/kxr_controller/node_scripts/rcb4_ros_bridge.py @@ -349,6 +349,7 @@ def run(self): break try: av = self.arm.angle_vector() + torque_vector = self.arm.servo_error() except RuntimeError as e: self.unsubscribe() rospy.signal_shutdown('Disconnected {}.'.format(e)) @@ -362,6 +363,8 @@ def run(self): msg.position.append( np.deg2rad( av[self.id_to_index[servo_id]])) + msg.effort.append( + torque_vector[self.id_to_index[servo_id]]) msg.name.append(name) self.current_joint_states_pub.publish(msg)