Merge pull request #102 from hello-robot/develop

Release candidate v0.1.10

.github/workflows/pr_clone_run_test.sh

@@ -0,0 +1,17 @@
+cd repos
+git clone $3 stretch_body_PR_testing
+cd stretch_body_PR_testing
+git fetch origin pull/$1/head:$2
+git checkout $2
+cd body
+declare -a TestFiles=("test_arm" "test_dxl_comms" "test_end_of_arm" "test_hello_utils" "test_robot_params" "test_timing_stats" "test_base" "test_dynamixel_hello_XL430" "test_end_of_arm_tools" "test_lift" "test_robot" "test_timing_stats" "test_arm" "test_device" "test_dynamixel_XL430" "test_hello_utils" "test_pimu" "test_steppers" )
+declare -a TestFiles2=("test_hello_utils")
+declare -a TestFiles3=("test_robot_params")
+# Iterate the string array using for loop
+for fileName in ${TestFiles2[@]}; do
+   python -m unittest test.$fileName
+done
+cd ../..
+rm -rf stretch_body_PR_testing
+exit
+~^Z

.github/workflows/stretch_ssh_pr_checkout.yml

@@ -0,0 +1,35 @@
+name: Checkout new PR
+on:
+  pull_request_target:
+jobs:
+  Checkout_PR_and_run_tests:
+    runs-on: ubuntu-18.04
+    steps:
+      - name: Repository checkout
+        uses: actions/checkout@v2
+
+      - name: save the ssh key for access to robot to the file
+        run: 'echo "$SSH_KEY" > the_key'
+        shell: bash
+        env:
+          SSH_KEY: ${{ secrets.STRETCH_SSH_KEY_GITHUB_ACTION }}
+      - run: chmod 400 the_key
+
+      - name: Print out PR reference values
+        run: |
+          echo github.base_ref: ${{ github.base_ref }}
+          echo github.head_ref: ${{ github.head_ref }}
+          echo github.ref: ${{ github.ref }}
+
+      - name: Replace PR number, Branch name and Cloning URL
+        run: sed -e "s/\$1/${PR_NUMBER}/g" -e "s|\$2|${BRANCH_NAME}|g" -e "s|\$3|${CLONE_URL}|g" .github/workflows/pr_checkout_commands.sh > ssh_commands_pr_info.sh; cat ssh_commands_pr_info.sh
+        env:
+          PR_NUMBER: ${{ github.event.number }}
+          BRANCH_NAME: ${{ github.head_ref }}
+          CLONE_URL: ${{ github.event.repository.clone_url }}
+
+      - name: Log into stretch using ssh and run tests
+        run: ssh -i the_key hello-robot@"${STRETCH_IP}" -p "$STRETCH_PORT" -tt -o StrictHostKeyChecking=no < ./ssh_commands_pr_info.sh
+        env:
+          STRETCH_IP: 72.134.86.243
+          STRETCH_PORT: 43771

body/stretch_body/arm.py

@@ -27,15 +27,20 @@ def __init__(self):
     # ###########  Device Methods #############
 
     def startup(self):
-        return self.motor.startup()
+        success= self.motor.startup()
+        self.__update_status()
+        return success
 
     def stop(self):
         self.motor.stop()
 
     def pull_status(self):
         self.motor.pull_status()
-        self.status['timestamp_pc']=time.time()
-        self.status['pos']= self.motor_rad_to_translate(self.status['motor']['pos'])
+        self.__update_status()
+
+    def __update_status(self):
+        self.status['timestamp_pc'] = time.time()
+        self.status['pos'] = self.motor_rad_to_translate(self.status['motor']['pos'])
         self.status['vel'] = self.motor_rad_to_translate(self.status['motor']['vel'])
         self.status['force'] = self.motor_current_to_translate_force(self.status['motor']['current'])
 

body/stretch_body/base.py

@@ -29,8 +29,9 @@ def __init__(self):
     # ###########  Device Methods #############
 
     def startup(self):
