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_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 setUp(self):
super().setUp()
self.dynamic_cube = Shape('dynamic_cube')
self.cubes_under_dummy = Dummy('cubes_under_dummy')
self.cube0 = Shape('cube0')
self.dummy = Dummy('dummy')
self.simple_model = Shape('simple_model')
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 __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:
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__(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 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 __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__(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 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__(self,
continuous_control=True,
obs_lowdim=True,
rpa=3,
frames=4):
self.pr = PyRep()
SCENE_FILE = join(dirname(abspath(__file__)), 'reacher_v2.ttt')
self.pr.launch(SCENE_FILE, headless=True)
self.pr.start()
self.continuous_control = continuous_control
self.target = Shape('target')
self.tip = Dummy('Reacher_tip')
self.camera = VisionSensor('Vision_sensor')
self.agent = Reacher()
self.done = False
self.rpa = rpa
self.obs_lowdim = obs_lowdim
self.frames = frames
self.prev_obs = []
self.steps_ep = 0
self.increment = 4 * pi / 180 # to radians
self.action_all = [[self.increment, self.increment],
[-self.increment, -self.increment],
[0, self.increment], [0, -self.increment],
[self.increment, 0], [-self.increment, 0],
[-self.increment, self.increment],
[self.increment, -self.increment]]
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)
def init_episode(self, index: int) -> List[str]:
b = SpawnBoundary([self.boundary])
for obj in self.jars:
b.sample(obj, min_distance=0.01)
success = ProximitySensor('success')
success.set_position([0.0, 0.0, 0.05],
relative_to=self.jars[index % 2],
reset_dynamics=False)
w3 = Dummy('waypoint3')
w3.set_orientation([-np.pi, 0, -np.pi], reset_dynamics=False)
w3.set_position([0.0, 0.0, 0.125],
relative_to=self.jars[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.lid, success)]
self.register_success_conditions(self.conditions)
return [
'close the %s jar' % target_color_name,
'screw on the %s jar lid' % target_color_name,
'grasping the lid, lift it from the table and use it to seal '
'the %s jar' % target_color_name,
'pick up the lid from the table and put it on the %s jar' %
target_color_name
]
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 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 _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_visualize(self):
mobile = YouBot()
waypoint = Dummy('youBot_waypoint')
path = mobile.get_linear_path(waypoint.get_position(),
waypoint.get_orientation()[-1])
# Check that it does not error
path.visualize()
def __init__(self,
scene_name,
obs_lowdim=True,
rpa=6,
reward_dense=True,
demonstration_mode=False,
boundary=0):
super().__init__(scene_name, reward_dense, boundary)
# Arm init and handles
self.arm = robot_arm()
self.mobile_base = robot_base()
self.arm_start_pos = self.arm.get_joint_positions()
self.tip = self.arm.get_tip()
self.config_tree = self.arm.get_configuration_tree()
self.action_space = 4
self.prev_tip_pos = np.array(self.tip.get_position())
self.action = [0 for _ in range(self.action_space)]
self.done = False
self.obs_lowdim = obs_lowdim
self.rpa = rpa
self.demonstration_mode = demonstration_mode
self.target_base = Dummy('robot_target_base')
self.arm.set_motor_locked_at_zero_velocity(1)
self.arm.set_control_loop_enabled(0)
self.arm.set_joint_target_velocities([0, 0, 0, 0])
self.mobile_base.set_motor_locked_at_zero_velocity(1)
self.mobile_base.set_joint_target_velocities([0, 0, 0, 0])
def render(self, mode='human'):
if self._gym_cam is 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())
self._gym_cam.set_render_mode(RenderMode.OPENGL3_WINDOWED)
def test_get_linear_path_visualize(self):
arm = Panda()
waypoint = Dummy('Panda_waypoint')
path = arm.get_linear_path(
waypoint.get_position(), waypoint.get_orientation())
# Check that it does not error
path.visualize()
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_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 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 test_solve_ik_via_sampling(self):
arm = Panda()
waypoint = Dummy('Panda_waypoint')
configs = arm.solve_ik_via_sampling(
waypoint.get_position(), waypoint.get_orientation(), max_configs=5)
self.assertIsNotNone(configs)
self.assertEqual(len(configs), 5)
current_config = arm.get_joint_positions()
# Checks correct config (last)
arm.set_joint_positions(configs[-1])
self.assertTrue(np.allclose(
arm.get_tip().get_pose(), waypoint.get_pose(), atol=0.001))
# Checks correct config (first)
arm.set_joint_positions(configs[0])
self.assertTrue(np.allclose(
arm.get_tip().get_pose(), waypoint.get_pose(), atol=0.001))
# Checks order
prev_config_dist = 0
for config in configs:
config_dist = sum(
[(c - f)**2 for c, f in zip(current_config, config)])
# This test requires that the metric scale for each joint remains at
# 1.0 in _getConfigDistance lua function
self.assertLessEqual(prev_config_dist, config_dist)
prev_config_dist = config_dist
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))
def init_task(self) -> None:
self.shape_sorter = Shape('shape_sorter')
self.success_sensor = ProximitySensor('success')
self.shapes = [Shape(ob.replace(' ', '_')) for ob in SHAPE_NAMES]
self.drop_points = [
Dummy('%s_drop_point' % ob.replace(' ', '_'))
for ob in SHAPE_NAMES]
self.grasp_points = [
Dummy('%s_grasp_point' % ob.replace(' ', '_'))
for ob in SHAPE_NAMES]
self.waypoint1 = Dummy('waypoint1')
self.waypoint4 = Dummy('waypoint4')
self.register_graspable_objects(self.shapes)
self.register_waypoint_ability_start(0, self._set_grasp)
self.register_waypoint_ability_start(3, self._set_drop)
self.boundary = SpawnBoundary([Shape('boundary')])
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 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))
