def init_episode(self, index: int) -> List[str]:
b = SpawnBoundary([self.boundary])
success = ProximitySensor('success')
for obj in self.jars:
b.sample(obj, min_distance=0.01)
w0 = Dummy('waypoint0')
w0.set_orientation([-np.pi, 0, -np.pi], reset_dynamics=False)
w0.set_position([0, 0, 0.1],
relative_to=self.lids[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.jars[index % 2].set_color(target_color_rgb)
other_index = {0: 1, 1: 0}
self.jars[other_index[index % 2]].set_color(distractor_color_rgb)
self.conditions += [DetectedCondition(self.lids[index % 2], success)]
self.register_success_conditions(self.conditions)
return [
'open the %s jar' % target_color_name,
'unscrew the %s jar' % target_color_name,
'grasp the lid of the %s jar, unscrew it in an anti_clockwise '
'direction until it is removed from the jar, and leave it on '
'the table top' % target_color_name,
'remove the lid from the %s jam jar and set it down on the '
'table' % target_color_name
]
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]
class PlaceHangerOnRack(Task):
def init_task(self) -> None:
self.hanger_holder = Shape('hanger_holder')
hanger = Shape('clothes_hanger')
success_detector = ProximitySensor('success_detector0')
self.register_success_conditions([
NothingGrasped(self.robot.gripper),
DetectedCondition(hanger, success_detector)
])
self.register_graspable_objects([hanger])
self.workspace_boundary = SpawnBoundary([Shape('workspace')])
def init_episode(self, index: int) -> List[str]:
self.workspace_boundary.clear()
self.workspace_boundary.sample(self.hanger_holder)
return [
'pick up the hanger and place in on the rack'
'put the hanger on the rack', 'hang the hanger up',
'grasp the hanger and hang it up'
]
def variation_count(self) -> int:
return 1
def is_static_workspace(self) -> bool:
return True
class PlugChargerInPowerSupply(Task):
def init_task(self) -> None:
sensor_right_hole = ProximitySensor('sensor1')
sensor_left_hole = ProximitySensor('sensor2')
charger = Shape('charger')
self.charger_base = Dummy('charger_base')
charger_right_stick = Shape('detector1')
charger_left_stick = Shape('detector2')
self.boundary = SpawnBoundary([Shape('charger_boundary')])
self.register_graspable_objects([charger])
self.register_success_conditions([
DetectedCondition(charger_right_stick, sensor_right_hole),
DetectedCondition(charger_left_stick, sensor_left_hole),
NothingGrasped(self.robot.gripper)
])
def init_episode(self, index: int) -> List[str]:
self.boundary.clear()
self.boundary.sample(self.charger_base)
return [
'plug charger in power supply',
'pick up the charger and plug in it',
'plug the charger into the mains',
'lift the charger up to the wall and plug it in',
'plug the charger into the wall'
]
def variation_count(self) -> int:
return 1
def base_rotation_bounds(self) -> Tuple[List[float], List[float]]:
return [0, 0, -3.14 / 4.], [0, 0, 3.14 / 4.]
class PutKnifeOnChoppingBoard(Task):
def init_task(self) -> None:
knife = Shape('knife')
success = ProximitySensor('success')
self.knife_block = Shape('knife_block')
self.chopping_board = Shape('chopping_board')
self.register_graspable_objects([knife])
cond_set = ConditionSet([DetectedCondition(knife, success),
NothingGrasped(self.robot.gripper)],
order_matters=True)
self.register_success_conditions([cond_set])
self.boundary = SpawnBoundary([Shape('boundary')])
def init_episode(self, index: int) -> List[str]:
self.boundary.clear()
self.boundary.sample(self.knife_block)
return ['put the knife on the chopping board',
'slide the knife out of the knife block and put it down on the '
'chopping board',
'place the knife on the chopping board',
'pick up the knife and leave it on the chopping board',
'move the knife from the holder to the chopping board']
def variation_count(self) -> int:
return 1
def init_episode(self, index: int) -> List[str]:
pillar0 = Shape('hannoi_square_pillar0')
pillar1 = Shape('hannoi_square_pillar1')
pillar2 = Shape('hannoi_square_pillar2')
spokes = [pillar0, pillar1, pillar2]
color_name, color_rgb = colors[index]
chosen_pillar = np.random.choice(spokes)
chosen_pillar.set_color(color_rgb)
_, _, z = self.success_centre.get_position()
x, y, _ = chosen_pillar.get_position()
self.success_centre.set_position([x, y, z])
color_choices = np.random.choice(list(range(index)) +
list(range(index + 1, len(colors))),
size=2,
replace=False)
spokes.remove(chosen_pillar)
for spoke, i in zip(spokes, color_choices):
name, rgb = colors[i]
spoke.set_color(rgb)
boundary1 = Shape('hannoi_square_boundary0')
square_ring = Shape('hannoi_square_ring')
b = SpawnBoundary([boundary1])
b.sample(square_ring)
return [
'put the ring on the %s spoke' % color_name,
'slide the ring onto the %s colored spoke' % color_name,
'place the ring onto the %s spoke' % color_name
]
def init_episode(self, index: int) -> List[str]:
b = SpawnBoundary([self.boundary])
for obj in self.jars:
b.sample(obj, min_distance=0.01)
success = ProximitySensor('success')
success.set_position([0.0, 0.0, 0.05],
relative_to=self.jars[index % 2],
reset_dynamics=False)
w3 = Dummy('waypoint3')
w3.set_orientation([-np.pi, 0, -np.pi], reset_dynamics=False)
w3.set_position([0.0, 0.0, 0.125],
relative_to=self.jars[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.jars[index % 2].set_color(target_color_rgb)
other_index = {0: 1, 1: 0}
self.jars[other_index[index % 2]].set_color(distractor_color_rgb)
self.conditions += [DetectedCondition(self.lid, success)]
self.register_success_conditions(self.conditions)
return [
'close the %s jar' % target_color_name,
'screw on the %s jar lid' % target_color_name,
'grasping the lid, lift it from the table and use it to seal '
'the %s jar' % target_color_name,
'pick up the lid from the table and put it on the %s jar' %
target_color_name
]
def init_episode(self, index: int) -> List[str]:
detectors = [ProximitySensor('success%d' % i) for i in range(2)]
self.register_success_conditions([
self.joint_conditions[index],
DetectedCondition(self.remote, detectors[0]),
DetectedCondition(self.remote, detectors[1])
])
self.register_graspable_objects([self.remote])
w6 = Dummy('waypoint6')
_, _, z = w6.get_position(relative_to=self.remote)
rel_pos_lst = [
Shape('target_button_wrap1').get_position(relative_to=self.remote),
Shape('target_button_wrap2').get_position(relative_to=self.remote)
]
rel_pos_lst[index % 2][2] = z
w6.set_position(rel_pos_lst[index],
relative_to=self.remote,
reset_dynamics=False)
b = SpawnBoundary([self.boundary])
b.sample(self.remote)
btn = {0: 'plus', 1: 'minus'}
chnl = {0: 'up', 1: 'minus'}
return [
'turn the channel %s' % chnl[index],
'change the television channel %s' % chnl[index],
'point the remote at the tv and press the %s button to turn '
'the channel %s' % (btn[index], chnl[index]),
'using the tv remote, ensure it is facing the television and '
'press the %s button to increment the channel %s by one' %
(btn[index], chnl[index]),
'find the %s button on the remote, rotate the remote such that'
' it is pointed at the tv, then press the button to change '
'the channel %s' % (chnl[index], btn[index])
]
def init_episode(self, index: int) -> List[str]:
color_name, color_rgb = colors[index]
self.target.set_color(color_rgb)
color_choices = np.random.choice(list(range(index)) +
list(range(index + 1, len(colors))),
size=2,
replace=False)
for ob, i in zip([self.distractor0, self.distractor1], color_choices):
name, rgb = colors[i]
ob.set_color(rgb)
b = SpawnBoundary([self.boundaries])
for ob in [self.target, self.distractor0, self.distractor1]:
b.sample(ob,
min_distance=0.2,
min_rotation=(0, 0, 0),
max_rotation=(0, 0, 0))
# change the position of the robot
q = self.robot.arm.get_joint_positions()
new_q = np.array([1.2, 0, 0, 0, 0, 0, 0]) + q
self.robot.arm.set_joint_positions(new_q)
self.step_in_episode = 0
return [
'reach the %s target' % color_name,
'touch the %s ball with the panda gripper' % color_name,
'reach the %s sphere' % color_name
]
class PutTrayInOven(Task):
def init_task(self) -> None:
success_detector = ProximitySensor('success')
self.tray = Shape('tray')
self.register_graspable_objects([self.tray])
self.register_success_conditions(
[DetectedCondition(self.tray, success_detector),
NothingGrasped(self.robot.gripper)])
self.boundary = SpawnBoundary([Shape('oven_tray_boundary')])
def init_episode(self, index: int) -> List[str]:
self.boundary.clear()
self.boundary.sample(
self.tray, min_rotation=(0, 0, 0), max_rotation=(0, 0, 0))
return ['put tray in oven',
'place the tray in the oven',
'open the oven, then slide the tray in',
'open the oven door, pick up the tray, and put it down on the '
'oven shelf']
def variation_count(self) -> int:
return 1
def base_rotation_bounds(self) -> Tuple[List[float], List[float]]:
return [0, 0, -3.14 / 4.], [0, 0, 3.14 / 4.]
def boundary_root(self) -> Object:
return Shape('oven_boundary_root')
class PutToiletRollOnStand(Task):
def init_task(self) -> None:
roll = Shape('toilet_roll')
self.boundary_roll = SpawnBoundary([Shape('boundary_roll')])
self.boundary_stand = SpawnBoundary([Shape('boundary_stand')])
self.roll_box = Shape('toilet_roll_box')
self.stand_base = Shape('stand_base')
success_sensor = ProximitySensor('success')
self.register_graspable_objects([roll])
self.register_success_conditions(
[DetectedCondition(roll, success_sensor),
NothingGrasped(self.robot.gripper)])
def init_episode(self, index: int) -> List[str]:
self.boundary_roll.clear()
self.boundary_stand.clear()
self.boundary_roll.sample(self.roll_box)
self.boundary_stand.sample(self.stand_base)
return ['put toilet roll on stand',
'place the toilet roll on the stand',
'slide the roll onto its holder']
def variation_count(self) -> int:
return 1
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]]
class TvOn(Task):
def init_task(self) -> None:
self._remote = Shape('tv_remote')
self._spawn_boundary = SpawnBoundary([Shape('spawn_boundary')])
self.register_graspable_objects([self._remote])
self.register_success_conditions(
[JointCondition(Joint('target_button_joint0'), 0.003)])
def init_episode(self, index: int) -> List[str]:
self._spawn_boundary.clear()
self._spawn_boundary.sample(self._remote)
return [
'turn on the TV',
'point the remote control at the television and turn on the '
'television',
'pick up the remote and rotate it such that the front of the '
'remote is pointed straight at the television, then set the '
'remote down and press the power button down in order to switch'
' on the TV',
'find the power button at the top of the remote, ensure the '
'remote is pointed at the tv, then turn the tv on'
]
def variation_count(self) -> int:
return 1
def base_rotation_bounds(
self
) -> Tuple[Tuple[float, float, float], Tuple[float, float, float]]:
return (0.0, 0.0, -0.5 * np.pi), (0.0, 0.0, +0.5 * np.pi)
class PutPlateInColoredDishRack(Task):
def init_task(self) -> None:
plate = Shape('plate')
self.dish_rack = Shape('dish_rack')
self.plate_stand = Shape('plate_stand')
self.success_sensor = ProximitySensor('success')
self.success_poses = [Dummy('success_pos%d' % i) for i in range(3)]
self.pillars = [Shape('dish_rack_pillar%d' % i) for i in range(6)]
self.boundary = SpawnBoundary([Shape('boundary')])
self.register_graspable_objects([plate])
cond_set = ConditionSet([
DetectedCondition(plate, self.success_sensor),
NothingGrasped(self.robot.gripper)
],
order_matters=True)
self.register_success_conditions([cond_set])
def init_episode(self, index: int) -> List[str]:
color_name, _ = colors[index]
shuffled_pillar_indexes = list(range(3))
np.random.shuffle(shuffled_pillar_indexes)
# Color the other spokes
color_choices = [index] + list(
np.random.choice(
list(range(index)) + list(range(index + 1, len(colors))),
size=2,
replace=False))
for pillar_i, color_i in zip(shuffled_pillar_indexes, color_choices):
_, rgb = colors[color_i]
self.pillars[pillar_i * 2].set_color(rgb)
self.pillars[1 + pillar_i * 2].set_color(rgb)
# Move the success detector to the correct spot
success_pos = self.success_poses[shuffled_pillar_indexes[0]]
self.success_sensor.set_position(success_pos.get_position())
self.success_sensor.set_orientation(success_pos.get_orientation())
self.boundary.clear()
self.boundary.sample(self.plate_stand,
min_rotation=(0, 0, 0.5),
max_rotation=(0, 0, 0.5))
self.boundary.sample(self.dish_rack, min_distance=0.2)
return [
'put the plate between the %s pillars of the dish rack' #
% color_name,
'place the plate in the the %s section of the drying rack' %
color_name,
'pick up the plate and leave it between the %s spokes on the '
'dish rack' % color_name
]
def variation_count(self) -> int:
return len(colors)
def base_rotation_bounds(self) -> Tuple[List[float], List[float]]:
return [0, 0, 0], [0, 0, 0]
def init_episode(self, index: int) -> List[str]:
color_name, color_rgb = colors[index]
self.target.set_color(color_rgb)
b = SpawnBoundary([self.boundaries])
for ob in [self.target]:
b.sample(ob,
min_distance=0.2,
min_rotation=(0, 0, 0),
max_rotation=(0, 0, 0))
return ['reach the %s target' % color_name]
class WipeDesk(Task):
def init_task(self) -> None:
self.dirt_spots = []
self.sponge = Shape('sponge')
self.sensor = ProximitySensor('sponge_sensor')
self.register_graspable_objects([self.sponge])
boundaries = [Shape('dirt_boundary')]
_, _, self.z_boundary = boundaries[0].get_position()
self.b = SpawnBoundary(boundaries)
def init_episode(self, index: int) -> List[str]:
self._place_dirt()
self.register_success_conditions([EmptyCondition(self.dirt_spots)])
return [
'wipe dirt off the desk', 'use the sponge to clean up the desk',
'remove the dirt from the desk',
'grip the sponge and wipe it back and forth over any dirt you '
'see', 'clean up the mess', 'wipe the dirt up'
]
def variation_count(self) -> int:
return 1
def step(self) -> None:
for d in self.dirt_spots:
if self.sensor.is_detected(d):
self.dirt_spots.remove(d)
d.remove()
def cleanup(self) -> None:
for d in self.dirt_spots:
d.remove()
self.dirt_spots = []
def _place_dirt(self):
for i in range(DIRT_POINTS):
spot = Shape.create(type=PrimitiveShape.CUBOID,
size=[.005, .005, .001],
mass=0,
static=True,
respondable=False,
renderable=True,
color=[0.58, 0.29, 0.0])
spot.set_parent(self.get_base())
spot.set_position([-1, -1, self.z_boundary + 0.001])
self.b.sample(spot,
min_distance=0.00,
min_rotation=(0.00, 0.00, 0.00),
max_rotation=(0.00, 0.00, 0.00))
self.dirt_spots.append(spot)
self.b.clear()
def init_episode(self, index: int) -> List[str]:
self.register_success_conditions([self.condition])
self.register_graspable_objects([self.remote])
b = SpawnBoundary([self.boundary])
b.sample(self.remote)
return ['turn on the TV',
'point the remote control at the television and turn on the '
'television',
'pick up the remote and rotate it such that the front of the '
'remote is pointed straight at the television, then set the '
'remote down and press the power button down in order to switch'
' on the TV',
'find the power button at the top of the remote, ensure the '
'remote is pointed at the tv, then turn the tv on']
class BlockPyramid(Task):
def init_task(self) -> None:
self.blocks = [Shape('block_pyramid_block%d' % i) for i in range(6)]
self.distractors = [
Shape('block_pyramid_distractor_block%d' % i) for i in range(6)
]
success_detectors = [
ProximitySensor('block_pyramid_success_block%d' % i)
for i in range(3)
]
cond_set = ConditionSet([
DetectedSeveralCondition(self.blocks, success_detectors[0], 3),
DetectedSeveralCondition(self.blocks, success_detectors[1], 2),
DetectedSeveralCondition(self.blocks, success_detectors[2], 1),
NothingGrasped(self.robot.gripper)
])
self.register_success_conditions([cond_set])
self.register_graspable_objects(self.blocks + self.distractors)
self.spawn_boundary = SpawnBoundary(
[Shape('block_pyramid_boundary%d' % i) for i in range(4)])
def init_episode(self, index: int) -> List[str]:
color_name, color_rgb = colors[index]
for obj in self.blocks:
obj.set_color(color_rgb)
color_choice = np.random.choice(list(range(index)) +
list(range(index + 1, len(colors))),
size=1,
replace=False)[0]
name, rgb = colors[color_choice]
for obj in self.distractors:
obj.set_color(rgb)
self.spawn_boundary.clear()
for ob in self.blocks + self.distractors:
self.spawn_boundary.sample(ob, min_distance=0.08)
return [
'stack %s blocks in a pyramid' % color_name,
'create a pyramid with the %s objects' % color_name,
'make a pyramid out of %s cubes' % color_name,
'position the %s blocks in the shape of a pyramid' % color_name,
'use the %s blocks to build a pyramid' % color_name
]
def variation_count(self) -> int:
return len(colors)
def init_episode(self, index: int) -> List[str]:
self.cups_placed = -1
self.cups_to_place = 1 + index % MAX_CUPS_TO_PLACE
self.w1.set_position(self.w1_rel_pos,
relative_to=self.cups[0],
reset_dynamics=False)
self.w1.set_orientation(self.w1_rel_ori,
relative_to=self.cups[0],
reset_dynamics=False)
b = SpawnBoundary([self.cups_boundary])
for c in self.cups:
b.sample(c,
ignore_collisions=False,
min_distance=0.10,
min_rotation=(0.00, 0.00, -3.14),
max_rotation=(0.00, 0.00, +3.14))
if c == self.cups[0]:
self.w1.set_position(self.w1_rel_pos,
relative_to=c,
reset_dynamics=False)
self.w1.set_orientation(self.w1_rel_ori,
relative_to=c,
reset_dynamics=False)
for i in range(self.cups_to_place):
self.success_conditions.append(
DetectedCondition(self.cups[i], self.success_detectors[i]))
self.register_success_conditions(self.success_conditions)
self.register_waypoint_ability_start(0, self._move_above_next_target)
self.register_waypoints_should_repeat(self._repeat)
if self.cups_to_place == 1:
return [
'place 1 cup on the cup holder'
'pick up one cup and put it on the mug tree',
'move 1 mug from the table to the cup holder',
'pick up one cup and slide its handle onto a spoke on the '
'mug holder'
]
else:
return [
'place %d cups on the cup holder' % self.cups_to_place,
'pick up %d cups and place them on the holder' %
self.cups_to_place,
'move %d cups from the table to the mug tree' %
self.cups_to_place,
'pick up %d mugs and slide their handles onto the cup '
'holder spokes' % self.cups_to_place
]
def init_episode(self, index: int) -> List[str]:
target_index = (index + 1) % self.variation_count()
source_name, source_rgb = colors[index]
target_name, target_rgb = colors[target_index]
self.cup_source_visual.set_color(source_rgb)
self.cup_target_visual.set_color(target_rgb)
options = (list(range(index)) + list(range(index + 1, len(colors))))
options.remove(target_index)
color_choices = np.random.choice(options, size=3, replace=False)
for obj, color_index in zip(self.distractors_vis, color_choices):
_, rgb = colors[color_index]
obj.set_color(rgb)
b = SpawnBoundary([Shape('boundary')])
b.sample(self.cup_source, min_distance=0.12)
b.sample(self.cup_target, min_distance=0.12)
[b.sample(d, min_distance=0.12) for d in self.distractors]
# Make the waypoints always be the same orientation
self.cup_target_base.set_orientation([0.] * 3)
self.drops = []
conditions = []
for i in range(LIQUID_BALLS):
drop = Shape.create(PrimitiveShape.SPHERE,
mass=0.0001,
size=[0.005, 0.005, 0.005])
drop.set_parent(self.cup_source)
drop.set_color([0.1, 0.1, 0.9])
drop.set_position(list(np.random.normal(0, 0.0005, size=(3, ))),
relative_to=self.cup_source)
self.drops.append(drop)
conditions.append(DetectedCondition(drop, self.success_detector))
self.register_success_conditions([ConditionSet(conditions)])
return [
'pour liquid from the %s cup to the %s cup' %
(source_name, target_name),
'pour liquid from the %s mug to the %s mug' %
(source_name, target_name),
'pour the contents of the %s mug into the %s one' %
(source_name, target_name),
'pick up the %s cup and pour the liquid into the %s one' %
(source_name, target_name)
]
class PutGroceriesInCupboard(Task):
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 init_episode(self, index: int) -> List[str]:
self.boundary.clear()
[self.boundary.sample(g, min_distance=0.1) for g in self.groceries]
self.waypoint1.set_pose(self.grasp_points[index].get_pose())
self.register_success_conditions(
[DetectedCondition(self.groceries[index],
ProximitySensor('success')),
NothingGrasped(self.robot.gripper)])
return ['put the %s in the cupboard' % GROCERY_NAMES[index],
'pick up the %s and place it in the cupboard'
% GROCERY_NAMES[index],
'move the %s to the bottom shelf' % GROCERY_NAMES[index],
'put away the %s in the cupboard' % GROCERY_NAMES[index]]
def variation_count(self) -> int:
return len(GROCERY_NAMES)
def boundary_root(self) -> Object:
return Shape('boundary_root')
def base_rotation_bounds(self) -> Tuple[Tuple[float, float, float],
Tuple[float, float, float]]:
return (0.0, 0.0, -1.), (0.0, 0.0, 1.)
class ChangeChannel(Task):
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 init_episode(self, index: int) -> List[str]:
detectors = [ProximitySensor('success%d' % i) for i in range(2)]
self.register_success_conditions([
self._joint_conditions[index],
DetectedCondition(self._remote, detectors[0]),
DetectedCondition(self._remote, detectors[1])
])
x, y, _ = self._target_buttons[index % 2].get_position()
self._w6.set_position([x, y, self._w6z])
self._spawn_boundary.clear()
self._spawn_boundary.sample(self._remote)
btn = ['plus', 'minus']
chnl = ['up', 'minus']
return ['turn the channel %s' % chnl[index],
'change the television channel %s' % chnl[index],
'point the remote at the tv and press the %s button to turn '
'the channel %s' % (btn[index], chnl[index]),
'using the tv remote, ensure it is facing the television and '
'press the %s button to increment the channel %s by one'
% (btn[index], chnl[index]),
'find the %s button on the remote, rotate the remote such that'
' it is pointed at the tv, then press the button to change '
'the channel %s' % (chnl[index], btn[index])]
def variation_count(self) -> int:
return 2
def base_rotation_bounds(self) -> Tuple[Tuple[float, float, float],
Tuple[float, float, float]]:
return (0.0, 0.0, -0.5*np.pi), (0.0, 0.0, +0.5 * np.pi)
def init_episode(self, index: int) -> List[str]:
color_name, color_rgb = colors[index]
self.target.set_color(color_rgb)
b = SpawnBoundary([self.boundaries])
for ob in [self.target]:
b.sample(ob,
min_distance=0.2,
min_rotation=(0, 0, 0),
max_rotation=(0, 0, 0))
joint_pos = [
-5.435585626401007e-05, 0, 3.4842159948311746e-05,
-2.453382730484009, 0.00016987835988402367, 3.0057222843170166,
0.785248875617981
]
gripper_pos = [0.0004903227090835571, 0.0007378421723842621]
self.robot.arm.set_joint_positions(joint_pos)
self.robot.gripper.set_joint_positions(gripper_pos)
return ['reach the %s target' % color_name]
def init_episode(self, index: int) -> List[str]:
self._variation_index = index
self.target_pepper_index = index
while len(self.success_conditions) > 1:
self.success_conditions.pop()
self.success_conditions.append(
DetectedCondition(self.peppers[self.target_pepper_index],
self.success_detector))
self.register_success_conditions(self.success_conditions)
b = SpawnBoundary([self.boundary])
for p in self.peppers:
b.sample(p,
ignore_collisions=False,
min_distance=0.05,
min_rotation=(0.00, 0.00, -3.14),
max_rotation=(0.00, 0.00, +3.14))
self.w0.set_position(
self.w0_rel_pos,
relative_to=self.peppers[self.target_pepper_index],
reset_dynamics=False)
self.w0.set_orientation(
self.w0_rel_ori,
relative_to=self.peppers[self.target_pepper_index],
reset_dynamics=False)
index_dic = {0: 'green', 1: 'red', 2: 'yellow'}
if self.target_pepper_index in range(3):
return [
'remove the %s pepper from the weighing scales and place it '
'on the table' % index_dic[self.target_pepper_index],
'take the %s pepper off of the scales' %
index_dic[self.target_pepper_index],
'lift the %s pepper off of the tray and set it down on the '
'table' % index_dic[self.target_pepper_index],
'grasp the %s pepper and move it to the table top' %
index_dic[self.target_pepper_index],
'take the %s object off of the scales tray' %
index_dic[self.target_pepper_index],
'put the %s item on the item' %
index_dic[self.target_pepper_index]
]
else:
raise ValueError('Invalid pepper index, should not be here')
return -1
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)
ProximitySensor('success').set_position(
[0, 0, 0],
relative_to=self.holders[index % 2],
reset_dynamics=False)
w1 = Dummy('waypoint1')
w1.set_orientation([-np.pi, 0, -np.pi], reset_dynamics=False)
w4 = Dummy('waypoint4')
w4.set_orientation([-np.pi, 0, +3.735 * 10**(-1)],
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.bulb, ProximitySensor('success')),
NothingGrasped(self.robot.gripper)
])
return [
'put the bulb in the %s holder' % target_color_name,
'screw the bulb out and leave it in the %s stand' %
target_color_name,
'remove the bulb from the lamp and put it in the %s stand' %
target_color_name,
'take the light bulb out of the lamp and place it in the %s '
'holder' % target_color_name,
'grasp the light bulb, twist it anti clockwise until it is no '
'longer attached to the lamp, and drop it into the %s stand' %
target_color_name
]
class PickUpCup(Task):
def init_task(self) -> None:
self.cup1 = Shape('cup1')
self.cup2 = Shape('cup2')
self.cup1_visual = Shape('cup1_visual')
self.cup2_visual = Shape('cup2_visual')
self.boundary = SpawnBoundary([Shape('boundary')])
self.success_sensor = ProximitySensor('success')
self.register_graspable_objects([self.cup1, self.cup2])
self.register_success_conditions([
DetectedCondition(self.cup1, self.success_sensor, negated=True),
GraspedCondition(self.robot.gripper, self.cup1),
])
def init_episode(self, index: int) -> List[str]:
self.variation_index = index
target_color_name, target_rgb = colors[index]
random_idx = np.random.choice(len(colors))
while random_idx == index:
random_idx = np.random.choice(len(colors))
_, other1_rgb = colors[random_idx]
self.cup1_visual.set_color(target_rgb)
self.cup2_visual.set_color(other1_rgb)
self.boundary.clear()
self.boundary.sample(self.cup2, min_distance=0.1)
self.boundary.sample(self.success_sensor, min_distance=0.1)
return [
'pick up the %s cup' % target_color_name,
'grasp the %s cup and lift it' % target_color_name,
'lift the %s cup' % target_color_name
]
def variation_count(self) -> int:
return len(colors)
def init_episode(self, index: int) -> List[str]:
self.target_pepper_index = index
while len(self.success_conditions) > 2:
self.success_conditions.pop()
self.success_conditions.append(
DetectedCondition(self.peppers[self.target_pepper_index],
self.success_detector))
self.register_success_conditions(self.success_conditions)
b = SpawnBoundary([self.boundary])
for p in self.peppers:
b.sample(p,
ignore_collisions=False,
min_distance=0.12,
min_rotation=(0.00, 0.00, -3.14),
max_rotation=(0.00, 0.00, +3.14))
self.w0.set_position(
self.w0_rel_pos,
relative_to=self.peppers[self.target_pepper_index],
reset_dynamics=False)
self.w0.set_orientation(
self.w0_rel_ori,
relative_to=self.peppers[self.target_pepper_index],
reset_dynamics=False)
pepper = {0: 'green', 1: 'red', 2: 'yellow'}
return [
'weigh the %s pepper' % pepper[index],
'pick up the %s pepper and set it down on the weighing scales' %
pepper[index],
'put the %s pepper on the scales' % pepper[index],
'place the %s pepper onto the scales tray' % pepper[index],
'lift up the %s pepper onto the weighing scales' % pepper[index],
'grasp the %s pepper from above and lower it onto the tray' %
pepper[index]
]
class PlaceShapeInShapeSorter(Task):
def init_task(self) -> None:
self.shape_sorter = Shape('shape_sorter')
self.success_sensor = ProximitySensor('success')
self.shapes = [Shape(ob.replace(' ', '_')) for ob in SHAPE_NAMES]
self.drop_points = [
Dummy('%s_drop_point' % ob.replace(' ', '_'))
for ob in SHAPE_NAMES]
self.grasp_points = [
Dummy('%s_grasp_point' % ob.replace(' ', '_'))
for ob in SHAPE_NAMES]
self.waypoint1 = Dummy('waypoint1')
self.waypoint4 = Dummy('waypoint4')
self.register_graspable_objects(self.shapes)
self.register_waypoint_ability_start(0, self._set_grasp)
self.register_waypoint_ability_start(3, self._set_drop)
self.boundary = SpawnBoundary([Shape('boundary')])
def init_episode(self, index) -> List[str]:
self.variation_index = index
shape = SHAPE_NAMES[index]
self.register_success_conditions(
[DetectedCondition(self.shapes[index], self.success_sensor)])
self.boundary.clear()
[self.boundary.sample(s, min_distance=0.05) for s in self.shapes]
return ['put the %s in the shape sorter' % shape,
'pick up the %s and put it in the sorter' % shape,
'place the %s into its slot in the shape sorter' % shape,
'slot the %s into the shape sorter' % shape]
def variation_count(self) -> int:
return len(SHAPE_NAMES)
def _set_grasp(self, _):
gp = self.grasp_points[self.variation_index]
self.waypoint1.set_pose(gp.get_pose())
def _set_drop(self, _):
dp = self.drop_points[self.variation_index]
self.waypoint4.set_pose(dp.get_pose())
class PutKnifeInKnifeBlock(Task):
def init_task(self) -> None:
knife = Shape('knife')
self.knife_base = Dummy('knife_base')
success = ProximitySensor('success')
self.knife_block = Shape('knife_block')
self.chopping_board = Shape('chopping_board')
self.register_graspable_objects([knife])
set = ConditionSet([
DetectedCondition(knife, success),
NothingGrasped(self.robot.gripper)
],
order_matters=True)
self.register_success_conditions([set])
self.boundary = SpawnBoundary([Shape('boundary')])
self.knife_bound = SpawnBoundary([Shape('knife_boundary')])
def init_episode(self, index: int) -> List[str]:
self.knife_bound.clear()
self.boundary.clear()
self.boundary.sample(self.knife_block)
self.boundary.sample(self.chopping_board)
self.knife_bound.sample(self.knife_base)
return [
'put the knife in the knife block',
'slide the knife into its slot in the knife block',
'place the knife in the knife block',
'pick up the knife and leave it in its holder',
'move the knife from the chopping board to the holder'
]
def variation_count(self) -> int:
return 1
def base_rotation_bounds(self) -> Tuple[List[float], List[float]]:
return [0, 0, 0], [0, 0, 0]
class Scene(object):
"""Controls what is currently in the vrep scene. This is used for making
sure that the tasks are easily reachable. This may be just replaced by
environment. Responsible for moving all the objects. """
def __init__(self,
pyrep: PyRep,
robot: Robot,
obs_config=ObservationConfig()):
self._pyrep = pyrep
self._robot = robot
self._obs_config = obs_config
self._active_task = None
self._inital_task_state = None
self._start_arm_joint_pos = robot.arm.get_joint_positions()
if robot.is_grip():
self._starting_gripper_joint_pos = robot.gripper.get_joint_positions(
)
else:
self._starting_gripper_joint_pos = None
self._workspace = Shape('workspace')
self._workspace_boundary = SpawnBoundary([self._workspace])
self._cam_over_shoulder_left = VisionSensor('cam_over_shoulder_left')
self._cam_over_shoulder_right = VisionSensor('cam_over_shoulder_right')
self._cam_wrist = VisionSensor('cam_wrist')
self._cam_front = VisionSensor('cam_front')
self._cam_over_shoulder_left_mask = VisionSensor(
'cam_over_shoulder_left_mask')
self._cam_over_shoulder_right_mask = VisionSensor(
'cam_over_shoulder_right_mask')
self._cam_wrist_mask = VisionSensor('cam_wrist_mask')
self._cam_front_mask = VisionSensor('cam_front_mask')
self._has_init_task = self._has_init_episode = False
self._variation_index = 0
self._initial_robot_state = (robot.arm.get_configuration_tree(),
robot.gripper.get_configuration_tree())
# Set camera properties from observation config
self._set_camera_properties()
x, y, z = self._workspace.get_position()
minx, maxx, miny, maxy, _, _ = self._workspace.get_bounding_box()
self._workspace_minx = x - np.fabs(minx)
self._workspace_maxx = x + maxx
self._workspace_miny = y - np.fabs(miny)
self._workspace_maxy = y + maxy
self._workspace_minz = z
self._workspace_maxz = z + 1.0 # 1M above workspace
def load(self, task: Task) -> None:
"""Loads the task and positions at the centre of the workspace.
:param task: The task to load in the scene.
"""
task.load() # Load the task in to the scene
# Set at the centre of the workspace
task.get_base().set_position(self._workspace.get_position())
self._inital_task_state = task.get_state()
self._active_task = task
self._initial_task_pose = task.boundary_root().get_orientation()
self._has_init_task = self._has_init_episode = False
self._variation_index = 0
def unload(self) -> None:
"""Clears the scene. i.e. removes all tasks. """
if self._active_task is not None:
self._robot.gripper.release()
if self._has_init_task:
self._active_task.cleanup_()
self._active_task.unload()
self._active_task = None
self._variation_index = 0
def init_task(self) -> None:
self._active_task.init_task()
self._inital_task_state = self._active_task.get_state()
self._has_init_task = True
self._variation_index = 0
def init_episode(self,
index: int,
randomly_place: bool = True,
max_attempts: int = 5) -> List[str]:
"""Calls the task init_episode and puts randomly in the workspace.
"""
self._variation_index = index
if not self._has_init_task:
self.init_task()
# Try a few times to init and place in the workspace
attempts = 0
descriptions = None
while attempts < max_attempts:
descriptions = self._active_task.init_episode(index)
try:
if (randomly_place
and not self._active_task.is_static_workspace()):
self._place_task()
self._active_task.validate()
break
except (BoundaryError, WaypointError) as e:
self._active_task.cleanup_()
self._active_task.restore_state(self._inital_task_state)
attempts += 1
if attempts >= max_attempts:
raise e
# Let objects come to rest
[self._pyrep.step() for _ in range(STEPS_BEFORE_EPISODE_START)]
self._has_init_episode = True
return descriptions
def reset(self) -> None:
"""Resets the joint angles. """
self._robot.gripper.release()
arm, gripper = self._initial_robot_state
self._pyrep.set_configuration_tree(arm)
self._pyrep.set_configuration_tree(gripper)
self._robot.arm.set_joint_positions(self._start_arm_joint_pos,
disable_dynamics=True)
self._robot.arm.set_joint_target_velocities(
[0] * len(self._robot.arm.joints))
if self._robot.is_grip():
self._robot.gripper.set_joint_positions(
self._starting_gripper_joint_pos, disable_dynamics=True)
self._robot.gripper.set_joint_target_velocities(
[0] * len(self._robot.gripper.joints))
else:
self._robot.gripper.release()
if self._active_task is not None and self._has_init_task:
self._active_task.cleanup_()
self._active_task.restore_state(self._inital_task_state)
self._active_task.set_initial_objects_in_scene()
def get_observation(self) -> Observation:
tip = self._robot.arm.get_tip()
joint_forces = None
if self._obs_config.joint_forces:
fs = self._robot.arm.get_joint_forces()
vels = self._robot.arm.get_joint_target_velocities()
joint_forces = self._obs_config.joint_forces_noise.apply(
np.array([-f if v < 0 else f for f, v in zip(fs, vels)]))
ee_forces_flat = None
if self._obs_config.gripper_touch_forces:
ee_forces = self._robot.gripper.get_touch_sensor_forces()
ee_forces_flat = []
for eef in ee_forces:
ee_forces_flat.extend(eef)
ee_forces_flat = np.array(ee_forces_flat)
lsc_ob = self._obs_config.left_shoulder_camera
rsc_ob = self._obs_config.right_shoulder_camera
wc_ob = self._obs_config.wrist_camera
fc_ob = self._obs_config.front_camera
lsc_mask_fn = (rgb_handles_to_mask
if lsc_ob.masks_as_one_channel else lambda x: x)
rsc_mask_fn = (rgb_handles_to_mask
if rsc_ob.masks_as_one_channel else lambda x: x)
wc_mask_fn = (rgb_handles_to_mask
if wc_ob.masks_as_one_channel else lambda x: x)
fc_mask_fn = (rgb_handles_to_mask
if fc_ob.masks_as_one_channel else lambda x: x)
def get_rgb_depth(sensor: VisionSensor, get_rgb: bool, get_depth: bool,
rgb_noise: NoiseModel, depth_noise: NoiseModel,
depth_in_meters: bool):
rgb = depth = None
if sensor is not None and (get_rgb or get_depth):
sensor.handle_explicitly()
if get_rgb:
rgb = sensor.capture_rgb()
if rgb_noise is not None:
rgb = rgb_noise.apply(rgb)
if get_depth:
depth = sensor.capture_depth(depth_in_meters)
if depth_noise is not None:
depth = depth_noise.apply(depth)
return rgb, depth
def get_mask(sensor: VisionSensor, mask_fn):
mask = None
if sensor is not None:
sensor.handle_explicitly()
mask = mask_fn(sensor.capture_rgb())
return mask
left_shoulder_rgb, left_shoulder_depth = get_rgb_depth(
self._cam_over_shoulder_left, lsc_ob.rgb, lsc_ob.depth,
lsc_ob.rgb_noise, lsc_ob.depth_noise, lsc_ob.depth_in_meters)
right_shoulder_rgb, right_shoulder_depth = get_rgb_depth(
self._cam_over_shoulder_right, rsc_ob.rgb, rsc_ob.depth,
rsc_ob.rgb_noise, rsc_ob.depth_noise, rsc_ob.depth_in_meters)
wrist_rgb, wrist_depth = get_rgb_depth(self._cam_wrist, wc_ob.rgb,
wc_ob.depth, wc_ob.rgb_noise,
wc_ob.depth_noise,
wc_ob.depth_in_meters)
front_rgb, front_depth = get_rgb_depth(self._cam_front, fc_ob.rgb,
fc_ob.depth, fc_ob.rgb_noise,
fc_ob.depth_noise,
fc_ob.depth_in_meters)
left_shoulder_mask = get_mask(self._cam_over_shoulder_left_mask,
lsc_mask_fn) if lsc_ob.mask else None
right_shoulder_mask = get_mask(self._cam_over_shoulder_right_mask,
rsc_mask_fn) if rsc_ob.mask else None
wrist_mask = get_mask(self._cam_wrist_mask,
wc_mask_fn) if wc_ob.mask else None
front_mask = get_mask(self._cam_front_mask,
fc_mask_fn) if fc_ob.mask else None
obs = Observation(
left_shoulder_rgb=left_shoulder_rgb,
left_shoulder_depth=left_shoulder_depth,
right_shoulder_rgb=right_shoulder_rgb,
right_shoulder_depth=right_shoulder_depth,
wrist_rgb=wrist_rgb,
wrist_depth=wrist_depth,
front_rgb=front_rgb,
front_depth=front_depth,
left_shoulder_mask=left_shoulder_mask,
right_shoulder_mask=right_shoulder_mask,
wrist_mask=wrist_mask,
front_mask=front_mask,
joint_velocities=(self._obs_config.joint_velocities_noise.apply(
np.array(self._robot.arm.get_joint_velocities()))
if self._obs_config.joint_velocities else None),
joint_positions=(self._obs_config.joint_positions_noise.apply(
np.array(self._robot.arm.get_joint_positions()))
if self._obs_config.joint_positions else None),
joint_forces=(joint_forces
if self._obs_config.joint_forces else None),
gripper_open=(self._robot.gripper.if_open()
if self._obs_config.gripper_open else None),
gripper_pose=(np.array(tip.get_pose())
if self._obs_config.gripper_pose else None),
gripper_matrix=(np.reshape(tip.get_matrix(), (3, 4))
if self._obs_config.gripper_matrix else None),
gripper_touch_forces=(ee_forces_flat
if self._obs_config.gripper_touch_forces else
None),
gripper_joint_positions=(
np.array(self._robot.gripper.get_joint_positions())
if self._obs_config.gripper_joint_positions else None),
wrist_camera_matrix=(np.reshape(self._cam_wrist.get_matrix(),
(3, 4))
if self._cam_wrist.still_exists() else None),
task_low_dim_state=(self._active_task.get_low_dim_state() if
self._obs_config.task_low_dim_state else None))
obs = self._active_task.decorate_observation(obs)
return obs
def step(self):
self._pyrep.step()
self._active_task.step()
def get_demo(self,
record: bool = True,
callable_each_step: Callable[[Observation], None] = None,
randomly_place: bool = True) -> Demo:
"""Returns a demo (list of observations)"""
if not self._has_init_task:
self.init_task()
if not self._has_init_episode:
self.init_episode(self._variation_index,
randomly_place=randomly_place)
self._has_init_episode = False
waypoints = self._active_task.get_waypoints()
if len(waypoints) == 0:
raise NoWaypointsError('No waypoints were found.',
self._active_task)
demo = []
if record:
self._pyrep.step() # Need this here or get_force doesn't work...
demo.append(self.get_observation())
while True:
success = False
for i, point in enumerate(waypoints):
point.start_of_path()
try:
path = point.get_path()
except ConfigurationPathError as e:
raise DemoError(
'Could not get a path for waypoint %d.' % i,
self._active_task) from e
ext = point.get_ext()
path.visualize()
done = False
success = False
while not done:
done = path.step()
self.step()
self._demo_record_step(demo, record, callable_each_step)
success, term = self._active_task.success()
point.end_of_path()
path.clear_visualization()
if len(ext) > 0:
contains_param = False
start_of_bracket = -1
gripper = self._robot.gripper
if 'open_gripper(' in ext:
gripper.release()
start_of_bracket = ext.index('open_gripper(') + 13
contains_param = ext[start_of_bracket] != ')'
if not contains_param:
done = False
while not done:
done = gripper.actuate(1.0, 0.04)
self._pyrep.step()
self._active_task.step()
if self._obs_config.record_gripper_closing:
self._demo_record_step(
demo, record, callable_each_step)
elif 'close_gripper(' in ext:
start_of_bracket = ext.index('close_gripper(') + 14
contains_param = ext[start_of_bracket] != ')'
if not contains_param:
done = False
while not done:
done = gripper.actuate(0.0, 0.04)
self._pyrep.step()
self._active_task.step()
if self._obs_config.record_gripper_closing:
self._demo_record_step(
demo, record, callable_each_step)
if contains_param:
rest = ext[start_of_bracket:]
num = float(rest[:rest.index(')')])
done = False
while not done:
done = gripper.actuate(num, 0.04)
self._pyrep.step()
self._active_task.step()
if self._obs_config.record_gripper_closing:
self._demo_record_step(demo, record,
callable_each_step)
if 'close_gripper(' in ext:
for g_obj in self._active_task.get_graspable_objects():
gripper.grasp(g_obj)
self._demo_record_step(demo, record, callable_each_step)
if not self._active_task.should_repeat_waypoints() or success:
break
# Some tasks may need additional physics steps
# (e.g. ball rowling to goal)
if not success:
for _ in range(10):
self._pyrep.step()
self._active_task.step()
self._demo_record_step(demo, record, callable_each_step)
success, term = self._active_task.success()
if success:
break
success, term = self._active_task.success()
if not success:
raise DemoError('Demo was completed, but was not successful.',
self._active_task)
return Demo(demo)
def get_observation_config(self) -> ObservationConfig:
return self._obs_config
def check_target_in_workspace(self, target_pos: np.ndarray) -> bool:
x, y, z = target_pos
return (self._workspace_maxx > x > self._workspace_minx
and self._workspace_maxy > y > self._workspace_miny
and self._workspace_maxz > z > self._workspace_minz)
def _demo_record_step(self, demo_list, record, func):
if record:
demo_list.append(self.get_observation())
if func is not None:
func(self.get_observation())
def _set_camera_properties(self) -> None:
def _set_rgb_props(rgb_cam: VisionSensor, rgb: bool, depth: bool,
conf: CameraConfig):
if not (rgb or depth):
rgb_cam.remove()
else:
rgb_cam.set_explicit_handling(1)
rgb_cam.set_resolution(conf.image_size)
rgb_cam.set_render_mode(conf.render_mode)
def _set_mask_props(mask_cam: VisionSensor, mask: bool,
conf: CameraConfig):
if not mask:
mask_cam.remove()
else:
mask_cam.set_explicit_handling(1)
mask_cam.set_resolution(conf.image_size)
_set_rgb_props(self._cam_over_shoulder_left,
self._obs_config.left_shoulder_camera.rgb,
self._obs_config.left_shoulder_camera.depth,
self._obs_config.left_shoulder_camera)
_set_rgb_props(self._cam_over_shoulder_right,
self._obs_config.right_shoulder_camera.rgb,
self._obs_config.right_shoulder_camera.depth,
self._obs_config.right_shoulder_camera)
_set_rgb_props(self._cam_wrist, self._obs_config.wrist_camera.rgb,
self._obs_config.wrist_camera.depth,
self._obs_config.wrist_camera)
_set_rgb_props(self._cam_front, self._obs_config.front_camera.rgb,
self._obs_config.front_camera.depth,
self._obs_config.front_camera)
_set_mask_props(self._cam_over_shoulder_left_mask,
self._obs_config.left_shoulder_camera.mask,
self._obs_config.left_shoulder_camera)
_set_mask_props(self._cam_over_shoulder_right_mask,
self._obs_config.right_shoulder_camera.mask,
self._obs_config.right_shoulder_camera)
_set_mask_props(self._cam_wrist_mask,
self._obs_config.wrist_camera.mask,
self._obs_config.wrist_camera)
_set_mask_props(self._cam_front_mask,
self._obs_config.front_camera.mask,
self._obs_config.front_camera)
def _place_task(self) -> None:
self._workspace_boundary.clear()
# Find a place in the robot workspace for task
self._active_task.boundary_root().set_orientation(
self._initial_task_pose)
min_rot, max_rot = self._active_task.base_rotation_bounds()
self._workspace_boundary.sample(self._active_task.boundary_root(),
min_rotation=min_rot,
max_rotation=max_rot)