-        return self.left_wheel.startup() and self.right_wheel.startup()
-
+        success=self.left_wheel.startup() and self.right_wheel.startup()
+        self.__update_status()
+        return success
 
     def stop(self):
         self.left_wheel.stop()
@@ -288,6 +289,9 @@ def pull_status(self):
         """
         self.left_wheel.pull_status()
         self.right_wheel.pull_status()
+        self.__update_status()
+
+    def __update_status(self):
         self.status['timestamp_pc'] = time.time()
 
         p0 = self.status['left_wheel']['pos']

body/stretch_body/dynamixel_X_chain.py

@@ -75,7 +75,8 @@ def startup(self):
                                 self.logger.error('Dynamixel X sync read initialization failed.')
                                 raise DynamixelCommError
                 for mk in self.motors.keys():
-                    self.motors[mk].startup()
+                    if not self.motors[mk].startup():
+                        raise DynamixelCommError
                     self.status[mk] = self.motors[mk].status
                 self.pull_status()
             except DynamixelCommError:

body/stretch_body/dynamixel_hello_XL430.py

@@ -129,31 +129,37 @@ def do_ping(self, verbose=False):
         return self.motor.do_ping(verbose)
 
     def startup(self):
-        if self.motor.do_ping(verbose=False):
-            self.hw_valid = True
-            self.motor.disable_torque()
-            if self.params['use_multiturn']:
-                self.motor.enable_multiturn()
+        try:
+            if self.motor.do_ping(verbose=False):
+                self.hw_valid = True
+                self.motor.disable_torque()
+                if self.params['use_multiturn']:
+                    self.motor.enable_multiturn()
+                else:
+                    self.motor.enable_pos()
+                    if self.params['range_t'][0]<0 or self.params['range_t'][1]>4095:
+                        self.logger.warning('Warning: Invalid position range for %s'%self.name)
+                self.motor.set_pwm_limit(self.params['pwm_limit'])
+                self.motor.set_temperature_limit(self.params['temperature_limit'])
+                self.motor.set_min_voltage_limit(self.params['min_voltage_limit'])
+                self.motor.set_max_voltage_limit(self.params['max_voltage_limit'])
+                self.motor.set_P_gain(self.params['pid'][0])
+                self.motor.set_I_gain(self.params['pid'][1])
+                self.motor.set_D_gain(self.params['pid'][2])
+                self.motor.set_return_delay_time(self.params['return_delay_time'])
+                self.motor.set_profile_velocity(self.rad_per_sec_to_ticks(self.params['motion']['default']['vel']))
+                self.motor.set_profile_acceleration(self.rad_per_sec_sec_to_ticks(self.params['motion']['default']['accel']))
+                self.v_des=self.params['motion']['default']['vel']
+                self.a_des=self.params['motion']['default']['accel']
+                self.is_calibrated=self.motor.is_calibrated()
+                self.enable_torque()
+                self.pull_status()
+                return True
             else:
-                self.motor.enable_pos()
-                if self.params['range_t'][0]<0 or self.params['range_t'][1]>4095:
-                    self.logger.warning('Warning: Invalid position range for %s'%self.name)
-            self.motor.set_pwm_limit(self.params['pwm_limit'])
-            self.motor.set_temperature_limit(self.params['temperature_limit'])
-            self.motor.set_min_voltage_limit(self.params['min_voltage_limit'])
-            self.motor.set_max_voltage_limit(self.params['max_voltage_limit'])
-            self.motor.set_P_gain(self.params['pid'][0])
-            self.motor.set_I_gain(self.params['pid'][1])
-            self.motor.set_D_gain(self.params['pid'][2])
-            self.motor.set_return_delay_time(self.params['return_delay_time'])
-            self.motor.set_profile_velocity(self.rad_per_sec_to_ticks(self.params['motion']['default']['vel']))
-            self.motor.set_profile_acceleration(self.rad_per_sec_sec_to_ticks(self.params['motion']['default']['accel']))
-            self.v_des=self.params['motion']['default']['vel']
-            self.a_des=self.params['motion']['default']['accel']
-            self.is_calibrated=self.motor.is_calibrated()
-            self.enable_torque()
-            return True
-        else:
+                self.logger.warning('DynamixelHelloXL430 Ping failed... %s' % self.name)
+                print('DynamixelHelloXL430 Ping failed...', self.name)
+                return False
+        except DynamixelCommError:
             self.logger.warning('DynamixelHelloXL430 Ping failed... %s' % self.name)
             print('DynamixelHelloXL430 Ping failed...', self.name)
             return False

body/stretch_body/hello_utils.py

@@ -4,7 +4,8 @@
 import os
 import time
 import logging
-import numpy
+import numpy as np
+
 
 def print_stretch_re_use():
     print("For use with S T R E T C H (TM) RESEARCH EDITION from Hello Robot Inc.\n")
@@ -117,79 +118,115 @@ def pretty_print_dict(title, d):
 
 
 class LoopStats():
+    """Track timing statistics for control loops
     """
-    Track timing statistics for control loops
-    """
-    def __init__(self,loop_name,target_loop_rate):
-        self.loop_name=loop_name
-        self.target_loop_rate=target_loop_rate
-        self.ts_loop_start=None
-        self.ts_loop_end=None
-        self.status={'loop_rate_hz':0, 'loop_rate_avg_hz':0, 'loop_rate_min_hz':10000000, 'loop_rate_max_hz':0,'loop_rate_std':0, 'execution_time_ms':0, 'loop_warns':0}
-        self.logger = logging.getLogger()
-        self.n_log=100
-        self.log_idx=0
-        self.rate_log=None
-        self.log_rate_hz=1.0
-        self.warned_yet=False
-        self.sleep_time_s =0.001
-        self.loop_cycles=0
+
+    def __init__(self, loop_name, target_loop_rate):
+        self.loop_name = loop_name
+        self.target_loop_rate = target_loop_rate
+        self.ts_loop_start = None
+        self.ts_loop_end = None
+        self.last_ts_loop_end = None
+        self.status = {'execution_time_s': 0,
+                       'curr_rate_hz': 0,
+                       'avg_rate_hz': 0,
+                       'supportable_rate_hz': 0,
+                       'min_rate_hz': float('inf'),
+                       'max_rate_hz': 0,
+                       'std_rate_hz': 0,
+                       'missed_loops': 0,
+                       'num_loops': 0}
+        self.logger = logging.getLogger(self.loop_name)
+        self.curr_rate_history = []
+        self.supportable_rate_history = []
+        self.n_history = 100
+        self.debug_freq = 50
+        self.sleep_time_s = 0.0
 
     def pretty_print(self):
