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