diff --git a/hello_helpers/src/hello_helpers/gripper_conversion.py b/hello_helpers/src/hello_helpers/gripper_conversion.py new file mode 100644 index 0000000..458b87e --- /dev/null +++ b/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 diff --git a/stretch_core/nodes/stretch_driver b/stretch_core/nodes/stretch_driver index 8c7b371..1aa5cee 100755 --- a/stretch_core/nodes/stretch_driver +++ b/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