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 test_remove(self):
self.dynamic_cube.remove()
self.simple_model.remove()
self.assertFalse(self.dynamic_cube.still_exists())
self.assertFalse(self.simple_model.still_exists())
self.assertFalse(Object.exists('dynamic_cube'))
self.assertFalse(Object.exists('simple_model'))
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, handle: Object):
super(Human, self).__init__()
self.handle = handle
handle.value = 0
children = handle.get_objects_in_tree(handle)
for child in children:
name = child.get_name()
if 'Bill_goalPosCylinder' in name:
self.dummy_handle = child.get_parent()
def get_object(name_or_handle: str) -> 'Object':
"""Gets object retrieved by name.
:return: The object.
"""
name = Object.get_object_name(name_or_handle)
object_type = Object.get_object_type(name)
cls = object_type_to_class[object_type]
return cls(name)
def sample_reset_pos(area: Object):
minx, maxx, miny, maxy, _, _ = area.get_bounding_box()
pose = area.get_pose()
print('Surface pose: ', pose)
x = np.random.uniform(minx, maxx) + pose[0]
y = np.random.uniform(miny, maxy) + pose[1]
z = pose[2] + 0.05
return x, y, z
def grasp(self, obj: Object) -> bool:
"""Grasp object if it is detected.
Note: The does not actuate the gripper, but instead simply attaches the
detected object to the gripper to 'fake' a grasp.
:param obj: The object to grasp if detected.
:return: True if the object was detected/grasped.
"""
detected = self._proximity_sensor.is_detected(obj)
if detected:
self._grasped_objects.append(obj)
self._old_parents.append(obj.get_parent())
obj.set_parent(self._attach_point, keep_in_place=True)
return detected
def subtract_object(self, obj: Object, options: int = 0) -> None:
"""Subtract object from the octree.
:param obj: Object to subtract.
:param options: Object subtraction options.
"""
sim.simSubtractObjectFromOctree(self._handle, obj.get_handle(), options)
return
def grasp(self, obj: Object) -> bool:
"""Attach the object to the suction cup if it is detected.
Note: The does not move the object up to the suction cup. Therefore, the
proximity sensor should have a short range in order for the suction
grasp to look realistic.
:param obj: The object to grasp if detected.
:return: True if the object was detected/grasped.
"""
detected = self._proximity_sensor.is_detected(obj)
# Check if detected and that we are not already grasping it.
if detected and obj not in self._grasped_objects:
self._grasped_objects.append(obj)
self._old_parents.append(obj.get_parent()) # type: ignore
obj.set_parent(self._attach_point, keep_in_place=True)
return detected
def _get_waypoints(self, validating=False) -> List[Waypoint]:
waypoint_name = 'waypoint%d'
waypoints = []
additional_waypoint_inits = []
i = 0
while True:
name = waypoint_name % i
if not Object.exists(name):
# There are no more waypoints...
break
ob_type = Object.get_object_type(name)
way = None
if ob_type == ObjectType.DUMMY:
waypoint = Dummy(name)
start_func = None
end_func = None
if i in self._waypoint_abilities_start:
start_func = self._waypoint_abilities_start[i]
if i in self._waypoint_abilities_end:
end_func = self._waypoint_abilities_end[i]
way = Point(waypoint,
self.robot,
start_of_path_func=start_func,
end_of_path_func=end_func)
elif ob_type == ObjectType.PATH:
cartestian_path = CartesianPath(name)
way = PredefinedPath(cartestian_path, self.robot)
else:
raise WaypointError(
'%s is an unsupported waypoint type %s' % (name, ob_type),
self)
if name in self._waypoint_additional_inits and not validating:
additional_waypoint_inits.append(
(self._waypoint_additional_inits[name], way))
waypoints.append(way)
i += 1
# Check if all of the waypoints are feasible
feasible, way_i = self._feasible(waypoints)
if not feasible:
raise WaypointError(
"Infeasible episode. Can't reach waypoint %d." % way_i, self)
for func, way in additional_waypoint_inits:
func(way)
return waypoints
def __init__(self, boundary: Object):
self._boundary = boundary
self._is_plane = False
self._contained_objects = []
if boundary.is_model():
(minx, maxx, miny,
maxy, minz, maxz) = boundary.get_model_bounding_box()
else:
minx, maxx, miny, maxy, minz, maxz = boundary.get_bounding_box()
self._boundary_bbox = BoundingBox(minx, maxx, miny, maxy, minz, maxz)
height = np.abs(maxz - minz)
if height == 0:
height = 1.0
self._is_plane = True
self._area = np.abs(maxx - minx) * np.abs(maxy - miny) * height
def __init__(self, handle: Object):
super(HumanOnPath, self).__init__()
self.handle = handle
handle.value = 0
children = self.handle.get_objects_in_tree()
for child in children:
name = child.get_name()
if 'Bill_base' in name:
self.human_handle = child
def is_detected(self, obj: Object) -> bool:
"""Checks whether the proximity sensor detects the indicated object.
:param obj: The object to detect.
:return: Bool indicating if the object was detected.
"""
state, point = vrep.simCheckProximitySensor(self._handle,
obj.get_handle())
return state == 1
def load(self) -> Object:
if Object.exists(self.get_name()):
return Dummy(self.get_name())
ttm_file = os.path.join(os.path.dirname(os.path.abspath(__file__)),
'../task_ttms/%s.ttm' % self.name)
if not os.path.isfile(ttm_file):
raise FileNotFoundError(
'The following is not a valid task .ttm file: %s' % ttm_file)
self._base_object = self.pyrep.import_model(ttm_file)
return self._base_object
def __init__(self, waypoint: Object, robot: Robot, ignore_collisions=False,
start_of_path_func=None, end_of_path_func=None):
self._waypoint = waypoint
self._robot = robot
self._ext = waypoint.get_extension_string()
self._ignore_collisions = ignore_collisions
self._linear_only = False
self._start_of_path_func = start_of_path_func
self._end_of_path_func = end_of_path_func
if len(self._ext) > 0:
self._ignore_collisions = 'ignore_collision' in self._ext
self._linear_only = 'linear' in self._ext
def get_objects_in_tree(self, root_object=None, *args, **kwargs
) -> List[Object]:
"""Retrieves the objects in a given hierarchy tree.
:param root_object: The root object in the tree. Pass None to retrieve
all objects in the configuration tree. :py:class:`Object` or `int`.
:param object_type: The object type to retrieve.
One of :py:class:`.ObjectType`.
:param exclude_base: Exclude the tree base from the returned list.
:param first_generation_only: Include in the returned list only the
object's first children. Otherwise, entire hierarchy is returned.
:return: A list of objects in the hierarchy tree.
"""
return Object._get_objects_in_tree(root_object, *args, **kwargs)
def __init__(self, handle: Object):
super(Human, self).__init__()
self.handle = handle
handle.value = 0
children = handle.get_objects_in_tree(handle)
for child in children:
name = child.get_name()
if 'Bill_goalPosCylinder' in name:
self.dummy_handle = child.get_parent()
# self.dummy_handle._set_property(prop_type: int, value: bool) -> None:
pos = self.handle.get_position()
self.ss1 = Shape.create(type=PrimitiveShape.CONE,
color=[1, 0, 0],
size=[0.75, 0.75, 0.0015],
position=[pos[0], pos[1], 0],
orientation=[3.14, 0, 3.14])
self.ss1.set_color([1, 0, 0])
self.ss1.set_position([pos[0], pos[1], 0])
self.ss1.set_orientation([3.14, 0, 3.14])
self.ss1.set_dynamic(False)
self.ss1.set_renderable(False)
self.set_model(True)
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_position_within_boundary(self, obj: Object, obj_bbox: BoundingBox
) -> List[float]:
x = np.random.uniform(
self._boundary_bbox.min_x + np.abs(obj_bbox.min_x),
self._boundary_bbox.max_x - np.abs(obj_bbox.max_x))
y = np.random.uniform(
self._boundary_bbox.min_y + np.abs(obj_bbox.min_y),
self._boundary_bbox.max_y - np.abs(obj_bbox.max_y))
if self._is_plane:
_, _, z = obj.get_position(self._boundary)
else:
z = np.random.uniform(
self._boundary_bbox.min_z + np.abs(obj_bbox.min_z),
self._boundary_bbox.max_z - np.abs(obj_bbox.max_z))
return [x, y, z]
def insert_object(self, obj: Object, color: Optional[float] = None,
options: int = 0) -> None:
"""Inserts object into the octree.
:param obj: Object to insert.
:param color: A list containing RGB data, or None.
:param options: Object insertion options.
"""
if color is not None:
if not isinstance(color, list):
raise ValueError(
'Octree.insert_object: color parameter not a list.')
elif len(color) is not 3:
raise ValueError(
'Octree.insert_object: color parameter not an RGB list.')
sim.simInsertObjectIntoOctree(self._handle, obj.get_handle(), options,
color, 0)
return
def add(self, obj: Object, ignore_collisions: bool = False,
min_rotation: tuple = (0.0, 0.0, -3.14),
max_rotation: tuple = (0.0, 0.0, 3.14),
min_distance: float = 0.01) -> int:
"""Returns true if can add and adds it
rotation_limits: how mush we allow it to rotate from its original
position"""
# Rotate the bounding box randomly
if obj.is_model():
bb = obj.get_model_bounding_box()
else:
bb = obj.get_bounding_box()
obj_bbox = BoundingBox(*bb)
rotation = np.random.uniform(list(min_rotation), list(max_rotation))
obj_bbox = obj_bbox.rotate(rotation)
if not obj_bbox.within_boundary(self._boundary_bbox, self._is_plane):
return -1
new_pos = self._get_position_within_boundary(obj, obj_bbox)
obj.set_position(new_pos, self._boundary)
obj.rotate(list(rotation))
new_pos = np.array(new_pos)
if not ignore_collisions:
for contained_obj in self._contained_objects:
# Check for collision between each child
for cont_ob in contained_obj.get_objects_in_tree(
exclude_base=False):
for placing_ob in obj.get_objects_in_tree(
exclude_base=False):
if placing_ob.check_collision(cont_ob):
return -2
dist = np.linalg.norm(
new_pos - contained_obj.get_position(self._boundary))
if dist < min_distance:
return -3
self._contained_objects.append(obj)
return 1
def init_task(self) -> None:
self.target_block = Shape('pick_and_lift_target')
self.target = Shape("success_visual")
self.target.set_renderable(False)
self.register_graspable_objects([self.target_block])
self.boundary = SpawnBoundary([Shape('pick_and_lift_boundary')])
self.success_detector = ProximitySensor('pick_and_lift_success')
self.front_camera_exists = Object.exists('cam_front')
if self.front_camera_exists:
self.front_camera = VisionSensor('cam_front')
self.init_front_camera_position = self.front_camera.get_position()
self.init_front_camera_orientation = self.front_camera.get_orientation(
)
self.panda_base = Shape("Panda_link0_visual")
cond_set = ConditionSet([
GraspedCondition(self.robot.gripper, self.target_block),
DetectedCondition(self.target_block, self.success_detector)
])
self.register_success_conditions([cond_set])
def init_task(self) -> None:
self.target = Shape('target')
self.distractor0 = Shape('distractor0')
self.distractor1 = Shape('distractor1')
self.target.set_renderable(True)
self.distractor0.set_renderable(False)
self.distractor1.set_renderable(False)
self.front_camera_exists = Object.exists('cam_front')
if self.front_camera_exists:
self.front_camera = VisionSensor('cam_front')
#curr_pos = self.front_camera.get_position()
# new_pos = curr_pos + np.array([-1.25, 1.6 , -0.35])
# self.front_camera.set_position(new_pos)
# curr_ori = self.front_camera.get_orientation()
# new_ori = np.array([1.6, 0, 0])
# self.front_camera.set_orientation(new_ori)
# front pos
#new_pos = curr_pos + np.array([0.0, 0.0 , -0.2])
#self.front_camera.set_position(new_pos)
#curr_ori = self.front_camera.get_orientation()
#new_ori = curr_ori + np.array([0, -0.25, 0])
#self.front_camera.set_orientation(new_ori)
self.step_in_episode = 0
self.obstacle = Shape.create(type=PrimitiveShape.SPHERE,
size=[0.3, 0.3, 0.3],
renderable=True,
respondable=True,
static=True,
position=[0.1, 0.1, 1.4],
color=[0, 0, 0])
self.boundaries = Shape('boundary')
success_sensor = ProximitySensor('success')
self.register_success_conditions(
[DetectedCondition(self.robot.arm.get_tip(), success_sensor)])
def __init__(self, gripper: Gripper, object: Object):
self._gripper = gripper
self._object_handle = object.get_handle()
def test_object_exists(self):
yes = Object.exists('dynamic_cube')
no = Object.exists('dynamic_cubeee')
self.assertTrue(yes)
self.assertFalse(no)
def test_get_object_name(self):
self.assertEqual(Object.get_object_name('dynamic_cube'),
'dynamic_cube')
self.assertEqual(
Object.get_object_name(self.dynamic_cube.get_handle()),
'dynamic_cube')
def test_get_object_type(self):
self.assertEqual(Object.get_object_type('dynamic_cube'),
ObjectType.SHAPE)
self.assertEqual(Object.get_object_type('dummy'), ObjectType.DUMMY)
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):
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
def get_object(self, name: str):
return Object.get_object(name)
