Moved helper class GripperConversion to helpers module

hello_helpers/src/hello_helpers/gripper_conversion.py

@@ -0,0 +1,59 @@
+#!/usr/bin/env python
+
+class GripperConversion:
+    def __init__(self):
+        # robotis position values (gripper.py)
+        #      0 is very close to closed (fingers almost or barely touching)
+        #     50 is maximally open
+        #   -100 is maximally closed (maximum force applied to the object - might be risky for a large object)
+        #
+        # aperture is 12.5 cm wide when open (0.125 m, 125 mm)
+        #
+        # finger angle
+        #   0.0 just barely closed
+        #   fully opened is
+
+        # from stretch_gripper.xacro
+        # scale_finger_length = 0.9
+        # scale_finger_length * 0.19011
+        # = 0.171099
+        self.finger_length_m = 0.171
+
+        self.open_aperture_m = 0.09 #0.125
+        self.closed_aperture_m = 0.0
+
+        self.open_robotis = 70.0
+        self.closed_robotis = 0.0
+
+        self.robotis_to_aperture_slope = ((self.open_aperture_m - self.closed_aperture_m) / (self.open_robotis - self.closed_robotis))
+
+    def robotis_to_aperture(self, robotis_in):
+        # linear model
+        aperture_m = (self.robotis_to_aperture_slope * (robotis_in - self.closed_robotis)) + self.closed_aperture_m
+        return aperture_m
+
+    def aperture_to_robotis(self, aperture_m):
+        # linear model
+        robotis_out = ((aperture_m - self.closed_aperture_m) / self.robotis_to_aperture_slope) + self.closed_robotis
+        return robotis_out
+
+    def aperture_to_finger_rad(self, aperture_m):
+        # arc length / radius = ang_rad
+        finger_rad = (aperture_m/2.0)/self.finger_length_m
+        return finger_rad
+
+    def finger_rad_to_aperture(self, finger_rad):
+        aperture_m = 2.0 * (finger_rad * self.finger_length_m)
+        return aperture_m
+
+    def finger_to_robotis(self, finger_ang_rad):
+        aperture_m = self.finger_rad_to_aperture(finger_ang_rad)
+        robotis_out = self.aperture_to_robotis(aperture_m)
+        return robotis_out
+
+    def status_to_all(self, gripper_status):
+        aperture_m = self.robotis_to_aperture(gripper_status['pos_pct'])
+        finger_rad = self.aperture_to_finger_rad(aperture_m)
+        finger_effort = gripper_status['effort']
+        finger_vel = (self.robotis_to_aperture_slope * gripper_status['vel'])/2.0
+        return aperture_m, finger_rad, finger_effort, finger_vel

stretch_core/nodes/stretch_driver

@@ -26,6 +26,7 @@ from sensor_msgs.msg import JointState, Imu, MagneticField
 from std_msgs.msg import Header
 
 import hello_helpers.hello_misc as hm
+from hello_helpers.gripper_conversion import GripperConversion
 
 
 GRIPPER_DEBUG = False
@@ -50,65 +51,6 @@ def bound_ros_command(bounds, ros_pos, clip_ros_tolerance):
     return ros_pos
 
 
-class GripperConversion:
-    def __init__(self):
-        # robotis position values (gripper.py)
-        #      0 is very close to closed (fingers almost or barely touching)
-        #     50 is maximally open
-        #   -100 is maximally closed (maximum force applied to the object - might be risky for a large object)
-        #
-        # aperture is 12.5 cm wide when open (0.125 m, 125 mm)
-        #
-        # finger angle
-        #   0.0 just barely closed
-        #   fully opened is
-
-        # from stretch_gripper.xacro
-        # scale_finger_length = 0.9
-        # scale_finger_length * 0.19011
-        # = 0.171099
-        self.finger_length_m = 0.171
-
-        self.open_aperture_m = 0.09 #0.125
-        self.closed_aperture_m = 0.0
-
-        self.open_robotis = 70.0
-        self.closed_robotis = 0.0
-
-        self.robotis_to_aperture_slope = ((self.open_aperture_m - self.closed_aperture_m) / (self.open_robotis - self.closed_robotis))
-
-    def robotis_to_aperture(self, robotis_in):
-        # linear model
-        aperture_m = (self.robotis_to_aperture_slope * (robotis_in - self.closed_robotis)) + self.closed_aperture_m
-        return aperture_m
-
-    def aperture_to_robotis(self, aperture_m):
-        # linear model
-        robotis_out = ((aperture_m - self.closed_aperture_m) / self.robotis_to_aperture_slope) + self.closed_robotis
-        return robotis_out
-
-    def aperture_to_finger_rad(self, aperture_m):
-        # arc length / radius = ang_rad
-        finger_rad = (aperture_m/2.0)/self.finger_length_m
-        return finger_rad
-
-    def finger_rad_to_aperture(self, finger_rad):
-        aperture_m = 2.0 * (finger_rad * self.finger_length_m)
-        return aperture_m
-
-    def finger_to_robotis(self, finger_ang_rad):
-        aperture_m = self.finger_rad_to_aperture(finger_ang_rad)
-        robotis_out = self.aperture_to_robotis(aperture_m)
-        return robotis_out
-
-    def status_to_all(self, gripper_status):
-        aperture_m = self.robotis_to_aperture(gripper_status['pos_pct'])
-        finger_rad = self.aperture_to_finger_rad(aperture_m)
-        finger_effort = gripper_status['effort']
-        finger_vel = (self.robotis_to_aperture_slope * gripper_status['vel'])/2.0
-        return aperture_m, finger_rad, finger_effort, finger_vel
-
-
 class SimpleCommandGroup:
     def __init__(self, joint_name, acceptable_joint_error=0.015, clip_ros_tolerance=0.001):
         self.clip_ros_tolerance = clip_ros_tolerance