def test_sensor_noise_robot(self):
obs_config = ObservationConfig(
joint_velocities_noise=GaussianNoise(0.01),
joint_forces=False,
task_low_dim_state=False)
scene = Scene(self.pyrep, self.robot, obs_config)
scene.load(ReachTarget(self.pyrep, self.robot))
obs1 = scene.get_observation()
obs2 = scene.get_observation()
self.assertTrue(
np.array_equal(obs1.joint_positions, obs2.joint_positions))
self.assertFalse(
np.array_equal(obs1.joint_velocities, obs2.joint_velocities))
def test_sensor_noise_images(self):
cam_config = CameraConfig(rgb_noise=GaussianNoise(0.05, (.0, 1.)))
obs_config = ObservationConfig(left_shoulder_camera=cam_config,
joint_forces=False,
task_low_dim_state=False)
scene = Scene(self.pyrep, self.robot, obs_config)
scene.load(ReachTarget(self.pyrep, self.robot))
obs1 = scene.get_observation()
obs2 = scene.get_observation()
self.assertTrue(
np.array_equal(obs1.right_shoulder_rgb, obs2.right_shoulder_rgb))
self.assertFalse(
np.array_equal(obs1.left_shoulder_rgb, obs2.left_shoulder_rgb))
self.assertTrue(obs1.left_shoulder_rgb.max() <= 1.0)
self.assertTrue(obs1.left_shoulder_rgb.min() >= 0.0)
print('(+) run the simulator')
print('(n) for new task.')
print('(s) to save the .ttm')
print('(r) to rename the task')
inp = input()
if inp == 'q':
break
if pr.running:
if inp == '+':
pr.stop()
pr.step_ui()
elif inp == 'p':
[(pr.step(), scene.get_observation()) for _ in range(100)]
elif inp == 'd':
loaded_task.new_demo()
elif inp == 'v':
loaded_task.new_variation()
elif inp == 'e':
loaded_task.new_episode()
else:
if inp == '+':
loaded_task.reload_python()
loaded_task.reset_variation()
pr.start()
pr.step_ui()
elif inp == 'n':
inp = input('Do you want to save the current task first?\n')
if inp == 'y':
class EnvironmentImpl(Environment):
"""Each environment has a scene."""
@staticmethod
def all_task_names():
return tuple(o[0] for o in getmembers(tasks) if isclass(o[1]))
def __init__(self, task_name: str, obs_config: ObservationConfig = ObservationConfig(task_low_dim_state=True),
action_mode: ActionMode = ActionMode(),
arm_name: str = "Panda", gripper_name: str = "Panda_gripper"):
super(EnvironmentImpl, self).__init__(task_name)
self._arm_name = arm_name
self._gripper_name = gripper_name
self._action_mode = action_mode
self._obs_config = obs_config
# TODO: modify the task/robot/arm/gripper to support early instantiation before v-rep launched
self._task = None
self._pyrep = None
self._robot = None
self._scene = None
self._variation_number = 0
self._reset_called = False
self._prev_ee_velocity = None
self._update_info_dict()
def init(self, display=False):
if self._pyrep is not None:
self.finalize()
with suppress_std_out_and_err():
self._pyrep = PyRep()
# TODO: TTT_FILE should be defined by robot, but now robot depends on launched pyrep
self._pyrep.launch(join(DIR_PATH, TTT_FILE), headless=not display)
self._pyrep.set_simulation_timestep(0.005)
# TODO: Load arm and gripper from name
self._robot = Robot(Panda(), PandaGripper())
self._scene = Scene(self._pyrep, self._robot, self._obs_config)
self._set_arm_control_action()
# Str comparison because sometimes class comparison doesn't work.
if self._task is not None:
self._task.unload()
self._task = self._get_class_by_name(self._task_name, tasks)(self._pyrep, self._robot)
self._scene.load(self._task)
self._pyrep.start()
def finalize(self):
with suppress_std_out_and_err():
self._pyrep.shutdown()
self._pyrep = None
def reset(self, random: bool = True) -> StepDict:
logging.info('Resetting task: %s' % self._task.get_name())
self._scene.reset()
try:
# TODO: let desc be constant
desc = self._scene.init_episode(self._variation_number, max_attempts=MAX_RESET_ATTEMPTS, randomly_place=random)
except (BoundaryError, WaypointError) as e:
raise TaskEnvironmentError(
'Could not place the task %s in the scene. This should not '
'happen, please raise an issues on this task.'
% self._task.get_name()) from e
ctr_loop = self._robot.arm.joints[0].is_control_loop_enabled()
locked = self._robot.arm.joints[0].is_motor_locked_at_zero_velocity()
self._robot.arm.set_control_loop_enabled(False)
self._robot.arm.set_motor_locked_at_zero_velocity(True)
self._reset_called = True
self._robot.arm.set_control_loop_enabled(ctr_loop)
self._robot.arm.set_motor_locked_at_zero_velocity(locked)
# Returns a list o f descriptions and the first observation
return {'s': self._scene.get_observation().get_low_dim_data(), "opt": desc}
def step(self, last_step: StepDict) -> (StepDict, bool):
# returns observation, reward, done, info
if not self._reset_called:
raise RuntimeError("Call 'reset' before calling 'step' on a task.")
assert 'a' in last_step, "Key 'a' for action not in last_step, maybe you passed a wrong dict ?"
# action should contain 1 extra value for gripper open close state
arm_action = np.array(last_step['a'][:-1])
ee_action = last_step['a'][-1]
current_ee = (1.0 if self._robot.gripper.get_open_amount()[0] > 0.9 else 0.0)
if ee_action > 0.0:
ee_action = 1.0
elif ee_action < -0.0:
ee_action = 0.0
if self._action_mode.arm == ArmActionMode.ABS_JOINT_VELOCITY:
self._assert_action_space(arm_action, (len(self._robot.arm.joints),))
self._robot.arm.set_joint_target_velocities(arm_action)
elif self._action_mode.arm == ArmActionMode.DELTA_JOINT_VELOCITY:
self._assert_action_space(arm_action, (len(self._robot.arm.joints),))
cur = np.array(self._robot.arm.get_joint_velocities())
self._robot.arm.set_joint_target_velocities(cur + arm_action)
elif self._action_mode.arm == ArmActionMode.ABS_JOINT_POSITION:
self._assert_action_space(arm_action, (len(self._robot.arm.joints),))
self._robot.arm.set_joint_target_positions(arm_action)
elif self._action_mode.arm == ArmActionMode.DELTA_JOINT_POSITION:
self._assert_action_space(arm_action, (len(self._robot.arm.joints),))
cur = np.array(self._robot.arm.get_joint_positions())
self._robot.arm.set_joint_target_positions(cur + arm_action)
elif self._action_mode.arm == ArmActionMode.ABS_EE_POSE:
self._assert_action_space(arm_action, (7,))
self._ee_action(list(arm_action))
elif self._action_mode.arm == ArmActionMode.DELTA_EE_POSE:
self._assert_action_space(arm_action, (7,))
a_x, a_y, a_z, a_qx, a_qy, a_qz, a_qw = arm_action
x, y, z, qx, qy, qz, qw = self._robot.arm.get_tip().get_pose()
new_rot = Quaternion(a_qw, a_qx, a_qy, a_qz) * Quaternion(qw, qx, qy, qz)
qw, qx, qy, qz = list(new_rot)
new_pose = [a_x + x, a_y + y, a_z + z] + [qx, qy, qz, qw]
self._ee_action(list(new_pose))
elif self._action_mode.arm == ArmActionMode.ABS_EE_VELOCITY:
self._assert_action_space(arm_action, (7,))
pose = self._robot.arm.get_tip().get_pose()
new_pos = np.array(pose) + (arm_action * DT)
self._ee_action(list(new_pos))
elif self._action_mode.arm == ArmActionMode.DELTA_EE_VELOCITY:
self._assert_action_space(arm_action, (7,))
if self._prev_ee_velocity is None:
self._prev_ee_velocity = np.zeros((7,))
self._prev_ee_velocity += arm_action
pose = self._robot.arm.get_tip().get_pose()
pose = np.array(pose)
new_pose = pose + (self._prev_ee_velocity * DT)
self._ee_action(list(new_pose))
elif self._action_mode.arm == ArmActionMode.ABS_JOINT_TORQUE:
self._assert_action_space(arm_action, (len(self._robot.arm.joints),))
self._torque_action(arm_action)
elif self._action_mode.arm == ArmActionMode.DELTA_JOINT_TORQUE:
cur = np.array(self._robot.arm.get_joint_forces())
new_action = cur + arm_action
self._torque_action(new_action)
else:
raise RuntimeError('Unrecognised action mode.')
if current_ee != ee_action:
done = False
while not done:
done = self._robot.gripper.actuate(ee_action, velocity=0.04)
self._pyrep.step()
self._task.step()
if ee_action == 0.0:
# If gripper close action, the check for grasp.
for g_obj in self._task.get_graspable_objects():
self._robot.gripper.grasp(g_obj)
else:
# If gripper opem action, the check for ungrasp.
self._robot.gripper.release()
self._scene.step()
success, terminate = self._task.success()
last_step['r'] = int(success)
next_step = {'s': self._scene.get_observation().get_low_dim_data(), "opt": None}
return last_step, next_step, terminate
def name(self) -> str:
return self._task_name
# ------------- private methods ------------- #
def _update_info_dict(self):
# update info dict
self._info["action mode"] = self._action_mode
self._info["observation mode"] = self._obs_config
# TODO: action dim should related to robot, not action mode, here we fixed it temporally
self._info["action dim"] = (8,)
self._info["action low"] = np.zeros(self._info["action dim"], dtype=np.float32) - 1.
self._info["action high"] = np.zeros(self._info["action dim"], dtype=np.float32) + 1.
self._info["state dim"] = (73,)
self._info["state low"] = np.zeros(self._info["state dim"], dtype=np.float32) - 100.
self._info["state high"] = np.zeros(self._info["state dim"], dtype=np.float32) + 100.
self._info["reward low"] = -np.inf
self._info["reward high"] = np.inf
def _set_arm_control_action(self):
self._robot.arm.set_control_loop_enabled(True)
if self._action_mode.arm in (ArmActionMode.ABS_JOINT_VELOCITY, ArmActionMode.DELTA_JOINT_VELOCITY):
self._robot.arm.set_control_loop_enabled(False)
self._robot.arm.set_motor_locked_at_zero_velocity(True)
elif self._action_mode.arm in (ArmActionMode.ABS_JOINT_POSITION, ArmActionMode.DELTA_JOINT_POSITION,
ArmActionMode.ABS_EE_POSE, ArmActionMode.DELTA_EE_POSE,
ArmActionMode.ABS_EE_VELOCITY, ArmActionMode.DELTA_EE_VELOCITY):
self._robot.arm.set_control_loop_enabled(True)
elif self._action_mode.arm in (ArmActionMode.ABS_JOINT_TORQUE, ArmActionMode.DELTA_JOINT_TORQUE):
self._robot.arm.set_control_loop_enabled(False)
else:
raise RuntimeError('Unrecognised action mode.')
def sample_variation(self) -> int:
self._variation_number = np.random.randint(0, self._task.variation_count())
return self._variation_number
def _assert_action_space(self, action, expected_shape):
if np.shape(action) != expected_shape:
raise RuntimeError(
'Expected the action shape to be: %s, but was shape: %s' % (
str(expected_shape), str(np.shape(action))))
def _torque_action(self, action):
self._robot.arm.set_joint_target_velocities([(TORQUE_MAX_VEL if t < 0 else -TORQUE_MAX_VEL) for t in action])
self._robot.arm.set_joint_forces(np.abs(action))
def _ee_action(self, action):
try:
joint_positions = self._robot.arm.solve_ik(action[:3], quaternion=action[3:])
self._robot.arm.set_joint_target_positions(joint_positions)
except IKError as e:
raise InvalidActionError("Could not find a path.") from e
self._pyrep.step()
@staticmethod
def _get_class_by_name(class_name: str, model: ModuleType) -> TaskClass:
all_class_dict = {}
for o in getmembers(model):
if isclass(o[1]):
all_class_dict[o[0]] = o[1]
if class_name not in all_class_dict:
raise NotImplementedError(f"No class {class_name} found in {model.__name__} !")
return all_class_dict[class_name]