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