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)