def __init__(self, count: int, name: str, joint_names: List[str]):
super().__init__(count, name, joint_names)
prox_name = '%s_attachProxSensor' % name
attach_name = '%s_attachPoint' % name
prox_name = prox_name if count == 0 else prox_name + '%d' % (count - 1)
attach_name = attach_name if count == 0 else attach_name + '%d' % (
count - 1)
self._proximity_sensor = ProximitySensor(prox_name)
self._attach_point = ForceSensor(attach_name)
self._old_parents = []
self._grasped_objects = []
self._prev_positions = [None] * len(joint_names)
self._prev_vels = [None] * len(joint_names) # Used to stop oscillating
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_task(self) -> None:
plate = Shape('plate')
fork = Shape('fork')
knife = Shape('knife')
spoon = Shape('spoon')
glass = Shape('glass')
self.register_success_conditions([
DetectedCondition(plate, ProximitySensor('plate_detector')),
DetectedCondition(fork, ProximitySensor('fork_detector')),
DetectedCondition(knife, ProximitySensor('knife_detector')),
DetectedCondition(spoon, ProximitySensor('spoon_detector')),
DetectedCondition(glass, ProximitySensor('glass_detector')),
NothingGrasped(self.robot.gripper)
])
self.register_graspable_objects([plate, fork, knife, spoon, glass])
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_task(self) -> None:
success_sensor = ProximitySensor('success')
usb = Shape('usb')
usb_tip = Shape('tip')
self.register_graspable_objects([usb])
self.register_success_conditions(
[DetectedCondition(usb_tip, success_sensor)])
def init_task(self):
wine_bottle = Shape('wine_bottle')
self.register_graspable_objects([wine_bottle])
self.register_success_conditions([
DetectedCondition(wine_bottle, ProximitySensor('success')),
NothingGrasped(self.robot.gripper)
])
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 TestProximitySensors(TestCore):
def setUp(self):
super().setUp()
self.sensor = ProximitySensor('proximity_sensor')
def test_read(self):
self.pyrep.step()
distance, object = self.sensor.read()
self.pyrep.step()
self.assertAlmostEqual(distance, 0.1)
def test_is_detected(self):
ob1 = Shape('simple_model')
ob2 = Shape('dynamic_cube')
self.assertTrue(self.sensor.is_detected(ob1))
self.assertFalse(self.sensor.is_detected(ob2))
def init_task(self) -> None:
cup = Shape('cup')
self.register_graspable_objects([cup])
self.register_success_conditions([
JointCondition(Joint('joint'), np.deg2rad(1)),
DetectedCondition(cup, ProximitySensor('success'))
])
def __init__(self):
self.initial_pos = Dummy('target0')
self.final_pos = Dummy('target1')
self.obstacle0 = Shape('Cylinder')
self.obstacle1 = Shape('Cylinder0')
self.obstacle2 = Shape('Cylinder1')
self.sensor = ProximitySensor('Panda_sensor')
def init_task(self) -> None:
piece2 = Shape('solve_puzzle_piece2')
self.register_graspable_objects([piece2])
self.register_success_conditions([
NothingGrasped(self.robot.gripper),
DetectedCondition(piece2, ProximitySensor('success'))
])
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_task(self) -> None:
success_sensor = ProximitySensor('success')
frame = Shape('frame')
frame_detector = Shape('frame_detector')
self.register_graspable_objects([frame])
self.register_success_conditions(
[DetectedCondition(frame_detector, success_sensor)])
def init_task(self) -> None:
phone = Shape('phone')
self.register_graspable_objects([phone])
self.register_success_conditions([
DetectedCondition(phone, ProximitySensor('success')),
NothingGrasped(self.robot.gripper)
])
def init_task(self) -> None:
bottle = Shape('bottle')
success_sensor = ProximitySensor('success')
self.register_graspable_objects([bottle])
self.register_success_conditions(
[DetectedCondition(bottle, success_sensor),
NothingGrasped(self.robot.gripper)])
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]]
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:
lid = Shape('lid')
self.register_success_conditions([
DetectedCondition(lid, ProximitySensor('sensor_handle')),
# DetectedCondition(lid, ProximitySensor('sensor_grill_top'),
# negated=True)
])
def init_task(self):
success_sensor = ProximitySensor('success')
umbrella = Shape('umbrella')
self.register_graspable_objects([umbrella])
self.register_success_conditions(
[DetectedCondition(umbrella, success_sensor),
NothingGrasped(self.robot.gripper)])
def init_task(self) -> None:
needle_minute = Shape('clock_needle_minute')
success_detector = ProximitySensor('detector_minute0')
self.register_success_conditions([
NothingGrasped(self.robot.gripper),
DetectedCondition(needle_minute, success_detector)
])
def init_task(self) -> None:
roll = Shape('toilet_roll')
success_sensor = ProximitySensor('success')
self.register_graspable_objects([roll])
self.register_success_conditions(
[DetectedCondition(roll, success_sensor, negated=True),
NothingGrasped(self.robot.gripper)])
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_task(self) -> None:
spatula = Shape('scoop_with_spatula_spatula')
self.register_graspable_objects([spatula])
self.register_success_conditions([
DetectedCondition(Shape('Cuboid'), ProximitySensor('success')),
GraspedCondition(self.robot.gripper, spatula)
])
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.target = Shape('target')
self.boundaries = Shape('boundary')
success_sensor = ProximitySensor('success')
self.register_success_conditions(
[DetectedCondition(self.robot.arm.get_tip(), success_sensor)])
self.thresh = 0.04
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_task(self) -> None:
success_detector = ProximitySensor('success')
tray = Shape('tray')
self.register_graspable_objects([tray])
self.register_success_conditions([
DetectedCondition(tray, success_detector, negated=True),
NothingGrasped(self.robot.gripper)
])
def init_task(self) -> None:
stick = Shape('hockey_stick')
self.register_success_conditions([
DetectedCondition(Shape('hockey_ball'),
ProximitySensor('success')),
GraspedCondition(self.robot.gripper, stick)
])
self.register_graspable_objects([stick])
def init_task(self) -> None:
self.block = Shape('block')
self.success_sensor = ProximitySensor('success')
self.register_success_conditions([
DetectedCondition(self.block, self.success_sensor)])
self.target = Shape('target')
self.workspace = Shape('workspace')
self.target_boundary = SpawnBoundary([Shape('target_boundary')])
def init_task(self) -> None:
self.lid = Shape('saucepan_lid_grasp_point')
success_detector = ProximitySensor('success')
self.register_graspable_objects([self.lid])
cond_set = ConditionSet([
GraspedCondition(self.robot.gripper, self.lid),
DetectedCondition(self.lid, success_detector)
])
self.register_success_conditions([cond_set])
