Merge pull request #21 from hello-robot/bug_fix/tilt_backlash

bug fix for tilt backlash

stretch_core/nodes/command_groups.py

@@ -209,7 +209,8 @@ class HeadTiltCommandGroup(SimpleCommandGroup):
         if self.active:
             _, tilt_error = self.update_execution(robot_status, backlash_state=kwargs['backlash_state'])
             robot.head.move_by('head_tilt', tilt_error, v_r=self.goal['velocity'], a_r=self.goal['acceleration'])
-            if tilt_error > (self.head_tilt_backlash_transition_angle + self.head_tilt_calibrated_offset):
+            #if tilt_error > (self.head_tilt_backlash_transition_angle + self.head_tilt_calibrated_offset):
+            if self.goal['position'] > (self.head_tilt_backlash_transition_angle + self.head_tilt_calibrated_offset):
                 kwargs['backlash_state']['head_tilt_looking_up'] = True
             else:
                 kwargs['backlash_state']['head_tilt_looking_up'] = False

stretch_core/nodes/joint_trajectory_server.py

@@ -143,6 +143,9 @@ class JointTrajectoryAction:
                 named_errors = [c.update_execution(robot_status, success_callback=self.success_callback,
                                                    backlash_state=self.node.backlash_state)
                                 for c in command_groups]
+                # It's not clear how this could ever happen. The
+                # groups in command_groups.py seem to return
+                # (self.name, self.error) or None, rather than True.
                 if any(ret == True for ret in named_errors):
                     self.node.robot_mode_rwlock.release_read()
                     return