diff --git a/body/stretch_body/dynamixel_hello_XL430.py b/body/stretch_body/dynamixel_hello_XL430.py index 5b189a81..534b0e6d 100644 --- a/body/stretch_body/dynamixel_hello_XL430.py +++ b/body/stretch_body/dynamixel_hello_XL430.py @@ -106,6 +106,8 @@ def __init__(self, name, chain=None, usb=None): self.total_range = abs(self.ticks_to_world_rad(self.params['range_t'][0]) - self.ticks_to_world_rad(self.params['range_t'][1])) self.in_collision_stop = {'pos': False, 'neg': False} self.ts_collision_stop = {'pos': 0.0, 'neg': 0.0} + self.forced_collision_stop_override = {'pos': False, 'neg':False} + self._in_collision_stop_override = False except KeyError: self.motor=None @@ -449,17 +451,29 @@ def step_collision_avoidance(self,in_collision): # print('Invalid IN_COLLISION for joint %s'%self.name) # return + if True in self.forced_collision_stop_override.values(): + if not self._in_collision_stop_override: + self.quick_stop() + self._in_collision_stop_override = True + return + + if not True in self.forced_collision_stop_override.values(): + self._in_collision_stop_override = False + + for dir in ['pos','neg']: if in_collision[dir] and not self.in_collision_stop[dir]: # Stop current motion self.ts_collision_stop[dir] = time.time() - self.quick_stop() + if not self.was_runstopped: + self.quick_stop() self.in_collision_stop[dir] = True #Reset if out of collision (at least 1s after collision) - if self.in_collision_stop[dir] and not in_collision[dir] and time.time()-self.ts_collision_stop[dir]>1.0: + if self.in_collision_stop[dir] and not in_collision[dir] and time.time()-self.ts_collision_stop[dir]>1: self.in_collision_stop[dir] = False + def get_braking_distance(self,acc=None): """Compute distance to brake the joint from the current velocity""" v_curr = self.status['vel'] @@ -585,6 +599,14 @@ def set_velocity(self,v_des,a_des=None): self.enable_torque() v = min(self.params['motion']['max']['vel'],abs(v_des)) + if self.forced_collision_stop_override['pos'] and v_des>0: + self.logger.warning(f"set_velocity in Forced Collision stop. Motion disabled in direction {'pos'} for {self.name}. Not executing move_to") + return + + if self.forced_collision_stop_override['neg'] and v_des<0: + self.logger.warning(f"set_velocity in Forced Collision stop. Motion disabled in direction {'neg'} for {self.name}. Not executing move_to") + return + if self.in_collision_stop['pos'] and v_des>0: self.logger.warning('set_velocity in collision . Motion disabled in direction %s for %s. Not executing set_velocity'%('pos',self.name)) return @@ -707,6 +729,13 @@ def move_to(self,x_des, v_des=None, a_des=None): print('Dynamixel not calibrated:', self.name) return + if self.forced_collision_stop_override['pos'] and self.status['pos']x_des: + self.logger.warning(f"move_to in Forced Collision stop. Motion disabled in direction {'neg'} for {self.name}. Not executing move_to") + return if self.in_collision_stop['pos'] and self.status['pos'] 0.9: + robot.lift.move_by(-0.2) + robot.push_command() + time.sleep(0.25) + while robot.lift.motor.status['is_moving']: + time.sleep(0.1) + + if not (2 > self.motors['wrist_yaw'].status['pos'] > 0): + self.move_to('wrist_pitch',-1.57) + + def switch_to_portrait_mode(self): + self.portrait_orientation = True + self.reset_tablet_orientation() + + def switch_to_landscape_mode(self): + self.portrait_orientation = False + self.reset_tablet_orientation() + + def reset_tablet_orientation(self): + if self.portrait_orientation: + self.motors['wrist_roll'].move_to(1.57) + else: + self.motors['wrist_roll'].move_to(self.params['stow']['wrist_roll']) + + def stow(self): + # Fold Arm, Wrist yaw turns left making the tabled screen face forward. + print('--------- Stowing %s ----'%self.name) + self.reset_tablet_orientation() + self.move_to('wrist_yaw', self.params['stow']['wrist_yaw']) + time.sleep(1) + self.move_to('wrist_pitch', self.params['stow']['wrist_pitch']) + time.sleep(1) + + def home(self): + # Tablet should face completely downwards during homing + self.motors['wrist_pitch'].move_to(-1.57) + while self.motors['wrist_pitch'].motor.is_moving(): + time.sleep(0.2) + self.reset_tablet_orientation() + self.motors['wrist_yaw'].home() + self.motors['wrist_pitch'].move_to(self.params['stow']['wrist_pitch']) + self.motors['wrist_yaw'].move_to(1.57) + time.sleep(2) + + def step_sentry(self, robot): + super().step_sentry(robot) + if robot.collision.running: + wrist_p = self.get_joint('wrist_pitch').status['pos'] + wrist_y = self.get_joint('wrist_yaw').status['pos'] + # TODO: Add more special conditions around this part + if wrist_p > -0.2 and wrist_y < 0.18: + # print("In Special Stop") + self.get_joint('wrist_yaw').forced_collision_stop_override = {'pos': False, 'neg':True} + self.get_joint('wrist_pitch').forced_collision_stop_override = {'pos': True, 'neg':False} + + else: + # print("Out Special Stop") + self.get_joint('wrist_yaw').forced_collision_stop_override = {'pos': False, 'neg':False} + self.get_joint('wrist_pitch').forced_collision_stop_override = {'pos': False, 'neg':False} \ No newline at end of file diff --git a/body/stretch_body/gamepad_controller.py b/body/stretch_body/gamepad_controller.py index 3f6306cb..9ca6b38f 100755 --- a/body/stretch_body/gamepad_controller.py +++ b/body/stretch_body/gamepad_controller.py @@ -334,7 +334,7 @@ def main(): for k in state.keys(): print(k, ' : ', state[k]) print('------------------------------') - time.sleep(1.0) + time.sleep(0.1) except (KeyboardInterrupt, SystemExit): pass diff --git a/body/stretch_body/gamepad_joints.py b/body/stretch_body/gamepad_joints.py index 5734f84d..7e55b19c 100644 --- a/body/stretch_body/gamepad_joints.py +++ b/body/stretch_body/gamepad_joints.py @@ -439,10 +439,6 @@ def command_stick_to_motion(self, x, robot): x (float): Range [-1.0,+1.0], control servo speed robot (robot.Robot): Valid robot instance """ - if 'wrist' in self.name: - motor = robot.end_of_arm.get_joint(self.name) - if 'head' in self.name: - motor = robot.head.get_joint(self.name) if abs(x)= 2: + if time.time() - self._last_fn_btn_press >= 2 and not self.currently_stowing: self.do_single_beep(robot) self._last_fn_btn_press = None - self.stow_robot(robot) + threading.Thread(target=self.stow_robot,args=(robot,),daemon=True).start() + # self.stow_robot(robot) else: self._last_fn_btn_press = None @@ -412,11 +416,13 @@ def _safety_stop(self, robot): def stow_robot(self, robot): if robot.is_homed(): # Reset motion params as fast for xbox + self.currently_stowing = True v = robot.end_of_arm.motors['wrist_yaw'].params['motion']['default']['vel'] a = robot.end_of_arm.motors['wrist_yaw'].params['motion']['default']['accel'] robot.end_of_arm.motors['wrist_yaw'].set_motion_params(v, a) robot.stow() self.do_single_beep(robot) + self.currently_stowing = False def stop(self): if self._needs_robot_startup: diff --git a/body/stretch_body/head.py b/body/stretch_body/head.py index ab17229e..cff65f6e 100644 --- a/body/stretch_body/head.py +++ b/body/stretch_body/head.py @@ -56,6 +56,14 @@ def move_by(self, joint, x_r, v_r=None, a_r=None): """ self.motors[joint].move_by(x_r,v_r,a_r) + def set_velocity(self, joint, v_r, a_r=None): + """ + joint: name of joint (string) + v_r: commanded velocity (rad/s). + a_r: acceleration motion profile (rad/s^2) + """ + self.motors[joint].set_velocity(v_r, a_r) + def home(self): if self.motors['head_pan'].params['req_calibration']: with self.pt_lock: diff --git a/body/stretch_body/prismatic_joint.py b/body/stretch_body/prismatic_joint.py index 02ffb1fc..d71e72b3 100644 --- a/body/stretch_body/prismatic_joint.py +++ b/body/stretch_body/prismatic_joint.py @@ -39,6 +39,7 @@ def __init__(self,name,usb=None): self.dist_to_min_max = None # track dist to min,max limits self.vel_brake_zone_thresh = 0.02 # initial/minimum brake zone thresh value self._prev_set_vel_ts = None + self._prev_collision_update_ts = None self.watchdog_enabled = False self.total_range = abs(self.params['range_m'][1] - self.params['range_m'][0]) self.in_collision_stop = {'pos': False, 'neg': False} @@ -590,7 +591,7 @@ def step_collision_avoidance(self,in_collision): # if in_collision['pos'] and in_collision['neg']: # print('Invalid IN_COLLISION for joint %s'%self.name) # return - + for dir in ['pos','neg']: if in_collision[dir] and not self.in_collision_stop[dir]: # Stop current motion @@ -598,10 +599,12 @@ def step_collision_avoidance(self,in_collision): self.push_command() self.in_collision_stop[dir] = True self.ts_collision_stop[dir]=time.time() + self.collision_till_zero_vel_counter = 0 #Reset if out of collision (at least 1s after collision) if self.in_collision_stop[dir] and not in_collision[dir] and time.time()-self.ts_collision_stop[dir]>1.0: self.in_collision_stop[dir] = False + diff --git a/body/stretch_body/robot.py b/body/stretch_body/robot.py index 6609a75f..ad0da735 100644 --- a/body/stretch_body/robot.py +++ b/body/stretch_body/robot.py @@ -156,8 +156,8 @@ def step(self): if self.robot.params['use_sentry']: if (self.titr % self.sentry_downrate_int) == 0: self.robot._step_sentry() - if self.robot.is_homed(): - self.robot.collision.step() + # if self.robot.is_homed(): + # self.robot.collision.step() if (self.titr % self.trajectory_downrate_int) == 0: self.robot._update_trajectory_non_dynamixel() self.stats.mark_loop_end() @@ -170,6 +170,36 @@ def run(self): self.step() self.robot.logger.debug('Shutting down SystemMonitorThread') +class CollisionMonitorThread(threading.Thread): + """ + This thread runs at 25Hz. + It updates the status data of the Devices. + It also steps the Sentry, Monitor, and Collision functions + """ + def __init__(self, robot, target_rate_hz=25.0): + threading.Thread.__init__(self, name = self.__class__.__name__) + self.robot=robot + self.robot_update_rate_hz = target_rate_hz + self.shutdown_flag = threading.Event() + self.stats = hello_utils.LoopStats(loop_name='CollisionMonitorThread',target_loop_rate=self.robot_update_rate_hz) + self.titr=0 + self.running=False + + def step(self): + self.titr = self.titr + 1 + self.stats.mark_loop_start() + if self.robot.is_homed(): + self.robot.collision.step() + self.stats.mark_loop_end() + + def run(self): + self.running=True + while not self.shutdown_flag.is_set(): + self.stats.wait_until_ready_to_run() + if not self.shutdown_flag.is_set(): + self.step() + self.robot.logger.debug('Shutting down CollisionMonitorThread') + class Robot(Device): """ API to the Stretch Robot @@ -228,6 +258,7 @@ def __init__(self): self.sys_thread = None self.dxl_head_thread = None self.event_loop_thread = None + self.collision_mgmt_thread = None self.eoa_name= self.params['tool'] module_name = self.robot_params[self.eoa_name]['py_module_name'] @@ -309,6 +340,7 @@ def startup(self,start_non_dxl_thread=True,start_dxl_thread=True,start_sys_mon_t self.dxl_end_of_arm_thread = DXLEndOfArmStatusThread(self,target_rate_hz=self.params['rates']['DXLStatusThread_Hz']) self.sys_thread = SystemMonitorThread(self, target_rate_hz=self.params['rates']['SystemMonitorThread_Hz']) self.dxl_head_thread = DXLHeadStatusThread(self, target_rate_hz=self.params['rates']['DXLStatusThread_Hz']) + self.collision_mgmt_thread = CollisionMonitorThread(self, target_rate_hz=100) if start_non_dxl_thread: self.non_dxl_thread.setDaemon(True) @@ -327,6 +359,10 @@ def startup(self,start_non_dxl_thread=True,start_dxl_thread=True,start_sys_mon_t self.sys_thread.setDaemon(True) self.sys_thread.start() + if start_sys_mon_thread and self.collision_mgmt_thread: + self.collision_mgmt_thread.setDaemon(True) + self.collision_mgmt_thread.start() + return success def stop(self): @@ -352,6 +388,10 @@ def stop(self): if self.sys_thread.running: self.sys_thread.shutdown_flag.set() self.sys_thread.join(1) + if self.collision_mgmt_thread: + if self.collision_mgmt_thread.running: + self.collision_mgmt_thread.shutdown_flag.set() + self.collision_mgmt_thread.join(1) for k in self.devices: if self.devices[k] is not None: self.logger.debug('Shutting down %s'%k) @@ -526,6 +566,7 @@ def stow(self): Cause the robot to move to its stow position Blocking. """ + self.disable_collision_mgmt() self.head.move_to('head_pan', self.get_stow_pos('head_pan')) self.head.move_to('head_tilt',self.get_stow_pos('head_pan')) @@ -540,10 +581,9 @@ def stow(self): while not self.lift.motor.status['near_pos_setpoint'] and time.time() - ts < 4.0: time.sleep(0.1) lift_stowed=True - - # Wrist pitch should be lifted up before closing the arm - if 'wrist_pitch' in self.end_of_arm.joints: - self.end_of_arm.move_to('wrist_pitch', self.end_of_arm.params['stow']['wrist_pitch']) + + # Run pre stow specific to each end of arm + self.end_of_arm.pre_stow(self) #Bring in arm before bring down print('--------- Stowing Arm ----') @@ -569,23 +609,32 @@ def stow(self): #Make sure wrist yaw is done before exiting while self.end_of_arm.motors['wrist_yaw'].motor.is_moving(): time.sleep(0.1) + self.enable_collision_mgmt() def home(self): """ Cause the robot to home its joints by moving to hardstops Blocking. """ + self.disable_collision_mgmt() if self.head is not None: print('--------- Homing Head ----') self.head.home() # Wrist pitch should be angled somewhere in between the lift leaving # the base of its range and before it reaches the top of its range. - def _angle_pitch(): + def _angle_pitch(angle): time.sleep(1) # wait 1 sec to leave bottom of lift's range if 'wrist_pitch' in self.end_of_arm.joints: - self.end_of_arm.move_to('wrist_pitch', self.end_of_arm.params['stow']['wrist_pitch']) - threading.Thread(target=_angle_pitch).start() + self.end_of_arm.move_to('wrist_pitch', angle) + + # If gripper present + if 'stretch_gripper' in self.end_of_arm.joints: + threading.Thread(target=_angle_pitch,args=(self.end_of_arm.params['stow']['wrist_pitch'],)).start() + + # No gripper but tablet or accessory present in dw3 wrist, wrist pitch should face downward + elif 'wrist_pitch' in self.end_of_arm.joints: + threading.Thread(target=_angle_pitch,args=(-1.57,)).start() # Home the lift if self.lift is not None: @@ -600,6 +649,8 @@ def _angle_pitch(): # Home the end-of-arm if self.end_of_arm is not None: self.end_of_arm.home() + + self.enable_collision_mgmt() #Let user know it is done self.pimu.trigger_beep() self.push_command() diff --git a/body/stretch_body/robot_collision.py b/body/stretch_body/robot_collision.py index 697bfb76..30627214 100644 --- a/body/stretch_body/robot_collision.py +++ b/body/stretch_body/robot_collision.py @@ -7,6 +7,11 @@ import time import threading import chime +import random +from stretch_body.robot_params import RobotParams +import multiprocessing +import signal +import ctypes try: # works on ubunut 22.04 @@ -82,6 +87,85 @@ def check_pts_in_AABB_cube(cube, pts): return True return False +def check_AABB_in_AABB_from_pts(pts1, pts2): + """ + Check if an AABB intersects another AABB from the given two sets of points + """ + print(pts1.shape,pts2.shape) + xmax_1 = max(pts1[:, 0]) + xmin_1 = min(pts1[:, 0]) + ymax_1 = max(pts1[:, 1]) + ymin_1 = min(pts1[:, 1]) + zmax_1 = max(pts1[:, 2]) + zmin_1 = min(pts1[:, 2]) + + xmax_2 = max(pts2[:, 0]) + xmin_2 = min(pts2[:, 0]) + ymax_2 = max(pts2[:, 1]) + ymin_2 = min(pts2[:, 1]) + zmax_2 = max(pts2[:, 2]) + zmin_2 = min(pts2[:, 2]) + + cx = xmin_1<=xmax_2 and xmax_1>=xmin_2 + cy = ymin_1<=ymax_2 and ymax_1>=ymin_2 + cz = zmin_1<=zmax_2 and zmax_1>=zmin_2 + + return cx and cy and cz + + +def check_mesh_triangle_edges_in_cube(mesh_triangles,cube): + # Check a set of mesh's triangles intersect an AABB cube + while len(mesh_triangles): + # choose a random triangle indices + random_index = random.randint(0, len(mesh_triangles) - 1) + points = mesh_triangles[random_index] + mesh_triangles.pop(random_index) + # Barycentric Coordinates based points based edge points interpolation + if check_pts_in_AABB_cube(cube,sample_points_on_triangle_edges(np.array(points))): + return True + return False + +def get_triangle_edge_barycentric_coords(N): + """ + Generate a Barycentric coordinate vectors of N points of a triangle edges + This matrix is to be used as a constant. + """ + barycentric_coords = [] + nums = np.linspace(0,1,num=N) + for n1 in nums: + for n2 in nums: + for n3 in nums: + c = False + if abs(n3)<0.001: + c = True + if abs(n2)<0.001: + c = True + if abs(n1)<0.001: + c = True + if c and abs((1-n2-n3)-n1)<0.001: + barycentric_coords.append([n1,n2,n3]) + + return np.array(barycentric_coords) + +BARYCENTRIC_COORDS = get_triangle_edge_barycentric_coords(25) # Sample a NX3 Barycentric Coord vector matrix + +def sample_points_on_triangle_edges(points): + # Convert barycentric coordinates to Cartesian coordinates + points = BARYCENTRIC_COORDS[:, 0][:, np.newaxis] * points[0] \ + + BARYCENTRIC_COORDS[:, 1][:, np.newaxis] * points[1] \ + + BARYCENTRIC_COORDS[:, 2][:, np.newaxis] * points[2] + return points + +def scale_cuboid_points(vertices,scale_factor): + # Calculate the centroid of the cuboid + centroid = np.mean(vertices, axis=0) + + # Scale the vertices relative to the centroid + scaled_vertices = centroid + scale_factor * (vertices - centroid) + + # Convert the scaled vertices back to a list of tuples and return + return scaled_vertices + def check_ppd_edges_in_cube(cube,cube_edge,edge_indices): if len(edge_indices)!=12: print('Invalid PPD provided to check_ppd_edges_in_cube') @@ -101,6 +185,22 @@ def check_ppd_edges_in_cube(cube,cube_edge,edge_indices): return True return False +def closest_pair_3d(points1, points2): + """ + Find the closest pair of 3D points from two lists of 3D points. + """ + closest_distance = float('inf') + closest_pair = None + + for p1 in points1: + for p2 in points2: + dist = np.linalg.norm(p1-p2) + if dist < closest_distance: + closest_distance = dist + closest_pair = (p1, p2) + + return closest_pair, closest_distance + # ####################################################################### class CollisionLink: @@ -172,6 +272,16 @@ def find_edge_indices_PPD(self): lens.sort() print('LENS',lens) return q + + def get_triangles(self): + triangles_idx=self.mesh.cells_dict['triangle'] + triangles = [] + for t_set in triangles_idx: + p1 = self.pose[t_set[0]] + p2 = self.pose[t_set[1]] + p3 = self.pose[t_set[2]] + triangles.append([p1,p2,p3]) + return triangles def check_AABB(self,pts): """ @@ -199,13 +309,14 @@ def check_AABB(self,pts): return True class CollisionPair: - def __init__(self, name,link_pts,link_cube,detect_as): + def __init__(self, name,link_pts,link_cube,detect_as, cube_scale=1.2): self.in_collision=False self.was_in_collision=False self.link_cube=link_cube self.link_pts=link_pts self.detect_as=detect_as self.name=name + self.cube_scale = cube_scale self.is_valid=self.link_cube.is_valid and self.link_pts.is_valid and self.link_cube.is_aabb if not self.is_valid: print('Dropping monitor of collision pair %s'%self.name_id) @@ -222,17 +333,30 @@ def pretty_print(self): class CollisionJoint: - def __init__(self, joint_name,motor): + def __init__(self, joint_name): self.name=joint_name - self.motor=motor self.active_collisions=[] self.collision_pairs=[] self.collision_dirs={} self.in_collision={'pos':False,'neg':False} self.was_in_collision = {'pos': False, 'neg': False} + self.last_in_collision_cnt = 0 + def add_collision_pair(self,motion_dir, collision_pair): self.collision_pairs.append(collision_pair) self.collision_dirs[collision_pair.name]=motion_dir + + def update_last_joint_cfg_thresh(self,thresh): + self.in_collision['last_joint_cfg_thresh'] = thresh + + + def update_collision_pair_min_dist(self,pair_name): + for cp in self.collision_pairs: + if cp.name == pair_name: + _,dist = closest_pair_3d(cp.link_cube.pose,cp.link_pts.pose) + self.in_collision['las_cp_min_dist'] = {'pair_name':pair_name,'dist':dist} + return + def pretty_print(self): print('-------Collision Joint: %s-----------------'%self.name) for cp in self.collision_pairs: @@ -243,9 +367,166 @@ def pretty_print(self): for ac in self.active_collisions: print('Active Collision: %s' % ac) +def _collision_compute_worker(name, shared_is_running, shared_joint_cfg, shared_collision_status, exit_event): + signal.signal(signal.SIGINT, signal_handler) + signal.signal(signal.SIGTERM, signal_handler) + collision_compute = RobotCollisionCompute(name) + collision_compute.startup() + collision_joints_status = {} + time.sleep(0.5) + while not exit_event.is_set(): + try: + if shared_is_running.value: + collision_compute.step(shared_joint_cfg) + for joint_name in collision_compute.collision_joints: + collision_joints_status[joint_name] = collision_compute.collision_joints[joint_name].in_collision + shared_collision_status.put(collision_joints_status) + except (BrokenPipeError,ConnectionResetError): + pass + +def signal_handler(signal_received, frame): + exit(0) class RobotCollisionMgmt(Device): - def __init__(self,robot): + def __init__(self,robot,name='robot_collision_mgmt'): + self.name = name + self.robot = robot + self.shared_joint_cfg = multiprocessing.Queue() + self.shared_collision_status = multiprocessing.Queue() + self.shared_is_running = multiprocessing.Value(ctypes.c_bool, False) + self.exit_event = multiprocessing.Event() + self.collision_compute_proccess = multiprocessing.Process(target=_collision_compute_worker, + args=(self.name, + self.shared_is_running, + self.shared_joint_cfg, + self.shared_collision_status, + self.exit_event,),daemon=True) + self.running = False + self.robot_params = RobotParams().get_params()[1] + self.collision_status = {} + self.brake_joints = {} + + def startup(self): + self.collision_compute_proccess.start() + + def stop(self): + self.exit_event.set() + self.shared_is_running.set(False) + self.collision_compute_proccess.terminate() + self.collision_compute_proccess.join() + + def step(self): + try: + self.shared_is_running.value = self.running + if self.running: + config = self.get_joint_configuration(self.brake_joints) + self.shared_joint_cfg.put(config) + self.collision_status.update(self.shared_collision_status.get()) + for j in self.collision_status.keys(): + jm = self.get_joint_motor(j) + jm.step_collision_avoidance(self.collision_status[j]) + if True in self.collision_status[j].values(): + self.brake_joints[j] = True + else: + self.brake_joints[j] = False + + except (BrokenPipeError,ConnectionResetError): + pass + + def get_joint_motor(self,joint_name): + if joint_name=='lift': + return self.robot.lift + if joint_name=='arm': + return self.robot.arm + if joint_name=='head_pan': + return self.robot.head.get_joint('head_pan') + if joint_name=='head_tilt': + return self.robot.head.get_joint('head_tilt') + #Try the tool + return self.robot.end_of_arm.get_joint(joint_name) + + # def get_joint_configuration(self,braked=False): + # """ + # Construct a dictionary of robot's current pose + # """ + # s = self.robot.get_status() + # kbd = self.robot_params['robot_collision_mgmt'][self.robot.params['model_name']]['k_brake_distance'] + # if braked: + # da=kbd['arm']*self.robot.arm.get_braking_distance()/4.0 + # dl=kbd['lift']*self.robot.lift.get_braking_distance() + # dhp = kbd['head_pan'] * self.robot.head.get_joint('head_pan').get_braking_distance() + # dht = kbd['head_tilt'] * self.robot.head.get_joint('head_tilt').get_braking_distance() + # else: + # da=0.0 + # dl=0.0 + # dhp=0.0 + # dht=0.0 + + # configuration = { + # 'joint_lift': dl+s['lift']['pos'], + # 'joint_arm_l0': da+s['arm']['pos']/4.0, + # 'joint_arm_l1': da+s['arm']['pos']/4.0, + # 'joint_arm_l2': da+s['arm']['pos']/4.0, + # 'joint_arm_l3': da+s['arm']['pos']/4.0, + # 'joint_head_pan': dhp+s['head']['head_pan']['pos'], + # 'joint_head_tilt': dht+s['head']['head_tilt']['pos'] + # } + + # configuration.update(self.robot.end_of_arm.get_joint_configuration(braked)) + # return configuration + + def get_joint_configuration(self,brake_joints={}): + """ + Construct a dictionary of robot's current pose + """ + s = self.robot.get_status() + kbd = self.robot_params['robot_collision_mgmt'][self.robot.params['model_name']]['k_brake_distance'] + da=0.0 + dl=0.0 + dhp=0.0 + dht=0.0 + try: + if brake_joints['arm']: + da=kbd['arm']*self.robot.arm.get_braking_distance()/4.0 + except KeyError: + da=0.0 + try: + if brake_joints['lift']: + dl=kbd['lift']*self.robot.lift.get_braking_distance() + except KeyError: + dl=0.0 + try: + if brake_joints['head_pan']: + dhp = kbd['head_pan'] * self.robot.head.get_joint('head_pan').get_braking_distance() + except KeyError: + dhp=0.0 + try: + if brake_joints['head_tilt']: + dht = kbd['head_tilt'] * self.robot.head.get_joint('head_tilt').get_braking_distance() + except KeyError: + dht=0.0 + + configuration = { + 'joint_lift': dl+s['lift']['pos'], + 'joint_arm_l0': da+s['arm']['pos']/4.0, + 'joint_arm_l1': da+s['arm']['pos']/4.0, + 'joint_arm_l2': da+s['arm']['pos']/4.0, + 'joint_arm_l3': da+s['arm']['pos']/4.0, + 'joint_head_pan': dhp+s['head']['head_pan']['pos'], + 'joint_head_tilt': dht+s['head']['head_tilt']['pos'] + } + + configuration.update(self.robot.end_of_arm.get_joint_configuration(brake_joints)) + return configuration + + def enable(self): + self.running=True + + def disable(self): + self.running=False + +class RobotCollisionCompute(Device): + def __init__(self,name='robot_collision_mgmt'): """ RobotCollisionMgmt monitors for collisions between links. It utilizes the Collision mesh for collision estimation. @@ -258,29 +539,26 @@ def __init__(self,robot): Each link includes a parameter "scale_pct" which allows the mesh size to be expanded by a percentage around its centroid enabling the ability to increase the safety zone. """ - Device.__init__(self, name='robot_collision_mgmt') - self.robot = robot + Device.__init__(self, name) self.collision_joints = {} self.collision_links = {} self.collision_pairs = {} - #chime.theme('big-sur') #'material')# - self.running=False + chime.theme('big-sur') #'material') self.urdf=None + self.prev_loop_start_ts = None + self.robot_params = RobotParams().get_params()[1] def pretty_print(self): for j in self.collision_joints: self.collision_joints[j].pretty_print() - def enable(self): - self.running=True - def disable(self): - self.running=False + def startup(self,threaded=False): - Device.startup(self, threaded=False) + Device.startup(self, threaded) pkg = str(importlib_resources.files("stretch_urdf")) # .local/lib/python3.10/site-packages/stretch_urdf) - model_name = self.robot.params['model_name'] - eoa_name= self.robot.params['tool'] + model_name = self.robot_params['robot']['model_name'] + eoa_name= self.robot_params['robot']['tool'] urdf_name = pkg + '/%s/stretch_description_%s_%s.urdf' % (model_name, model_name, eoa_name) mesh_path = pkg + '/%s/' % (model_name) @@ -299,7 +577,7 @@ def startup(self,threaded=False): #Construct collision pairs cp_dict = self.params[model_name]['collision_pairs'] - cp_dict.update(self.robot.end_of_arm.params['collision_mgmt']['collision_pairs']) + cp_dict.update(self.robot_params[eoa_name]['collision_mgmt']['collision_pairs']) for cp_name in cp_dict: cp=cp_dict[cp_name] #Eg {'link_pts': 'link_head_tilt', 'link_cube': 'link_arm_l4','detect_as':'pts'} @@ -313,12 +591,14 @@ def startup(self,threaded=False): link_pts=self.collision_links[cp['link_pts']], link_cube=self.collision_links[cp['link_cube']], detect_as=cp['detect_as']) + if 'cube_scale' in list(cp.keys()): + self.collision_pairs[cp_name].cube_scale = cp['cube_scale'] #Assign collision pairs to each joint #Include those of standard robot body plus its defined tool # EG collision_joints={'lift':[{collision_1},{collision_2...}],'head_pan':[...]} cj_dict=self.params[model_name]['joints'] - eoa_cj_dict=self.robot.end_of_arm.params['collision_mgmt']['joints'] + eoa_cj_dict=self.robot_params[eoa_name]['collision_mgmt']['joints'] for tt in eoa_cj_dict: if tt in cj_dict: @@ -327,40 +607,32 @@ def startup(self,threaded=False): cj_dict[tt]=eoa_cj_dict[tt] for joint_name in cj_dict: - self.collision_joints[joint_name]=CollisionJoint(joint_name,self.get_joint_motor(joint_name)) + self.collision_joints[joint_name]=CollisionJoint(joint_name) cp_list = cj_dict[joint_name] for cp in cp_list: #eg cp={'motion_dir': 'pos', 'collision_pair': 'link_head_tilt_TO_link_arm_l4'} self.collision_joints[joint_name].add_collision_pair(motion_dir=cp['motion_dir'], collision_pair=self.collision_pairs[cp['collision_pair']]) - - def get_joint_motor(self,joint_name): - if joint_name=='lift': - return self.robot.lift - if joint_name=='arm': - return self.robot.arm - if joint_name=='head_pan': - return self.robot.head.get_joint('head_pan') - if joint_name=='head_tilt': - return self.robot.head.get_joint('head_tilt') - #Try the tool - return self.robot.end_of_arm.get_joint(joint_name) - + def step(self,cfg=None): """ Check for interference between cube pairs """ - if self.urdf is None or not self.running: + # if self.prev_loop_start_ts: + # print(f"[{self.name}] Step exec time: {(time.perf_counter()-self.prev_loop_start_ts)*1000}ms") + + if self.urdf is None: return - if cfg is None: - cfg = self.get_joint_configuration(braked=True)#_braked() + # if cfg is None: + # cfg = self.get_joint_configuration(braked=True)#_braked() # Update forward kinematics of links - lfk = self.urdf.link_fk(cfg=cfg, links=self.collision_links.keys(), use_names=True) + _cfg = cfg.get() + lfk = self.urdf.link_fk(cfg=_cfg, links=self.collision_links.keys(), use_names=True) # Update poses of links based on fk - for link_name in lfk: + for link_name in lfk: self.collision_links[link_name].set_pose(lfk[link_name].dot( self.collision_links[link_name].points.transpose()).transpose()) @@ -371,18 +643,27 @@ def step(self,cfg=None): for joint_name in self.collision_joints: self.collision_joints[joint_name].active_collisions=[] self.collision_joints[joint_name].was_in_collision = self.collision_joints[joint_name].in_collision.copy() - self.collision_joints[joint_name].in_collision = {'pos': False, 'neg': False} + # print(f"[{joint_name}] Was in Collision cnt: {self.collision_joints[joint_name].last_in_collision_cnt}") + # Release Collision Joints in_collision mode onlt after 100 cycles + if self.collision_joints[joint_name].in_collision['pos'] or self.collision_joints[joint_name].in_collision['neg']: + self.collision_joints[joint_name].last_in_collision_cnt = self.collision_joints[joint_name].last_in_collision_cnt + 1 + if self.collision_joints[joint_name].last_in_collision_cnt > 20: + self.collision_joints[joint_name].in_collision['pos'] = False + self.collision_joints[joint_name].in_collision['neg'] = False + self.collision_joints[joint_name].last_in_collision_cnt = 0 # Test for collisions across all collision pairs for pair_name in self.collision_pairs: cp=self.collision_pairs[pair_name] if cp.is_valid: cp.was_in_collision=cp.in_collision if cp.detect_as=='pts': - cp.in_collision=check_pts_in_AABB_cube(cube=cp.link_cube.pose,pts=cp.link_pts.pose) - # elif cp.detect_as=='edges': - # print('Checking', cp.name) - # cp.in_collision = check_ppd_edges_in_cube(cube=cp.link_cube.pose, cube_edge=cp.link_pts.pose,edge_indices=cp.link_pts.edge_indices_ppd) + cp.in_collision=check_pts_in_AABB_cube(cube = scale_cuboid_points(cp.link_cube.pose,cp.cube_scale), + pts = cp.link_pts.pose) + # cp.in_collision=check_AABB_in_AABB_from_pts(pts1=cp.link_cube.pose,pts2=cp.link_pts.pose) + elif cp.detect_as=='edges': + cp.in_collision = check_mesh_triangle_edges_in_cube(mesh_triangles = cp.link_pts.get_triangles(), + cube = scale_cuboid_points(cp.link_cube.pose,cp.cube_scale)) else: cp.in_collision =False #cp.pretty_print() @@ -393,10 +674,10 @@ def step(self,cfg=None): # Beep on new collision if not self.collision_pairs[pair_name].was_in_collision and self.collision_pairs[pair_name].in_collision: - print('New collision pair event: %s'%pair_name) - # print('\a') + print(f'New collision pair event: {pair_name} [{time.time()}]' ) self.alert() + # print(f"From Process: Normal CFG = {normalized_joint_status_thresh}") #Now update joint state for joint_name in self.collision_joints: cj = self.collision_joints[joint_name] @@ -404,9 +685,10 @@ def step(self,cfg=None): if cp.in_collision: cj.active_collisions.append(cp.name) #Add collision to joint cj.in_collision[cj.collision_dirs[cp.name]] = True - #Finally, update the collision state for each joint - self.collision_joints[joint_name].motor.step_collision_avoidance(self.collision_joints[joint_name].in_collision) - + # print(f"From Process: {joint_name} = {self.collision_joints[joint_name].in_collision}") + # self.collision_joints[joint_name].motor.step_collision_avoidance(self.collision_joints[joint_name].in_collision) + self.prev_loop_start_ts = time.perf_counter() + def alert(self): threading.Thread(target=chime.warning,daemon=True).start() @@ -427,37 +709,6 @@ def was_link_in_collsion(self,link_name): return False - def get_joint_configuration(self,braked=False): - """ - Construct a dictionary of robot's current pose - """ - s = self.robot.get_status() - - kbd = self.params[self.robot.params['model_name']]['k_brake_distance'] - if braked: - da=kbd['arm']*self.robot.arm.get_braking_distance()/4.0 - dl=kbd['lift']*self.robot.lift.get_braking_distance() - dhp = kbd['head_pan'] * self.robot.head.get_joint('head_pan').get_braking_distance() - dht = kbd['head_tilt'] * self.robot.head.get_joint('head_tilt').get_braking_distance() - else: - da=0.0 - dl=0.0 - dhp=0.0 - dht=0.0 - - configuration = { - 'joint_lift': dl+s['lift']['pos'], - 'joint_arm_l0': da+s['arm']['pos']/4.0, - 'joint_arm_l1': da+s['arm']['pos']/4.0, - 'joint_arm_l2': da+s['arm']['pos']/4.0, - 'joint_arm_l3': da+s['arm']['pos']/4.0, - 'joint_head_pan': dhp+s['head']['head_pan']['pos'], - 'joint_head_tilt': dht+s['head']['head_tilt']['pos'] - } - - configuration.update(self.robot.end_of_arm.get_joint_configuration(braked)) - return configuration - class RobotCollision(Device): """ Deprecated. Keep shell class (for now) to avoid breaking legacy code. diff --git a/body/stretch_body/robot_params_SE3.py b/body/stretch_body/robot_params_SE3.py index bd5e2e1c..97a8b94c 100644 --- a/body/stretch_body/robot_params_SE3.py +++ b/body/stretch_body/robot_params_SE3.py @@ -252,6 +252,9 @@ 'current_float_A': 0.04, 'current_limit_A': 1.0} +SE3_wrist_roll_DW3_tablet = SE3_wrist_roll_DW3 +SE3_wrist_roll_DW3_tablet['float_on_stop'] = 0 + # ######### EndOfArm Defn ############## """ Define the EndOfArm DynamixelXChain parameters @@ -274,18 +277,24 @@ 'stretch_gripper':0.0 }, 'collision_mgmt': { - 'k_brake_distance': {'wrist_pitch': 0.25, 'wrist_yaw': 0.25, 'wrist_roll': 0.25, 'stretch_gripper': 0.0}, + 'k_brake_distance': {'wrist_pitch': 3.0, 'wrist_yaw': 4.0, 'wrist_roll': 2.0, 'stretch_gripper': 0.0}, 'collision_pairs': { - 'link_gripper_fingertip_left_TO_link_lift': {'link_pts': 'link_gripper_fingertip_left', 'link_cube': 'link_lift','detect_as': 'pts'}, - 'link_gripper_s3_body_TO_base_link': {'link_pts': 'link_gripper_s3_body', 'link_cube': 'base_link','detect_as': 'pts'}, - 'link_wrist_pitch_TO_base_link': {'link_pts': 'link_wrist_pitch', 'link_cube': 'base_link','detect_as': 'pts'}, - 'link_wrist_yaw_bottom_TO_base_link': {'link_pts': 'link_wrist_yaw_bottom', 'link_cube': 'base_link','detect_as': 'pts'}, - 'link_gripper_finger_left_TO_base_link': {'link_pts': 'link_gripper_finger_left','link_cube': 'base_link', 'detect_as': 'pts'}, - 'link_gripper_finger_right_TO_base_link': {'link_pts': 'link_gripper_finger_right','link_cube': 'base_link', 'detect_as': 'pts'}, - 'link_gripper_fingertip_left_TO_base_link': {'link_pts': 'link_gripper_fingertip_left','link_cube': 'base_link', 'detect_as': 'pts'}, - 'link_gripper_fingertip_right_TO_base_link': {'link_pts': 'link_gripper_fingertip_right','link_cube': 'base_link', 'detect_as': 'pts'}, - 'link_gripper_s3_body_TO_link_arm_l0': {'link_pts': 'link_gripper_s3_body', 'link_cube': 'link_arm_l0','detect_as': 'pts'}, - 'link_gripper_s3_body_TO_link_arm_l1': {'link_pts': 'link_gripper_s3_body', 'link_cube': 'link_arm_l1','detect_as': 'pts'}}, + 'link_gripper_fingertip_left_TO_link_lift': {'link_pts': 'link_gripper_fingertip_left', 'link_cube': 'link_lift','detect_as': 'edges'}, + 'link_gripper_s3_body_TO_base_link': {'link_pts': 'link_gripper_s3_body', 'link_cube': 'base_link','detect_as': 'edges'}, + 'link_wrist_pitch_TO_base_link': {'link_pts': 'link_wrist_pitch', 'link_cube': 'base_link','detect_as': 'edges'}, + 'link_wrist_yaw_bottom_TO_base_link': {'link_pts': 'link_wrist_yaw_bottom', 'link_cube': 'base_link','detect_as': 'edges'}, + 'link_gripper_finger_left_TO_base_link': {'link_pts': 'link_gripper_finger_left','link_cube': 'base_link', 'detect_as': 'edges'}, + 'link_gripper_finger_right_TO_base_link': {'link_pts': 'link_gripper_finger_right','link_cube': 'base_link', 'detect_as': 'edges'}, + 'link_gripper_fingertip_left_TO_base_link': {'link_pts': 'link_gripper_fingertip_left','link_cube': 'base_link', 'detect_as': 'edges'}, + 'link_gripper_fingertip_right_TO_base_link': {'link_pts': 'link_gripper_fingertip_right','link_cube': 'base_link', 'detect_as': 'edges'}, + 'link_gripper_s3_body_TO_link_arm_l0': {'link_pts': 'link_gripper_s3_body', 'link_cube': 'link_arm_l0','detect_as': 'edges'}, + 'link_gripper_s3_body_TO_link_arm_l1': {'link_pts': 'link_gripper_s3_body', 'link_cube': 'link_arm_l1','detect_as': 'edges'}, + 'link_gripper_s3_body_TO_link_head_tilt': {'link_cube': 'link_head_tilt', 'link_pts': 'link_gripper_s3_body','detect_as': 'edges'}, + 'link_gripper_finger_left_TO_link_head_tilt': {'link_cube': 'link_head_tilt', 'link_pts': 'link_gripper_finger_left','detect_as': 'edges'}, + 'link_gripper_finger_right_TO_link_head_tilt': {'link_cube': 'link_head_tilt', 'link_pts': 'link_gripper_finger_right','detect_as': 'edges'}, + 'link_gripper_finger_right_link_arm_l0': {'link_pts': 'link_gripper_finger_right', 'link_cube': 'link_arm_l0','detect_as': 'edges'}, + 'link_gripper_finger_left_TO_link_arm_l1': {'link_pts': 'link_gripper_finger_left', 'link_cube': 'link_arm_l1','detect_as': 'edges'},}, + 'joints': {'arm': [{'motion_dir': 'neg', 'collision_pair': 'link_gripper_fingertip_right_TO_base_link'}, {'motion_dir': 'neg', 'collision_pair': 'link_gripper_s3_body_TO_base_link'}, {'motion_dir': 'neg', 'collision_pair': 'link_wrist_pitch_TO_base_link'}], @@ -295,21 +304,43 @@ {'motion_dir': 'neg', 'collision_pair': 'link_gripper_finger_left_TO_base_link'}, {'motion_dir': 'neg', 'collision_pair': 'link_gripper_finger_right_TO_base_link'}, {'motion_dir': 'neg', 'collision_pair': 'link_gripper_fingertip_left_TO_base_link'}, - {'motion_dir': 'neg', 'collision_pair': 'link_gripper_fingertip_right_TO_base_link'}], + {'motion_dir': 'neg', 'collision_pair': 'link_gripper_fingertip_right_TO_base_link'}, + {'motion_dir': 'pos','collision_pair': 'link_gripper_s3_body_TO_link_head_tilt'}, + {'motion_dir': 'pos','collision_pair': 'link_gripper_finger_left_TO_link_head_tilt'}, + {'motion_dir': 'pos','collision_pair': 'link_gripper_finger_right_TO_link_head_tilt'}], 'wrist_pitch': [{'motion_dir': 'neg', 'collision_pair': 'link_gripper_s3_body_TO_base_link'}, {'motion_dir': 'neg', 'collision_pair': 'link_gripper_finger_left_TO_base_link'}, {'motion_dir': 'neg','collision_pair': 'link_gripper_finger_right_TO_base_link'}, {'motion_dir': 'neg','collision_pair': 'link_gripper_fingertip_left_TO_base_link'}, {'motion_dir': 'neg','collision_pair': 'link_gripper_fingertip_right_TO_base_link'}, + {'motion_dir': 'neg','collision_pair': 'link_gripper_s3_body_TO_link_head_tilt'}, + {'motion_dir': 'neg','collision_pair': 'link_gripper_finger_left_TO_link_head_tilt'}, + {'motion_dir': 'neg','collision_pair': 'link_gripper_finger_right_TO_link_head_tilt'}, {'motion_dir': 'pos', 'collision_pair': 'link_gripper_s3_body_TO_link_arm_l0'}, - {'motion_dir': 'pos', 'collision_pair': 'link_gripper_s3_body_TO_link_arm_l1'}], + {'motion_dir': 'pos', 'collision_pair': 'link_gripper_s3_body_TO_link_arm_l1'}, + {'motion_dir': 'pos','collision_pair': 'link_gripper_s3_body_TO_link_head_tilt'}, + {'motion_dir': 'pos','collision_pair': 'link_gripper_finger_left_TO_link_head_tilt'}, + {'motion_dir': 'pos','collision_pair': 'link_gripper_finger_right_TO_link_head_tilt'}, + {'motion_dir': 'pos','collision_pair': 'link_gripper_fingertip_left_TO_link_lift'}], 'wrist_roll': [{'motion_dir': 'neg', 'collision_pair': 'link_gripper_s3_body_TO_link_arm_l1'}, - {'motion_dir': 'pos', 'collision_pair': 'link_gripper_s3_body_TO_link_arm_l0'}], + {'motion_dir': 'neg', 'collision_pair': 'link_gripper_s3_body_TO_link_arm_l0'}, + {'motion_dir': 'pos', 'collision_pair': 'link_gripper_s3_body_TO_link_arm_l0'}, + # {'motion_dir': 'pos', 'collision_pair': 'link_gripper_s3_body_TO_link_arm_l1'}, + {'motion_dir': 'pos', 'collision_pair': 'link_gripper_s3_body_TO_base_link'}, + {'motion_dir': 'neg', 'collision_pair': 'link_gripper_s3_body_TO_base_link'}], 'wrist_yaw': [{'motion_dir': 'pos', 'collision_pair': 'link_gripper_fingertip_left_TO_link_lift'}, {'motion_dir': 'neg', 'collision_pair': 'link_gripper_finger_right_TO_base_link'}, {'motion_dir': 'pos', 'collision_pair': 'link_gripper_finger_left_TO_base_link'}, {'motion_dir': 'neg', 'collision_pair': 'link_gripper_s3_body_TO_link_arm_l0'}, - {'motion_dir': 'pos', 'collision_pair': 'link_gripper_s3_body_TO_link_arm_l1'}]}}, + {'motion_dir': 'pos', 'collision_pair': 'link_gripper_s3_body_TO_link_arm_l1'}, + # {'motion_dir': 'pos','collision_pair': 'link_gripper_s3_body_TO_link_head_tilt'}, + {'motion_dir': 'pos', 'collision_pair': 'link_gripper_s3_body_TO_base_link'}, + {'motion_dir': 'neg', 'collision_pair': 'link_gripper_s3_body_TO_base_link'}, + {'motion_dir': 'pos','collision_pair': 'link_gripper_finger_left_TO_link_head_tilt'}, + {'motion_dir': 'pos','collision_pair': 'link_gripper_finger_right_TO_link_head_tilt'}, + {'motion_dir': 'neg','collision_pair': 'link_gripper_s3_body_TO_link_head_tilt'}, + {'motion_dir': 'pos','collision_pair': 'link_gripper_finger_left_TO_link_head_tilt'}, + {'motion_dir': 'neg','collision_pair': 'link_gripper_finger_right_TO_link_head_tilt'}]}}, 'devices': { 'wrist_pitch': { @@ -377,6 +408,90 @@ } }} + +SE3_eoa_wrist_dw3_tool_tablet_12in={ + 'py_class_name': 'EOA_Wrist_DW3_Tool_Tablet_12in', + 'py_module_name': 'stretch_body.end_of_arm_tools', + 'use_group_sync_read': 1, + 'retry_on_comm_failure': 1, + 'portrait_orientation': 0, + 'baud': 115200, + 'dxl_latency_timer': 64, + 'wrist': 'eoaw_dw3', + 'tool': 'eoat_nil', + 'stow': { + 'arm': 0.0, + 'lift': 0.3, + 'wrist_pitch': 0.0, + 'wrist_roll': 0.0, + 'wrist_yaw': 1.57 + }, + 'collision_mgmt': { + 'k_brake_distance': {'wrist_pitch': 0.5, 'wrist_yaw':1.5, 'wrist_roll': 0.25}, + 'collision_pairs': { + 'link_DW3_tablet_12in_TO_base_link': {'link_cube': 'base_link', 'link_pts': 'link_DW3_tablet_12in', 'detect_as': 'edges'}, + 'link_DW3_tablet_12in_TO_link_arm_l0': {'link_cube': 'link_arm_l0', 'link_pts': 'link_DW3_tablet_12in','detect_as': 'edges'}, + 'link_DW3_tablet_12in_TO_link_arm_l1': {'link_cube': 'link_arm_l1', 'link_pts': 'link_DW3_tablet_12in','detect_as': 'edges'}, + 'link_DW3_tablet_12in_TO_link_head_tilt': {'link_cube': 'link_head_tilt', 'link_pts': 'link_DW3_tablet_12in','detect_as': 'edges', 'cube_scale': 1.2}, + 'link_wrist_pitch_TO_base_link': {'link_pts': 'link_wrist_pitch', 'link_cube': 'base_link','detect_as': 'edges'}, + 'link_wrist_yaw_bottom_TO_base_link': {'link_pts': 'link_wrist_yaw_bottom', 'link_cube': 'base_link','detect_as': 'edges'}, + 'link_DW3_tablet_12in_TO_link_lift': {'link_pts': 'link_DW3_tablet_12in', 'link_cube': 'link_lift','detect_as': 'edges'},}, + + 'joints': {'arm': [{'motion_dir': 'neg', 'collision_pair': 'link_DW3_tablet_12in_TO_base_link'}, + {'motion_dir': 'neg', 'collision_pair': 'link_wrist_pitch_TO_base_link'}, + {'motion_dir': 'neg', 'collision_pair': 'link_DW3_tablet_12in_TO_link_head_tilt'}], + + 'lift': [{'motion_dir': 'neg', 'collision_pair': 'link_DW3_tablet_12in_TO_base_link'}, + {'motion_dir': 'neg', 'collision_pair': 'link_wrist_pitch_TO_base_link'}, + {'motion_dir': 'neg', 'collision_pair': 'link_wrist_yaw_bottom_TO_base_link'}, + {'motion_dir': 'pos', 'collision_pair': 'link_DW3_tablet_12in_TO_link_head_tilt'}], + + 'wrist_pitch': [{'motion_dir': 'neg', 'collision_pair': 'link_DW3_tablet_12in_TO_base_link'}, + # {'motion_dir': 'pos', 'collision_pair': 'link_DW3_tablet_12in_TO_base_link'}, + # {'motion_dir': 'pos', 'collision_pair': 'link_DW3_tablet_12in_TO_link_arm_l0'}, + {'motion_dir': 'pos', 'collision_pair': 'link_DW3_tablet_12in_TO_link_arm_l1'}, + # {'motion_dir': 'neg', 'collision_pair': 'link_DW3_tablet_12in_TO_link_head_tilt'}, + {'motion_dir': 'pos', 'collision_pair': 'link_DW3_tablet_12in_TO_link_head_tilt'} + ], + + 'wrist_roll': [{'motion_dir': 'neg', 'collision_pair': 'link_DW3_tablet_12in_TO_link_arm_l1'}, + {'motion_dir': 'pos', 'collision_pair': 'link_DW3_tablet_12in_TO_link_arm_l0'}], + + 'wrist_yaw': [ + {'motion_dir': 'neg', 'collision_pair': 'link_DW3_tablet_12in_TO_link_arm_l0'}, + {'motion_dir': 'neg', 'collision_pair': 'link_DW3_tablet_12in_TO_link_arm_l1'}, + # {'motion_dir': 'neg', 'collision_pair': 'link_DW3_tablet_12in_TO_link_arm_l0'}, + {'motion_dir': 'pos', 'collision_pair': 'link_DW3_tablet_12in_TO_link_lift'}, + {'motion_dir': 'pos', 'collision_pair': 'link_DW3_tablet_12in_TO_link_arm_l1'}, + {'motion_dir': 'neg', 'collision_pair': 'link_DW3_tablet_12in_TO_base_link'}, + {'motion_dir': 'pos', 'collision_pair': 'link_DW3_tablet_12in_TO_base_link'}, + {'motion_dir': 'pos', 'collision_pair': 'link_DW3_tablet_12in_TO_link_head_tilt'}, + # {'motion_dir': 'neg', 'collision_pair': 'link_DW3_tablet_12in_TO_link_head_tilt'}, + ], + + 'wrist_roll': [{'motion_dir': 'neg', 'collision_pair': 'link_DW3_tablet_12in_TO_link_arm_l0'}, + {'motion_dir': 'pos', 'collision_pair': 'link_DW3_tablet_12in_TO_link_arm_l1'}] + } + }, + + 'devices': { + 'wrist_pitch': { + 'py_class_name': 'WristPitch', + 'py_module_name': 'stretch_body.wrist_pitch', + 'device_params': 'SE3_wrist_pitch_DW3' + }, + 'wrist_roll': { + 'py_class_name': 'WristRoll', + 'py_module_name': 'stretch_body.wrist_roll', + 'device_params': 'SE3_wrist_roll_DW3_tablet' + }, + 'wrist_yaw': { + 'py_class_name': 'WristYaw', + 'py_module_name': 'stretch_body.wrist_yaw', + 'device_params': 'SE3_wrist_yaw_DW3' + } + }} + # ###################################33 # Baseline Nominal Params nominal_params={ @@ -384,9 +499,10 @@ #Each EOA will get expanded at runtime into its full parameter dictionary # Eg, supported_eoa.tool_none --> adds the wrist_yaw param dict to nominal_params # Add all formally supported EOA to this list - 'supported_eoa': ['eoa_wrist_dw3_tool_nil','eoa_wrist_dw3_tool_sg3'], + 'supported_eoa': ['eoa_wrist_dw3_tool_nil','eoa_wrist_dw3_tool_sg3', 'eoa_wrist_dw3_tool_tablet_12in'], 'eoa_wrist_dw3_tool_nil': SE3_eoa_wrist_dw3_tool_nil, 'eoa_wrist_dw3_tool_sg3': SE3_eoa_wrist_dw3_tool_sg3, + 'eoa_wrist_dw3_tool_tablet_12in': SE3_eoa_wrist_dw3_tool_tablet_12in, 'arm':{ 'usb_name': '/dev/hello-motor-arm', 'use_vel_traj': 1, @@ -812,7 +928,7 @@ 'robot_collision_mgmt': { 'max_mesh_points': 48, 'SE3': { - 'k_brake_distance': {'lift': 0.75, 'arm': 0.125, 'wrist_yaw': 0.125, 'head_pan': 0.125, 'head_tilt': 0.125}, + 'k_brake_distance': {'lift': 1.75, 'arm': 1.125, 'wrist_yaw': 0.125, 'head_pan': 0.125, 'head_tilt': 0.125}, 'collision_pairs':{'link_head_tilt_TO_link_arm_l4':{'link_pts': 'link_head_tilt', 'link_cube': 'link_arm_l4','detect_as':'pts'}, 'link_arm_l0_TO_base_link':{'link_pts': 'link_arm_l0', 'link_cube': 'base_link','detect_as':'pts'}},