diff --git a/.github/workflows/build_test.yaml b/.github/workflows/build_test.yaml index 24ea081d..529b5b62 100644 --- a/.github/workflows/build_test.yaml +++ b/.github/workflows/build_test.yaml @@ -14,14 +14,14 @@ jobs: steps: - uses: actions/checkout@v4 - - name: Configure Git to trust the directory + - name: Configure working direcotry run: | echo "Current directory is $(pwd)" echo "Present files are $(ls)" git config --global --add safe.directory `pwd` - - name: Install dependencies and code + - name: Install Stretch Body run: | pip3 install -e body/ - - name: Run tests + - name: Run Stretch Body Tests run: | pytest-3 body/test/test_foo.py \ No newline at end of file diff --git a/body/test/robot_tests/test_robot_home_stow.py b/body/test/robot_tests/test_robot_home_stow.py new file mode 100644 index 00000000..3654f99a --- /dev/null +++ b/body/test/robot_tests/test_robot_home_stow.py @@ -0,0 +1,159 @@ +#!/usr/bin/env python3 +import stretch_body.robot as robot +import time +import unittest + + +class Test_BRI_robot_home_stow(unittest.TestCase): + """ + Test to Verify Robot's homing and stowing behaviour + """ + + @classmethod + def setUpClass(self): + self.robot = robot.Robot() + self.robot.startup() + + @classmethod + def tearDownClass(self): + self.robot.stop() + + def check_joint_poses(self, poses_dict, check_name): + self.robot.pull_status() + + arm_pose = self.robot.status['arm']['pos'] + self.assertAlmostEqual(arm_pose, poses_dict['arm'], places=2, msg='%s Value "arm" Different' % check_name) + + lift_pose = self.robot.status['lift']['pos'] + self.assertAlmostEqual(lift_pose, poses_dict['lift'], places=2, msg='%s Value "lift" Different' % check_name) + + head_pan_pose = self.robot.status['head']['head_pan']['pos'] + self.assertAlmostEqual(head_pan_pose, poses_dict['head_pan'], places=1, + msg='%s Value "head_pan" Different' % check_name) + + head_tilt_pose = self.robot.status['head']['head_tilt']['pos'] + self.assertAlmostEqual(head_tilt_pose, poses_dict['head_tilt'], places=1, + msg='%s Value "head_tilt" Different' % check_name) + + if self.robot.params['tool'] == 'tool_stretch_dex_wrist': + wrist_yaw_pose = self.robot.status['end_of_arm']['wrist_yaw']['pos'] + self.assertAlmostEqual(wrist_yaw_pose, poses_dict['wrist_yaw'], places=1, + msg='%s Value "wrist_yaw" Different' % check_name) + + wrist_roll_pose = self.robot.status['end_of_arm']['wrist_roll']['pos'] + self.assertAlmostEqual(wrist_roll_pose, poses_dict['wrist_roll'], places=1, + msg='%s Value "wrist_roll" Different' % check_name) + wrist_pitch_pose = self.robot.status['end_of_arm']['wrist_pitch']['pos'] + self.assertAlmostEqual(wrist_pitch_pose, poses_dict['wrist_pitch'], places=1, + msg='%s Value "wrist_pitch" Different' % check_name) + + gripper_pose = self.robot.status['end_of_arm']['stretch_gripper']['pos'] + self.assertAlmostEqual(gripper_pose, poses_dict['stretch_gripper'], places=1, + msg='%s Value "stretch_gripper" Different' % check_name) + + if self.robot.params['tool'] == 'eoa_wrist_dw3_tool_sg3': + wrist_yaw_pose = self.robot.status['end_of_arm']['wrist_yaw']['pos'] + self.assertAlmostEqual(wrist_yaw_pose, poses_dict['wrist_yaw'], places=1, + msg='%s Value "wrist_yaw" Different' % check_name) + + wrist_roll_pose = self.robot.status['end_of_arm']['wrist_roll']['pos'] + self.assertAlmostEqual(wrist_roll_pose, poses_dict['wrist_roll'], places=1, + msg='%s Value "wrist_roll" Different' % check_name) + wrist_pitch_pose = self.robot.status['end_of_arm']['wrist_pitch']['pos'] + self.assertAlmostEqual(wrist_pitch_pose, poses_dict['wrist_pitch'], places=1, + msg='%s Value "wrist_pitch" Different' % check_name) + + gripper_pose = self.robot.status['end_of_arm']['stretch_gripper']['pos'] + self.assertAlmostEqual(gripper_pose, poses_dict['stretch_gripper'], places=1, + msg='%s Value "stretch_gripper" Different' % check_name) + + if self.robot.params['tool'] == 'tool_stretch_gripper': + wrist_yaw_pose = self.robot.status['end_of_arm']['wrist_yaw']['pos'] + self.assertAlmostEqual(wrist_yaw_pose, poses_dict['wrist_yaw'], places=1, + msg='%s Value "wrist_yaw" Different' % check_name) + + gripper_pose = self.robot.status['end_of_arm']['stretch_gripper']['pos'] + self.assertAlmostEqual(gripper_pose, poses_dict['stretch_gripper'], places=1, + msg='%s Value "stretch_gripper" Different' % check_name) + + def test_robot_home(self): + """ + Manually verify robot homing and assert joint poses are ~equal to homing poses. + """ + self.robot.home() + time.sleep(1.5) + + home_poses = {'head_pan': 0, + 'head_tilt': 0, + 'arm': 0.1, + 'lift': 0.6} + + if self.robot.params['tool'] == 'tool_stretch_dex_wrist': + home_poses['wrist_yaw'] = 0 + home_poses['wrist_roll'] = 0 + home_poses['wrist_pitch'] = 0 + home_poses['stretch_gripper'] = 0 + + if self.robot.params['tool'] == 'eoa_wrist_dw3_tool_sg3': + home_poses['wrist_yaw'] = 0 + home_poses['wrist_roll'] = self.bringup_test.robot_params[self.robot.params['tool']]['stow']['wrist_roll'] + home_poses['wrist_pitch'] = self.bringup_test.robot_params[self.robot.params['tool']]['stow']['wrist_pitch'] + home_poses['stretch_gripper'] = 0 + + if self.robot.params['tool'] == 'tool_stretch_gripper': + home_poses['wrist_yaw'] = 0 + home_poses['stretch_gripper'] = 0 + + if self.robot.params['tool'] == 'eoa_wrist_dw3_tool_nil': + home_poses['wrist_yaw'] = 0 + home_poses['wrist_roll'] = self.bringup_test.robot_params[self.robot.params['tool']]['stow']['wrist_roll'] + home_poses['wrist_pitch'] = self.bringup_test.robot_params[self.robot.params['tool']]['stow']['wrist_pitch'] + + self.bringup_test.log_params('home_poses', home_poses) + self.check_joint_poses(home_poses, 'Homing') + + def test_robot_stow(self): + """ + Manually verify robot stowing and assert joint poses are ~equal to stowing poses. + """ + print('Stowing robot.') + self.robot.stow() + time.sleep(1.5) + + stow_poses = {} + stow_poses['arm'] = self.robot.get_stow_pos('arm') + stow_poses['lift'] = self.robot.get_stow_pos('lift') + stow_poses['head_pan'] = self.robot.get_stow_pos('head_pan') + stow_poses['head_tilt'] = self.robot.get_stow_pos('head_tilt') + + + if self.robot.params['tool'] == 'tool_stretch_dex_wrist': + stow_poses['wrist_yaw'] = self.robot.get_stow_pos('wrist_yaw') + stow_poses['wrist_roll'] = self.robot.get_stow_pos('wrist_roll') + stow_poses['wrist_pitch'] = self.robot.get_stow_pos('wrist_pitch') + stow_poses['stretch_gripper'] = self.robot.get_stow_pos('stretch_gripper') + + + if self.robot.params['tool'] == 'eoa_wrist_dw3_tool_sg3': + stow_poses['wrist_yaw'] = self.robot.get_stow_pos('wrist_yaw') + stow_poses['wrist_roll'] = self.robot.get_stow_pos('wrist_roll') + stow_poses['wrist_pitch'] = self.robot.get_stow_pos('wrist_pitch') + stow_poses['stretch_gripper'] = self.robot.get_stow_pos('stretch_gripper') + + + if self.robot.params['tool'] == 'tool_stretch_gripper': + stow_poses['wrist_yaw'] = self.robot.get_stow_pos('wrist_yaw') + stow_poses['stretch_gripper'] = self.robot.get_stow_pos('stretch_gripper') + + if self.robot.params['tool'] == 'eoa_wrist_dw3_tool_nil': + stow_poses['wrist_yaw'] = self.robot.get_stow_pos('wrist_yaw') + stow_poses['wrist_roll'] = self.robot.get_stow_pos('wrist_roll') + stow_poses['wrist_pitch'] = self.robot.get_stow_pos('wrist_pitch') + + self.bringup_test.log_params('stow_poses', stow_poses) + self.check_joint_poses(stow_poses, 'Stowing') + + +test_suite = unittest.TestSuite() +test_suite.addTest(Test_BRI_robot_home_stow('test_robot_home')) +test_suite.addTest(Test_BRI_robot_home_stow('test_robot_stow')) \ No newline at end of file