def __init__(self, variation):
self.wp0 = Dummy('waypoint0')
self.wp1 = Dummy('waypoint1')
self.button_wp0 = Dummy('target_button0')
self.joint0 = Joint('target_button_joint0')
if variation == '2button':
self.button_wp1 = Dummy('target_button1')
self.joint1 = Joint('target_button_joint1')
def init_task(self) -> None:
self.anchors = [Dummy('waypoint_anchor_%s' % opt)
for opt in OPTIONS]
self.joints = [Joint('drawer_joint_%s' % opt)
for opt in OPTIONS]
self.waypoint1 = Dummy('waypoint1')
self.item = Shape('item')
self.register_graspable_objects([self.item])
def init_task(self) -> None:
self.groceries = [Shape(name.replace(' ', '_'))
for name in GROCERY_NAMES]
self.grasp_points = [Dummy('%s_grasp_point' % name.replace(' ', '_'))
for name in GROCERY_NAMES]
self.waypoint1 = Dummy('waypoint1')
self.register_graspable_objects(self.groceries)
self.boundary = SpawnBoundary([Shape('workspace')])
def test_solve_ik_via_jacobian(self):
arm = Panda()
waypoint = Dummy('Panda_waypoint')
new_config = arm.solve_ik_via_jacobian(
waypoint.get_position(), waypoint.get_orientation())
arm.set_joint_positions(new_config)
self.assertTrue(np.allclose(
arm.get_tip().get_pose(), waypoint.get_pose(), atol=0.001))
def test_get_linear_path_and_get_end(self):
arm = Panda()
waypoint = Dummy('Panda_waypoint')
path = arm.get_linear_path(
waypoint.get_position(), waypoint.get_orientation())
path.set_to_end()
self.assertTrue(np.allclose(
arm.get_tip().get_position(), waypoint.get_position(), atol=0.001))
class SnakeRobot(RobotComponent):
"""Base class representing a snake-liked robot with movement support.
"""
def __init__(self,
count: int,
name: str,
num_joints: int,
base_name: str = None,
max_velocity=1.0,
max_acceleration=4.0,
max_jerk=1000):
"""
Count is used for when we have multiple copies of arms
:param max_jerk: The second derivative of the velocity.
"""
joint_names = ['%s_joint%d' % (name, i + 1) for i in range(num_joints)]
super().__init__(count, name, joint_names, base_name)
# Used for movement
self.max_velocity = max_velocity
self.max_acceleration = max_acceleration
self.max_jerk = max_jerk
# handles
suffix = '' if count == 1 else '#%d' % (count - 2)
self._snake_head = Dummy('%s_head%s' % (name, suffix))
self._snake_tail = Dummy('%s_tail%s' % (name, suffix))
self._collision_collection = sim.simGetCollectionHandle(
'%s_collection%s' % (name, suffix))
def get_snake_head(self):
return self._snake_head
def get_snake_tail(self):
return self._snake_tail
def get_snake_head_pos(self):
return self._snake_head.get_position()
def get_snake_tail_pos(self):
return self._snake_tail.get_position()
def get_snake_joints_pos(self):
pos = [
self.joints[i].get_position()
for i in range(self.get_joint_count())
]
return pos
def get_snake_angle(self):
head_pos = self.get_snake_head_pos()[:2]
tail_pos = self.get_snake_tail_pos()[:2]
k = (tail_pos[1] - head_pos[1]) / (tail_pos[0] - head_pos[0] + 1e-8)
angle_rad = np.arctan(k)
return angle_rad
def init_state(self):
pass
def KinovaArm_setCenterOfTool(self, pose, referencedTo):
target = Dummy("target")
parent_frame_object = Shape('gen3')
position = target.get_position(parent_frame_object)
#target.set_position([position[0] + pose.x / 1000, position[1] + pose.y / 1000, position[2] + pose.z / 1000], parent_frame_object)
target.set_position([
position[0] + pose.x / 1000, position[1] + pose.y / 1000,
position[2] + pose.z / 1000
], parent_frame_object)
def test_merge_objects(self):
top = Dummy('cubes_under_dummy')
self.assertEqual(
len(top.get_objects_in_tree(exclude_base=True)), 3)
cubes = [Shape('cube%d' % i) for i in range(3)]
ob = self.pyrep.merge_objects(cubes)
self.assertIsInstance(ob, Object)
self.assertEqual(
len(top.get_objects_in_tree(exclude_base=True)), 1)
class LightBulbIn(Task):
def init_task(self) -> None:
self.bulbs_visual = [Shape('light_bulb%d' % i) for i in range(2)]
self.bulb_glass_visual = [Shape('bulb%d' % i) for i in range(2)]
self.holders = [Shape('bulb_holder%d' % i) for i in range(2)]
self.bulbs = [Shape('bulb_phys%d' % i) for i in range(2)]
self.conditions = [NothingGrasped(self.robot.gripper)]
self.register_graspable_objects(self.bulbs)
self.boundary = Shape('spawn_boundary')
def init_episode(self, index: int) -> List[str]:
self._variation_index = index
b = SpawnBoundary([self.boundary])
for holder in self.holders:
b.sample(holder, min_distance=0.01)
self.w1 = Dummy('waypoint1')
self.w1.set_position([0, 0, +10*10**(-3)],
relative_to=self.bulb_glass_visual[index % 2],
reset_dynamics=False)
target_color_name, target_color_rgb = colors[index]
color_choice = np.random.choice(
list(range(index)) + list(
range(index + 1, len(colors))),
size=1, replace=False)[0]
_, distractor_color_rgb = colors[color_choice]
self.holders[index % 2].set_color(target_color_rgb)
other_index = {0: 1, 1: 0}
self.holders[other_index[index % 2]].set_color(distractor_color_rgb)
self.register_success_conditions([DetectedCondition(
self.bulbs[index % 2],
ProximitySensor('success')),
NothingGrasped(self.robot.gripper)])
return ['screw in the %s light bulb' % target_color_name,
'screw the light bulb from the %s holder into the lamp'
% target_color_name,
'pick up the light bulb from the %s stand, lift it up to just '
'above the lamp, then screw it down into the lamp in a '
'clockwise fashion' % target_color_name,
'put the light bulb from the %s casing into the lamp'
% target_color_name]
def variation_count(self) -> int:
return len(colors)
def step(self) -> None:
if DetectedCondition(self.bulbs[self._variation_index % 2],
ProximitySensor('success')).condition_met() \
== (True, True):
self.bulb_glass_visual[self._variation_index % 2].set_color(
[1.0, 1.0, 0.0])
def cleanup(self) -> None:
if self.bulb_glass_visual:
[self.bulb_glass_visual[i].set_color([1.0, 1.0, 1.0])
for i in range(2)]
def __init__(self,
task_class,
observation_mode='state',
render_mode: Union[None, str] = None):
self._observation_mode = observation_mode
self._render_mode = render_mode
obs_config = ObservationConfig()
if observation_mode == 'state':
obs_config.set_all_high_dim(False)
obs_config.set_all_low_dim(True)
elif observation_mode == 'vision':
obs_config.set_all(True)
else:
raise ValueError('Unrecognised observation_mode: %s.' %
observation_mode)
action_mode = ActionMode(ArmActionMode.ABS_JOINT_VELOCITY)
self.env = Environment(action_mode,
obs_config=obs_config,
headless=True)
self.env.launch()
self.task = self.env.get_task(task_class)
_, obs = self.task.reset()
self.action_space = spaces.Box(low=-1.0,
high=1.0,
shape=(self.env.action_size, ))
if observation_mode == 'state':
self.observation_space = spaces.Box(
low=-np.inf, high=np.inf, shape=obs.get_low_dim_data().shape)
elif observation_mode == 'vision':
self.observation_space = spaces.Dict({
"state":
spaces.Box(low=-np.inf,
high=np.inf,
shape=obs.get_low_dim_data().shape),
"left_shoulder_rgb":
spaces.Box(low=0, high=1, shape=obs.left_shoulder_rgb.shape),
"right_shoulder_rgb":
spaces.Box(low=0, high=1, shape=obs.right_shoulder_rgb.shape),
"wrist_rgb":
spaces.Box(low=0, high=1, shape=obs.wrist_rgb.shape),
"front_rgb":
spaces.Box(low=0, high=1, shape=obs.front_rgb.shape),
})
if render_mode is not None:
# Add the camera to the scene
cam_placeholder = Dummy('cam_cinematic_placeholder')
self._gym_cam = VisionSensor.create([640, 360])
self._gym_cam.set_pose(cam_placeholder.get_pose())
if render_mode == 'human':
self._gym_cam.set_render_mode(RenderMode.OPENGL3_WINDOWED)
else:
self._gym_cam.set_render_mode(RenderMode.OPENGL3)
def __init__(self, headless, control_mode='joint_velocity'):
self.headless = headless
self.reward_offset = 10.0
self.reward_range = self.reward_offset
self.penalty_offset = 1
#self.penalty_offset = 1.
self.fall_down_offset = 0.1
self.metadata = [] #gym env argument
self.control_mode = control_mode
#launch and setup the scene, and set the proxy variables in present of the counterparts in the scene
self.pr = PyRep()
if control_mode == 'end_position':
SCENE_FILE = join(dirname(abspath(__file__)),
'./scenes/UnicarRobot_ik.ttt')
elif control_mode == 'joint_velocity':
SCENE_FILE = join(dirname(abspath(__file__)),
'./scenes/UnicarRobot.ttt')
self.pr.launch(SCENE_FILE, headless=headless)
self.pr.start()
self.agent = UnicarRobotArm() #drehkranz + UR10
#self.gripper = UnicarRobotGreifer()#suction
#self.suction = UnicarRobotGreifer()
self.suction = Shape("UnicarRobotGreifer_body_sub0")
self.proximity_sensor = ProximitySensor('UnicarRobotGreifer_sensor')
self.table = Shape('UnicarRobotTable')
self.carbody = Shape('UnicarRobotCarbody')
if control_mode == 'end_position':
self.agent.set_control_loop_enabled(True)
self.action_space = np.zeros(4)
elif control_mode == 'joint_velocity':
self.agent.set_control_loop_enabled(False)
self.action_space = np.zeros(7)
else:
raise NotImplementedError
#self.observation_space = np.zeros(17)
self.observation_space = np.zeros(20)
self.agent.set_motor_locked_at_zero_velocity(True)
self.target = Shape("UnicarRobotTarget") #Box
self.agent_ee_tip = self.agent.get_tip()
self.tip_target = Dummy('UnicarRobotArm_target')
self.tip_pos = self.agent_ee_tip.get_position()
# set a proper initial robot gesture or tip position
if control_mode == 'end_position':
initial_pos = [0, 1.5, 1.6]
self.tip_target.set_position(initial_pos)
#one big step for rotatin is enough, with reset_dynamics = True, set the rotation instantaneously
self.tip_target.set_orientation([0, 0, 0], reset_dynamics=True)
elif control_mode == 'joint_velocity':
self.initial_joint_positions = [0, 0, 0, 0, 0, 0, 0]
self.agent.set_joint_positions(self.initial_joint_positions)
self.pr.step()
self.initial_tip_positions = self.agent_ee_tip.get_position()
self.initial_target_positions = self.target.get_position()
def test_get_linear_path_and_get_end(self):
mobile = YouBot()
waypoint = Dummy('youBot_waypoint')
path = mobile.get_linear_path(waypoint.get_position(),
waypoint.get_orientation()[-1])
path.set_to_end()
self.assertTrue(
np.allclose(mobile.get_position()[:2],
waypoint.get_position()[:2],
atol=0.001))
def init_task(self) -> None:
self.large_container = Shape('large_container')
self.target_container0 = Shape('small_container0')
self.target_container1 = Shape('small_container1')
self.success_detector0 = ProximitySensor('success0')
self.success_detector1 = ProximitySensor('success1')
self.target_waypoint = Dummy('waypoint3')
self.spawn_boundary = SpawnBoundary([Shape('spawn_boundary')])
self.register_waypoint_ability_start(1, self._move_above_object)
self.register_waypoints_should_repeat(self._repeat)
self.bin_objects = []
def __init__(self, ep_length=100):
"""
Pollos environment for testing purposes
:param dim: (int) the size of the dimensions you want to learn
:param ep_length: (int) the length of each episodes in timesteps
"""
# 5 points in 3D space forming the trajectory
self.action_space = Box(low=-0.3, high=0.3, shape=(5,3), dtype=np.float32)
self.observation_space = Box(low=np.array([-0.1, -0.2, 0, -1.5, 0.0, 0.0, -0.5, -0.2, -0.2]),
high=np.array([0.1, 1.7, 2.5, 0, 0.0, 0.0, 0.5, 0.2, 1.0]))
#
self.pr = PyRep()
self.pr.launch(SCENE_FILE, headless=False)
self.pr.start()
self.agent = UR10()
self.agent.max_velocity = 1.2
self.joints = [Joint('UR10_joint1'), Joint('UR10_joint2'), Joint('UR10_joint3'), Joint('UR10_joint4'), Joint('UR10_joint5'), Joint('UR10_joint6')]
self.joints_limits = [[j.get_joint_interval()[1][0],j.get_joint_interval()[1][0]+j.get_joint_interval()[1][1]]
for j in self.joints]
print(self.joints_limits)
self.agent_hand = Shape('hand')
self.agent.set_control_loop_enabled(True)
self.agent.set_motor_locked_at_zero_velocity(True)
self.initial_agent_tip_position = self.agent.get_tip().get_position()
self.initial_agent_tip_quaternion = self.agent.get_tip().get_quaternion()
self.target = Dummy('UR10_target')
self.initial_joint_positions = self.agent.get_joint_positions()
self.pollo_target = Dummy('pollo_target')
self.pollo = Shape('pollo')
self.table_target = Dummy('table_target')
self.initial_pollo_position = self.pollo.get_position()
self.initial_pollo_orientation = self.pollo.get_quaternion()
self.table_target = Dummy('table_target')
# objects
self.scene_objects = [Shape('table0'), Shape('Plane'), Shape('Plane0'), Shape('ConcretBlock')]
#self.agent_tip = self.agent.get_tip()
self.initial_distance = np.linalg.norm(np.array(self.initial_pollo_position)-np.array(self.initial_agent_tip_position))
# camera
self.camera = VisionSensor('kinect_depth')
self.camera_matrix_extrinsics = vrep.simGetObjectMatrix(self.camera.get_handle(),-1)
self.np_camera_matrix_extrinsics = np.delete(np.linalg.inv(self.vrepToNP(self.camera_matrix_extrinsics)), 3, 0)
width = 640.0
height = 480.0
angle = math.radians(57.0)
focalx_px = (width/2.0) / math.tan(angle/2.0)
focaly_px = (height/2.0) / math.tan(angle/2.0)
self.np_camera_matrix_intrinsics = np.array([[-focalx_px, 0.0, 320.0],
[0.0, -focalx_px, 240.0],
[0.0, 0.0, 1.0]])
self.reset()
def init_task(self) -> None:
self._remote = Shape('tv_remote')
self._joint_conditions = [
JointCondition(Joint('target_button_joint1'), 0.003),
JointCondition(Joint('target_button_joint2'), 0.003)
]
self._w6 = Dummy('waypoint6')
self._w6z = self._w6.get_position()[2]
self.register_graspable_objects([self._remote])
self._spawn_boundary = SpawnBoundary([Shape('spawn_boundary')])
self._target_buttons = [
Shape('target_button_wrap1'), Shape('target_button_wrap2')]
def test_get_linear_path_and_step(self):
arm = Panda()
waypoint = Dummy('Panda_waypoint')
path = arm.get_linear_path(
waypoint.get_position(), waypoint.get_orientation())
self.assertIsNotNone(path)
done = False
while not done:
done = path.step()
self.pyrep.step()
self.assertTrue(np.allclose(
arm.get_tip().get_position(), waypoint.get_position(), atol=0.01))
def create_object(obj_path, scaling_factor=0.001):
obj_name, ext = get_file_name(obj_path)
if not ext == ".obj":
print("[ERROR] please check obj file {}".format(obj_path))
exit()
obj = Shape.import_mesh(obj_path, scaling_factor=scaling_factor)
frame = Dummy.create()
frame.set_parent(obj)
instruction_frame = Dummy.create()
instruction_frame.set_position([0, 0, 0], relative_to=obj)
instruction_frame.set_parent(obj)
return ObjObject(obj, frame, instruction_frame)
def KinovaArm_getCenterOfTool(self, referencedTo):
ret = RoboCompKinovaArm.TPose()
parent_frame_object = Shape('gen3')
tip = Dummy("tip")
pos = tip.get_position(parent_frame_object)
rot = tip.get_orientation(parent_frame_object)
ret.x = pos[0]
ret.y = pos[1]
ret.z = pos[2]
ret.rx = rot[0]
ret.ry = rot[1]
ret.rz = rot[2]
return ret
def __init__(self, count: int, num_propellers: int,
distance_from_target: float, name: str):
"""Count is used for when we have multiple copies of mobile bases.
:param count: used for multiple copies of robots
:param num_propellers: number of actuated propellers
:param name: string with robot name (same as base in vrep model).
"""
joint_names = [
'%s_propeller_joint%s' % (name, str(i + 1))
for i in range(num_propellers)
]
super().__init__(count, name, joint_names)
self.dist1 = distance_from_target
self.num_propellers = num_propellers
self.static_propeller_velocity = 5.335
self.pParam = 2
self.iParam = 0
self.dParam = 0
self.vParam = -2
self.cumul = 0
self.lastE = 0
self.pAlphaE = 0
self.pBetaE = 0
self.psp2 = 0
self.psp1 = 0
self.prevEuler = 0
suffix = '' if count == 0 else '#%d' % (count - 1)
propeller_names = [
'%s_propeller_respondable%s%s' % (name, str(i + 1), suffix)
for i in range(self.num_propellers)
]
self.propellers = self.get_propeller_handles(
propeller_names) # use handles instead of shapes here
# Motion planning handles
self.intermediate_target_base = Dummy('%s_intermediate_target_base%s' %
(name, suffix))
self.target_base = Shape('%s_target_base%s' % (name, suffix))
self._object_base = vrep.simGetObjectHandle(
'%s_base%s' % (name, suffix)
) # object instead of collection of objects for quadracopter
# Robot parameters and handle
self.z_pos = self.get_position()[2]
self.target_z = self.target_base.get_position()[-1]
def _move_above_next_target(self, waypoint):
self.checkers_placed += 1
self.target_index = self.target_indexes[self.checkers_placed]
if self.checkers_placed > self.checkers_to_setup:
raise RuntimeError('Should not be here')
w1 = Dummy('waypoint1')
# self.w2.set_parent(target_checkers[self.checkers_placed]
unplaced_x, unplaced_y, unplaced_z = self.target_checkers[
self.checkers_placed].get_position()
z_offset_pickup = 0
w1.set_position([unplaced_x, unplaced_y, unplaced_z - z_offset_pickup],
reset_dynamics=False)
w4 = Dummy('waypoint4')
target_x, target_y, target_z = self.checkers_starting_pos_list[
self.target_index]
z_offset_placement = 1.00 * 10**(-3)
w4.set_position([target_x - z_offset_placement, target_y, target_z],
relative_to=self.chess_board,
reset_dynamics=False)
if self.checkers_to_setup > 1:
if self.target_index == self.target_indexes[0]:
self.target_index = self.target_indexes[1]
if self.target_index == self.target_indexes[1] \
and self.checkers_to_setup == 3:
self.target_index = self.target_indexes[2]
def _move_above_next_target(self, waypoint):
if self.cups_placed > self.cups_to_place:
raise RuntimeError('Should not be here, all cups should have been '
'placed')
move_index = self.cups_placed if self.cups_placed > -1 else 0
next_move_index = self.cups_placed + 1 if self.cups_placed > -1 else 0
if self.cups_placed > -1:
w4 = Dummy('waypoint4')
w4_x, w4_y, w4_z = w4.get_position(
relative_to=self.spokes[move_index])
w4_alpha, w4_beta, w4_gamma = w4.get_orientation(
relative_to=self.spokes[move_index])
self.w1.set_position(self.w1_rel_pos,
relative_to=self.cups[next_move_index],
reset_dynamics=False)
self.w1.set_orientation(self.w1_rel_ori,
relative_to=self.cups[next_move_index],
reset_dynamics=False)
w4.set_position([w4_x, w4_y, w4_z],
relative_to=self.spokes[next_move_index],
reset_dynamics=False)
w4.set_orientation([w4_alpha, w4_beta, w4_gamma],
relative_to=self.spokes[next_move_index],
reset_dynamics=False)
######
self.cups_placed += 1
def test_get_linear_path_and_step(self):
mobile = YouBot()
waypoint = Dummy('youBot_waypoint')
path = mobile.get_linear_path(waypoint.get_position(),
waypoint.get_orientation()[-1])
self.assertIsNotNone(path)
done = False
while not done:
done = path.step()
self.pyrep.step()
self.assertTrue(
np.allclose(mobile.get_position()[:2],
waypoint.get_position()[:2],
atol=0.001))
def init_task(self) -> None:
self.left_start = Dummy('waypoint0')
self.left_end = Dummy('waypoint1')
self.right_start = Dummy('waypoint5')
self.right_end = Dummy('waypoint6')
self.left_joint = Joint('left_joint')
self.right_joint = Joint('right_joint')
def init_task(self) -> None:
self.drops = []
self.cup_target_base = Dummy('cup_target_base')
self.cup_source = Shape('cup_source')
self.cup_target = Shape('cup_target')
self.cup_source_visual = Shape('cup_source_visual')
self.cup_target_visual = Shape('cup_target_visual')
self.distractors = [Shape('cup_distractor%d' % i) for i in range(3)]
self.distractors_vis = [
Shape('cup_distractor_visual%d' % i) for i in range(3)
]
self.success_detector = ProximitySensor('success')
self.register_graspable_objects([self.cup_source])
def test_get_objects_in_tree(self):
with warnings.catch_warnings(record=True) as w:
warnings.simplefilter('always')
objects = self.pyrep.get_objects_in_tree()
self.assertNotEqual(len(w), 0)
for obj in objects:
self.assertIsInstance(obj, Object)
dummys = [Dummy('nested_dummy%d' % i) for i in range(3)]
for root_obj in [dummys[0], dummys[0].get_handle()]:
objects = self.pyrep.get_objects_in_tree(
root_obj, exclude_base=False, first_generation_only=False)
self.assertListEqual(objects, dummys)
for obj in objects:
self.assertIs(type(obj), Dummy)
self.assertListEqual(
self.pyrep.get_objects_in_tree(root_obj,
exclude_base=True,
first_generation_only=False),
dummys[1:])
self.assertListEqual(
self.pyrep.get_objects_in_tree(root_obj,
exclude_base=False,
first_generation_only=True),
dummys[:-1])
def to_type(handle: int) -> Object:
"""Converts an object handle to the correct sub-type.
:param handle: The internal handle of an object.
:return: The sub-type of this object.
"""
t = sim.simGetObjectType(handle)
if t == sim.sim_object_shape_type:
return Shape(handle)
elif t == sim.sim_object_dummy_type:
return Dummy(handle)
elif t == sim.sim_object_path_type:
return CartesianPath(handle)
elif t == sim.sim_object_joint_type:
return Joint(handle)
elif t == sim.sim_object_visionsensor_type:
return VisionSensor(handle)
elif t == sim.sim_object_forcesensor_type:
return ForceSensor(handle)
elif t == sim.sim_object_proximitysensor_type:
return ProximitySensor(handle)
elif t == sim.sim_object_camera_type:
return Camera(handle)
elif t == sim.sim_object_octree_type:
return Octree(handle)
raise ValueError
def init_task(self) -> None:
self.shelf = {0: 'bottom', 1: 'middle', 2: 'top'}
self.shelf_alt = {0: 'first', 1: 'second', 2: 'third'}
self.money_list = [Shape('dollar_stack%d' % i) for i in range(3)]
self.register_graspable_objects(self.money_list)
self.success_conditions = [NothingGrasped(self.robot.gripper)]
self.w1 = Dummy('waypoint1')
self.w1_rel_pos = [
-2.7287 * 10**(-4), -2.3246 * 10**(-6), +4.5627 * 10**(-2)
]
self.w1_rel_ori = [-3.1416, 7.2824 * 10**(-1), -2.1265 * 10**(-2)]
self.w3 = Dummy('waypoint3')
self.place_boundary = Shape('placement_boundary')
self.pick_boundary = Shape('money_boundary')
self.safe = Shape('safe_body')
self.money_ori = self.money_list[0].get_orientation()
def init_episode(self, index: int) -> List[str]:
self.target_time_index = index
target_times_dic = {0: '12:15', 1: '12:30', 2: '14:45'}
target_time_str = target_times_dic[self.target_time_index]
success_detector_lst = [
ProximitySensor('detector_hour%d' % self.target_time_index),
ProximitySensor('detector_minute%d' % self.target_time_index)
]
while len(self.success_conditions) > 1:
self.success_conditions.pop()
self.success_conditions += \
[DetectedCondition(self.needle_hour, success_detector_lst[0]),
DetectedCondition(self.needle_minute, success_detector_lst[1])]
self.register_success_conditions(self.success_conditions)
self.w2 = Dummy('waypoint2')
w2_starting_ori = self.w2.get_orientation(
relative_to=self.needle_pivot)
w2_target_ori = w2_starting_ori * 6
w2_target_ori[2] -= self.target_rotation_dic[self.target_time_index]
self.w2.set_orientation(w2_target_ori,
relative_to=self.needle_pivot,
reset_dynamics=False)
_, _, w2_set_gamma = self.w2.get_orientation(
relative_to=self.needle_pivot)
self.w2_gamma_delta = w2_set_gamma - w2_starting_ori[2]
self.current_15_angle = 0
self.total_rotation_min = 0
self.lots_of_15 = 0
self.lock_updates = 0
return [
'change the clock to show time %s' % target_time_str,
'adjust the time to %s' % target_time_str,
'change the clock to %s' % target_time_str,
'set the clock to %s' % target_time_str,
'turn the knob on the back of the clock until the time shows %s' %
target_time_str,
'rotate the wheel on the clock to make it show %s' %
target_time_str,
'make the clock say %s' % target_time_str
]
def init_task(self) -> None:
self.needle = Shape('scales_meter_needle')
self.needle_pivot = Shape('scales_meter_pivot')
self.top_plate = Shape('scales_tray_visual')
self.joint = Joint('scales_joint')
_, _, starting_z = self.top_plate.get_position()
self.top_plate_starting_z = starting_z
self.needle_starting_ori = self.needle.get_orientation(
relative_to=self.needle_pivot)
self.peppers = [Shape('pepper%d' % i) for i in range(3)]
self.register_graspable_objects(self.peppers)
self.boundary = Shape('peppers_boundary')
self.success_detector = ProximitySensor('success_detector')
self.success_conditions = [NothingGrasped(self.robot.gripper)]
self.w0 = Dummy('waypoint0')
self.w0_rel_pos = self.w0.get_position(relative_to=self.peppers[0])
self.w0_rel_ori = self.w0.get_orientation(relative_to=self.peppers[0])
def init_episode(self, index: int) -> List[str]:
self.target_shelf = index
w4 = Dummy('waypoint4')
target_dummy_name = 'dummy_shelf' + str(self.target_shelf)
target_pos_dummy = Dummy(target_dummy_name)
target_pos = target_pos_dummy.get_position()
w4.set_position(target_pos, reset_dynamics=False)
self.success_detector = ProximitySensor(
('success_detector' + str(self.target_shelf))
)
while len(self.success_conditions) > 1:
self.success_conditions.pop()
self.success_conditions.append(
DetectedCondition(self.money, self.success_detector)
)
self.register_success_conditions(self.success_conditions)
b = SpawnBoundary([self.money_boundary])
b.sample(self.money,
min_rotation=(0.00, 0.00, 0.00),
max_rotation=(0.00, 0.00, +0.5 * np.pi))
return ['put the money away in the safe on the %s shelf'
% self.index_dic[index],
'leave the money on the %s shelf on the safe'
% self.index_dic[index],
'place the stack of bank notes on the %s shelf of the safe'
% self.index_dic[index]]
