class TurnTap(Task):
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_episode(self, index: int) -> List[str]:
option = OPTIONS[index]
if option == 'right':
self.left_start.set_position(self.right_start.get_position())
self.left_start.set_orientation(self.right_start.get_orientation())
self.left_end.set_position(self.right_end.get_position())
self.left_end.set_orientation(self.right_end.get_orientation())
self.register_success_conditions(
[JointCondition(self.right_joint, 1.57)])
else:
self.register_success_conditions(
[JointCondition(self.left_joint, 1.57)])
return [
'turn %s tap' % option,
'rotate the %s tap' % option,
'grasp the %s tap and turn it' % option
]
def variation_count(self) -> int:
return 2
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 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 _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 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))
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 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 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 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 update_joystick(self, datos):
adv = advR = 0.0
rot = rotR = 0.0
side = sideR = 0.0
print(datos.buttons)
for x in datos.buttons:
if x.name == "mode":
self.mode += x.step
if self.mode % 2 == 1:
self.bloqueo = True
else:
self.bloqueo = False
for x in datos.axes:
print(x.name + "" + str(x.value))
if x.name == "X_axis":
adv = x.value if np.abs(x.value) > 1 else 0
advR = x.value if np.abs(x.value) > 1 else 0
if x.name == "Y_axis":
rot = x.value if np.abs(x.value) > 0.01 else 0
rotR = x.value if np.abs(x.value) > 0.01 else 0
if x.name == "Z_axis":
side = x.value if np.abs(x.value) > 1 else 0
sideR = x.value if np.abs(x.value) > 1 else 0
if x.name == "gripper":
if x.value > 1:
self.pr.script_call("open@RG2", 1)
print("abriendo")
if x.value < -1:
print("cerrando")
self.pr.script_call("close@RG2", 1)
dummy = Dummy("target")
parent_frame_object = None
position = dummy.get_position()
orientation = dummy.get_orientation()
if self.bloqueo == False:
print("modo0\n\n")
dummy.set_position([
position[0] + adv / 1000, position[1] + rot / 1000,
position[2] + side / 1000
], parent_frame_object)
else:
print("modo1\n\n")
dummy.set_orientation([
orientation[0] + advR / 1000, orientation[1] + rotR / 1000,
orientation[2] + sideR / 1000
], parent_frame_object)
def test_get_nonlinear_path(self):
mobile = YouBot()
waypoint = Dummy('youBot_waypoint')
path = mobile.get_nonlinear_path(waypoint.get_position(),
waypoint.get_orientation()[-1])
self.assertIsNotNone(path)
class OpenWindow(Task):
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_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 variation_count(self) -> int:
return 2
def step(self) -> None:
if not self.left_unlocked:
self.left_unlocked = self.left_unlocked_cond.condition_met()[0]
if self.left_unlocked:
self.left_frame.set_dynamic(True)
if not self.right_unlocked:
self.right_unlocked = self.right_unlocked_cond.condition_met()[0]
if self.right_unlocked:
self.right_frame.set_dynamic(True)
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')
class ChangeClock(Task):
def init_task(self) -> None:
self.needle_hour = Shape('clock_needle_hour')
self.needle_minute = Shape('clock_needle_minute')
self.needle_crank = Shape('clock_needle_crank')
self.needle_pivot = Shape('clock_needle_pivot')
self.register_graspable_objects([self.needle_crank])
_, _, self.needle_crank_starting_gamma = \
self.needle_crank.get_orientation()
self.hour_starting_ori = \
self.needle_hour.get_orientation(relative_to=self.needle_pivot)
self.minute_starting_ori = \
self.needle_minute.get_orientation(relative_to=self.needle_pivot)
self.target_rotation_dic = {
0: (1 / 24) * np.pi,
1: (2 / 24) * np.pi,
2: (59 / 24) * np.pi
}
self.success_conditions = [NothingGrasped(self.robot.gripper)]
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 variation_count(self) -> int:
return NUM_TARGET_TIMES
def step(self) -> None:
_, _, current_gamma = self.needle_crank.get_orientation()
crank_gamma_delta = \
current_gamma - self.needle_crank_starting_gamma
new_hour_ori = self.hour_starting_ori.copy()
new_min_ori = self.minute_starting_ori.copy()
if ((self.target_rotation_dic[self.target_time_index] % (
2 * np.pi)) - 0.05) \
<= -crank_gamma_delta <= \
((self.target_rotation_dic[self.target_time_index] % (
2 * np.pi)) + 0.05):
new_hour_ori[1] -= \
self.target_rotation_dic[self.target_time_index]
new_min_ori[1] -= (12) * \
self.target_rotation_dic[self.target_time_index]
else:
new_hour_ori[1] += 12 * crank_gamma_delta / 12
new_min_ori[1] += 12 * crank_gamma_delta
self.needle_hour.set_orientation(new_hour_ori,
relative_to=self.needle_pivot,
reset_dynamics=False)
self.needle_minute.set_orientation(new_min_ori,
relative_to=self.needle_pivot,
reset_dynamics=False)
def base_rotation_bounds(self) -> Tuple[List[float], List[float]]:
return [0.0, 0.0, 0], [0, 0, +0.4 * np.pi]
class TakeOffWeighingScales(Task):
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._variation_index = index
self.target_pepper_index = index
while len(self.success_conditions) > 1:
self.success_conditions.pop()
self.success_conditions.append(
DetectedCondition(self.peppers[self.target_pepper_index],
self.success_detector))
self.register_success_conditions(self.success_conditions)
b = SpawnBoundary([self.boundary])
for p in self.peppers:
b.sample(p,
ignore_collisions=False,
min_distance=0.09,
min_rotation=(0.00, 0.00, -3.14),
max_rotation=(0.00, 0.00, +3.14))
self.w0.set_position(
self.w0_rel_pos,
relative_to=self.peppers[self.target_pepper_index],
reset_dynamics=False)
self.w0.set_orientation(
self.w0_rel_ori,
relative_to=self.peppers[self.target_pepper_index],
reset_dynamics=False)
index_dic = {0: 'green', 1: 'red', 2: 'yellow'}
if self.target_pepper_index in range(3):
return [
'remove the %s pepper from the weighing scales and place it '
'on the table' % index_dic[self.target_pepper_index],
'take the %s pepper off of the scales' %
index_dic[self.target_pepper_index],
'lift the %s pepper off of the tray and set it down on the '
'table' % index_dic[self.target_pepper_index],
'grasp the %s pepper and move it to the table top' %
index_dic[self.target_pepper_index],
'take the %s object off of the scales tray' %
index_dic[self.target_pepper_index],
'put the %s item on the item' %
index_dic[self.target_pepper_index]
]
else:
raise ValueError('Invalid pepper index, should not be here')
def variation_count(self) -> int:
return UNIQUE_PEPPERS_TO_TAKE_OFF
def step(self) -> None:
_, _, pos_z = self.top_plate.get_position()
dz = self.top_plate_starting_z - pos_z
d_alpha = -130 * dz
new_needle_ori = [
self.needle_starting_ori[0] + d_alpha, self.needle_starting_ori[1],
self.needle_starting_ori[2]
]
self.needle.set_orientation(new_needle_ori,
relative_to=self.needle_pivot,
reset_dynamics=False)
def base_rotation_bounds(self) -> Tuple[List[float], List[float]]:
return [0.0, 0.0, -0.25 * np.pi], [0.0, 0.0, +0.25 * np.pi]
class RemoveCups(Task):
def init_task(self) -> None:
self.cups_removed = -1
self.cups = [Shape('mug%d' % i) for i in range(3)]
self.spokes = [
Shape('place_cups_holder_spoke%d' % i) for i in range(3)
]
self.holder = Shape('place_cups_holder_base')
self.holder_boundary = Shape('tree_boundary')
self.register_graspable_objects(self.cups)
self.success_detectors = [
ProximitySensor('success_detector%d' % i) for i in range(3)
]
self.w1 = Dummy('waypoint1')
self.w1_rel_pos = self.w1.get_position(relative_to=self.cups[0])
self.w1_rel_ori = self.w1.get_orientation(relative_to=self.cups[0])
self.w2 = Dummy('waypoint2')
self.w2_rel_pos = self.w2.get_position(relative_to=self.spokes[0])
self.w2_rel_ori = self.w2.get_orientation(relative_to=self.spokes[0])
self.w5 = Dummy('waypoint5')
self.w5_rel_pos = self.w5.get_orientation(
relative_to=self.success_detectors[0])
self.w5_new_pos = self.w5.get_position()
self.w5_new_pos_saved = self.w5_new_pos
self.success_conditions = [NothingGrasped(self.robot.gripper)]
def init_episode(self, index: int) -> List[str]:
self.cups_removed = -1
self.cups_to_remove = 1 + index % MAX_CUPS_TO_REMOVE
self.w5_new_pos = self.w5_new_pos_saved
self.w1.set_position(self.w1_rel_pos,
relative_to=self.cups[0],
reset_dynamics=False)
for i in range(self.cups_to_remove):
self.success_conditions.append(
DetectedCondition(self.cups[i], self.success_detectors[i]))
self.register_success_conditions(self.success_conditions)
self.register_waypoint_ability_start(0, self._move_above_next_target)
self.register_waypoints_should_repeat(self._repeat)
if self.cups_to_remove == 1:
return [
'remove 1 cup from the cup holder and place it on the '
'table', 'remove one cup from the mug holder',
'pick up 1 cup from the mug tree and place it on the table',
'slide 1 mug off of its spoke on the cup holder and leave '
'it on the table top'
]
else:
return [
'remove %d cups from the cup holder and place it on the '
'table' % self.cups_to_remove,
'remove %d cups from the cup holder' % self.cups_to_remove,
'pick up %d cups from the mug tree and place them on the '
'table' % self.cups_to_remove,
'slide %d mugs off of their spokes on the cup holder and '
'leave them on the table top' % self.cups_to_remove
]
def variation_count(self) -> int:
return MAX_CUPS_TO_REMOVE
def _move_above_next_target(self, waypoint):
if self.cups_removed > self.cups_to_remove:
raise RuntimeError('Should not be here, all cups should have been '
'removed')
move_index = self.cups_removed if self.cups_removed > -1 else 0
next_move_index = self.cups_removed + 1 if self.cups_removed > -1 else 0
if self.cups_removed > -1:
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)
self.w2.set_position(self.w2_rel_pos,
relative_to=self.spokes[next_move_index],
reset_dynamics=False)
self.w2.set_orientation(self.w2_rel_ori,
relative_to=self.spokes[next_move_index],
reset_dynamics=False)
new_x, new_y, _ = self.success_detectors[
next_move_index].get_position()
self.w5_new_pos[0] = new_x
self.w5_new_pos[1] = new_y
self.w5.set_position(self.w5_new_pos, reset_dynamics=False)
######
self.cups_removed += 1
######
def cleanup(self) -> None:
self.cups_removed = -1
def _repeat(self):
return self.cups_removed < self.cups_to_remove - 1
def base_rotation_bounds(self) -> Tuple[List[float], List[float]]:
return [0.0, 0.0, 0.0], [0.0, 0.0, 0.0]
def init_episode(self, index: int) -> List[str]:
self.target_shelf = index
target_dummy_name = 'dummy_shelf' + str(self.target_shelf)
target_pos_dummy = Dummy(target_dummy_name)
target_pos = target_pos_dummy.get_position()
target_ori = target_pos_dummy.get_orientation()
self.w1.set_position(target_pos, reset_dynamics=False)
self.w1.set_orientation(target_ori, reset_dynamics=False)
b_place = SpawnBoundary([self.place_boundary])
b_place.sample(self.w3,
min_rotation=(0.0, 0.0, -np.pi / 12),
max_rotation=(0.0, 0.0, +np.pi / 12),
min_distance=0)
self.success_detector = ProximitySensor('success_detector')
while len(self.success_conditions) > 1:
self.success_conditions.pop()
self.success_conditions.append(
DetectedCondition(self.money_list[self.target_shelf],
self.success_detector))
self.register_success_conditions(self.success_conditions)
# self.w1.set_parent(target_pos_dummy, keep_in_place=True)
rel_target_pos = self.w1.get_position(
relative_to=self.money_list[index])
rel_target_ori = self.w1.get_orientation(
relative_to=self.money_list[index])
b_pick = SpawnBoundary([self.pick_boundary])
b_pick.sample(self.money_list[index],
min_rotation=(0.0, 0.0, -np.pi / 10),
max_rotation=(0.0, 0.0, 0.0),
min_distance=0)
for m in self.money_list:
b_pick.sample(m,
min_rotation=(0.0, 0.0, -np.pi / 10),
max_rotation=(0.0, 0.0, 0.0),
min_distance=0)
self.w1.set_position(rel_target_pos,
relative_to=self.money_list[index],
reset_dynamics=False)
self.w1.set_orientation(rel_target_ori,
relative_to=self.money_list[index],
reset_dynamics=False)
"""
if self.target_shelf == 0:
return ['take the money out of the bottom shelf and place it on '
'the table']
elif self.target_shelf == 1:
return ['take the money out of the middle shelf and place it on '
'the table']
elif self.target_shelf == 2:
return ['take the money out of the top shelf and place it on '
'the table']
else:
raise ValueError('Invalid target_shelf index, should not be here')
return -1
"""
return [
'take the money out of the %s shelf and place it on the '
'table' % self.shelf[index],
'take the stack of money out of the %s shelf and place it on '
'the table' % self.shelf_alt[index],
'take one of the stacks of money out of the %s bit of the safe' %
self.shelf[index],
'take the money off of the %s shelf of the safe' %
self.shelf_alt[index],
'grasp the bank notes in the %s and remove it from the safe' %
self.shelf[index],
'get the money from the %s shelf' % self.shelf_alt[index],
'retrieve the stack of bank notes from the %s of the safe' %
self.shelf[index],
'locate the money on the %s shelf, grasp it, remove if from the'
' safe, and set it on the table' % self.shelf[index],
'put the money on the %s shelf down on the table' %
self.shelf_alt[index]
]
class EnvPollos(Env):
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
"""
self.vrep = vrep
logging.basicConfig(level=logging.DEBUG)
# they have to be simmetric. We interpret actions as angular increments
#self.action_space = Box(low=np.array([-0.1, -1.7, -2.5, -1.5, 0.0, 0.0]),
# high=np.array([0.1, 1.7, 2.5, 1.5, 0.0, 0.0]))
inc = 0.1
self.action_space = Box(low=np.array([-inc, -inc, -inc, -inc, 0,
-inc]),
high=np.array([inc, inc, inc, inc, 0, inc]))
self.observation_space = Box(low=np.array([-0.5, 0.0, -0.2]),
high=np.array([0.5, 1.0, 1.0]))
#
self.pr = PyRep()
self.pr.launch(SCENE_FILE, headless=False)
self.pr.start()
self.agent = UR10()
self.agent.max_velocity = 1
self.agent.set_control_loop_enabled(True)
self.agent.set_motor_locked_at_zero_velocity(True)
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]
self.high_joints_limits = [0.1, 1.7, 2.7, 0.0, 0.02, 0.3]
self.low_joints_limits = [-0.1, -0.2, 0.0, -1.5, -0.02, -0.5]
#self.agent_hand = Shape('hand')
self.initial_agent_tip_position = self.agent.get_tip().get_position()
self.initial_agent_tip_quaternion = self.agent.get_tip(
).get_quaternion()
self.initial_agent_tip_euler = self.agent.get_tip().get_orientation()
self.target = Dummy('UR10_target')
self.initial_target_orientation = self.target.get_orientation()
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')
#self.succion = Shape('suctionPad')
self.waypoints = [Dummy('dummy_gancho%d' % i) for i in range(4)]
# 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 reset(self):
pos = list(
np.random.uniform([-0.1, -0.1, 0.0], [0.1, 0.1, 0.1]) +
self.initial_pollo_position)
self.pollo.set_position(pos)
self.pollo.set_quaternion(self.initial_pollo_orientation)
self.agent.set_joint_positions(self.initial_joint_positions)
self.initial_epoch_time = time.time()
while True: # wait for arm to stop
self.pr.step() # Step the physics simulation
a = self.agent.get_joint_velocities()
if not np.any(np.where(np.fabs(a) < 0.1, False, True)):
break
print("ENV RESET DONE")
return self._get_state()
def step(self, action):
if action is None:
self.pr.step()
return self._get_state(), 0.0, False, {}
# check for nan
if np.any(np.isnan(action)):
print(action)
return self._get_state(), -1.0, False, {}
# action[1] = np.interp(action[1], [-1,7,1.7], [-0.2,1.7])
# action[2] = np.interp(action[2], [-2.5,2.5], [0,2.5])
# action[3] = np.interp(action[3], [-1.5,1.5], [-1.5,0])
# add actions as increments to current joints value
new_joints = np.array(
self.agent.get_joint_positions()) + np.array(action)
# check limits
if np.any(np.greater(new_joints, self.high_joints_limits)) or np.any(
np.less(new_joints, self.low_joints_limits)):
logging.debug("New joints value out of limits r=-10")
return self._get_state(), -10.0, True, {}
# move the robot and wait to stop
self.agent.set_joint_target_positions(new_joints)
reloj = time.time()
while True: # wait for arm to stop
self.pr.step() # Step the physics simulation
a = self.agent.get_joint_velocities()
if not np.any(np.where(np.fabs(a) < 0.1, False,
True)) or (time.time() - reloj) > 3:
break
# compute relevant magnitudes
np_pollo_target = np.array(self.pollo_target.get_position())
np_robot_tip_position = np.array(self.agent.get_tip().get_position())
dist = np.linalg.norm(np_pollo_target - np_robot_tip_position)
altura = np.clip((np_pollo_target[2] - self.initial_pollo_position[2]),
0, 0.5)
for obj in self.scene_objects: # colisión con la mesa
if self.agent_hand.check_collision(obj):
logging.debug("Collision with objects r=-10")
return self._get_state(), -10.0, True, {}
if altura > 0.3: # pollo en mano
logging.debug("Height reached !!! r=0")
return self._get_state(), 30.0, True, {}
elif dist > self.initial_distance: # mano se aleja
logging.debug("Hand moving away from chicken r=-10")
return self._get_state(), -10.0, True, {}
if time.time() - self.initial_epoch_time > 5: # too much time
logging.debug("Time out. End of epoch r=-10")
return self._get_state(), -10.0, True, {}
# Reward
#pollo_height = np.exp(-altura*20) # para 0.3m pollo_height = 0.002, para 0m pollo_height = 1
reward = altura - dist
logging.debug("New joints value out of limits r=-10")
return self._get_state(), reward, False, {}
def close(self):
self.pr.stop()
self.pr.shutdown()
def render(self):
print("RENDER")
np_pollo_en_camara = self.getPolloEnCamara()
# capture depth image
depth = self.camera.capture_rgb()
circ = plt.Circle(
(int(np_pollo_en_camara[0]), int(np_pollo_en_camara[1])), 10)
plt.clf()
fig = plt.gcf()
ax = fig.gca()
ax.add_artist(circ)
ax.imshow(depth, cmap='hot')
plt.pause(0.000001)
# Aux
# transform env.pollo_target.get_position() to camera coordinates and project pollo_en_camera a image coordinates
def getPolloEnCamara(self):
np_pollo_target = np.array(self.pollo_target.get_position())
np_pollo_target_cam = self.np_camera_matrix_extrinsics.dot(
np.append([np_pollo_target], 1.0))
np_pollo_en_camara = self.np_camera_matrix_intrinsics.dot(
np_pollo_target_cam)
np_pollo_en_camara = np_pollo_en_camara / np_pollo_en_camara[2]
np_pollo_en_camara = np.delete(np_pollo_en_camara, 2)
return np_pollo_en_camara
def getPolloEnCamaraEx(self):
np_pollo_target = np.array(self.pollo_target.get_position())
np_pollo_en_camara = self.np_camera_matrix_extrinsics.dot(
np.append([np_pollo_target], 1.0))
return np_pollo_en_camara
def _get_state(self):
# Return state containing arm joint angles/velocities & target position
# return (self.agent.get_joint_positions() +
# self.agent.get_joint_velocities() +
# self.pollo_target.get_position())
p = self.getPolloEnCamaraEx()
j = self.agent.get_joint_positions()
#r = np.array([p[0],p[1],p[2],j[0],j[1],j[2],j[3],j[4],j[5]])
r = np.array([p[0], p[1], p[2]])
return r
def vrepToNP(self, c):
return np.array([[c[0], c[4], c[8], c[3]], [c[1], c[5], c[9], c[7]],
[c[2], c[6], c[10], c[11]], [0, 0, 0, 1]])
def test_get_linear_path(self):
arm = Panda()
waypoint = Dummy('Panda_waypoint')
path = arm.get_linear_path(
waypoint.get_position(), waypoint.get_orientation())
self.assertIsNotNone(path)
class TakeMoneyOutSafe(Task):
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_shelf = index
target_dummy_name = 'dummy_shelf' + str(self.target_shelf)
target_pos_dummy = Dummy(target_dummy_name)
target_pos = target_pos_dummy.get_position()
target_ori = target_pos_dummy.get_orientation()
self.w1.set_position(target_pos, reset_dynamics=False)
self.w1.set_orientation(target_ori, reset_dynamics=False)
b_place = SpawnBoundary([self.place_boundary])
b_place.sample(self.w3,
min_rotation=(0.0, 0.0, -np.pi / 12),
max_rotation=(0.0, 0.0, +np.pi / 12),
min_distance=0)
self.success_detector = ProximitySensor('success_detector')
while len(self.success_conditions) > 1:
self.success_conditions.pop()
self.success_conditions.append(
DetectedCondition(self.money_list[self.target_shelf],
self.success_detector))
self.register_success_conditions(self.success_conditions)
# self.w1.set_parent(target_pos_dummy, keep_in_place=True)
rel_target_pos = self.w1.get_position(
relative_to=self.money_list[index])
rel_target_ori = self.w1.get_orientation(
relative_to=self.money_list[index])
b_pick = SpawnBoundary([self.pick_boundary])
b_pick.sample(self.money_list[index],
min_rotation=(0.0, 0.0, -np.pi / 10),
max_rotation=(0.0, 0.0, 0.0),
min_distance=0)
for m in self.money_list:
b_pick.sample(m,
min_rotation=(0.0, 0.0, -np.pi / 10),
max_rotation=(0.0, 0.0, 0.0),
min_distance=0)
self.w1.set_position(rel_target_pos,
relative_to=self.money_list[index],
reset_dynamics=False)
self.w1.set_orientation(rel_target_ori,
relative_to=self.money_list[index],
reset_dynamics=False)
"""
if self.target_shelf == 0:
return ['take the money out of the bottom shelf and place it on '
'the table']
elif self.target_shelf == 1:
return ['take the money out of the middle shelf and place it on '
'the table']
elif self.target_shelf == 2:
return ['take the money out of the top shelf and place it on '
'the table']
else:
raise ValueError('Invalid target_shelf index, should not be here')
return -1
"""
return [
'take the money out of the %s shelf and place it on the '
'table' % self.shelf[index],
'take the stack of money out of the %s shelf and place it on '
'the table' % self.shelf_alt[index],
'take one of the stacks of money out of the %s bit of the safe' %
self.shelf[index],
'take the money off of the %s shelf of the safe' %
self.shelf_alt[index],
'grasp the bank notes in the %s and remove it from the safe' %
self.shelf[index],
'get the money from the %s shelf' % self.shelf_alt[index],
'retrieve the stack of bank notes from the %s of the safe' %
self.shelf[index],
'locate the money on the %s shelf, grasp it, remove if from the'
' safe, and set it on the table' % self.shelf[index],
'put the money on the %s shelf down on the table' %
self.shelf_alt[index]
]
def variation_count(self) -> int:
return NUM_SHELVES_IN_SAFE
def cleanup(self) -> None:
for m in self.money_list:
m.set_orientation(self.money_ori, reset_dynamics=False)
def base_rotation_bounds(self) -> Tuple[List[float], List[float]]:
return (0.0, 0.0, -0.5 * np.pi), (0.0, 0.0, +0.5 * np.pi)
