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])