-        print('--------- TimingStats %s -----------'%self.loop_name)
-        print('Target rate: %f'%self.target_loop_rate)
-        print('Current rate (Hz): %f' % self.status['loop_rate_hz'])
-        print('Average rate (Hz): %f' % self.status['loop_rate_avg_hz'])
-        print('Min rate (Hz): %f' % self.status['loop_rate_min_hz'])
-        print('Max rate (Hz): %f' % self.status['loop_rate_max_hz'])
-        print('Current execution time (ms): %f' % self.status['execution_time_ms'])
-        print('Execution time supports rate of (Hz) %f'%(1000.0/self.status['execution_time_ms']))
-        print('Warnings: %d out of %d'%(self.status['loop_warns'],self.loop_cycles))
+        print('--------- TimingStats %s -----------' % self.loop_name)
+        print('Target rate (Hz): %.2f' % self.target_loop_rate)
+        print('Current rate (Hz): %.2f' % self.status['curr_rate_hz'])
+        print('Average rate (Hz): %.2f' % self.status['avg_rate_hz'])
+        print('Standard deviation of rate history (Hz): %.2f' % self.status['std_rate_hz'])
+        print('Min rate (Hz): %.2f' % self.status['min_rate_hz'])
+        print('Max rate (Hz): %.2f' % self.status['max_rate_hz'])
+        print('Supportable rate (Hz): %.2f' % self.status['supportable_rate_hz'])
+        print('Warnings: %d out of %d' % (self.status['missed_loops'], self.status['num_loops']))
 
     def mark_loop_start(self):
         self.ts_loop_start=time.time()
 
     def mark_loop_end(self):
