def test_get_object(self):
self.assertEqual(Object.get_object('dynamic_cube'), self.dynamic_cube)
self.assertEqual(Object.get_object('dummy'), self.dummy)
self.assertEqual(Object.get_object(self.dynamic_cube.get_handle()),
self.dynamic_cube)
self.assertEqual(Object.get_object(self.dummy.get_handle()),
self.dummy)
def _get_objects_in_tree(root_object=None,
object_type=ObjectType.ALL,
exclude_base=True,
first_generation_only=False) -> List['Object']:
if root_object is None:
root_object = sim.sim_handle_scene
elif isinstance(root_object, Object):
root_object = root_object.get_handle()
elif not isinstance(root_object, int):
raise ValueError('root_object must be None, int or Object')
options = 0
if exclude_base:
options |= 1
if first_generation_only:
options |= 2
handles = sim.simGetObjectsInTree(root_object, object_type.value,
options)
objects = []
for handle in handles:
try:
objects.append(Object.get_object(handle))
except KeyError:
# e.g., CAMERA and LIGHT are not officially supported
name = Object.get_object_name(handle)
type = Object.get_object_type(name)
warnings.warn("Object ({}, '{}') has {}, "
'which is not supported'.format(
handle, name, type))
return objects
def __init__(self, proxy_map):
super(SpecificWorker, self).__init__(proxy_map)
self.data = {
'walls': [],
'goal': [],
'humans': None,
'robot_position': None,
'robot_orientation': None,
'simulator_mutex': threading.RLock()
}
self.soda = SODA(proxy_map,
self.data,
scene_file='dataset_lowres_top.ttt')
self.min_max_data = {
"min_humans": 0,
"max_humans": 4,
"min_wandering_humans": 0,
"max_wandering_humans": 4,
"min_tables": 0,
"max_tables": 4,
"min_plants": 0,
"max_plants": 4,
"min_relations": 0,
"max_relations": 4
}
self.data, self.wandering_humans = self.soda.room_setup(
self.min_max_data['min_humans'],
self.min_max_data['min_wandering_humans'],
self.min_max_data['min_plants'],
self.min_max_data['min_tables'],
self.min_max_data['min_relations'],
self.min_max_data['max_humans'],
self.min_max_data['max_wandering_humans'],
self.min_max_data['max_plants'],
self.min_max_data['max_tables'],
self.min_max_data['max_relations'],
robot_random_pose=False,
show_goal=False,
show_relations=False)
#Loop
self.adv = self.rot = 0.
self.last_ten = time.time() - 10
self.last_point_one = time.time() - 10
self.end_simulation = False
self.vision_sensor = Object.get_object('Vision_sensor')
def get_object(self, name: str):
return Object.get_object(name)
def resetTask(task):
obs = task.get_observation()
obj_poses = obj_pose_sensor.get_poses()
surface = Object.get_object('worksurface')
#get items in cupboard
in_cupboard = []
print('started reseting')
for k in [
'crackers', 'mustard', 'coffee', 'sugar', 'spam', 'tuna', 'soup',
'strawberry_jello', 'chocolate_jello'
]:
if check_if_in_cupboard(k, obj_poses):
in_cupboard.append(k)
#drop anything in hand
actions = agent2.ungrasp_object(obs)
obs, reward, terminal = task.step(actions)
#move to start position
actions = agent2.move_to_pos([0.25, 0, 1], False)
obs, reward, terminal = task.step(actions)
print('moved to start')
while len(in_cupboard) > 0:
#move to above object location
if (len(in_cupboard) > 1):
random = np.random.randint(len(in_cupboard) - 1)
print(random)
obj = in_cupboard[random]
else:
obj = in_cupboard[0]
actions = agent2.move_above_cabinet(obj_poses, obj, False)
obs, reward, terminal = task.step(actions)
print('move above cabinet')
target_obj = Object.get_object(obj)
#attempt straight grasp
grasped = False
actions = agent2.move_to_cabinet_object(obj_poses, obj, False)
prev_forces = obs.joint_forces
while np.linalg.norm(obs.gripper_pose -
actions[:-1]) > 0.01 and not grasped and np.sum(
np.abs(obs.joint_forces - prev_forces)) <= 50:
prev_forces = obs.joint_forces
print('stepping to target')
obs, reward, terminate = task.step(actions)
grasped = task._robot.gripper.grasp(target_obj)
print(obj, grasped)
#if failed kick the object to the back of the line and try another
#if (not grasped):
# in_cupboard.append(obj)
# print('kicking')
# continue
#remove from cabinet
actions = agent2.move_above_cabinet_num(obs, obj_poses, 5)
obs, reward, terminal = task.step(actions)
print('moved above cabinet_num')
#place on table
print('place on table')
# Go to post-grasp location
actions = [0.25, 0, 0.99, 0, 1, 0, 0, 0]
prev_forces = obs.joint_forces
while np.linalg.norm(obs.gripper_pose - actions[:-1]) > 0.01 and (
np.sum(np.abs(obs.joint_forces - prev_forces)) <= 50):
prev_forces = obs.joint_forces
print('stepping to post-grasp staging')
obs, reward, terminate = task.step(actions)
print('moved to post-grasp location', actions, obs.gripper_pose)
while grasped:
reset_x, reset_y, reset_z = sample_reset_pos(surface)
print('Randomly chosen reset location: ', reset_x, ', ', reset_y)
_, _, _, _, target_zmin, target_zmax = target_obj.get_bounding_box(
)
actions = [
reset_x, reset_y, target_zmax - target_zmin + reset_z, 0, 1, 0,
0, 0
]
print('Reset location actions: ', actions)
try:
prev_forces = obs.joint_forces
while np.linalg.norm(
obs.gripper_pose - actions[:-1]) > 0.01 and (np.sum(
np.abs(obs.joint_forces - prev_forces)) <= 50):
prev_forces = obs.joint_forces
print('stepping to reset location')
obs, reward, terminate = task.step(actions)
except ConfigurationPathError:
print('Bad choice! Pick again.')
continue
print('moved to reset location', actions, obs.gripper_pose)
task._robot.gripper.release()
grasped = False
print('nextobject')
in_cupboard.clear()
obj_poses = obj_pose_sensor.get_poses()
for k in [
'crackers', 'mustard', 'coffee', 'sugar', 'spam', 'tuna',
'soup', 'strawberry_jello', 'chocolate_jello'
]:
if check_if_in_cupboard(k, obj_poses):
in_cupboard.append(k)
#open hand
actions = agent2.ungrasp_object(obs)
obs, reward, terminal = task.step(actions)
#move to start position
actions = agent2.move_to_pos([0.25, 0, 1])
obs, reward, terminal = task.step(actions)
print('finished resetting')
descriptions = None
#original reset task
#descriptions, obs = task.reset()
return descriptions, obs
print('moved above object')
actions = agent2.grasp_object(obs)
obs, reward, terminal = task.step(actions)
print('grasp object')
actions = agent2.move_above_cabinet_num(obs, obj_poses,
1 + item_number)
obs, reward, terminal = task.step(actions)
print('moved above cabinet')
actions = agent2.move_into_cabinet(obs, obj_poses, 1 + item_number)
obs, reward, terminal = task.step(actions)
print('moved into cabinet')
target_obj = Object.get_object(item)
while (not task._robot.gripper._grasped_objects == []):
task._robot.gripper.release()
actions = agent2.ungrasp_object(obs)
obs, reward, terminal = task.step(actions)
print('ungrasp object')
actions = agent2.move_above_cabinet_num(obs, obj_poses,
1 + item_number)
obs, reward, terminal = task.step(actions)
print('moved above cabinet num')
resetTask(task)
condition = False
if (condition):
for obj in objs:
name = obj.get_name()
pose = obj.get_pose()
obj_poses[name] = pose
return obj_poses
if __name__ == "__main__":
action_mode = ActionMode(ArmActionMode.ABS_EE_POSE_PLAN) # See rlbench/action_modes.py for other action modes
env = Environment(action_mode, '', ObservationConfig(), False)
# available tasks: EmptyContainer, PlayJenga, PutGroceriesInCupboard, SetTheTable
task = env.get_task(PutGroceriesInCupboard)
surface = Object.get_object('worksurface')
print('Surface bounding box: ', surface.get_bounding_box())
agent = AutonAgentAbsolute_Mode()
obj_pose_sensor = HonestObjectPoseSensor(env)
descriptions, obs = task.reset()
print(descriptions)
for _ in range(5):
# Go to staging location
actions = [0.25, 0, 0.99, 0, 1, 0, 0, 0]
while np.linalg.norm(obs.gripper_pose - actions[:-1]) > 0.01:
print('stepping to staging')
obs, reward, terminate = task.step(actions)
