diff --git a/hello_helpers/src/hello_helpers/hello_misc.py b/hello_helpers/src/hello_helpers/hello_misc.py index bcbc7e0..6058012 100644 --- a/hello_helpers/src/hello_helpers/hello_misc.py +++ b/hello_helpers/src/hello_helpers/hello_misc.py @@ -2,8 +2,9 @@ from __future__ import print_function -import time import os +import sys +import time import glob import math diff --git a/stretch_core/nodes/command_groups.py b/stretch_core/nodes/command_groups.py index cdb28f9..01893ce 100644 --- a/stretch_core/nodes/command_groups.py +++ b/stretch_core/nodes/command_groups.py @@ -166,7 +166,11 @@ class SimpleCommandGroup: class HeadPanCommandGroup(SimpleCommandGroup): - def __init__(self, range_rad, head_pan_calibrated_offset, head_pan_calibrated_looked_left_offset): + def __init__(self, range_rad, head_pan_calibrated_offset, head_pan_calibrated_looked_left_offset, robot=None): + if range_rad is None and robot is not None: + range_ticks = robot.head.motors['head_pan'].params['range_t'] + range_rad = (robot.head.motors['head_pan'].ticks_to_world_rad(range_ticks[1]), + robot.head.motors['head_pan'].ticks_to_world_rad(range_ticks[0])) SimpleCommandGroup.__init__(self, 'joint_head_pan', range_rad, acceptable_joint_error=0.15) self.head_pan_calibrated_offset = head_pan_calibrated_offset self.head_pan_calibrated_looked_left_offset = head_pan_calibrated_looked_left_offset @@ -199,7 +203,11 @@ class HeadPanCommandGroup(SimpleCommandGroup): class HeadTiltCommandGroup(SimpleCommandGroup): def __init__(self, range_rad, head_tilt_calibrated_offset, head_tilt_calibrated_looking_up_offset, - head_tilt_backlash_transition_angle): + head_tilt_backlash_transition_angle, robot=None): + if range_rad is None and robot is not None: + range_ticks = robot.head.motors['head_tilt'].params['range_t'] + range_rad = (robot.head.motors['head_tilt'].ticks_to_world_rad(range_ticks[1]), + robot.head.motors['head_tilt'].ticks_to_world_rad(range_ticks[0])) SimpleCommandGroup.__init__(self, 'joint_head_tilt', range_rad, acceptable_joint_error=0.52) self.head_tilt_calibrated_offset = head_tilt_calibrated_offset self.head_tilt_calibrated_looking_up_offset = head_tilt_calibrated_looking_up_offset @@ -231,7 +239,11 @@ class HeadTiltCommandGroup(SimpleCommandGroup): class WristYawCommandGroup(SimpleCommandGroup): - def __init__(self, range_rad): + def __init__(self, range_rad, robot=None): + if range_rad is None and robot is not None: + range_ticks = robot.end_of_arm.motors['wrist_yaw'].params['range_t'] + range_rad = (robot.end_of_arm.motors['wrist_yaw'].ticks_to_world_rad(range_ticks[1]), + robot.end_of_arm.motors['wrist_yaw'].ticks_to_world_rad(range_ticks[0])) SimpleCommandGroup.__init__(self, 'joint_wrist_yaw', range_rad) def init_execution(self, robot, robot_status, **kwargs): @@ -251,7 +263,13 @@ class WristYawCommandGroup(SimpleCommandGroup): class GripperCommandGroup(SimpleCommandGroup): - def __init__(self, range_robotis): + def __init__(self, range_robotis, robot=None): + if range_robotis is None and robot is not None: + range_ticks = robot.end_of_arm.motors['stretch_gripper'].params['range_t'] + range_rad = (robot.end_of_arm.motors['stretch_gripper'].ticks_to_world_rad(range_ticks[0]), + robot.end_of_arm.motors['stretch_gripper'].ticks_to_world_rad(range_ticks[1])) + range_robotis = (robot.end_of_arm.motors['stretch_gripper'].world_rad_to_pct(range_rad[0]), + robot.end_of_arm.motors['stretch_gripper'].world_rad_to_pct(range_rad[1])) SimpleCommandGroup.__init__(self, None, None, acceptable_joint_error=1.0) self.gripper_joint_names = ['joint_gripper_finger_left', 'joint_gripper_finger_right', 'gripper_aperture'] self.gripper_conversion = GripperConversion() @@ -331,8 +349,9 @@ class GripperCommandGroup(SimpleCommandGroup): class TelescopingCommandGroup(SimpleCommandGroup): - def __init__(self, range_m, wrist_extension_calibrated_retracted_offset): - #SimpleCommandGroup.__init__(self, 'wrist_extension', range_m, acceptable_joint_error=0.005) + def __init__(self, range_m, wrist_extension_calibrated_retracted_offset, robot=None): + if range_m is None and robot is not None: + range_m = tuple(robot.arm.params['range_m']) SimpleCommandGroup.__init__(self, 'wrist_extension', range_m, acceptable_joint_error=0.008) self.wrist_extension_calibrated_retracted_offset = wrist_extension_calibrated_retracted_offset self.telescoping_joints = ['joint_arm_l3', 'joint_arm_l2', 'joint_arm_l1', 'joint_arm_l0'] @@ -448,7 +467,9 @@ class TelescopingCommandGroup(SimpleCommandGroup): class LiftCommandGroup(SimpleCommandGroup): - def __init__(self, range_m): + def __init__(self, range_m, robot=None): + if range_m is None and robot is not None: + range_m = tuple(robot.lift.params['range_m']) SimpleCommandGroup.__init__(self, 'joint_lift', range_m) def init_execution(self, robot, robot_status, **kwargs): diff --git a/stretch_core/nodes/joint_trajectory_server.py b/stretch_core/nodes/joint_trajectory_server.py index 05661f2..0c2756a 100644 --- a/stretch_core/nodes/joint_trajectory_server.py +++ b/stretch_core/nodes/joint_trajectory_server.py @@ -1,6 +1,8 @@ #! /usr/bin/env python from __future__ import print_function +import importlib + import rospy import actionlib from control_msgs.msg import FollowJointTrajectoryAction @@ -25,35 +27,37 @@ class JointTrajectoryAction: self.feedback = FollowJointTrajectoryFeedback() self.result = FollowJointTrajectoryResult() - r = self.node.robot - head_pan_range_ticks = r.head.motors['head_pan'].params['range_t'] - head_pan_range_rad = (r.head.motors['head_pan'].ticks_to_world_rad(head_pan_range_ticks[1]), - r.head.motors['head_pan'].ticks_to_world_rad(head_pan_range_ticks[0])) - head_tilt_range_ticks = r.head.motors['head_tilt'].params['range_t'] - head_tilt_range_rad = (r.head.motors['head_tilt'].ticks_to_world_rad(head_tilt_range_ticks[1]), - r.head.motors['head_tilt'].ticks_to_world_rad(head_tilt_range_ticks[0])) - wrist_yaw_range_ticks = r.end_of_arm.motors['wrist_yaw'].params['range_t'] - wrist_yaw_range_rad = (r.end_of_arm.motors['wrist_yaw'].ticks_to_world_rad(wrist_yaw_range_ticks[1]), - r.end_of_arm.motors['wrist_yaw'].ticks_to_world_rad(wrist_yaw_range_ticks[0])) - gripper_range_ticks = r.end_of_arm.motors['stretch_gripper'].params['range_t'] - gripper_range_rad = (r.end_of_arm.motors['stretch_gripper'].ticks_to_world_rad(gripper_range_ticks[0]), - r.end_of_arm.motors['stretch_gripper'].ticks_to_world_rad(gripper_range_ticks[1])) - gripper_range_robotis = (r.end_of_arm.motors['stretch_gripper'].world_rad_to_pct(gripper_range_rad[0]), - r.end_of_arm.motors['stretch_gripper'].world_rad_to_pct(gripper_range_rad[1])) - - self.head_pan_cg = HeadPanCommandGroup(head_pan_range_rad, - self.node.head_pan_calibrated_offset_rad, - self.node.head_pan_calibrated_looked_left_offset_rad) - self.head_tilt_cg = HeadTiltCommandGroup(head_tilt_range_rad, + # Setup main command groups + self.head_pan_cg = HeadPanCommandGroup(None, self.node.head_pan_calibrated_offset_rad, + self.node.head_pan_calibrated_looked_left_offset_rad, + self.node.robot) + self.head_tilt_cg = HeadTiltCommandGroup(None, self.node.head_tilt_calibrated_offset_rad, self.node.head_tilt_calibrated_looking_up_offset_rad, - self.node.head_tilt_backlash_transition_angle_rad) - self.wrist_yaw_cg = WristYawCommandGroup(wrist_yaw_range_rad) - self.gripper_cg = GripperCommandGroup(gripper_range_robotis) - self.telescoping_cg = TelescopingCommandGroup(tuple(r.arm.params['range_m']), - self.node.wrist_extension_calibrated_retracted_offset_m) - self.lift_cg = LiftCommandGroup(tuple(r.lift.params['range_m'])) + self.node.head_tilt_backlash_transition_angle_rad, + self.node.robot) + self.telescoping_cg = TelescopingCommandGroup(None, + self.node.wrist_extension_calibrated_retracted_offset_m, + self.node.robot) + self.lift_cg = LiftCommandGroup(None, self.node.robot) self.mobile_base_cg = MobileBaseCommandGroup(virtual_range_m=(-0.5, 0.5)) + self.command_groups = [self.head_pan_cg, self.head_tilt_cg, self.telescoping_cg, + self.lift_cg, self.mobile_base_cg] + + # Setup end of arm + for j in self.node.robot.end_of_arm.joints: + if j == "stretch_gripper": + self.gripper_cg = GripperCommandGroup(None, self.node.robot) + self.command_groups.append(self.gripper_cg) + elif j == "wrist_yaw": + self.wrist_yaw_cg = WristYawCommandGroup(None, self.node.robot) + self.command_groups.append(self.wrist_yaw_cg) + else: + module_name = self.node.robot.end_of_arm.params['devices'][j].get('ros_py_module_name') + class_name = self.node.robot.end_of_arm.params['devices'][j].get('ros_py_class_name') + if module_name and class_name: + endofarm_device = getattr(importlib.import_module(module_name), class_name)(None, self.node.robot) + self.command_groups.append(endofarm_device) def execute_cb(self, goal): with self.node.robot_stop_lock: