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)
