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')
class OpenDoor(Task): def init_task(self) -> None: self._door_joint = Joint('door_frame_joint') self.register_success_conditions( [JointCondition(self._door_joint, np.deg2rad(25))]) self.door_unlock_cond = JointCondition(Joint('door_handle_joint'), np.deg2rad(25)) def init_episode(self, index: int) -> List[str]: self._door_unlocked = False self._door_joint.set_motor_locked_at_zero_velocity(True) return [ 'open the door', 'grip the handle and push the door open', 'use the handle to open the door' ] def variation_count(self) -> int: return 1 def step(self) -> None: if not self._door_unlocked: self._door_unlocked = self.door_unlock_cond.condition_met()[0] if self._door_unlocked: self._door_joint.set_motor_locked_at_zero_velocity(False) 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('boundary_root')
def init_episode(self, index: int) -> List[str]: self.right_frame.set_dynamic(False) self.left_frame.set_dynamic(False) self.left_unlocked = False self.right_unlocked = False option = OPTIONS[index] if option == 'right': self.waypoint0.set_position(self.waypoint0_.get_position()) self.waypoint1.set_position(self.waypoint1_.get_position()) self.waypoint2.set_position(self.waypoint2_.get_position()) self.waypoint2.set_orientation(self.waypoint2_.get_orientation()) self.waypoint3.set_position(self.waypoint3_.get_position()) self.waypoint3.set_orientation(self.waypoint3_.get_orientation()) self.register_success_conditions( [JointCondition(Joint('right_window_joint'), np.deg2rad(50))]) else: self.register_success_conditions( [JointCondition(Joint('left_window_joint'), np.deg2rad(50))]) return [ 'open %s window' % option, 'rotate the handle to unlock the %s window, then open it' % option, 'push the %s window open' % option, 'use the handle to open the %s window' % option ]
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.door_main = Shape('door_main') self.door_main.set_dynamic(False) door_joint = Joint('door_frame_joint') handle_joint = Joint('door_handle_joint') self.register_success_conditions( [JointCondition(door_joint, np.deg2rad(25))]) self.door_unlock_cond = JointCondition(handle_joint, np.deg2rad(25))
def __init__(self, configs, simulator): self.sim = simulator.sim self.rgb_cam = VisionSensor("kinect_rgb") self.depth_cam = VisionSensor("kinect_depth") self.rgb_cam.set_render_mode(RenderMode.OPENGL3) self.depth_cam.set_render_mode(RenderMode.OPENGL3) # Pan and tilt related variables. self.pan_joint = Joint("LoCoBot_head_pan_joint") self.tilt_joint = Joint("LoCoBot_head_tilt_joint")
def init_task(self) -> None: self.tv = Shape('tv_frame') self.screen_on = Shape('tv_screen_on') self.screen_up = Shape('tv_screen_up') self.screen_down = Shape('tv_screen_down') self.remote = Shape('tv_remote') self.boundary = Shape('spawn_boundary') self.joint_conditions = [ JointCondition(Joint('target_button_joint1'), 0.005), JointCondition(Joint('target_button_joint2'), 0.005) ]
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__(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: cup = Shape('cup') self.register_graspable_objects([cup]) self.register_success_conditions([ JointCondition(Joint('joint'), np.deg2rad(1)), DetectedCondition(cup, ProximitySensor('success')) ])
def get_low_dim_state(self) -> np.ndarray: """Gets the pose and various other properties of objects in the task. :return: 1D array of low-dimensional task state. """ # Corner cases: # (1) Object has been deleted. # (2) Object has been grasped (and is now child of gripper). state = [] for obj, objtype in self._initial_objs_in_scene: if not obj.still_exists(): # It has been deleted empty_len = 7 if objtype == ObjectType.JOINT: empty_len += 1 elif objtype == ObjectType.FORCE_SENSOR: empty_len += 6 state.extend(np.zeros((empty_len, )).tolist()) else: state.extend(np.array(obj.get_pose())) if obj.get_type() == ObjectType.JOINT: state.extend( [Joint(obj.get_handle()).get_joint_position()]) elif obj.get_type() == ObjectType.FORCE_SENSOR: forces, torques = ForceSensor(obj.get_handle()).read() state.extend(forces + torques) return np.array(state).flatten()
def init_task(self): self.cup = Shape('cup') self.success_sensor = ProximitySensor('success') self.register_graspable_objects([Shape('cup')]) self.left_joint = Joint('left_joint') self.right_joint = Joint('right_joint') self.waypoint0 = Dummy('waypoint0') self.waypoint1 = Dummy('waypoint1') self.waypoint2 = Dummy('waypoint2') self.waypoint6 = Dummy('waypoint6') self.left_initial_waypoint = Dummy('left_initial_waypoint') self.left_close_waypoint = Dummy('left_close_waypoint') self.left_far_waypoint = Dummy('left_far_waypoint') self.place_cup_left_waypoint = Dummy('place_cup_left_waypoint')
def init_task(self) -> None: self.options = ['bottom', 'middle', 'top'] self.anchors = [ Dummy('waypoint_anchor_%s' % opt) for opt in self.options ] self.joints = [Joint('drawer_joint_%s' % opt) for opt in self.options] self.waypoint1 = Dummy('waypoint1')
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.needle_detector = ProximitySensor('needle_sensor') self.success_conditions = [ NothingGrasped(self.robot.gripper), DetectedCondition(self.needle, self.needle_detector) ] self.w0 = Dummy('waypoint0') self.w0_rel_pos = [ +2.6203 * 10**(-3), -1.7881 * 10**(-7), +1.5197 * 10**(-1) ] self.w0_rel_ori = [-3.1416, -1.7467 * 10**(-2), -3.1416]
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): self.waypoint0 = Dummy('waypoint0') self.waypoint1 = Dummy('waypoint1') self.waypoint2 = Dummy('waypoint2') self.waypoint3 = Dummy('waypoint3') self.waypoint0_ = Dummy('waypoint0_') self.waypoint1_ = Dummy('waypoint1_') self.waypoint2_ = Dummy('waypoint2_') self.waypoint3_ = Dummy('waypoint3_') self.right_unlocked_cond = JointCondition(Joint('right_handle_joint'), np.deg2rad(60)) self.left_unlocked_cond = JointCondition(Joint('left_handle_joint'), np.deg2rad(60)) self.right_frame = Shape('right_frame') self.left_frame = Shape('left_frame')
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: cap_detector = ProximitySensor("cap_detector") self.joint = Joint('joint') self.force_sensor = ForceSensor('Force_sensor') self.cap = Shape('cap') self.register_success_conditions( [DetectedCondition(self.cap, cap_detector, negated=True)]) self.cap_turned_condition = JointCondition( self.joint, np.deg2rad(150))
def __init__(self, count: int, num_wheels: int, num_lasers: int, name: str, laser_name: str): """Count is used for when we have multiple copies of mobile bases. :param count: used for multiple copies of robots :param num_wheels: number of actuated wheels :param num_laser: number of lasers :param name: string with robot name (same as base in vrep model). """ joint_names = [ '%s_m_joint%s' % (name, str(i + 1)) for i in range(num_wheels) ] super().__init__(count, name, joint_names) self.num_wheels = num_wheels suffix = '' if count == 0 else '#%d' % (count - 1) wheel_names = [ '%s_wheel%s%s' % (name, str(i + 1), suffix) for i in range(self.num_wheels) ] self.wheels = [Shape(name) for name in wheel_names] joint_slipping_names = [ '%s_slipping_m_joint%s%s' % (name, str(i + 1), suffix) for i in range(self.num_wheels) ] self.joints_slipping = [ Joint(jsname) for jsname in joint_slipping_names ] laser_suffixs = ['', '#0'] self.lasers = [Laser(laser_name, suffix) for suffix in laser_suffixs] ''' # Motion planning handles self.intermediate_target_base = Dummy( '%s_intermediate_target_base%s' % (name, suffix)) self.target_base = Dummy('%s_target_base%s' % (name, suffix)) self._collision_collection = vrep.simGetCollectionHandle( '%s_base%s' % (name, suffix)) ''' # Robot parameters and handle self.z_pos = self.get_position()[2] #self.target_z = self.target_base.get_position()[-1] #self.wheel_radius = self.wheels[0].get_bounding_box()[5] # Z self.w = np.linalg.norm( np.array(self.wheels[0].get_position()) - np.array(self.wheels[3].get_position())) / 2. self.l = np.linalg.norm( np.array(self.wheels[0].get_position()) - np.array(self.wheels[1].get_position())) / 2.
def __init__(self, count: int, name: str, joint_names: List[str], base_name: str = None): suffix = '' if count == 0 else '#%d' % (count - 1) super().__init__( name + suffix if base_name is None else base_name + suffix) self._num_joints = len(joint_names) # Joint handles self.joints = [Joint(jname + suffix) for jname in joint_names]
def init_task(self) -> None: screw_driver = Shape('screw_driver') self.block = Shape('block') self.register_graspable_objects([screw_driver]) screw_joint = Joint('screw_joint') cond_set = ConditionSet([ GraspedCondition(self.robot.gripper, screw_driver), JointCondition(screw_joint, 1.4)], # about 90 degrees order_matters=True) self.register_success_conditions([cond_set])
def _create_object(self, name): """Creates pyrep object for the correct type""" # TODO implement other types handle = sim.simGetObjectHandle(name) o_type = sim.simGetObjectType(handle) if ObjectType(o_type) == ObjectType.JOINT: return Joint(handle) elif ObjectType(o_type) == ObjectType.SHAPE: return Shape(handle) else: return None
def init_task(self) -> None: bottle_detector = ProximitySensor("bottle_detector") cap_detector = ProximitySensor("cap_detector") bottle = Shape('bottle') self.joint = Joint('joint') self.cap = Shape('cap') self.register_success_conditions( [DetectedCondition(bottle, bottle_detector), DetectedCondition(self.cap, cap_detector, negated=True), NothingGrasped(self.robot.gripper)]) self.cap_turned_condition = JointCondition( self.joint, np.deg2rad(150))
def __init__(self, pr): #super(ArmECM, self).__init__(scenePath) """self.pr = PyRep() self.pr.launch(scenePath) self.pr.start() self.pr.step()""" self.pr = pr self.base_handle = Shape('L0_respondable_ECM') self.j1_handle = Joint('J1_ECM') self.j2_handle = Joint('J2_ECM') self.j3_handle = Joint('J3_ECM') self.j4_handle = Joint('J4_ECM') self.left_cam = VisionSensor('Vision_sensor_left') self.right_cam = VisionSensor('Vision_sensor_right')
def get_low_dim_state(self) -> np.ndarray: """Gets the pose and various other properties of objects in the task. :return: 1D array of low-dimensional task state. """ objs = self.get_base().get_objects_in_tree(exclude_base=True, first_generation_only=False) state = [] for obj in objs: state.extend(np.array(obj.get_pose())) if obj.get_type() == ObjectType.JOINT: state.extend([Joint(obj.get_handle()).get_joint_position()]) elif obj.get_type() == ObjectType.FORCE_SENSOR: forces, torques = ForceSensor(obj.get_handle()).read() state.extend(forces + torques) return np.array(state).flatten()
def __init__(self, min_distance, max_distance, default_joint_limit_type="none"): self.cam_left = VisionSensor("vs_cam_left#") self.cam_right = VisionSensor("vs_cam_right#") self.tilt_left = Joint("vs_eye_tilt_left#") self.tilt_right = Joint("vs_eye_tilt_right#") self.pan_left = Joint("vs_eye_pan_left#") self.pan_right = Joint("vs_eye_pan_right#") self.min_distance = min_distance self.max_distance = max_distance self.default_joint_limit_type = default_joint_limit_type self._pan_max = rad(10) self._tilt_max = rad(10) self._tilt_speed = 0 self._pan_speed = 0 self.episode_reset()
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_task(self) -> None: self.buttons_pushed = 0 self.color_variation_index = 0 self.target_buttons = [Shape('push_buttons_target%d' % i) for i in range(3)] self.target_topPlates = [Shape('target_button_topPlate%d' % i) for i in range(3)] self.target_joints = [Joint('target_button_joint%d' % i) for i in range(3)] self.target_wraps = [Shape('target_button_wrap%d' % i) for i in range(3)] self.boundaries = Shape('push_buttons_boundary') # goal_conditions merely state joint conditions for push action for # each button regardless of whether the task involves pushing it self.goal_conditions = [JointCondition(self.target_joints[n], 0.005) for n in range(3)] self.register_waypoint_ability_start(0, self._move_above_next_target) self.register_waypoints_should_repeat(self._repeat)
def __init__(self, count: int, num_wheels: int, distance_from_target: float, name: str, max_velocity: float = 4, max_velocity_rotation: float = 6, max_acceleration: float = 0.035): """Init. :param count: used for multiple copies of robots. :param num_wheels: number of actuated wheels. :param distance_from_target: offset from target. :param name: string with robot name (same as base in CoppeliaSim model). :param max_velocity: bounds x,y velocity for motion planning. :param max_velocity_rotation: bounds yaw velocity for motion planning. :param max_acceleration: bounds acceleration for motion planning. """ super().__init__(count, num_wheels, name) suffix = '' if count == 0 else '#%d' % (count - 1) self.paramP = 20 self.paramO = 10 # self.paramO = 30 self.previous_forw_back_vel = 0 self.previous_left_right_vel = 0 self.previous_rot_vel = 0 self.accelF = max_acceleration self.maxV = max_velocity self.max_v_rot = max_velocity_rotation self.dist1 = distance_from_target joint_slipping_names = [ '%s_slipping_m_joint%s%s' % (name, str(i + 1), suffix) for i in range(self.num_wheels) ] self.joints_slipping = [ Joint(jsname) for jsname in joint_slipping_names ]
def __init__(self, joint: Joint, position: float): """in radians if revoloute, or meters if prismatic""" self._joint = joint self._original_pos = joint.get_joint_position() self._pos = position