def evaluate_state(
trajectory: Trajectory, time_index: int, actual_position: Position
):
"""Compute cost of a given cube pose at a given time step. Less is better.
The cost is computed using :func:`move_cube.evaluate_state` (difficulty=3)
for the goal position that is active at the given time_index.
Args:
trajectory: The trajectory based on which the cost is computed.
time_index: Index of the time step that is evaluated.
actual_position: Cube position at the specified time step.
Returns:
Cost of the actual position w.r.t. to the goal position of the active
step in the trajectory. Lower value means that the actual pose is
closer to the goal. Zero if actual == goal.
"""
active_goal = get_active_goal(trajectory, time_index)
# wrap positions in Pose objects
actual_pose = move_cube.Pose(position=actual_position)
goal_pose = move_cube.Pose(position=active_goal)
return move_cube.evaluate_state(goal_pose, actual_pose, GOAL_DIFFICULTY)
def test_evaluate_state_difficulty_4_rotation_around_y(self):
difficulty = 4
pose_origin = move_cube.Pose()
pose_rot_only_y = move_cube.Pose(
orientation=Rotation.from_euler("y", 0.42).as_quat()
)
pose_rot_without_y = move_cube.Pose(
orientation=Rotation.from_euler("yz", [0.0, 0.42]).as_quat()
)
pose_rot_with_y = move_cube.Pose(
orientation=Rotation.from_euler("yz", [0.2, 0.42]).as_quat()
)
# needs to be zero for exact match
cost = move_cube.evaluate_state(pose_origin, pose_origin, difficulty)
self.assertEqual(cost, 0)
cost_only_y = move_cube.evaluate_state(
pose_origin, pose_rot_only_y, difficulty
)
self.assertEqual(cost_only_y, 0)
cost_without_y = move_cube.evaluate_state(
pose_origin, pose_rot_without_y, difficulty
)
cost_with_y = move_cube.evaluate_state(
pose_origin, pose_rot_with_y, difficulty
)
self.assertAlmostEqual(cost_without_y, cost_with_y)
def test_evaluate_state_difficulty_4(self):
difficulty = 4
pose_origin = move_cube.Pose()
pose_trans = move_cube.Pose(position=[1, 2, 3])
pose_rot = move_cube.Pose(
orientation=Rotation.from_euler("z", 0.42).as_quat()
)
pose_both = move_cube.Pose(
[1, 2, 3], Rotation.from_euler("z", 0.42).as_quat()
)
# needs to be zero for exact match
cost = move_cube.evaluate_state(pose_origin, pose_origin, difficulty)
self.assertEqual(cost, 0)
# None-zero if there is translation, rotation or both
self.assertNotEqual(
move_cube.evaluate_state(pose_origin, pose_trans, difficulty), 0
)
self.assertNotEqual(
move_cube.evaluate_state(pose_origin, pose_rot, difficulty), 0
)
self.assertNotEqual(
move_cube.evaluate_state(pose_origin, pose_both, difficulty), 0
)
def get_synced_log_data(logdir, goal, difficulty):
log = robot_fingers.TriFingerPlatformLog(os.path.join(logdir, "robot_data.dat"),
os.path.join(logdir, "camera_data.dat"))
log_camera = tricamera.LogReader(os.path.join(logdir, "camera_data.dat"))
stamps = log_camera.timestamps
obs = {'robot': [], 'cube': [], 'images': [], 't': [], 'desired_action': [],
'stamp': [], 'acc_reward': []}
ind = 0
acc_reward = 0.0
for t in range(log.get_first_timeindex(), log.get_last_timeindex()):
camera_observation = log.get_camera_observation(t)
acc_reward -= move_cube.evaluate_state(
move_cube.Pose(**goal), camera_observation.filtered_object_pose,
difficulty
)
if 1000 * log.get_timestamp_ms(t) >= stamps[ind]:
robot_observation = log.get_robot_observation(t)
obs['robot'].append(robot_observation)
obs['cube'].append(camera_observation.filtered_object_pose)
obs['images'].append([convert_image(camera.image)
for camera in camera_observation.cameras])
obs['desired_action'].append(log.get_desired_action(t))
obs['acc_reward'].append(acc_reward)
obs['t'].append(t)
obs['stamp'].append(log.get_timestamp_ms(t))
ind += 1
return obs
def compute_reward(logdir, simulation):
with open(os.path.join(logdir, "goal.json"), 'r') as f:
goal = json.load(f)
difficulty = goal['difficulty']
goal_pose = move_cube.Pose(position=np.array(goal['goal']['position']),
orientation=np.array(
goal['goal']['orientation']))
if (simulation == 0):
log = robot_fingers.TriFingerPlatformLog(
os.path.join(logdir, "robot_data.dat"),
os.path.join(logdir, "camera_data.dat"))
reward = 0.0
for t in range(log.get_first_timeindex(),
log.get_last_timeindex() + 1):
camera_observation = log.get_camera_observation(t)
reward -= evaluate_state(goal_pose,
camera_observation.filtered_object_pose,
difficulty)
return reward, True
else:
path = os.path.join(logdir, 'observations.pkl')
with open(path, 'rb') as handle:
observations = pkl.load(handle)
reward = 0.0
ex_state = move_cube.sample_goal(difficulty=-1)
for i in range(len(observations)):
ex_state.position = observations[i]["achieved_goal"]["position"]
ex_state.orientation = observations[i]["achieved_goal"][
"orientation"]
reward -= evaluate_state(goal_pose, ex_state, difficulty)
return reward, True
def test_evaluate_state_dict():
"""Test evaluate state using a dict instead of Pose."""
difficulty = 4
pose_origin = move_cube.Pose()
dict_origin = {"position": [0, 0, 0], "orientation": [0, 0, 0, 1]}
pose_pose = move_cube.Pose([1, 2, 3],
Rotation.from_euler("z", 0.42).as_quat())
dict_pose = {
"position": [1, 2, 3],
"orientation": Rotation.from_euler("z", 0.42).as_quat(),
}
# needs to be zero for exact match
pose_cost = move_cube.evaluate_state(pose_origin, pose_pose, difficulty)
dict_cost = move_cube.evaluate_state(dict_origin, dict_pose, difficulty)
assert pose_cost == dict_cost
def test_evaluate_state_difficulty_3():
difficulty = 3
pose_origin = move_cube.Pose()
pose_trans = move_cube.Pose(position=[1, 2, 3])
pose_rot = move_cube.Pose(
orientation=Rotation.from_euler("z", 0.42).as_quat())
pose_both = move_cube.Pose([1, 2, 3],
Rotation.from_euler("z", 0.42).as_quat())
# needs to be zero for exact match
cost = move_cube.evaluate_state(pose_origin, pose_origin, difficulty)
assert cost == 0
# None-zero if there is translation, rotation is ignored
assert move_cube.evaluate_state(pose_origin, pose_trans, difficulty) != 0
assert move_cube.evaluate_state(pose_origin, pose_rot, difficulty) == 0
assert move_cube.evaluate_state(pose_origin, pose_both, difficulty) != 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 main(logdir, video_path):
goal, difficulty = get_goal(logdir)
data = get_synced_log_data(logdir, goal, difficulty)
fps = len(data['t']) / (data['stamp'][-1] - data['stamp'][0])
video_recorder = VideoRecorder(fps)
cube_drawer = CubeDrawer(logdir)
initial_object_pose = move_cube.Pose(data['cube'][0].position,
data['cube'][0].orientation)
platform = trifinger_simulation.TriFingerPlatform(
visualization=True,
initial_object_pose=initial_object_pose,
)
markers = []
marker_cube_ori = VisualCubeOrientation(data['cube'][0].position,
data['cube'][0].orientation)
marker_goal_ori = VisualCubeOrientation(goal['position'], goal['orientation'])
visual_objects.CubeMarker(
width=0.065,
position=goal['position'],
orientation=goal['orientation']
)
p.configureDebugVisualizer(p.COV_ENABLE_GUI, 0)
p.resetDebugVisualizerCamera(cameraDistance=0.6, cameraYaw=0, cameraPitch=-40, cameraTargetPosition=[0,0,0])
for i, t in enumerate(data['t']):
platform.simfinger.reset_finger_positions_and_velocities(data['desired_action'][i].position)
platform.cube.set_state(data['cube'][i].position, data['cube'][i].orientation)
marker_cube_ori.set_state(data['cube'][i].position, data['cube'][i].orientation)
frame_desired = video_recorder.get_views()
frame_desired = cv2.cvtColor(frame_desired, cv2.COLOR_RGB2BGR)
platform.simfinger.reset_finger_positions_and_velocities(data['robot'][i].position)
frame_observed = video_recorder.get_views()
frame_observed = cv2.cvtColor(frame_observed, cv2.COLOR_RGB2BGR)
frame_real = np.concatenate(data['images'][i], axis=1)
frame_real_cube = np.concatenate(cube_drawer.add_cube(data['images'][i],
data['cube'][i]),
axis=1)
frame = vstack_frames((frame_desired, frame_observed, frame_real, frame_real_cube))
# frame = np.concatenate((frame_desired, frame_observed,
# frame_real, frame_real_cube), axis=0)
# add text
frame = add_text(frame, text="step: {:06d}".format(t), position=(10, 40))
frame = add_text(frame, text="acc reward: {:.3f}".format(data["acc_reward"][i]), position=(10, 70))
frame = add_text(
frame,
text="tip force {}".format(
np.array2string(data["robot"][i].tip_force, precision=3),
),
position=(10, 100),
)
video_recorder.add_frame(frame)
video_recorder.save_video(video_path)
def compute_reward_adaptive_behind(logdir, simulation, TOTALTIMESTEPS):
with open(os.path.join(logdir, "goal.json"), 'r') as f:
goal = json.load(f)
difficulty = goal['difficulty']
goal_pose = move_cube.Pose(position=np.array(goal['goal']['position']),
orientation=np.array(
goal['goal']['orientation']))
indice = goal['reachstart']
if (indice == -1):
return -10000, False
if (simulation == 0):
min_length = TOTALTIMESTEPS
log = robot_fingers.TriFingerPlatformLog(
os.path.join(logdir, "robot_data.dat"),
os.path.join(logdir, "camera_data.dat"))
reward = 0.0
count = 0
for t in range(log.get_last_timeindex() - TOTALTIMESTEPS,
log.get_last_timeindex() + 1):
count += 1
camera_observation = log.get_camera_observation(t)
reward -= evaluate_state(goal_pose,
camera_observation.filtered_object_pose,
difficulty)
if (count == min_length):
break
if (count == min_length):
return reward, True
else:
return -10000, False
else:
min_length = TOTALTIMESTEPS
path = os.path.join(logdir, 'observations.pkl')
with open(path, 'rb') as handle:
observations = pkl.load(handle)
reward = 0.0
count = 0
ex_state = move_cube.sample_goal(difficulty=-1)
for i in range(
len(observations) - TOTALTIMESTEPS - 1, len(observations)):
count += 1
ex_state.position = observations[i]["achieved_goal"]["position"]
ex_state.orientation = observations[i]["achieved_goal"][
"orientation"]
reward -= evaluate_state(goal_pose, ex_state, difficulty)
if (count == min_length):
break
if (count == min_length):
return reward, True
else:
return -10000, False
def compute_reward(logdir):
log = robot_fingers.TriFingerPlatformLog(os.path.join(logdir, "robot_data.dat"),
os.path.join(logdir, "camera_data.dat"))
with open(os.path.join(logdir, "goal.json"), 'r') as f:
goal = json.load(f)
difficulty = goal['difficulty']
goal_pose = move_cube.Pose(position=np.array(goal['goal']['position']),
orientation=np.array(goal['goal']['orientation']))
reward = 0.0
for t in range(log.get_first_timeindex(), log.get_last_timeindex() + 1):
camera_observation = log.get_camera_observation(t)
reward -= move_cube.evaluate_state(
goal_pose, camera_observation.filtered_object_pose, difficulty
)
return reward
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 reset(self):
# By changing the `_reset_*` method below you can switch between using
# the platform frontend, which is needed for the submission system, and
# the direct simulation, which may be more convenient if you want to
# pre-train locally in simulation.
self._reset_platform_frontend()
if self.sim_platform:
del self.sim_platform
initial_robot_position = TriFingerPlatform.spaces.robot_position.default
default_object_position = (
TriFingerPlatform.spaces.object_position.default)
default_object_orientation = (
TriFingerPlatform.spaces.object_orientation.default)
dummy_initial_object_pose = move_cube.Pose(
position=default_object_position,
orientation=default_object_orientation,
)
self.sim_platform = TriFingerPlatform(
visualization=False,
initial_robot_position=initial_robot_position,
initial_object_pose=dummy_initial_object_pose,
)
# self.goal_marker = visual_objects.CubeMarker(
# width=0.065,
# position=self.goal["position"],
# orientation=self.goal["orientation"],
# physicsClientId=self.sim_platform.simfinger._pybullet_client_id,
# )
# self._reset_direct_simulation()
self.step_count = 0
# need to already do one step to get initial observation
# TODO disable frameskip here?
observation, _, _, _ = self.step(self._initial_action)
return observation
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))
class TriFingerPlatform:
"""
Wrapper around the simulation providing the same interface as
``robot_interfaces::TriFingerPlatformFrontend``.
The following methods of the robot_interfaces counterpart are not
supported:
- get_robot_status()
- wait_until_timeindex()
"""
# Create the action and observation spaces
# ========================================
_n_joints = 9
_n_fingers = 3
_max_torque_Nm = 0.36
_max_velocity_radps = 10
spaces = SimpleNamespace()
spaces.robot_torque = SimpleNamespace(
low=np.full(_n_joints, -_max_torque_Nm, dtype=np.float32),
high=np.full(_n_joints, _max_torque_Nm, dtype=np.float32),
default=np.zeros(_n_joints, dtype=np.float32),
)
spaces.robot_position = SimpleNamespace(
low=np.array([-0.9, -1.57, -2.7] * _n_fingers, dtype=np.float32),
high=np.array([1.4, 1.57, 0.0] * _n_fingers, dtype=np.float32),
default=np.array(
[0.0, np.deg2rad(70), np.deg2rad(-130)] * _n_fingers,
dtype=np.float32,
),
)
spaces.robot_velocity = SimpleNamespace(
low=np.full(_n_joints, -_max_velocity_radps, dtype=np.float32),
high=np.full(_n_joints, _max_velocity_radps, dtype=np.float32),
default=np.zeros(_n_joints, dtype=np.float32),
)
spaces.object_position = SimpleNamespace(
low=np.array([-0.3, -0.3, 0], dtype=np.float32),
high=np.array([0.3, 0.3, 0.3], dtype=np.float32),
default=np.array([0, 0, move_cube._min_height], dtype=np.float32),
)
spaces.object_orientation = SimpleNamespace(
low=-np.ones(4, dtype=np.float32),
high=np.ones(4, dtype=np.float32),
default=move_cube.Pose().orientation,
)
# for convenience, we also create the respective gym spaces
spaces.robot_torque.gym = gym.spaces.Box(low=spaces.robot_torque.low,
high=spaces.robot_torque.high)
spaces.robot_position.gym = gym.spaces.Box(low=spaces.robot_position.low,
high=spaces.robot_position.high)
spaces.robot_velocity.gym = gym.spaces.Box(low=spaces.robot_velocity.low,
high=spaces.robot_velocity.high)
spaces.object_position.gym = gym.spaces.Box(
low=spaces.object_position.low, high=spaces.object_position.high)
spaces.object_orientation.gym = gym.spaces.Box(
low=spaces.object_orientation.low, high=spaces.object_orientation.high)
def __init__(
self,
visualization=False,
initial_robot_position=None,
initial_object_pose=None,
enable_cameras=False,
):
"""Initialize.
Args:
visualization (bool): Set to true to run visualization.
initial_robot_position: Initial robot joint angles
initial_object_pose: Initial pose for the manipulation object.
Can be any object with attributes ``position`` (x, y, z) and
``orientation`` (x, y, z, w). This is optional, if not set, a
random pose will be sampled.
enable_cameras (bool): Set to true to enable camera observations.
By default this is disabled as rendering of images takes a lot
of computational power. Therefore the cameras should only be
enabled if the images are actually used.
"""
#: Camera rate in frames per second. Observations of camera and
#: object pose will only be updated with this rate.
#: NOTE: This is currently not used!
self._camera_rate_fps = 30
#: Set to true to render camera observations
self.enable_cameras = enable_cameras
#: Simulation time step
self._time_step = 0.004
# first camera update in the first step
self._next_camera_update_step = 0
# Initialize robot, object and cameras
# ====================================
self.simfinger = SimFinger(
finger_type="trifingerpro",
time_step=self._time_step,
enable_visualization=visualization,
)
if initial_robot_position is None:
initial_robot_position = self.spaces.robot_position.default
self.simfinger.reset_finger_positions_and_velocities(
initial_robot_position)
if initial_object_pose is None:
initial_object_pose = move_cube.Pose(
position=self.spaces.object_position.default,
orientation=self.spaces.object_orientation.default,
)
self.cube = collision_objects.Block(
initial_object_pose.position,
initial_object_pose.orientation,
mass=0.020,
)
self.tricamera = camera.TriFingerCameras()
# Forward some methods for convenience
# ====================================
# forward "RobotFrontend" methods directly to simfinger
self.Action = self.simfinger.Action
self.get_desired_action = self.simfinger.get_desired_action
self.get_applied_action = self.simfinger.get_applied_action
self.get_timestamp_ms = self.simfinger.get_timestamp_ms
self.get_current_timeindex = self.simfinger.get_current_timeindex
self.get_robot_observation = self.simfinger.get_observation
# forward kinematics directly to simfinger
self.forward_kinematics = (
self.simfinger.pinocchio_utils.forward_kinematics)
# Initialize log
# ==============
self._action_log = {
"initial_robot_position": initial_robot_position.tolist(),
"initial_object_pose": {
"position": initial_object_pose.position.tolist(),
"orientation": initial_object_pose.orientation.tolist(),
},
"actions": [],
}
def get_time_step(self):
"""Get simulation time step in seconds."""
return self._time_step
def _compute_camera_update_step_interval(self):
return (1.0 / self._camera_rate_fps) / self._time_step
def append_desired_action(self, action):
"""
Call :meth:`pybullet.SimFinger.append_desired_action` and add the
action to the action log.
Arguments/return value are the same as for
:meth:`pybullet.SimFinger.append_desired_action`.
"""
# update camera and object observations only with the rate of the
# cameras
# next_t = self.get_current_timeindex() + 1
# has_camera_update = next_t >= self._next_camera_update_step
# if has_camera_update:
# self._next_camera_update_step += (
# self._compute_camera_update_step_interval()
# )
# self._object_pose_t = self._get_current_object_pose()
# if self.enable_cameras:
# self._camera_observation_t = (
# self._get_current_camera_observation()
# )
has_camera_update = True
self._object_pose_t = self._get_current_object_pose()
if self.enable_cameras:
self._camera_observation_t = self._get_current_camera_observation()
t = self.simfinger.append_desired_action(action)
# The correct timestamp can only be acquired now that t is given.
# Update it accordingly in the object and camera observations
if has_camera_update:
camera_timestamp_s = self.get_timestamp_ms(t) / 1000
self._object_pose_t.timestamp = camera_timestamp_s
if self.enable_cameras:
for i in range(len(self._camera_observation_t.cameras)):
self._camera_observation_t.cameras[
i].timestamp = camera_timestamp_s
# write the desired action to the log
self._action_log["actions"].append({
"t":
t,
"torque":
action.torque.tolist(),
"position":
action.position.tolist(),
"position_kp":
action.position_kp.tolist(),
"position_kd":
action.position_kd.tolist(),
})
return t
def _get_current_object_pose(self, t=None):
cube_state = self.cube.get_state()
pose = ObjectPose()
pose.position = np.asarray(cube_state[0])
pose.orientation = np.asarray(cube_state[1])
pose.confidence = 1.0
# NOTE: The timestamp can only be set correctly after time step t is
# actually reached. Therefore, this is set to None here and filled
# with the proper value later.
if t is None:
pose.timestamp = None
else:
pose.timestamp = self.get_timestamp_ms(t)
return pose
def get_object_pose(self, t):
"""Get object pose at time step t.
Args:
t: The time index of the step for which the object pose is
requested. Only the value returned by the last call of
:meth:`~append_desired_action` is valid.
Returns:
ObjectPose: Pose of the object. Values come directly from the
simulation without adding noise, so the confidence is 1.0.
Raises:
ValueError: If invalid time index ``t`` is passed.
"""
current_t = self.simfinger._t
if t < 0:
raise ValueError("Cannot access time index less than zero.")
elif t == current_t:
return self._object_pose_t
elif t == current_t + 1:
return self._get_current_object_pose(t)
else:
raise ValueError(
"Given time index t has to match with index of the current"
" step or the next one.")
def _get_current_camera_observation(self, t=None):
images = self.tricamera.get_images()
observation = TriCameraObservation()
# NOTE: The timestamp can only be set correctly after time step t
# is actually reached. Therefore, this is set to None here and
# filled with the proper value later.
if t is None:
timestamp = None
else:
timestamp = self.get_timestamp_ms(t)
for i, image in enumerate(images):
observation.cameras[i].image = image
observation.cameras[i].timestamp = timestamp
return observation
def get_camera_observation(self, t):
"""Get camera observation at time step t.
Args:
t: The time index of the step for which the observation is
requested. Only the value returned by the last call of
:meth:`~append_desired_action` is valid.
Returns:
TriCameraObservation: Observations of the three cameras. Images
are rendered in the simulation. Note that they are not optimized
to look realistically.
Raises:
ValueError: If invalid time index ``t`` is passed.
"""
if not self.enable_cameras:
raise RuntimeError(
"Cameras are not enabled. Create `TriFingerPlatform` with"
" `enable_cameras=True` if you want to use camera"
" observations.")
current_t = self.simfinger._t
if t < 0:
raise ValueError("Cannot access time index less than zero.")
elif t == current_t:
return self._camera_observation_t
elif t == current_t + 1:
return self._get_current_camera_observation(t)
else:
raise ValueError(
"Given time index t has to match with index of the current"
" step or the next one.")
def store_action_log(self, filename):
"""Store the action log to a JSON file.
Args:
filename (str): Path to the JSON file to which the log shall be
written. If the file exists already, it will be overwritten.
"""
# TODO should the log also contain intermediate observations (object
# and finger) for verification?
t = self.simfinger.get_current_timeindex()
object_pose = self.get_object_pose(t)
self._action_log["final_object_pose"] = {
"position": object_pose.position.tolist(),
"orientation": object_pose.orientation.tolist(),
}
with open(filename, "w") as fh:
json.dump(self._action_log, fh)
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(logdir, video_path):
custom_drawer = CustomDrawer(os.path.join(logdir, 'user/custom_data'))
goal, difficulty = get_goal(logdir)
data = get_synced_log_data(logdir, goal, difficulty)
fps = len(data['t']) / (data['stamp'][-1] - data['stamp'][0])
video_recorder = VideoRecorder(fps)
cube_drawer = CubeDrawer(logdir)
initial_object_pose = move_cube.Pose(data['cube'][0].position,
data['cube'][0].orientation)
platform = trifinger_simulation.TriFingerPlatform(
visualization=False,
initial_object_pose=initial_object_pose,
)
markers = []
marker_cube_ori = VisualCubeOrientation(data['cube'][0].position,
data['cube'][0].orientation)
marker_goal_ori = VisualCubeOrientation(goal['position'],
goal['orientation'])
visual_objects.CuboidMarker(
size=move_cube._CUBOID_SIZE,
position=goal['position'],
orientation=goal['orientation'],
pybullet_client_id=platform.simfinger._pybullet_client_id,
)
# if 'grasp_target_cube_pose' in custom_log:
# markers.append(
# visual_objects.CubeMarker(
# width=0.065,
# position=custom_log['grasp_target_cube_pose']['position'],
# orientation=custom_log['grasp_target_cube_pose']['orientation'],
# color=(0, 0, 1, 0.5),
# pybullet_client_id=platform.simfinger._pybullet_client_id,
# )
# )
# if 'pregrasp_tip_positions' in custom_log:
# for tip_pos in custom_log['pregrasp_tip_positions']:
# print(tip_pos)
# markers.append(
# SphereMarker(
# radius=0.015,
# position=tip_pos,
# color=(0, 1, 1, 0.5)
# )
# )
# if 'failure_target_tip_positions' in custom_log:
# for tip_pos in custom_log['failure_target_tip_positions']:
# print(tip_pos)
# markers.append(
# SphereMarker(
# radius=0.015,
# position=tip_pos,
# color=(1, 0, 0, 0.5)
# )
# )
# if 'pitch_grasp_positions' in custom_log:
# for tip_pos in custom_log['pitch_grasp_positions']:
# print(tip_pos)
# markers.append(
# SphereMarker(
# radius=0.015,
# position=tip_pos,
# color=(1, 1, 1, 0.5)
# )
# )
# if 'yaw_grasp_positions' in custom_log:
# for tip_pos in custom_log['yaw_grasp_positions']:
# print(tip_pos)
# markers.append(
# SphereMarker(
# radius=0.015,
# position=tip_pos,
# color=(0, 0, 0, 0.5)
# )
# )
p.configureDebugVisualizer(p.COV_ENABLE_GUI, 0)
p.resetDebugVisualizerCamera(cameraDistance=0.6,
cameraYaw=0,
cameraPitch=-40,
cameraTargetPosition=[0, 0, 0])
for i, t in enumerate(data['t']):
platform.simfinger.reset_finger_positions_and_velocities(
data['desired_action'][i].position)
platform.cube.set_state(data['cube'][i].position,
data['cube'][i].orientation)
marker_cube_ori.set_state(data['cube'][i].position,
data['cube'][i].orientation)
custom_drawer.draw(t)
frame_desired = video_recorder.get_views()
frame_desired = cv2.cvtColor(frame_desired, cv2.COLOR_RGB2BGR)
platform.simfinger.reset_finger_positions_and_velocities(
data['robot'][i].position)
frame_observed = video_recorder.get_views()
frame_observed = cv2.cvtColor(frame_observed, cv2.COLOR_RGB2BGR)
frame_real = np.concatenate(data['images'][i], axis=1)
frame_real_cube = np.concatenate(cube_drawer.add_cube(
data['images'][i], data['cube'][i]),
axis=1)
frame = vstack_frames(
(frame_desired, frame_observed, frame_real, frame_real_cube))
# frame = np.concatenate((frame_desired, frame_observed,
# frame_real, frame_real_cube), axis=0)
# add text
frame = add_text(frame,
text="step: {:06d}".format(t),
position=(10, 40))
frame = add_text(frame,
text="acc reward: {:.3f}".format(
data["acc_reward"][i]),
position=(10, 70))
frame = add_text(
frame,
text="tip force {}".format(
np.array2string(data["robot"][i].tip_force, precision=3), ),
position=(10, 100),
)
video_recorder.add_frame(frame)
video_recorder.save_video(video_path)
def test_get_cube_corner_positions():
# cube half width
chw = move_cube._CUBE_WIDTH / 2
# no transformation
expected_origin_corners = np.array([
[-chw, -chw, -chw],
[-chw, -chw, +chw],
[-chw, +chw, -chw],
[-chw, +chw, +chw],
[+chw, -chw, -chw],
[+chw, -chw, +chw],
[+chw, +chw, -chw],
[+chw, +chw, +chw],
])
origin = move_cube.Pose()
origin_corners = move_cube.get_cube_corner_positions(origin)
np.testing.assert_array_almost_equal(expected_origin_corners,
origin_corners)
# only translation
expected_translated_corners = np.array([
[-chw + 1, -chw + 2, -chw + 3],
[-chw + 1, -chw + 2, +chw + 3],
[-chw + 1, +chw + 2, -chw + 3],
[-chw + 1, +chw + 2, +chw + 3],
[+chw + 1, -chw + 2, -chw + 3],
[+chw + 1, -chw + 2, +chw + 3],
[+chw + 1, +chw + 2, -chw + 3],
[+chw + 1, +chw + 2, +chw + 3],
])
translated = move_cube.get_cube_corner_positions(
move_cube.Pose([1, 2, 3], [0, 0, 0, 1]))
np.testing.assert_array_almost_equal(expected_translated_corners,
translated)
# only rotation
rot_z90 = Rotation.from_euler("z", 90, degrees=True).as_quat()
expected_rotated_corners = np.array([
[+chw, -chw, -chw],
[+chw, -chw, +chw],
[-chw, -chw, -chw],
[-chw, -chw, +chw],
[+chw, +chw, -chw],
[+chw, +chw, +chw],
[-chw, +chw, -chw],
[-chw, +chw, +chw],
])
rotated = move_cube.get_cube_corner_positions(
move_cube.Pose([0, 0, 0], rot_z90))
np.testing.assert_array_almost_equal(expected_rotated_corners, rotated)
# both rotation and translation
expected_both_corners = np.array([
[+chw + 1, -chw + 2, -chw + 3],
[+chw + 1, -chw + 2, +chw + 3],
[-chw + 1, -chw + 2, -chw + 3],
[-chw + 1, -chw + 2, +chw + 3],
[+chw + 1, +chw + 2, -chw + 3],
[+chw + 1, +chw + 2, +chw + 3],
[-chw + 1, +chw + 2, -chw + 3],
[-chw + 1, +chw + 2, +chw + 3],
])
both = move_cube.get_cube_corner_positions(
move_cube.Pose([1, 2, 3], rot_z90))
np.testing.assert_array_almost_equal(expected_both_corners, both)
def __init__(
self,
visualization=False,
initial_robot_position=None,
initial_object_pose=None,
enable_cameras=False,
):
"""Initialize.
Args:
visualization (bool): Set to true to run visualization.
initial_robot_position: Initial robot joint angles
initial_object_pose: Initial pose for the manipulation object.
Can be any object with attributes ``position`` (x, y, z) and
``orientation`` (x, y, z, w). This is optional, if not set, a
random pose will be sampled.
enable_cameras (bool): Set to true to enable camera observations.
By default this is disabled as rendering of images takes a lot
of computational power. Therefore the cameras should only be
enabled if the images are actually used.
"""
#: Camera rate in frames per second. Observations of camera and
#: object pose will only be updated with this rate.
#: NOTE: This is currently not used!
self._camera_rate_fps = 30
#: Set to true to render camera observations
self.enable_cameras = enable_cameras
#: Simulation time step
self._time_step = 0.004
# first camera update in the first step
self._next_camera_update_step = 0
# Initialize robot, object and cameras
# ====================================
self.simfinger = SimFinger(
finger_type="trifingerpro",
time_step=self._time_step,
enable_visualization=visualization,
)
if initial_robot_position is None:
initial_robot_position = self.spaces.robot_position.default
self.simfinger.reset_finger_positions_and_velocities(
initial_robot_position)
if initial_object_pose is None:
initial_object_pose = move_cube.Pose(
position=self.spaces.object_position.default,
orientation=self.spaces.object_orientation.default,
)
self.cube = collision_objects.Block(
initial_object_pose.position,
initial_object_pose.orientation,
mass=0.020,
)
self.tricamera = camera.TriFingerCameras()
# Forward some methods for convenience
# ====================================
# forward "RobotFrontend" methods directly to simfinger
self.Action = self.simfinger.Action
self.get_desired_action = self.simfinger.get_desired_action
self.get_applied_action = self.simfinger.get_applied_action
self.get_timestamp_ms = self.simfinger.get_timestamp_ms
self.get_current_timeindex = self.simfinger.get_current_timeindex
self.get_robot_observation = self.simfinger.get_observation
# forward kinematics directly to simfinger
self.forward_kinematics = (
self.simfinger.pinocchio_utils.forward_kinematics)
# Initialize log
# ==============
self._action_log = {
"initial_robot_position": initial_robot_position.tolist(),
"initial_object_pose": {
"position": initial_object_pose.position.tolist(),
"orientation": initial_object_pose.orientation.tolist(),
},
"actions": [],
}
