Feature/tablet tool

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 {'pos'} for {self.name}. Not executing move_to")
+            return
+
+        if self.forced_collision_stop_override['neg'] 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']<x_des:
             self.logger.warning('move_to in collision. Motion disabled in direction %s for %s. Not executing move_to'%('pos',self.name))

body/stretch_body/end_of_arm.py

@@ -62,6 +62,15 @@ def move_by(self, joint, x_r, v_r=None, a_r=None):
         """
         with self.pt_lock:
             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)
+        """
+        with self.pt_lock:
+            self.motors[joint].set_velocity(v_r, a_r)
 
     def pose(self,joint, p,v_r=None, a_r=None):
         """
@@ -76,6 +85,9 @@ def pose(self,joint, p,v_r=None, a_r=None):
     def stow(self):
         pass #Override by specific tool
 
+    def pre_stow(self,robot=None):
+        pass #Override by specific tool
+
     def home(self, joint=None):
         """
         Home to hardstops
@@ -98,7 +110,7 @@ def is_tool_present(self,class_name):
                 return True
         return False
 
-    def get_joint_configuration(self,braked=False):
+    def get_joint_configuration(self,brake_joints={}):
         """
         Construct a dictionary of tools current pose (for robot_collision_mgmt)
         Keys match joint names in URDF
@@ -109,14 +121,16 @@ def get_joint_configuration(self,braked=False):
             jn = self.urdf_map[j]
             motor = self.get_joint(jn)
             dx = 0.0
-            if braked:
-                try:
+            try:
+                if brake_joints[j]:
                     dx = self.params['collision_mgmt']['k_brake_distance'][jn] * motor.get_braking_distance()
-                except KeyError:
-                    dx=0
+            except KeyError:
+                dx=0
             ret[j] = motor.status['pos'] + dx
         return ret
-
+
+    def pre_stow(self,robot=None):
+        pass
 
 
 

body/stretch_body/end_of_arm_tools.py

@@ -1,4 +1,6 @@
 from stretch_body.end_of_arm import *
+import threading
+import time
 
 # ############# STRETCH RE1 / RE2 #######################
 class ToolNone(EndOfArm):
@@ -73,6 +75,10 @@ def home(self):
         self.motors['wrist_roll'].move_to(0)
         self.motors['wrist_yaw'].home()
 
+    def pre_stow(self,robot=None):
+        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'])
+
 # ##########################################################3#
 class EOA_Wrist_DW3_Tool_NIL(EndOfArm):
     """
@@ -99,6 +105,11 @@ def home(self):
         self.motors['wrist_roll'].move_to(self.params['stow']['wrist_roll'])
         self.motors['wrist_yaw'].home()
 
