Deployed 2109ee9 to 0.3 with MkDocs 1.6.1 and mike 2.1.3

Author: github-actions[bot]

0.3/search/search_index.json

@@ -129,10 +129,10 @@ def is_trajectory_active(self):
                 return True
         return False
 
-    def follow_trajectory(self, v_r=None, a_r=None, req_calibration=False, move_to_start_point=True):
+    def follow_trajectory(self, v_r=None, a_r=None, move_to_start_point=True):
         success = True
         for motor in self.motors:
-            success = success and self.motors[motor].follow_trajectory(v_r, a_r, req_calibration, move_to_start_point)
+            success = success and self.motors[motor].follow_trajectory(v_r, a_r, move_to_start_point)
         return success
 
     def update_trajectory(self):

0.3/sitemap.xml

@@ -914,7 +914,7 @@ def get_trajectory_time_remaining(self):
         else:
             return max(0,self.trajectory.waypoints[-1].time - self.get_trajectory_ts())
 
-    def follow_trajectory(self, v_r=None, a_r=None, req_calibration=True, move_to_start_point=True):
+    def follow_trajectory(self, v_r=None, a_r=None, move_to_start_point=True):
         """Starts executing a waypoint trajectory
 
         `self.trajectory` must be populated with a valid trajectory before calling
@@ -926,8 +926,6 @@ def follow_trajectory(self, v_r=None, a_r=None, req_calibration=True, move_to_st
             velocity limit for trajectory in radians per second
         a_r : float
             acceleration limit for trajectory in radians per second squared
-        req_calibration : bool
-            whether to allow motion prior to homing
         move_to_start_point : bool
             whether to move to the trajectory's start to avoid a jump, this
             time to move doesn't count against the trajectory's timeline
@@ -936,10 +934,9 @@ def follow_trajectory(self, v_r=None, a_r=None, req_calibration=True, move_to_st
         if not self.hw_valid:
             self.logger.warning('Dynamixel connection to hardware not valid')
             return False
-        if req_calibration:
-            if not self.is_calibrated:
-                self.logger.warning('Dynamixel not homed')
-                return False
+        if self.params['req_calibration'] and not self.is_calibrated:
+            self.logger.warning('Dynamixel not homed')
+            return False
         if self._waypoint_ts is not None:
             self.logger.error('Dynamixel waypoint trajectory already active')
             return False
@@ -973,6 +970,10 @@ def follow_trajectory(self, v_r=None, a_r=None, req_calibration=True, move_to_st
             if not self.wait_until_at_setpoint():
                 self.logger.warning('Dynamixel unable to reach starting point')
                 return False
+        else:
+            if not np.isclose(self.trajectory[0].position, self.status['pos'], atol=self.params['distance_tol']):
+                self.logger.warning("Can't start joint traj: first waypoint doesn't match current position")
+                return False
 
         # start trajectory
         self._waypoint_ts = time.time()

0.3/sitemap.xml.gz

@@ -5,6 +5,7 @@
 import stretch_body.hello_utils as hu
 import time
 import sys
+import numpy as np
 
 
 class PrismaticJoint(Device):
@@ -480,8 +481,10 @@ def follow_trajectory(self, v_m=None, a_m=None, stiffness=None, contact_thresh_p
             if prev_sync_mode:
                 self.motor.enable_sync_mode()
                 self.push_command()
-            #print('WAIT')
-            #time.sleep(2.0)
+        else:
+            if not np.isclose(self.trajectory[0].position, self.status['pos'], atol=self.params['distance_tol']):
+                self.logger.warning("Can't start joint traj: first waypoint doesn't match current position")
+                return False
 
         self.traj_guarded_event=self.motor.status['guarded_event']
         # start trajectory

0.3/stretch-body/body/stretch_body/dynamixel_X_chain.py

@@ -490,10 +490,10 @@ def is_trajectory_active(self):
                self.head.is_trajectory_active()
 
 
-    def follow_trajectory(self):
+    def follow_trajectory(self, move_to_start_point=False):
         success = True
-        success = success and self.arm.follow_trajectory(move_to_start_point=False)
-        success = success and self.lift.follow_trajectory(move_to_start_point=False)
+        success = success and self.arm.follow_trajectory(move_to_start_point=move_to_start_point)
+        success = success and self.lift.follow_trajectory(move_to_start_point=move_to_start_point)
         success = success and self.base.follow_trajectory()
 
         # Check if need to do a motor sync by looking at if there's been a pimu sync signal sent
@@ -506,8 +506,17 @@ def follow_trajectory(self):
         if self.pimu.ts_last_motor_sync is None or sync_required:
             self.pimu.trigger_motor_sync()
 
-        success = success and self.end_of_arm.follow_trajectory(move_to_start_point=False)
-        success = success and self.head.follow_trajectory(move_to_start_point=False)
+        success = success and self.end_of_arm.follow_trajectory(move_to_start_point=move_to_start_point)
+        success = success and self.head.follow_trajectory(move_to_start_point=move_to_start_point)
+
+        # TODO: HACK! We need a way to queue waypoint trajectories to
+        # joints before starting execution. Currently, each joint's
+        # `follow_trajectory` does both. If arm's traj is valid and
+        # starts executing, but lift's traj isn't, the arm doesn't
+        # know that the lift didn't start and keeps going.
+        if not success:
+            self.stop_trajectory()
+
         return success
 
     def stop_trajectory(self):

0.3/stretch-body/body/stretch_body/dynamixel_hello_XL430.py

@@ -57,8 +57,13 @@ class RobotParams:
     4. stretch_configuration_params.yaml                | Robot specific data (eg, serial numbers and calibrations). Calibration tools may update these.
     5. stretch_user_params.yaml                         | User specific data (eg, contact thresholds, controller tunings, etc)
     """