-        self.loop_cycles+=1
-        #First two cycles initialize vars / log
+        self.status['num_loops'] += 1
+
+        # First two cycles initialize vars / log
         if not self.ts_loop_start:
             return
         if self.ts_loop_end is None:
-            self.ts_loop_end=time.time()
+            self.ts_loop_end = time.time()
             return
-        end_last=self.ts_loop_end
-        self.ts_loop_end=time.time()
-        self.status['execution_time_ms']=(self.ts_loop_end-self.ts_loop_start)*1000
-        self.status['loop_rate_hz']=1.0/(self.ts_loop_end-end_last)
-        self.status['loop_rate_min_hz']=min(self.status['loop_rate_hz'], self.status['loop_rate_min_hz'])
-        self.status['loop_rate_max_hz'] = max(self.status['loop_rate_hz'], self.status['loop_rate_max_hz'])
-        if type(self.rate_log)==type(None):
-            self.rate_log=numpy.array([self.status['loop_rate_hz']] * self.n_log)
-        self.rate_log[self.log_idx]=self.status['loop_rate_hz']
-        self.log_idx=(self.log_idx+1)%self.n_log
-        self.status['loop_rate_avg_hz']=numpy.average(self.rate_log)
-
-        self.sleep_time_s = (1 / self.target_loop_rate) - (self.status['execution_time_ms'] / 1000)
-        if self.sleep_time_s < .001:
-            self.status['loop_warns'] += 1
-            if not self.warned_yet:
-                self.warned_yet=True
-                self.logger.debug('Target loop rate of %f Hz for %s not possible. Capable of %.2f Hz' % (self.target_loop_rate,self.loop_name,(1000.0/self.status['execution_time_ms'])))
-
-
-    def display_rate_histogram(self):
+        if self.last_ts_loop_end is None:
+            self.last_ts_loop_end = self.ts_loop_end
+            self.ts_loop_end = time.time()
+            self.status['execution_time_s'] = self.ts_loop_end - self.ts_loop_start
+            self.status['curr_rate_hz'] = 1.0 / (self.ts_loop_end - self.last_ts_loop_end)
+            return
+
+        # Calculate average and supportable loop rate **must be done before marking loop end**
+        if len(self.curr_rate_history) >= self.n_history:
+            self.curr_rate_history.pop(0)
+        self.curr_rate_history.append(self.status['curr_rate_hz'])
+        self.status['avg_rate_hz'] = np.mean(self.curr_rate_history)
+        self.status['std_rate_hz'] = np.std(self.curr_rate_history)
+        if len(self.supportable_rate_history) >= self.n_history:
+            self.supportable_rate_history.pop(0)
+        self.supportable_rate_history.append(1.0 / self.status['execution_time_s'])
+        self.status['supportable_rate_hz'] = np.mean(self.supportable_rate_history)
+
+        # Log timing stats **must be done before marking loop end**
+        if self.status['num_loops'] % self.debug_freq == 0:
+            self.logger.debug('--------- TimingStats %s %d -----------' % (self.loop_name, self.status['num_loops']))
+            self.logger.debug('Target rate: %f' % self.target_loop_rate)
+            self.logger.debug('Current rate (Hz): %f' % self.status['curr_rate_hz'])
+            self.logger.debug('Average rate (Hz): %f' % self.status['avg_rate_hz'])
+            self.logger.debug('Standard deviation of rate history (Hz): %f' % self.status['std_rate_hz'])
+            self.logger.debug('Min rate (Hz): %f' % self.status['min_rate_hz'])
+            self.logger.debug('Max rate (Hz): %f' % self.status['max_rate_hz'])
+            self.logger.debug('Supportable rate (Hz): %f' % self.status['supportable_rate_hz'])
+            self.logger.debug('Standard deviation of supportable rate history (Hz): %f' % np.std(self.supportable_rate_history))
+            self.logger.debug('Warnings: %d out of %d' % (self.status['missed_loops'], self.status['num_loops']))
+            self.logger.debug('Sleep time (s): %f' % self.sleep_time_s)
+
+        # Calculate current loop rate & execution time
+        self.last_ts_loop_end = self.ts_loop_end
+        self.ts_loop_end = time.time()
+        self.status['execution_time_s'] = self.ts_loop_end - self.ts_loop_start
+        self.status['curr_rate_hz'] = 1.0 / (self.ts_loop_end - self.last_ts_loop_end)
+        self.status['min_rate_hz'] = min(self.status['curr_rate_hz'], self.status['min_rate_hz'])
+        self.status['max_rate_hz'] = max(self.status['curr_rate_hz'], self.status['max_rate_hz'])
+
+        # Calculate sleep time to achieve desired loop rate
+        self.sleep_time_s = (1 / self.target_loop_rate) - self.status['execution_time_s']
+        if self.sleep_time_s < 0.0:
+            self.status['missed_loops'] += 1
+            if self.status['missed_loops'] == 1:
+                self.logger.debug('Missed target loop rate of %.2f Hz for %s. Currently %.2f Hz' % (self.target_loop_rate, self.loop_name, self.status['curr_rate_hz']))
+
+    def generate_rate_histogram(self, save=None):
         import matplotlib.pyplot as plt
         fig, axs = plt.subplots(1, 1, sharey=True, tight_layout=True)
