def reset(self):
# reset simulation
del self.platform
self.platform = TriFingerPlatform(
visualization=self.visualization,
initial_robot_position=TriFingerPlatform.spaces.robot_position.
default,
initial_object_pose=self.initializer.get_initial_state(),
)
goal = self.initializer.get_goal()
self.goal = {
"position": goal.position,
"orientation": goal.orientation,
}
# visualize the goal
if self.visualization:
self.goal_marker = visual_objects.CubeMarker(
width=0.065,
position=goal.position,
orientation=goal.orientation,
)
self.info = {"difficulty": self.initializer.difficulty}
self.step_count = 0
return self._create_observation(0)
def reset(self):
"""Reset the environment."""
# hard-reset simulation
del self.platform
# initialize simulation
initial_robot_position = (
TriFingerPlatform.spaces.robot_position.default)
# initialize cube at the centre
initial_object_pose = mct.move_cube.Pose(
position=mct.INITIAL_CUBE_POSITION)
self.platform = TriFingerPlatform(
visualization=self.visualization,
initial_robot_position=initial_robot_position,
initial_object_pose=initial_object_pose,
)
# get goal trajectory from the initializer
trajectory = self.initializer.get_trajectory()
# visualize the goal
if self.visualization:
self.goal_marker = visual_objects.CubeMarker(
width=mct.move_cube._CUBE_WIDTH,
position=trajectory[0][1],
orientation=(0, 0, 0, 1),
pybullet_client_id=self.platform.simfinger._pybullet_client_id,
)
self.info = {"time_index": -1, "trajectory": trajectory}
self.step_count = 0
return self._create_observation(0)
def reset(self):
# reset simulation
del self.platform
# initialize simulation
initial_robot_position = (
TriFingerPlatform.spaces.robot_position.default)
initial_object_pose = self.initializer.get_initial_state()
goal_object_pose = self.initializer.get_goal()
self.platform = TriFingerPlatform(
visualization=self.visualization,
initial_robot_position=initial_robot_position,
initial_object_pose=initial_object_pose,
)
self.goal = {
"position": goal_object_pose.position,
"orientation": goal_object_pose.orientation,
}
# visualize the goal
if self.visualization:
self.goal_marker = visual_objects.CubeMarker(
width=0.065,
position=goal_object_pose.position,
orientation=goal_object_pose.orientation,
physicsClientId=self.platform.simfinger._pybullet_client_id,
)
self.info = {"difficulty": self.initializer.difficulty}
self.step_count = 0
return self._create_observation(0)
def reset(self):
# reset simulation
del self.platform
# initialize simulation
if self.initializer is None:
# if no initializer is given (which will be the case during training),
# we can initialize in any way desired. here, we initialize the cube always
# in the center of the arena, instead of randomly, as this appears to help
# training
initial_robot_position = (
TriFingerPlatform.spaces.robot_position.default
)
default_object_position = (
TriFingerPlatform.spaces.object_position.default
)
default_object_orientation = (
TriFingerPlatform.spaces.object_orientation.default
)
initial_object_pose = move_cube.Pose(
position=default_object_position,
orientation=default_object_orientation,
)
goal_object_pose = move_cube.sample_goal(difficulty=1)
else:
# if an initializer is given, i.e. during evaluation, we need to initialize
# according to it, to make sure we remain coherent with the standard CubeEnv.
# otherwise the trajectories produced during evaluation will be invalid.
initial_robot_position = (
TriFingerPlatform.spaces.robot_position.default
)
initial_object_pose = self.initializer.get_initial_state()
goal_object_pose = self.initializer.get_goal()
self.platform = TriFingerPlatform(
visualization=self.visualization,
initial_robot_position=initial_robot_position,
initial_object_pose=initial_object_pose,
)
self.goal = {
"position": goal_object_pose.position,
"orientation": goal_object_pose.orientation,
}
# visualize the goal
if self.visualization:
self.goal_marker = visual_objects.CubeMarker(
width=0.065,
position=goal_object_pose.position,
orientation=goal_object_pose.orientation,
pybullet_client_id=self.platform.simfinger._pybullet_client_id,
)
self.info = dict()
self.step_count = 0
return self._create_observation(0)
def test_timestamps_no_camera_delay():
platform = TriFingerPlatform(
visualization=False, enable_cameras=True, camera_delay_steps=0
)
action = platform.Action()
# ensure the camera frame rate is set to 10 Hz
assert platform.camera_rate_fps == 10
# compute camera update step interval based on configured rates
camera_update_step_interval = (
1 / platform.camera_rate_fps
) / platform._time_step
# robot time step in milliseconds
time_step_ms = platform._time_step * 1000
# First time step
t = platform.append_desired_action(action)
first_stamp_ms = platform.get_timestamp_ms(t)
first_stamp_s = first_stamp_ms / 1000
camera_obs = platform.get_camera_observation(t)
assert first_stamp_s == camera_obs.cameras[0].timestamp
assert first_stamp_s == camera_obs.cameras[1].timestamp
assert first_stamp_s == camera_obs.cameras[2].timestamp
# Test time stamps of observations t+1 (with the current implementation,
# the observation should be exactly the same as for t).
camera_obs_next = platform.get_camera_observation(t + 1)
assert first_stamp_s == camera_obs_next.cameras[0].timestamp
assert first_stamp_s == camera_obs_next.cameras[1].timestamp
assert first_stamp_s == camera_obs_next.cameras[2].timestamp
# Second time step
t = platform.append_desired_action(action)
second_stamp_ms = platform.get_timestamp_ms(t)
assert second_stamp_ms == first_stamp_ms + time_step_ms
# there should not be a new camera observation yet
camera_obs = platform.get_camera_observation(t)
assert first_stamp_s == camera_obs.cameras[0].timestamp
assert first_stamp_s == camera_obs.cameras[1].timestamp
assert first_stamp_s == camera_obs.cameras[2].timestamp
# do several steps until a new camera/object update is expected
# (-1 because there is already one action appended above for the
# "second time step")
for _ in range(int(camera_update_step_interval - 1)):
t = platform.append_desired_action(action)
nth_stamp_ms = platform.get_timestamp_ms(t)
nth_stamp_s = nth_stamp_ms / 1000
assert nth_stamp_ms > second_stamp_ms
camera_obs = platform.get_camera_observation(t)
assert nth_stamp_s == camera_obs.cameras[0].timestamp
assert nth_stamp_s == camera_obs.cameras[1].timestamp
assert nth_stamp_s == camera_obs.cameras[2].timestamp
def test_camera_timestamps_with_camera_delay_less_than_rate():
# A bit more complex example (probably redundant with the simple one above
# but since I already implemented it, let's keep it).
platform = TriFingerPlatform(
visualization=False, enable_cameras=True, camera_delay_steps=10
)
action = platform.Action()
# ensure the camera frame rate is set to 10 Hz
assert platform.camera_rate_fps == 10
assert platform._compute_camera_update_step_interval() == 100
first_stamp_s = platform.get_timestamp_ms(0) / 1000
# The start is a bit tricky because of the initial observation which has
# timestamp 0 but the next observation is triggered in the first step,
# resulting in the same timestamp. So first step 100 times, until the next
# camera update is triggered.
for i in range(100):
t = platform.append_desired_action(action)
# In each step, we still should see the camera observation from step 0
cameras = platform.get_camera_observation(t).cameras
assert first_stamp_s == cameras[0].timestamp, f"i={i}, t={t}"
assert first_stamp_s == cameras[1].timestamp, f"i={i}, t={t}"
assert first_stamp_s == cameras[2].timestamp, f"i={i}, t={t}"
assert t == 99
# one more step to trigger the next camera update
t = platform.append_desired_action(action)
second_stamp_s = platform.get_timestamp_ms(t) / 1000
# The next observation should be triggered now but due to the delay, we
# should still see the old observation for the next 9 steps
for i in range(9):
t = platform.append_desired_action(action)
cameras = platform.get_camera_observation(t).cameras
assert first_stamp_s == cameras[0].timestamp, f"i={i}, t={t}"
assert first_stamp_s == cameras[1].timestamp, f"i={i}, t={t}"
assert first_stamp_s == cameras[2].timestamp, f"i={i}, t={t}"
# after the next step, we should see an updated camera observation which
# again persists for 100 steps
for i in range(100):
t = platform.append_desired_action(action)
cameras = platform.get_camera_observation(t).cameras
assert second_stamp_s == cameras[0].timestamp, f"i={i}, t={t}"
assert second_stamp_s == cameras[1].timestamp, f"i={i}, t={t}"
assert second_stamp_s == cameras[2].timestamp, f"i={i}, t={t}"
# and now the next update should be there
t = platform.append_desired_action(action)
camera_obs = platform.get_camera_observation(t)
assert second_stamp_s < camera_obs.cameras[0].timestamp
def test_timestamps(self):
platform = TriFingerPlatform(visualization=False, enable_cameras=True)
action = platform.Action()
# ensure the camera frame rate is set to 10 Hz
self.assertEqual(platform.camera_rate_fps, 10)
# compute camera update step interval based on configured rates
camera_update_step_interval = (
1 / platform.camera_rate_fps) / platform._time_step
# robot time step in milliseconds
time_step_ms = platform._time_step * 1000
# First time step
t = platform.append_desired_action(action)
first_stamp_ms = platform.get_timestamp_ms(t)
first_stamp_s = first_stamp_ms / 1000
camera_obs = platform.get_camera_observation(t)
self.assertEqual(first_stamp_ms, camera_obs.cameras[0].timestamp)
self.assertEqual(first_stamp_ms, camera_obs.cameras[1].timestamp)
self.assertEqual(first_stamp_ms, camera_obs.cameras[2].timestamp)
# Test time stamps of observations t+1
camera_obs_next = platform.get_camera_observation(t + 1)
next_stamp_ms = first_stamp_ms + time_step_ms
self.assertEqual(next_stamp_ms, camera_obs_next.cameras[0].timestamp)
self.assertEqual(next_stamp_ms, camera_obs_next.cameras[1].timestamp)
self.assertEqual(next_stamp_ms, camera_obs_next.cameras[2].timestamp)
# Second time step
t = platform.append_desired_action(action)
second_stamp_ms = platform.get_timestamp_ms(t)
self.assertEqual(second_stamp_ms, first_stamp_ms + time_step_ms)
# there should not be a new camera observation yet
camera_obs = platform.get_camera_observation(t)
self.assertEqual(first_stamp_s, camera_obs.cameras[0].timestamp)
self.assertEqual(first_stamp_s, camera_obs.cameras[1].timestamp)
self.assertEqual(first_stamp_s, camera_obs.cameras[2].timestamp)
# do several steps until a new camera/object update is expected
# (-1 because there is already one action appended above for the
# "second time step")
for _ in range(int(camera_update_step_interval - 1)):
t = platform.append_desired_action(action)
nth_stamp_ms = platform.get_timestamp_ms(t)
nth_stamp_s = nth_stamp_ms / 1000
self.assertGreater(nth_stamp_ms, second_stamp_ms)
camera_obs = platform.get_camera_observation(t)
self.assertEqual(nth_stamp_s, camera_obs.cameras[0].timestamp)
self.assertEqual(nth_stamp_s, camera_obs.cameras[1].timestamp)
self.assertEqual(nth_stamp_s, camera_obs.cameras[2].timestamp)
def test_object_pose_observation(self):
Pose = namedtuple("Pose", ["position", "orientation"])
pose = Pose([0.1, -0.5, 0], [0, 0, 0.2084599, 0.97803091])
platform = TriFingerPlatform(initial_object_pose=pose)
t = platform.append_desired_action(platform.Action())
obs = platform.get_camera_observation(t)
np.testing.assert_array_equal(obs.object_pose.position, pose.position)
np.testing.assert_array_almost_equal(obs.object_pose.orientation,
pose.orientation)
def test_get_camera_observation_timeindex(self):
platform = TriFingerPlatform(enable_cameras=True)
# negative time index needs to be rejected
with self.assertRaises(ValueError):
platform.get_camera_observation(-1)
t = platform.append_desired_action(platform.Action())
try:
platform.get_camera_observation(t)
platform.get_camera_observation(t + 1)
except Exception:
self.fail()
with self.assertRaises(ValueError):
platform.get_camera_observation(t + 2)
def test_get_object_pose_timeindex(self):
platform = TriFingerPlatform()
# negative time index needs to be rejected
with self.assertRaises(ValueError):
platform.get_object_pose(-1)
t = platform.append_desired_action(platform.Action())
try:
platform.get_object_pose(t)
platform.get_object_pose(t + 1)
except Exception:
self.fail()
with self.assertRaises(ValueError):
platform.get_object_pose(t + 2)
def test_camera_timestamps_with_camera_delay_simple():
# Basic test with simple values: Camera update every 2 steps, delay of 1
# step.
platform = TriFingerPlatform(
visualization=False, enable_cameras=True, camera_delay_steps=1
)
action = platform.Action()
# set camera rate to 100 fps so we need less stepping in this test
platform.camera_rate_fps = 500
assert platform._compute_camera_update_step_interval() == 2
initial_stamp_s = platform.get_timestamp_ms(0) / 1000
# first step triggers camera (we get the initial observation at this point)
t = platform.append_desired_action(action)
assert t == 0
camera_obs = platform.get_camera_observation(t)
assert initial_stamp_s == camera_obs.cameras[0].timestamp, f"t={t}"
assert initial_stamp_s == camera_obs.cameras[1].timestamp, f"t={t}"
assert initial_stamp_s == camera_obs.cameras[2].timestamp, f"t={t}"
trigger1_stamp_s = platform.get_timestamp_ms(t) / 1000
# in second step observation is ready but still has stamp zero
t = platform.append_desired_action(action)
camera_obs = platform.get_camera_observation(t)
assert trigger1_stamp_s == camera_obs.cameras[0].timestamp, f"t={t}"
assert trigger1_stamp_s == camera_obs.cameras[1].timestamp, f"t={t}"
assert trigger1_stamp_s == camera_obs.cameras[2].timestamp, f"t={t}"
# in third step new observation is triggered but due to delay we still get
# the old one
t = platform.append_desired_action(action)
camera_obs = platform.get_camera_observation(t)
assert trigger1_stamp_s == camera_obs.cameras[0].timestamp, f"t={t}"
assert trigger1_stamp_s == camera_obs.cameras[1].timestamp, f"t={t}"
assert trigger1_stamp_s == camera_obs.cameras[2].timestamp, f"t={t}"
trigger2_stamp_s = platform.get_timestamp_ms(t) / 1000
assert trigger2_stamp_s > trigger1_stamp_s
# in forth step the new observation is ready
t = platform.append_desired_action(action)
camera_obs = platform.get_camera_observation(t)
assert trigger2_stamp_s == camera_obs.cameras[0].timestamp, f"t={t}"
assert trigger2_stamp_s == camera_obs.cameras[1].timestamp, f"t={t}"
assert trigger2_stamp_s == camera_obs.cameras[2].timestamp, f"t={t}"
# again trigger but we get previous observation due to delay
t = platform.append_desired_action(action)
camera_obs = platform.get_camera_observation(t)
assert trigger2_stamp_s == camera_obs.cameras[0].timestamp, f"t={t}"
assert trigger2_stamp_s == camera_obs.cameras[1].timestamp, f"t={t}"
assert trigger2_stamp_s == camera_obs.cameras[2].timestamp, f"t={t}"
trigger3_stamp_s = platform.get_timestamp_ms(t) / 1000
assert trigger3_stamp_s > trigger2_stamp_s
# and there should be an update again
t = platform.append_desired_action(action)
camera_obs = platform.get_camera_observation(t)
assert trigger3_stamp_s == camera_obs.cameras[0].timestamp, f"t={t}"
assert trigger3_stamp_s == camera_obs.cameras[1].timestamp, f"t={t}"
assert trigger3_stamp_s == camera_obs.cameras[2].timestamp, f"t={t}"
def test_camera_timestamps_with_camera_delay_more_than_rate():
# In this test the delay is higher than the camera rate, so this results in
# the effective rate to be reduced.
# Use small numbers (camera update interval 2 and delay 3) to keep the test
# manageable.
platform = TriFingerPlatform(
visualization=False, enable_cameras=True, camera_delay_steps=3
)
action = platform.Action()
# set high camera rate so we need less stepping in this test
platform.camera_rate_fps = 500
assert platform._compute_camera_update_step_interval() == 2
initial_stamp_s = platform.get_timestamp_ms(0) / 1000
# first step triggers camera (we get the initial observation at this point)
t = platform.append_desired_action(action)
assert t == 0
camera_obs = platform.get_camera_observation(t)
assert initial_stamp_s == camera_obs.cameras[0].timestamp, f"t={t}"
assert initial_stamp_s == camera_obs.cameras[1].timestamp, f"t={t}"
assert initial_stamp_s == camera_obs.cameras[2].timestamp, f"t={t}"
trigger1_stamp_s = platform.get_timestamp_ms(t) / 1000
# now it takes 3 steps until we actually see the new observation
t = platform.append_desired_action(action)
t = platform.append_desired_action(action)
camera_obs = platform.get_camera_observation(t)
assert initial_stamp_s == camera_obs.cameras[0].timestamp, f"t={t}"
assert initial_stamp_s == camera_obs.cameras[1].timestamp, f"t={t}"
assert initial_stamp_s == camera_obs.cameras[2].timestamp, f"t={t}"
t = platform.append_desired_action(action)
camera_obs = platform.get_camera_observation(t)
assert trigger1_stamp_s == camera_obs.cameras[0].timestamp, f"t={t}"
assert trigger1_stamp_s == camera_obs.cameras[1].timestamp, f"t={t}"
assert trigger1_stamp_s == camera_obs.cameras[2].timestamp, f"t={t}"
# Only now, one step later, the next update is triggered
t = platform.append_desired_action(action)
camera_obs = platform.get_camera_observation(t)
assert trigger1_stamp_s == camera_obs.cameras[0].timestamp, f"t={t}"
assert trigger1_stamp_s == camera_obs.cameras[1].timestamp, f"t={t}"
assert trigger1_stamp_s == camera_obs.cameras[2].timestamp, f"t={t}"
trigger2_stamp_s = platform.get_timestamp_ms(t) / 1000
assert trigger2_stamp_s > trigger1_stamp_s
# And again it takes 3 steps until we actually see the new observation
t = platform.append_desired_action(action)
t = platform.append_desired_action(action)
camera_obs = platform.get_camera_observation(t)
assert trigger1_stamp_s == camera_obs.cameras[0].timestamp, f"t={t}"
assert trigger1_stamp_s == camera_obs.cameras[1].timestamp, f"t={t}"
assert trigger1_stamp_s == camera_obs.cameras[2].timestamp, f"t={t}"
t = platform.append_desired_action(action)
camera_obs = platform.get_camera_observation(t)
assert trigger2_stamp_s == camera_obs.cameras[0].timestamp, f"t={t}"
assert trigger2_stamp_s == camera_obs.cameras[1].timestamp, f"t={t}"
assert trigger2_stamp_s == camera_obs.cameras[2].timestamp, f"t={t}"
def test_timestamps(self):
platform = TriFingerPlatform(visualization=False, enable_cameras=True)
action = platform.Action()
# compute camera update step interval based on configured rates
camera_update_step_interval = (
1 / platform._camera_rate_fps) / platform._time_step
# robot time step in milliseconds
time_step_ms = platform._time_step * 1000
# First time step
t = platform.append_desired_action(action)
first_stamp_ms = platform.get_timestamp_ms(t)
first_stamp_s = first_stamp_ms / 1000
object_pose = platform.get_object_pose(t)
camera_obs = platform.get_camera_observation(t)
self.assertEqual(first_stamp_ms, object_pose.timestamp)
self.assertEqual(first_stamp_ms, camera_obs.cameras[0].timestamp)
self.assertEqual(first_stamp_ms, camera_obs.cameras[1].timestamp)
self.assertEqual(first_stamp_ms, camera_obs.cameras[2].timestamp)
# Test time stamps of observations t+1
object_pose_next = platform.get_object_pose(t + 1)
camera_obs_next = platform.get_camera_observation(t + 1)
next_stamp_ms = first_stamp_ms + time_step_ms
self.assertEqual(next_stamp_ms, object_pose_next.timestamp)
self.assertEqual(next_stamp_ms, camera_obs_next.cameras[0].timestamp)
self.assertEqual(next_stamp_ms, camera_obs_next.cameras[1].timestamp)
self.assertEqual(next_stamp_ms, camera_obs_next.cameras[2].timestamp)
# XXX ===============================================================
# The following part of the test is disabled as currently everything is
# updated in each step (i.e. no lower update rate for camera and
# object).
return
# Second time step
t = platform.append_desired_action(action)
second_stamp_ms = platform.get_timestamp_ms(t)
self.assertEqual(second_stamp_ms, first_stamp_ms + time_step_ms)
# there should not be a new camera observation yet
object_pose = platform.get_object_pose(t)
camera_obs = platform.get_camera_observation(t)
self.assertEqual(first_stamp_s, object_pose.timestamp)
self.assertEqual(first_stamp_s, camera_obs.cameras[0].timestamp)
self.assertEqual(first_stamp_s, camera_obs.cameras[1].timestamp)
self.assertEqual(first_stamp_s, camera_obs.cameras[2].timestamp)
# do several steps until a new camera/object update is expected
for _ in range(int(camera_update_step_interval)):
t = platform.append_desired_action(action)
nth_stamp_ms = platform.get_timestamp_ms(t)
nth_stamp_s = nth_stamp_ms / 1000
self.assertGreater(nth_stamp_ms, second_stamp_ms)
object_pose = platform.get_object_pose(t)
camera_obs = platform.get_camera_observation(t)
self.assertEqual(nth_stamp_s, object_pose.timestamp)
self.assertEqual(nth_stamp_s, camera_obs.cameras[0].timestamp)
self.assertEqual(nth_stamp_s, camera_obs.cameras[1].timestamp)
self.assertEqual(nth_stamp_s, camera_obs.cameras[2].timestamp)