-    user_params_fn = hello_utils.get_fleet_directory()+'stretch_user_params.yaml'
-    config_params_fn = hello_utils.get_fleet_directory()+'stretch_configuration_params.yaml'
+    calibration_dir = hello_utils.get_fleet_directory()
+    if not exists(calibration_dir):
+        _valid_params=False
+        print('Calibration directory missing. Please contact Hello Robot Support')
+        sys.exit(1)
+    user_params_fn = calibration_dir+'stretch_user_params.yaml'
+    config_params_fn = calibration_dir+'stretch_configuration_params.yaml'
     if not hello_utils.check_file_exists(user_params_fn) or not hello_utils.check_file_exists(config_params_fn):
         _valid_params=False
         print('Please run tool RE1_migrate_params.py or verify if Stretch configuration YAML files are present before continuing.\nFor more details, see https://forum.hello-robot.com/t/425')

0.3/stretch-body/body/stretch_body/prismatic_joint.py

@@ -131,7 +131,8 @@
         'retry_on_comm_failure': 1,
         'baud': 115200,
         'enable_runstop': 1,
-        'disable_torque_on_stop': 1}
+        'disable_torque_on_stop': 1,
+        'distance_tol': 0.015}
 
 RE1V0_wrist_yaw={
     'device_info': "Standard wrist yaw shipped with original RE1",
@@ -177,7 +178,8 @@
     'baud': 115200,
     'enable_runstop': 1,
     'disable_torque_on_stop': 1,
-    'range_pad_t': [100.0, -100.0]}
+    'range_pad_t': [100.0, -100.0],
+    'distance_tol': 0.015}
 
 #Brought in from tool_share for DexWrist "2"
 RE1V0_wrist_pitch_DW2={
@@ -218,7 +220,8 @@
         'disable_torque_on_stop': 0,
         'float_on_stop': 1,
         'current_float_A': -0.13,
-        'current_limit_A': 2.5
+        'current_limit_A': 2.5,
+        'distance_tol': 0.03
     }
 
 RE1V0_wrist_roll_DW2={
@@ -259,7 +262,8 @@
         'disable_torque_on_stop': 0,
         'float_on_stop': 1,
         'current_float_A': 0.04,
-        'current_limit_A': 1.0
+        'current_limit_A': 1.0,
+        'distance_tol': 0.015
     }
 
 # ######### EndOfArm Defn ##############
@@ -397,7 +401,8 @@
                 'vel_m': 0.3,
                 'accel_m': 0.5},
                 'vel_brakezone_factor': 0.03},