+    def pre_stow(self,robot=None):
+        if robot:
+            robot.end_of_arm.move_to('wrist_pitch', robot.end_of_arm.params['stow']['wrist_pitch'])
+        else:
+            self.end_of_arm.move_to('wrist_pitch', self.end_of_arm.params['stow']['wrist_pitch'])
 
 class EOA_Wrist_DW3_Tool_SG3(EndOfArm):
     """
@@ -122,8 +133,119 @@ def stow(self):
         self.move_to('wrist_roll', self.params['stow']['wrist_roll'])
         self.move_to('wrist_yaw', self.params['stow']['wrist_yaw'])
         self.move_to('stretch_gripper', self.params['stow']['stretch_gripper'])
+
     def home(self):
         self.motors['wrist_pitch'].move_to(self.params['stow']['wrist_pitch'])
         self.motors['wrist_roll'].move_to(self.params['stow']['wrist_roll'])
         self.motors['wrist_yaw'].home()
         self.motors['stretch_gripper'].home()
+
+    def pre_stow(self,robot=None):
+        if robot:
+            robot.end_of_arm.move_to('wrist_pitch', robot.end_of_arm.params['stow']['wrist_pitch'])
+        else:
+            self.end_of_arm.move_to('wrist_pitch', self.end_of_arm.params['stow']['wrist_pitch'])
+
+class EOA_Wrist_DW3_Tool_Tablet_12in(EndOfArm):
+    """
+    Wrist Yaw / Pitch / Roll + 12in Tablet
+    """
+    def __init__(self, name='eoa_wrist_dw3_tool_tablet_12in'):
+        EndOfArm.__init__(self, name)
+        self.portrait_orientation = self.params['portrait_orientation']
+        #This maps from the name of a joint in the URDF to the name of the joint in Stretch Body
+        #It is used by CollisionMgmt
+        self.urdf_map={
+            'joint_wrist_yaw':'wrist_yaw',
+            'joint_wrist_pitch': 'wrist_pitch',
+            'joint_wrist_roll':'wrist_roll' #Not mapping fingers for collision mgmt yet
+        }
+
+    def move_by(self, joint, x_r, v_r=None, a_r=None, enable_wrist_roll = False):
+        # Lock wrist roll by default
+        if joint=='wrist_roll':
+            if enable_wrist_roll:
+                return EndOfArm.move_by(self, joint, x_r, v_r, a_r)
+            else:
+                return None
+        return EndOfArm.move_by(self, joint, x_r, v_r, a_r)
+
+    def move_to(self, joint, x_r, v_r=None, a_r=None, enable_wrist_roll = False):
+        # Lock wrist roll by default
+        if joint=='wrist_roll':
+            if enable_wrist_roll:
+                return EndOfArm.move_by(self, joint, x_r, v_r, a_r)
+            else:
+                return None
+        return EndOfArm.move_to(self, joint, x_r, v_r, a_r)
+
+    def set_velocity(self, joint, v_r, a_r=None, enable_wrist_roll = False):
+        # Lock wrist roll by default
+        if joint=='wrist_roll':
+            if enable_wrist_roll:
+                return EndOfArm.set_velocity(self, joint, v_r, a_r)
+            else:
+                return None
+        return EndOfArm.set_velocity(self, joint, v_r, a_r)
+
+    def pre_stow(self,robot=None):
+        if robot:
+            if robot.lift.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}

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
 

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)<self.dead_zone:
             x = 0
         acc = self.acc
@@ -452,7 +448,10 @@ def command_stick_to_motion(self, x, robot):
         v = self._process_stick_to_vel(x)
         if self.precision_mode:
             v = v*self.precision_scale_down
-        motor.set_velocity(v, acc)
+        if 'wrist' in self.name:
+            robot.end_of_arm.set_velocity(self.name,v, acc)
+        if 'head' in self.name:
+            robot.head.set_velocity(self.name,v, acc)
         self._prev_set_vel_ts = time.time()
 
     def command_button_to_motion(self,direction, robot):
@@ -462,17 +461,19 @@ def command_button_to_motion(self,direction, robot):
             direction (int): Direction integer -1 or +1
             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)
         vel = self.max_vel
         if self.precision_mode:
             vel = vel*self.precision_scale_down
         if direction==1:
-            motor.set_velocity(vel, self.acc)
+            if 'head' in self.name:
+                robot.head.set_velocity(self.name, vel, self.acc)
+            if 'wrist' in self.name:
+                robot.end_of_arm.set_velocity(self.name, vel, self.acc)
         elif direction==-1:
-            motor.set_velocity(-1*vel, self.acc)
+            if 'head' in self.name:
+                robot.head.set_velocity(self.name, -1*vel, self.acc)
+            if 'wrist' in self.name:
+                robot.end_of_arm.set_velocity(self.name, -1*vel, self.acc)
         self._prev_set_vel_ts = time.time()
 
     def stop_motion(self, robot):
@@ -483,11 +484,9 @@ def stop_motion(self, robot):
             robot (robot.Robot): Valid robot instance
         """
         if 'wrist' in self.name:
-            motor = robot.end_of_arm.get_joint(self.name)
+            robot.end_of_arm.set_velocity(self.name,0,self.params['motion']['max']['accel'])
         if 'head' in self.name:
-            motor = robot.head.get_joint(self.name)
-
-        motor.set_velocity(0,self.params['motion']['max']['accel'])
+            robot.head.set_velocity(self.name,0,self.params['motion']['max']['accel'])
 
     def _process_stick_to_vel(self, x):
         x = -1*x