def test_sample_goal_difficulty_4_no_initial_pose():
for i in range(1000):
goal = move_cube.sample_goal(difficulty=4)
# verify the goal is valid (i.e. within the allowed ranges)
try:
move_cube.validate_goal(goal)
except move_cube.InvalidGoalError as e:
pytest.fail(msg="Invalid goal: {} pose is {}, {}".format(
e, e.position, e.orientation), )
# verify the goal satisfies conditions of difficulty 2
assert goal.position[2] <= move_cube._max_height
assert goal.position[2] >= move_cube._min_height
def test_sample_goal_difficulty_1_no_initial_pose():
for i in range(1000):
goal = move_cube.sample_goal(difficulty=1)
# verify the goal is valid (i.e. within the allowed ranges)
try:
move_cube.validate_goal(goal)
except move_cube.InvalidGoalError as e:
pytest.fail(msg="Invalid goal: {} pose is {}, {}".format(
e, e.position, e.orientation), )
# verify the goal satisfies conditions of difficulty 1
# always on ground
assert goal.position[2] == (move_cube._CUBE_WIDTH / 2)
# no orientation
np.testing.assert_array_equal(goal.orientation, [0, 0, 0, 1])
def test_sample_goal_difficulty_4(self):
for i in range(1000):
goal = move_cube.sample_goal(difficulty=4)
# verify the goal is valid (i.e. within the allowed ranges)
try:
move_cube.validate_goal(goal)
except move_cube.InvalidGoalError as e:
self.fail(
msg="Invalid goal: {} pose is {}, {}".format(
e, e.position, e.orientation
),
)
# verify the goal satisfies conditions of difficulty 2
self.assertLessEqual(goal.position[2], move_cube._max_height)
self.assertGreaterEqual(goal.position[2], move_cube._min_height)
def test_sample_goal_difficulty_1(self):
for i in range(1000):
goal = move_cube.sample_goal(difficulty=1)
# verify the goal is valid (i.e. within the allowed ranges)
try:
move_cube.validate_goal(goal)
except move_cube.InvalidGoalError as e:
self.fail(
msg="Invalid goal: {} pose is {}, {}".format(
e, e.position, e.orientation
),
)
# verify the goal satisfies conditions of difficulty 1
# always on ground
self.assertEqual(goal.position[2], move_cube._CUBOID_HALF_SIZE[2])
# no orientation
np.testing.assert_array_equal(goal.orientation, [0, 0, 0, 1])
def __init__(self, difficulty, initial_state, goal):
"""Initialize.
Args:
difficulty (int): Difficulty level of the goal. This is still
needed even for a fixed goal, as it is also used for computing
the reward (the cost function is different for the different
levels).
initial_state (move_cube.Pose): Initial pose of the object.
goal (move_cube.Pose): Goal pose of the object.
Raises:
Exception: If initial_state or goal are not valid. See
:meth:`move_cube.validate_goal` for more information.
"""
move_cube.validate_goal(initial_state)
move_cube.validate_goal(goal)
self.difficulty = difficulty
self.initial_state = initial_state
self.goal = goal
def validate_goal(trajectory: Trajectory):
"""Checks if the given trajectory is valid, raises error if not.
Raises:
ValueError: If there are any structural issues.
move_cube.InvalidGoalError: If a position exceeds the allowed goal
space.
"""
previous_t = -1
if not trajectory:
raise ValueError("Trajectory is empty")
if trajectory[0][0] != 0:
raise ValueError("First goal does not start at t=0")
for i, (t, goal) in enumerate(trajectory):
if t <= previous_t:
raise ValueError(f"Goal {i} starts before previous goal")
previous_t = t
move_cube.validate_goal(move_cube.Pose(position=goal))
def test_validate_goal_dict():
"""Test validation of a goal that is passed as dict."""
half_width = move_cube._CUBE_WIDTH / 2
full_rotation = Rotation.from_euler("zxz", [0.42, 0.1, -2.3]).as_quat()
# a valid goal
try:
move_cube.validate_goal({
"position": [-0.12, 0.0, 0.06],
"orientation": full_rotation
})
except Exception as e:
pytest.fail("Valid goal was considered invalid because %s" % e)
# some invalid goals
# invalid values
with pytest.raises(ValueError):
move_cube.validate_goal({
"position": [0, 0],
"orientation": [0, 0, 0, 1]
})
# invalid positions
with pytest.raises(move_cube.InvalidGoalError):
move_cube.validate_goal({
"position": [0.3, 0, half_width],
"orientation": [0, 0, 0, 1]
})
# invalid dict keys
with pytest.raises(KeyError):
move_cube.validate_goal({
"wrong_key": [-0.12, 0.0, 0.06],
"orientation": full_rotation
})
def test_validate_goal():
half_width = move_cube._CUBE_WIDTH / 2
yaw_rotation = Rotation.from_euler("z", 0.42).as_quat()
full_rotation = Rotation.from_euler("zxz", [0.42, 0.1, -2.3]).as_quat()
# test some valid goals
try:
move_cube.validate_goal(
move_cube.Pose([0, 0, half_width], [0, 0, 0, 1]))
except Exception as e:
pytest.fail("Valid goal was considered invalid because %s" % e)
try:
move_cube.validate_goal(
move_cube.Pose([0.05, -0.1, half_width], yaw_rotation))
except Exception as e:
pytest.fail("Valid goal was considered invalid because %s" % e)
try:
move_cube.validate_goal(
move_cube.Pose([-0.12, 0.0, 0.06], full_rotation))
except Exception as e:
pytest.fail("Valid goal was considered invalid because %s" % e)
# test some invalid goals
# invalid values
with pytest.raises(ValueError):
move_cube.validate_goal(move_cube.Pose([0, 0], [0, 0, 0, 1]))
with pytest.raises(ValueError):
move_cube.validate_goal(move_cube.Pose([0, 0, 0], [0, 0, 1]))
# invalid positions
with pytest.raises(move_cube.InvalidGoalError):
move_cube.validate_goal(
move_cube.Pose([0.3, 0, half_width], [0, 0, 0, 1]))
with pytest.raises(move_cube.InvalidGoalError):
move_cube.validate_goal(
move_cube.Pose([0, -0.3, half_width], [0, 0, 0, 1]))
with pytest.raises(move_cube.InvalidGoalError):
move_cube.validate_goal(move_cube.Pose([0, 0, 0.3], [0, 0, 0, 1]))
with pytest.raises(move_cube.InvalidGoalError):
move_cube.validate_goal(move_cube.Pose([0, 0, 0], [0, 0, 0, 1]))
with pytest.raises(move_cube.InvalidGoalError):
move_cube.validate_goal(move_cube.Pose([0, 0, -0.01], [0, 0, 0, 1]))
# valid CoM position but rotation makes it reach out of valid range
with pytest.raises(move_cube.InvalidGoalError):
move_cube.validate_goal(
move_cube.Pose([0, 0, half_width], full_rotation))
def test_validate_goal(self):
on_ground_height = move_cube._CUBOID_HALF_SIZE[2]
yaw_rotation = Rotation.from_euler("z", 0.42).as_quat()
full_rotation = Rotation.from_euler("zxz", [0.42, 0.1, -2.3]).as_quat()
# test some valid goals
try:
move_cube.validate_goal(
move_cube.Pose([0, 0, on_ground_height], [0, 0, 0, 1])
)
except Exception as e:
self.fail("Valid goal was considered invalid because %s" % e)
try:
move_cube.validate_goal(
move_cube.Pose([0.05, -0.1, on_ground_height], yaw_rotation)
)
except Exception as e:
self.fail("Valid goal was considered invalid because %s" % e)
try:
move_cube.validate_goal(
move_cube.Pose([-0.12, 0.0, 0.06], full_rotation)
)
except Exception as e:
self.fail("Valid goal was considered invalid because %s" % e)
# test some invalid goals
# invalid values
with self.assertRaises(ValueError):
move_cube.validate_goal(move_cube.Pose([0, 0], [0, 0, 0, 1]))
with self.assertRaises(ValueError):
move_cube.validate_goal(move_cube.Pose([0, 0, 0], [0, 0, 1]))
# invalid positions
with self.assertRaises(move_cube.InvalidGoalError):
move_cube.validate_goal(
move_cube.Pose([0.3, 0, on_ground_height], [0, 0, 0, 1])
)
with self.assertRaises(move_cube.InvalidGoalError):
move_cube.validate_goal(
move_cube.Pose([0, -0.3, on_ground_height], [0, 0, 0, 1])
)
with self.assertRaises(move_cube.InvalidGoalError):
move_cube.validate_goal(move_cube.Pose([0, 0, 0.3], [0, 0, 0, 1]))
with self.assertRaises(move_cube.InvalidGoalError):
move_cube.validate_goal(move_cube.Pose([0, 0, 0], [0, 0, 0, 1]))
with self.assertRaises(move_cube.InvalidGoalError):
move_cube.validate_goal(
move_cube.Pose([0, 0, -0.01], [0, 0, 0, 1])
)
# valid CoM position but rotation makes it reach out of valid range
with self.assertRaises(move_cube.InvalidGoalError):
move_cube.validate_goal(
move_cube.Pose([0, 0, on_ground_height], full_rotation)
)
def main():
parser = argparse.ArgumentParser(description=__doc__)
parser.add_argument(
"--task",
required=True,
choices=["move_cube", "move_cube_on_trajectory", "rearrange_dice"],
help="Name of the task.",
)
parser.add_argument(
"--log-dir",
required=True,
type=pathlib.Path,
help="Directory containing the log files.",
)
args = parser.parse_args()
# some setup depending on the selected task
if args.task == "move_cube":
import trifinger_simulation.tasks.move_cube as task
compute_reward = compute_reward_move_cube
elif args.task == "move_cube_on_trajectory":
import trifinger_simulation.tasks.move_cube_on_trajectory as task
compute_reward = compute_reward_move_cube_on_trajectory
elif args.task == "rearrange_dice":
import trifinger_simulation.tasks.rearrange_dice as task
compute_reward = compute_reward_rearrange_dice
else:
raise ValueError(f"Invalid task {args.task}")
# dictionary with additional, task-specific data that is passed to the
# compute_reward function
additional_data = {}
# load goal from json file
try:
with open(args.log_dir / "goal.json", "r") as fh:
goal = json.load(fh)
task.validate_goal(goal["goal"])
except Exception as e:
print("Invalid goal file:", e)
sys.exit(1)
if args.task == "rearrange_dice":
# for the rearrange_dice task, we need to generate the goal mask
camera_params = load_camera_parameters(args.log_dir, "camera{id}.yml")
additional_data["goal_masks"] = task.generate_goal_mask(
camera_params, goal["goal"]
)
try:
robot_log = str(args.log_dir / "robot_data.dat")
camera_log = str(args.log_dir / "camera_data.dat")
if args.task in ("move_cube", "move_cube_on_trajectory"):
log = robot_fingers.TriFingerPlatformWithObjectLog(
robot_log, camera_log
)
else:
log = robot_fingers.TriFingerPlatformLog(robot_log, camera_log)
except Exception as e:
print("Failed to load logs:", e)
sys.exit(1)
# verify that the log is complete and matches the expected length
t_first = log.get_first_timeindex()
t_last = log.get_last_timeindex()
if t_first != 0:
print(
"Invalid log: First time index in robot log is {}. Expected 0.".format(
t_first
)
)
sys.exit(1)
if t_last != task.EPISODE_LENGTH - 1:
print(
"Invalid log: Last time index in robot log is {}. Expected {}.".format(
t_last, task.EPISODE_LENGTH - 1
)
)
sys.exit(1)
cumulative_reward = 0
for t in range(log.get_first_timeindex(), log.get_last_timeindex() + 1):
reward = compute_reward(task, log, t, goal, **additional_data)
cumulative_reward += reward
print("Cumulative Reward:", cumulative_reward)
