def _test_repeat_circ_pose(robot): """Test a circ motion Test Sequence: 1. Move to start position via ptp. 2. Move half a circle. 3. Repeat the previous command Expected Results: 1. Robot moves to start position. 2. Robot moves half a circle. 3. No INVALID_GOAL is returned from controller """ if _askPermission(_test_repeat_circ_pose.__name__) == 0: return robot.move(Ptp(goal=[0, 0, 0, 0, 0, 0])) robot.move(Ptp(goal=[0, -0.78, 0.78, 0, 1.56, 0])) robot.move(Ptp(goal=Pose(position=Point(-0.46, -0.21, 0.19), orientation=from_euler(0, -3.14, -0.25)))) robot.move(Circ(goal=Pose(position=Point(-0.460, 0, 0.19), orientation=from_euler(0, -3.14, -0.25)), vel_scale=PTP_VEL_PICK, interim=Point(-0.355, -0.105, 0.19))) robot.move(Circ(goal=Pose(position=Point(-0.460, 0, 0.19), orientation=from_euler(0, -3.14, -0.25)), vel_scale=PTP_VEL_PICK, interim=Point(-0.355, -0.105, 0.19))) _askSuccess(_test_repeat_circ_pose.__name__, 'Failed to create path object for circle. ' 'Circle : Plane for motion is not properly defined.')
def _test_circ_interim(robot): """Tests circ motions given an interim point. Test Sequence: 1. Move to start position. 2. Move 5/12 of a circle and back to the start position. 3. Move 5/8 of a circle. Expected Results: 1. Robot moves to start position. 2. Robot moves 5/12 of a circle and back to the start position. 3. Robot moves 5/8 of a circle. """ if _askPermission(_test_circ_interim.__name__) == 0: return START_POSE = Pose(position=Point(0.0, 0.4, 0.7), orientation=from_euler(-1.57, 0.0, 0.0)) INTERIM_POINT = Point(0.4, 0.0, 0.7) robot.move(Ptp(goal=[1.57, 0.0, -1.57, 0.0, 1.57, 0.0])) robot.move(Ptp(goal=START_POSE)) robot.move(Circ(goal=Pose(position=Point(0.35355, -0.18709, 0.7), orientation=from_euler(0.0, 0.0, 2.0951)), interim=INTERIM_POINT, vel_scale=VELOCITY_LOW, acc_scale=DEFAULT_ACC)) robot.move(Circ(goal=START_POSE, interim=INTERIM_POINT, vel_scale=VELOCITY_LOW, acc_scale=DEFAULT_ACC)) robot.move(Circ(goal=Pose(position=Point(-0.37204, -0.14693, 0.7), orientation=from_euler(0.0, 0.0, 3.9279)), interim=INTERIM_POINT, vel_scale=VELOCITY_LOW, acc_scale=DEFAULT_ACC)) _askSuccess(_test_circ_interim.__name__, 'The robot should have moved to a start position, then 5/12 of a circle, back to the start position' + ' and then 5/8 of a circle.')
def _test_lin_pos(robot): """Test a lin motion Test Sequence: 1. Move 15cm linear down. 2. Move 15cm linear up. Expected Results: 1. Robot moves 15cm linear down. 2. Robot moves 15cm linear up. """ if _askPermission(_test_lin_pos.__name__) == 0: return robot.move( Lin(goal=Pose(position=Point(0.0, 0.0, -0.15)), relative=True, vel_scale=PTP_VEL_PICK)) robot.move( Lin(goal=Pose(position=Point(0.0, 0.0, 0.15)), relative=True, vel_scale=PTP_VEL_PICK)) _askSuccess( _test_lin_pos.__name__, 'The robot tcp should have moved linear 15cm down and then linear 15cm up.' )
def _test_blend_pos(robot): """ Test a motion blending Test Sequence: 1. Move to start position using Ptp (upper "pick" position) 2. Generate square movement by blending 4 lin. Expected Results: 1. Robot moves to start position. 2. Robot moves in a square without intermediate stops. """ if _askPermission(_test_blend_pos.__name__) == 0: return robot.move(Ptp(goal=Pose(position=Point(-0.46, -0.21, 0.19), orientation=from_euler(0, -3.14, -0.25)))) seq_l = Sequence() seq_l.append(Lin(goal=Pose(position=Point(-0.460, 0, 0.19), orientation=from_euler(0, -3.14, -0.25)), vel_scale=PTP_VEL_PICK), blend_radius=0.05) seq_l.append(Lin(goal=Pose(position=Point(-0.3, 0, 0.19), orientation=from_euler(0, -3.14, -0.25)), vel_scale=PTP_VEL_PICK), blend_radius=0.05) seq_l.append(Lin(goal=Pose(position=Point(-0.3, -0.21, 0.19), orientation=from_euler(0, -3.14, -0.25)), vel_scale=PTP_VEL_PICK), blend_radius=0.05) seq_l.append(Lin(goal=Pose(position=Point(-0.46, -0.21, 0.19), orientation=from_euler(0, -3.14, -0.25)), vel_scale=PTP_VEL_PICK), blend_radius=0.0) robot.move(seq_l) _askSuccess(_test_blend_pos.__name__, 'The robot should moved to a start position and then in a square always retur' + 'ning to the start position. During the motion the robot does not stop.')
def _test_circ_center(robot): """Tests circ motions given a center point. Test Sequence: 1. Move to start position. 2. Move a quarter circle two times. 3. Move a quarter circle in the other direction two times. Expected Results: 1. Robot moves to start position. 2. Robot moves a quarter circle two times. 3. Robot moves a quarter circle in the other direction two times. """ if _askPermission(_test_circ_center.__name__) == 0: return START_POSE = Pose(position=Point(0.0, 0.4, 0.7), orientation=from_euler(0.0, 0.0, 0.0)) CENTER_POINT = Point(0.0, 0.0, 0.7) robot.move(Ptp(goal=[1.57, 0.0, -1.57, 0.0, 1.57, 0.0])) robot.move(Ptp(goal=START_POSE)) robot.move(Circ(goal=Pose(position=Point(0.4, 0.0, 0.7), orientation=from_euler(-1.57, 0.0, 0.0)), center=CENTER_POINT, vel_scale=VELOCITY_HIGH, acc_scale=DEFAULT_ACC)) robot.move(Circ(goal=Pose(position=Point(0.0, -0.4, 0.7), orientation=from_euler(-3.14, 0.0, 0.0)), center=CENTER_POINT, vel_scale=VELOCITY_HIGH, acc_scale=DEFAULT_ACC)) robot.move(Circ(goal=Pose(position=Point(0.4, 0.0, 0.7), orientation=from_euler(-1.57, 0.0, 0.0)), center=CENTER_POINT, vel_scale=VELOCITY_HIGH, acc_scale=DEFAULT_ACC)) robot.move(Circ(goal=START_POSE, center=CENTER_POINT, vel_scale=VELOCITY_HIGH, acc_scale=DEFAULT_ACC)) _askSuccess(_test_circ_center.__name__, 'The robot should have moved to a start position, then a quarter circle two times and then a quarter' + ' circle back two times.')
def _test_circ_pos(robot): """Test a circ motion Test Sequence: 1. Move to start position using Ptp (upper "pick" position) 2. Move half a circle. 3. Move half a circle back to origin. Expected Results: 1. Robot moves to start position. 2. Robot moves half a circle. 3. Robot moves half a circle back to origin. """ if _askPermission(_test_circ_pos.__name__) == 0: return robot.move(Ptp(goal=Pose(position=Point(-0.46, -0.21, 0.19), orientation=from_euler(0, -3.14, -0.25)))) robot.move(Circ(goal=Pose(position=Point(-0.460, 0, 0.19), orientation=from_euler(0, -3.14, -0.25)), vel_scale=PTP_VEL_PICK, interim=Point(-0.355, -0.105, 0.19))) robot.move(Circ(goal=Pose(position=Point(-0.460, -0.21, 0.19), orientation=from_euler(0, -3.14, -0.25)), vel_scale=PTP_VEL_PICK, interim=Point(-0.565, -0.105, 0.19))) _askSuccess(_test_circ_pos.__name__, 'The robot should have moved to a start position and then two times half a' + ' circle.')
def _test_too_large_gripper_pos(robot): """ Tests too large gripper position. Test Sequence: 1. Move to start position. 2. Perform gripper command. Expected Results: 1. Robot moves to start position. 2. Exception is thrown and error message is displayed. """ if _askPermission(_test_too_large_gripper_pos.__name__) == 0: return robot.move( Ptp(goal=[0, 0.007, -1.816, 0, 1.8236, 0], vel_scale=DEFAULT_PTP_VEL)) try: robot.move(Gripper(goal=GRIPPER_OPEN + 0.01)) except RobotMoveFailed: pass else: rospy.logerr('Invalid gripper position does not result in exception.') _askSuccess( _test_too_large_gripper_pos.__name__, 'There should be an error message stating that the gripper position violates the gripper limits.' )
def _test_ptp_pos(robot): """Tests a ptp motion. Test Sequence: 1. Move to start position. 2. Move away from the singularity. 3. Move to the upper "pick" position Expected Results: 1. Robot moves to start position. 2. Robot moves away from the singularity. 3. Robot moves to the upper "pick" position. """ if _askPermission(_test_ptp_pos.__name__) == 0: return robot.move(Ptp(goal=[0, 0, 0, 0, 0, 0])) robot.move(Ptp(goal=[0, -0.78, 0.78, 0, 1.56, 0])) robot.move( Ptp(goal=Pose(position=Point(-0.46, -0.21, 0.19), orientation=from_euler(0, -3.14, -0.25)))) _askSuccess( _test_ptp_pos.__name__, 'The robot should have moved to [0,0,0,0,0,0], after that away from [0,0,0,0,0,0] and then to the upper' + ' pick position.')
def _test_repeat_lin_pose(robot): """Tests repeating a lin pose motion. Test Sequence: 1. Move to start position via ptp. 2. Move to a pose via lin 3. Repeat the previous command Expected Results: 1. Robot moves to start position. 2. Robot moves to the given position. 3. No INVALID_GOAL is returned from controller """ if _askPermission(_test_repeat_lin_pose.__name__) == 0: return robot.move(Ptp(goal=[0, 0, 0, 0, 0, 0])) robot.move(Ptp(goal=[0, -0.78, 0.78, 0, 1.56, 0])) robot.move( Ptp(goal=Pose(position=Point(-0.46, -0.21, 0.19), orientation=from_euler(0, -3.14, -0.25)))) robot.move( Lin(goal=Pose(position=Point(-0.46, -0.21, 0.19)), vel_scale=PTP_VEL_PICK)) _askSuccess(_test_repeat_lin_pose.__name__, 'No INVALID_GOAL should appear from controller.')
def _test_repeat_ptp_in_sequence(robot): """Tests repeating a ptp joint motion in a sequence. Test Sequence: 1. Perform the following commands in a command sequence: - Move to start position. - Move away from the singularity via ptp joint. - Repeat the previous command. Expected Results: 1. Robot performs the following movements without INVALID_GOAL or other error returned from controller: - Moves to start position. - Moves away from the singularity. """ if _askPermission(_test_repeat_ptp_in_sequence.__name__) == 0: return seq = Sequence() seq.append(Ptp(goal=[0, 0, 0, 0, 0, 0]), blend_radius=0) seq.append(Ptp(goal=[0, -0.78, 0.78, 0, 1.56, 0]), blend_radius=0) seq.append(Ptp(goal=[0, -0.78, 0.78, 0, 1.56, 0]), blend_radius=0) robot.move(seq) _askSuccess( _test_repeat_ptp_in_sequence.__name__, 'No INVALID_GOAL or other error should appear from controller.')
def _test_circ_pos(robot): """Test a circ motion Test Sequence: 1. Move half a circle. 2. Move half a circle back to origin. Expected Results: 1. Robot moves half a circle. 2. Robot moves half a circle back to origin. """ if _askPermission(_test_circ_pos.__name__) == 0: return robot.move( Circ(goal=Pose(position=Point(-0.460, 0, 0.19), orientation=from_euler(0, -3.14, -0.25)), vel_scale=PTP_VEL_PICK, interim=Point(-0.355, -0.105, 0.19))) robot.move( Circ(goal=Pose(position=Point(-0.460, -0.21, 0.19), orientation=from_euler(0, -3.14, -0.25)), vel_scale=PTP_VEL_PICK, interim=Point(-0.565, -0.105, 0.19))) _askSuccess( _test_circ_pos.__name__, 'The robot should have moved in a half circle stop and move in half a circle' + ' to its original position')
def _test_seq_pos2(robot): """Tests a motion sequence. Together the two tests _test_seq_pos1 and _test_seq_pos2 contain all possible pairs of consecutive commands from the set of commands (PTP, LIN, CIRC). Test Sequence: 1. Execute sequence (PTP-CIRC-LIN-LIN-PTP) consisting of the following motions: a. Move away from the start position. b. Move a quarter circle arriving at the upper "pick" position. c. Move 15cm linear down. d. Move 15cm linear up. e. Move back to start position. Expected Results: 1. Robot performes sequence of the following motions stopping in between them: a. Robot moves away from the start position. b. Robot moves a quarter circle arriving at the upper "pick" position. c. Robot moves 15cm linear down. d. Robot moves 15cm linear up. e. Robot moves back to start position. """ if _askPermission(_test_seq_pos2.__name__) == 0: return x_pick = 0 y_pick = 0.25 z_pick_low = 0.25 z_pick_high = 0.4 x_pick2 = -0.25 y_pick2 = 0 x_center = y_center = 0 rot = Quaternion(0.0, 1.0, 0.0, 0.0) p_pick_high = Pose(position=Point(x_pick, y_pick, z_pick_high), orientation=rot) p_pick_low = Pose(position=Point(x_pick, y_pick, z_pick_low), orientation=rot) p_pick_high2 = Pose(position=Point(x_pick2, y_pick2, z_pick_high), orientation=rot) p_center = Pose(position=Point(x_center, y_center, z_pick_high)) seq_l = Sequence() seq_l.append(Ptp(goal=p_pick_high2)) seq_l.append(Circ(goal=p_pick_high, center=p_center.position)) seq_l.append(Lin(goal=p_pick_low)) seq_l.append(Lin(goal=p_pick_high)) seq_l.append(Ptp(goal=[0, 0, 0, 0, 0, 0])) robot.move(seq_l) _askSuccess( _test_seq_pos2.__name__, 'The robot should have moved away from the start position and then a quarter' + ' circle arriving at the upper pick position. After that the robot tcp' + ' should have moved linear 15cm down and then linear 15cm up. In the end the' + ' robot should have moved back to [0,0,0,0,0,0].')
def _test_pause_between_moves(robot): """Tests pausing the next motion when no motion is active. Test Sequence: 1. Start robot motion and wait for the motion to end. 2. Trigger pause. 3. Start robot motion and wait for 5 seconds. 4. Trigger resume. Expected Results: 1. Robot moves to goal. 2. Robot does not move. 3. Robot does not move. 4. Robot moves to goal. """ if _askPermission(_test_pause_between_moves.__name__) == 0: return # 1. Create simple ptp command and start thread for movement ptp = Ptp(goal=[0, -0.78, 0.78, 0, 1.56, 0], vel_scale=_DEFAULT_VEL_SCALE) move_thread = MoveThread(robot, ptp) move_thread.start() # Wait for thread to finish move_thread.join() # 2. Trigger pause robot.pause() # 3. Start another motion ptp = Ptp(goal=[0, 0, 0, 0, 0, 0], vel_scale=_DEFAULT_VEL_SCALE) move_thread = MoveThread(robot, ptp) move_thread.start() rospy.sleep(5.0) # 4. Trigger resume robot.resume() # Wait for thread to finish move_thread.join() _askSuccess( _test_pause_between_moves.__name__, 'The robot should have moved to the upper pick position and back to [0, 0, 0, 0, 0, 0] after ' 'approximately 5 seconds.')
def _test_closed_gripper_pos(robot): """ Tests smallest gripper position. Test Sequence: 1. Move to start position. 2. Perform gripper command. Expected Results: 1. Robot moves to start position. 2. Gripper closed. """ if _askPermission(_test_closed_gripper_pos.__name__) == 0: return robot.move( Ptp(goal=[0, 0.007, -1.816, 0, 1.8236, 0], vel_scale=DEFAULT_PTP_VEL)) robot.move(Gripper(goal=GRIPPER_CLOSED)) _askSuccess(_test_closed_gripper_pos.__name__, 'Gripper should be closed.')
def _test_ptp_pos(robot): """Tests a ptp motion. Test Sequence: 1. Move to zero position 2. Move away from the singularity. Expected Results: 1. Robot moves to zero position. 2. Robot moves away from the singularity. """ if _askPermission(_test_ptp_pos.__name__) == 0: return robot.move(Ptp(goal=[0, 0, 0, 0, 0, 0])) robot.move(Ptp(goal=[0, -0.78, 0.78, 0, 1.56, 0])) _askSuccess(_test_ptp_pos.__name__, 'The robot should have moved to [0,0,0,0,0,0] and after that away from [0,0,0,0,0,0].')
def _test_correct_gripper_pos(robot): """ Tests correct gripper position. Test Sequence: 1. Move to start position. 2. Perform gripper command. Expected Results: 1. Robot moves to start position. 2. Gripper moved to given position. """ if _askPermission(_test_correct_gripper_pos.__name__) == 0: return robot.move( Ptp(goal=[0, 0.007, -1.816, 0, 1.8236, 0], vel_scale=DEFAULT_PTP_VEL)) robot.move(Gripper(goal=GRIPPER_MIDDLE)) _askSuccess(_test_correct_gripper_pos.__name__, 'Gripper should have moved.')
def _test_stop(robot): """Tests stopping a robot motion. Test Sequence: 1. Start robot motion and wait for the motion to start. 2. Trigger stop. 3. Start robot motion with different goal. Expected Results: 1. Robot moves. 2. Robot stops motion before reaching goal. 3. Robot starts motion and moves to the other goal. """ if _askPermission(_test_stop.__name__) == 0: return _robot_motion_observer = RobotMotionObserver(_PLANNING_GROUP) # 1. Create simple ptp command and start thread for movement ptp = Ptp(goal=[0, -0.78, 0.78, 0, 1.56, 0], vel_scale=_DEFAULT_VEL_SCALE) move_thread = MoveThread(robot, ptp) move_thread.start() _robot_motion_observer.wait_motion_start( sleep_interval=_SLEEP_TIME_S, move_tolerance=_TOLERANCE_FOR_MOTION_DETECTION_RAD) # 2. Trigger stop robot.stop() # Wait for thread to finish move_thread.join() # 3. Trigger motion with different goal robot.move( Ptp(goal=[0.78, 0.0, 0.0, 0, 0, 0], relative=True, vel_scale=_DEFAULT_VEL_SCALE)) _askSuccess( _test_stop.__name__, 'The robot should have stopped before reaching the upper pick position and continued moving in ' 'another direction.')
def _test_repeat_ptp_joint(robot): """Tests repeating a ptp joint motion. Test Sequence: 1. Move to start position. 2. Move away from the singularity via ptp joint. 3. Repeat the previous command Expected Results: 1. Robot moves to start position. 2. Robot moves away from the singularity. 3. No INVALID_GOAL is returned from controller """ if _askPermission(_test_repeat_ptp_joint.__name__) == 0: return robot.move(Ptp(goal=[0, 0, 0, 0, 0, 0])) robot.move(Ptp(goal=[0, -0.78, 0.78, 0, 1.56, 0])) robot.move(Ptp(goal=[0, -0.78, 0.78, 0, 1.56, 0])) _askSuccess(_test_repeat_ptp_joint.__name__, 'No INVALID_GOAL should appear from controller.')
def _test_lin_pos(robot): """Test a lin motion Test Sequence: 1. Move to start position using Ptp (upper "pick" position) 2. Move 15cm linear down. 3. Move 15cm linear up. Expected Results: 1. Robot moves to the upper "pick" position 2. Robot moves 15cm linear down. 3. Robot moves 15cm linear up. """ if _askPermission(_test_lin_pos.__name__) == 0: return robot.move(Ptp(goal=Pose(position=Point(-0.46, -0.21, 0.19), orientation=from_euler(0, -3.14, -0.25)))) robot.move(Lin(goal=Pose(position=Point(0.0, 0.0, -0.15)), relative=True, vel_scale=PTP_VEL_PICK)) robot.move(Lin(goal=Pose(position=Point(0.0, 0.0, 0.15)), relative=True, vel_scale=PTP_VEL_PICK)) _askSuccess(_test_lin_pos.__name__, 'The robot should have moved to the upper pick position. Afterwards the tcp' + ' should have moved linear 15cm down and then linear 15cm up.')
def _test_circ_small_vertical(robot): """Tests circ motion on a vertical circle. Test Sequence: 1. Move to start position. 2. Move 1.99 PI along a small vertical circle. Expected Results: 1. Robot moves to start position. 2. Robot moves 1.99 PI along a small vertical circle. """ if _askPermission(_test_circ_small_vertical.__name__) == 0: return robot.move(Ptp(goal=[0.0, 0.0, 1.57, 0.0, 0.0, 0.0])) robot.move(Ptp(goal=Pose(position=Point(-0.5, 0.2, 0.4), orientation=from_euler(0.0, -1.57, -1.57)))) robot.move(Circ(goal=Pose(position=Point(-0.5, 0.19686, 0.39995), orientation=from_euler(0.0, -1.57, -1.57)), interim=Point(-0.5, 0.3, 0.5), vel_scale=VELOCITY_LOW, acc_scale=DEFAULT_ACC)) _askSuccess(_test_circ_small_vertical.__name__, 'The robot should have moved to a start position and then 1.99 PI along a small vertical circle.')
def _test_pause_resume(robot): """Tests pausing and resuming a robot motion. Test Sequence: 1. Start robot motion and wait for the motion to start. 2. Trigger pause and wait 5 seconds. 3. Trigger resume. Expected Results: 1. Robot moves. 2. Robot stops motion. 3. Robot starts motion again and moves to goal. """ if _askPermission(_test_pause_resume.__name__) == 0: return _robot_motion_observer = RobotMotionObserver(_PLANNING_GROUP) # 1. Create simple ptp command and start thread for movement ptp = Ptp(goal=[0, 0.39, -0.39, 0, 0.78, 0], vel_scale=_DEFAULT_VEL_SCALE) move_thread = MoveThread(robot, ptp) move_thread.start() _robot_motion_observer.wait_motion_start( sleep_interval=_SLEEP_TIME_S, move_tolerance=_TOLERANCE_FOR_MOTION_DETECTION_RAD) # 2. Trigger pause robot.pause() rospy.sleep(5.0) # 3. Trigger resume robot.resume() # Wait for thread to finish move_thread.join() _askSuccess( _test_pause_resume.__name__, 'The robot should have paused his movement and continued after approximately 5 seconds.' )
def _test_blend_pos(robot): """ Test a motion blending Test Sequence: 1. Generate square movement by blending 4 lin. Expected Results: 1. Robot moves in a square without intermediate stops. """ if _askPermission(_test_blend_pos.__name__) == 0: return seq_l = Sequence() seq_l.append(Lin(goal=Pose(position=Point(-0.460, 0, 0.19), orientation=from_euler(0, -3.14, -0.25)), vel_scale=PTP_VEL_PICK), blend_radius=0.05) seq_l.append(Lin(goal=Pose(position=Point(-0.3, 0, 0.19), orientation=from_euler(0, -3.14, -0.25)), vel_scale=PTP_VEL_PICK), blend_radius=0.05) seq_l.append(Lin(goal=Pose(position=Point(-0.3, -0.21, 0.19), orientation=from_euler(0, -3.14, -0.25)), vel_scale=PTP_VEL_PICK), blend_radius=0.05) seq_l.append(Lin(goal=Pose(position=Point(-0.46, -0.21, 0.19), orientation=from_euler(0, -3.14, -0.25)), vel_scale=PTP_VEL_PICK), blend_radius=0.0) robot.move(seq_l) _askSuccess( _test_blend_pos.__name__, 'The robot should move in a square back to its original position. During the' + ' motion the robot does not stop.')
def _test_pause_stop(robot): """Tests pausing and canceling a robot motion. Test Sequence: 1. Start robot motion and wait for the motion to start. 2. Trigger pause. 3. Trigger stop. Expected Results: 1. Robot moves. 2. Robot stops motion. 3. Move thread terminates. """ if _askPermission(_test_pause_stop.__name__) == 0: return _robot_motion_observer = RobotMotionObserver(_PLANNING_GROUP) # 1. Create simple ptp command and start thread for movement ptp = Ptp(goal=[0, -0.78, 0.78, 0, 1.56, 0], vel_scale=_DEFAULT_VEL_SCALE) move_thread = MoveThread(robot, ptp) move_thread.start() _robot_motion_observer.wait_motion_start( sleep_interval=_SLEEP_TIME_S, move_tolerance=_TOLERANCE_FOR_MOTION_DETECTION_RAD) # 2. Trigger pause robot.pause() # 3. Trigger stop robot.stop() # Wait for thread to finish move_thread.join() _askSuccess( _test_pause_stop.__name__, 'The robot should have paused his movement before reaching the upper pick position ' 'and the test should have terminated immediately.')
def _test_gripper_vel_scale(robot): """ Tests different velocity scaling of gripper command. Test Sequence: 1. Move to start position. 2. Perform gripper command with max velocity scale. 3. Perform gripper command with 0.1 velocity scale. Expected Results: 1. Robot moves to start position. 2. Gripper completely open with maximal speed. 3. Gripper completely closed with slower speed """ if _askPermission(_test_gripper_vel_scale.__name__) == 0: return robot.move( Ptp(goal=[0, 0.007, -1.816, 0, 1.8236, 0], vel_scale=DEFAULT_PTP_VEL)) robot.move(Gripper(goal=GRIPPER_CLOSED, vel_scale=0.1)) robot.move(Gripper(goal=GRIPPER_OPEN, vel_scale=1)) robot.move(Gripper(goal=GRIPPER_CLOSED, vel_scale=0.1)) _askSuccess(_test_gripper_vel_scale.__name__, 'Gripper should move with different speed.')
def _test_rob_gripper_cmd_combi(robot): """ Tests gripper command in combination with move commands. Test Sequence: For i=1:4 1. Move to start position. 2. Perform gripper command. 3. Move robot to different position. 4. Perform gripper command. Expected Results: For i=1:4 1. Robot moves to start position. 2. Gripper moves correctly. 3. Robot move to stated robot position. 4. Gripper moves correctly. """ if _askPermission(_test_rob_gripper_cmd_combi.__name__) == 0: return n = 5 grip_range = GRIPPER_MIDDLE - GRIPPER_CLOSED step_width = grip_range / n for i in range(0, n - 1): robot.move( Ptp(goal=[0, 0.007, -1.816, 0, 1.8236, 0], vel_scale=DEFAULT_PTP_VEL)) robot.move(Gripper(goal=GRIPPER_MIDDLE + (i * step_width))) robot.move( Ptp(goal=[0.5, 0.007, -1.816, 0, 1.8236, 0], vel_scale=DEFAULT_PTP_VEL)) robot.move(Gripper(goal=GRIPPER_CLOSED + (i * step_width))) _askSuccess(_test_rob_gripper_cmd_combi.__name__, 'Robot and gripper should have moved alternatingly.')
def _test_gripper_cmd_stress_test(robot): """ Stress test for gripper cmd. Test Sequence: 1. Move to start position. 2. Perform a large number of gripper command. Expected Results: 1. Robot moves to start position. 2. Gripper moves correctly. """ if _askPermission(_test_gripper_cmd_stress_test.__name__) == 0: return robot.move( Ptp(goal=[0, 0.007, -1.816, 0, 1.8236, 0], vel_scale=DEFAULT_PTP_VEL)) n = 30 grip_range = GRIPPER_OPEN - GRIPPER_CLOSED step_width = grip_range / n for i in range(1, n): robot.move(Gripper(goal=i * step_width)) _askSuccess(_test_gripper_cmd_stress_test.__name__, 'Gripper should have closed and then opened in %i steps' % n)