-        'set_safe_velocity': 1},
+        'set_safe_velocity': 1,
+        'distance_tol': 0.008},
     'base':{
         'usb_name_left_wheel': '/dev/hello-motor-left-wheel',
         'usb_name_right_wheel': '/dev/hello-motor-right-wheel',
@@ -496,7 +501,8 @@
         'stall_max_effort': 20.0,
         'stall_backoff': 0.017,
         'stall_max_time': 1.0,
-        'stall_min_vel': 0.1},
+        'stall_min_vel': 0.1,
+        'distance_tol': 0.15},
     'head_tilt':{
         'range_pad_t': [50.0, -50.0],
         'flip_encoder_polarity': 1,
@@ -542,7 +548,8 @@
         'stall_backoff': 0.017,
         'stall_max_effort': 20.0,
         'stall_max_time': 1.0,
-        'stall_min_vel': 0.1},
+        'stall_min_vel': 0.1,
+        'distance_tol': 0.52},
     'hello-motor-arm':{
         'gains':{
             'effort_LPF': 10.0,
@@ -719,7 +726,8 @@
                 'accel_m': 0.3},
             'vel_brakezone_factor': 0.02},
         'set_safe_velocity': 1,
-        'pinion_t': 12},
+        'pinion_t': 12,
+        'distance_tol': 0.015},
     'pimu':{
       'usb_name': '/dev/hello-pimu',
       'base_fan_off': 70,

0.3/stretch-body/body/stretch_body/robot.py

@@ -127,7 +127,8 @@
         'enable_runstop': 1,
         'disable_torque_on_stop': 1,
         'range_t': [0, 8022],
-        'zero_t': 5212}
+        'zero_t': 5212,
+        'distance_tol': 0.015}
 
 RE2V0_wrist_yaw={
     'device_info': "Standard wrist yaw shipped with original RE2",
@@ -175,7 +176,8 @@
     'disable_torque_on_stop': 1,
     'range_pad_t': [100.0, -100.0],
     'range_t': [0,9340],
-    'zero_t': 7005}
+    'zero_t': 7005,
+    'distance_tol': 0.015}
 
 #Brought in from tool_share for DexWrist "2"
 RE2V0_wrist_pitch_DW2={
@@ -216,7 +218,8 @@
         'disable_torque_on_stop': 0,
         'float_on_stop': 1,
         'current_float_A': -0.13,
-        'current_limit_A': 2.5
+        'current_limit_A': 2.5,
+        'distance_tol': 0.03
     }
 
 RE2V0_wrist_roll_DW2={
@@ -257,7 +260,8 @@
         'disable_torque_on_stop': 0,
         'float_on_stop': 1,
         'current_float_A': 0.04,
-        'current_limit_A': 1.0
+        'current_limit_A': 1.0,
+        'distance_tol': 0.015
     }
 
 # ######### EndOfArm Defn ##############
@@ -397,7 +401,8 @@
                 'vel_m': 0.4,
                 'accel_m': 0.4},
         'vel_brakezone_factor': 0.03},
-        'set_safe_velocity': 1},
+        'set_safe_velocity': 1,
+        'distance_tol': 0.008},
     'base':{
         'usb_name_left_wheel': '/dev/hello-motor-left-wheel',
         'usb_name_right_wheel': '/dev/hello-motor-right-wheel',
@@ -498,7 +503,8 @@
         'stall_min_vel': 0.1,
         'range_pad_t':[50.0,-50.0],
         'range_t': [0, 3827],
-        'zero_t': 1250},
+        'zero_t': 1250,
+        'distance_tol': 0.15},
     'head_tilt':{
         'flip_encoder_polarity': 1,
         'gr': 1.0,
@@ -546,7 +552,8 @@
         'stall_min_vel': 0.1,
         'range_pad_t': [50.0, -50.0],
         'range_t': [1775,3150],
-        'zero_t': 2048},
+        'zero_t': 2048,
+        'distance_tol': 0.52},
     'hello-motor-arm':{
         'gains':{
             'effort_LPF': 10.0,
@@ -723,7 +730,8 @@
               'vel_m': 0.15},
         'vel_brakezone_factor': 0.01},
         'set_safe_velocity': 1,
-          'pinion_t': 12},
+        'pinion_t': 12,
+        'distance_tol': 0.015},
     'pimu':{
       'usb_name': '/dev/hello-pimu',
       'base_fan_off': 70,