Merge pull request #27 from iory/effort

Support torque vector using difference of reference and actual vectors
iory · Jan 20, 2024 · c644047 · c644047
2 parents 26b3f6d + 7700c1a
commit c644047
Show file tree

Hide file tree

Showing 2 changed files with 12 additions and 0 deletions.
diff --git 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
@@ -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)