-        fig.suptitle('Distribution of loop rate (Hz). Target of %f '%self.target_loop_rate)
-        axs.hist(x=self.rate_log, bins='auto', color='#0504aa', alpha=0.7, rwidth=0.85)
-        plt.show()
+        fig.suptitle('Distribution of loop rate (Hz). Target of %.2f ' % self.target_loop_rate)
+        axs.hist(x=self.curr_rate_history, bins='auto', color='#0504aa', alpha=0.7, rwidth=0.85)
+        plt.show() if save is None else plt.savefig(save)
 
     def get_loop_sleep_time(self):
         """
-        :return: Time to sleep for to hit target loop rate
+        Returns
+        -------
+        float : Time to sleep for to hit target loop rate
         """
-        return max(.001,self.sleep_time_s)
-
+        return max(0.0, self.sleep_time_s)
 
 
 class ThreadServiceExit(Exception):

body/stretch_body/lift.py

@@ -25,17 +25,21 @@ def __init__(self):
         self.motor.set_motion_limits(self.translate_to_motor_rad(self.soft_motion_limits['current'][0]),
                                      self.translate_to_motor_rad(self.soft_motion_limits['current'][1]))
     # ###########  Device Methods #############
-
     def startup(self):
-        return self.motor.startup()
+        success= self.motor.startup()
+        self.__update_status()
+        return success
 
     def stop(self):
-        self.motor.stop() #Maintain current mode
+        self.motor.stop()
 
     def pull_status(self):
         self.motor.pull_status()
+        self.__update_status()
+
+    def __update_status(self):
         self.status['timestamp_pc'] = time.time()
-        self.status['pos']= self.motor_rad_to_translate_m(self.status['motor']['pos'])
+        self.status['pos'] = self.motor_rad_to_translate_m(self.status['motor']['pos'])
         self.status['vel'] = self.motor_rad_to_translate_m(self.status['motor']['vel'])
         self.status['force'] = self.motor_current_to_translate_force(self.status['motor']['current'])