def __init__(self):
self.time_step = 0.004
self.finger = sim_finger.SimFinger(finger_type=Cage.args.finger_type,
time_step=self.time_step,
enable_visualization=True,)
self.num_fingers = self.finger.number_of_fingers
_kwargs = {"physicsClientId": self.finger._pybullet_client_id}
if Cage.args.control_mode == "position":
self.position_goals = visual_objects.Marker(number_of_goals=self.num_fingers)
self.initial_robot_position=np.array([0.0, np.deg2rad(-60), np.deg2rad(-60)] * 3, dtype=np.float32,)
self.finger.reset_finger_positions_and_velocities(self.initial_robot_position)
self.initial_object_pose = move_cube.Pose(position=[0, 0, move_cube._min_height],
orientation=move_cube.Pose().orientation)
self.cube = collision_objects.Block(self.initial_object_pose.position,
self.initial_object_pose.orientation, mass=0.020, **_kwargs)
self.cube_z = 0.0325
self.workspace_radius = 0.18
self.tricamera = camera.TriFingerCameras(**_kwargs)
self.initial_marker_position = np.array([[ 0. , 0.17856407, self.cube_z],
[ 0.15464102, -0.08928203, self.cube_z],
[-0.15464102, -0.08928203, self.cube_z]])
self.position_goals.set_state(self.initial_marker_position)
self.marker_position = self.initial_marker_position
self.ik_module = geometric_ik.GeometricIK()
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 compute_reward(self, achieved_goal, desired_goal, info):
"""Compute the reward for the given achieved and desired goal.
Args:
achieved_goal : Current pose of the object.
desired_goal : Goal pose of the object.
info : An info dictionary containing a field "difficulty"
which specifies the difficulty level.
Returns:
float: The reward that corresponds to the provided achieved goal
w.r.t. to the desired goal. Note that the following should always
hold true::
ob, reward, done, info = env.step()
assert reward == env.compute_reward(
ob['achieved_goal'],
ob['desired_goal'],
info,
)
"""
if self.sparse_reward:
return -np.float32((move_cube.evaluate_state(
move_cube.Pose(desired_goal[0:3], desired_goal[3:7]),
move_cube.Pose(achieved_goal[0:3], achieved_goal[3:7]),
info['difficulty'],
) > 0.01))
else:
return move_cube.evaluate_state(
move_cube.Pose.from_dict(desired_goal),
move_cube.Pose.from_dict(achieved_goal),
info['difficulty'],
)
def _competition_reward(observation, difficulty):
object_pose = move_cube.Pose(
observation['object_position'],
observation['object_orientation']
)
goal_pose = move_cube.Pose(
observation['goal_object_position'],
observation['goal_object_orientation']
)
return -move_cube.evaluate_state(goal_pose, object_pose, difficulty)
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,
physicsClientId=self.platform.simfinger._pybullet_client_id,
)
self.info = {"difficulty": 1}
self.step_count = 0
return self._create_observation(0)
def test_validate_goal(self):
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:
self.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:
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, half_width], [0, 0, 0, 1])
)
with self.assertRaises(move_cube.InvalidGoalError):
move_cube.validate_goal(
move_cube.Pose([0, -0.3, half_width], [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, half_width], full_rotation)
)
def test_get_cube_corner_positions(self):
# 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)
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 __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": [],
}
