def DetectObjects(self, robot, **kw_args): """ Overriden method for detection_frame """ from prpy.perception.base import PerceptionException env = robot.GetEnv() # Detecting empty list will detect all possible objects detections = self._GetDetections([]) for (obj_name, obj_pose) in detections: if (obj_name not in self.query_to_kinbody_map): continue kinbody_name = self.query_to_kinbody_map[obj_name] if env.GetKinBody(kinbody_name) is None: from prpy.rave import add_object kinbody_file = '%s.kinbody.xml' % kinbody_name new_body = add_object( env, kinbody_name, os.path.join(self.kinbody_path, kinbody_file)) print kinbody_name body = env.GetKinBody(kinbody_name) body.SetTransform(obj_pose)
def DetectObject(self, robot, obj_name, **kw_args): """ Detect a single object @param robot The OpenRAVE robot @param obj_name The name of the object to detect @returns A kinbody placed in the detected location @throws PerceptionException if the object is not detected """ from prpy.perception.base import PerceptionException if obj_name not in self.kinbody_to_query_map: raise PerceptionException( 'The VNCC module cannot detect object {:s}'.format(obj_name)) query_name = self.kinbody_to_query_map[obj_name] obj_pose = self._GetDetection(query_name) if obj_pose is None: raise PerceptionException( 'Failed to detect object {:s}'.format(obj_name)) env = robot.GetEnv() if env.GetKinBody(obj_name) is None: from prpy.rave import add_object kinbody_file = '{:s}.kinbody.xml'.format(obj_name) new_body = add_object( env, obj_name, os.path.join(self.kinbody_path, kinbody_file)) body = env.GetKinBody(obj_name) body.SetTransform(obj_pose) return body
def DetectObject(self, robot, obj_name, **kw_args): """ Detect a single object @param robot The OpenRAVE robot @param obj_name The name of the object to detect @returns A kinbody placed in the detected location @throws PerceptionException if the object is not detected """ from prpy.perception.base import PerceptionException if obj_name not in self.kinbody_to_query_map: raise PerceptionException("The VNCC module cannot detect object {:s}".format(obj_name)) query_name = self.kinbody_to_query_map[obj_name] obj_pose = self._GetDetection(query_name) if obj_pose is None: raise PerceptionException("Failed to detect object {:s}".format(obj_name)) env = robot.GetEnv() if env.GetKinBody(obj_name) is None: from prpy.rave import add_object kinbody_file = "{:s}.kinbody.xml".format(obj_name) new_body = add_object(env, obj_name, os.path.join(self.kinbody_path, kinbody_file)) body = env.GetKinBody(obj_name) body.SetTransform(obj_pose) return body
def detect_objects(robot): """ Return table and glass objects relative to the robot. Add table and glass objects to the world. @param robot: OpenRAVE robot @return table and glass KinBodies """ env = robot.GetEnv() with env: robot_in_world = robot.GetTransform() table_in_world = numpy.dot(robot_in_world, TABLE_IN_ROBOT) table = add_object(env, 'table', 'furniture/table.kinbody.xml', table_in_world) glass = add_object(env, 'glass', 'objects/plastic_glass.kinbody.xml', numpy.dot(table_in_world, GLASS_IN_TABLE)) return table, glass
def DetectObject(self, robot, obj_name, **kw_args): """ Detect a single object @param robot The OpenRAVE robot @param obj_name The name of the object to detect @returns A kinbody placed in the detected location @throws PerceptionException if the object is not detected """ from prpy.perception.base import PerceptionException if obj_name not in self.kinbody_to_query_map: raise PerceptionException('The simtrack module cannot detect object %s', obj_name) query_name = self.kinbody_to_query_map[obj_name] obj_poses = self._GetDetections(query_name) if len(obj_poses is 0): raise PerceptionException('Failed to detect object %s', obj_name) obj_pose = None for (name, pose) in obj_poses: if (name is query_name): obj_pose = pose break; if (obj_pose is None): raise PerceptionException('Failed to detect object %s', obj_name) env = robot.GetEnv() if env.GetKinBody(obj_name) is None: from prpy.rave import add_object kinbody_file = '%s.kinbody.xml' % obj_name new_body = add_object( env, obj_name, os.path.join(self.kinbody_path, kinbody_file)) body = env.GetKinBody(obj_name) body.SetTransform(obj_pose) return body
def DetectObject(self, robot, obj_name, **kw_args): """ Detect a single object @param robot The OpenRAVE robot @param obj_name The name of the object to detect @returns A kinbody placed in the detected location @throws PerceptionException if the object is not detected """ from prpy.perception.base import PerceptionException if obj_name not in self.kinbody_to_query_map: raise PerceptionException( 'The simtrack module cannot detect object %s', obj_name) query_name = self.kinbody_to_query_map[obj_name] obj_poses = self._GetDetections(query_name) if len(obj_poses is 0): raise PerceptionException('Failed to detect object %s', obj_name) obj_pose = None for (name, pose) in obj_poses: if (name is query_name): obj_pose = pose break if (obj_pose is None): raise PerceptionException('Failed to detect object %s', obj_name) env = robot.GetEnv() if env.GetKinBody(obj_name) is None: from prpy.rave import add_object kinbody_file = '%s.kinbody.xml' % obj_name new_body = add_object( env, obj_name, os.path.join(self.kinbody_path, kinbody_file)) body = env.GetKinBody(obj_name) body.SetTransform(obj_pose) return body
#!/usr/bin/env python import humanpy import numpy import IPython from prpy.rave import add_object if __name__ == "__main__": env, robot = humanpy.initialize(attach_viewer=True) with env: fuze = add_object(env, 'fuze_bottle', 'objects/fuze_bottle.kinbody.xml') glass = add_object(env, 'plastic_glass', 'objects/plastic_glass.kinbody.xml') fuze_pose = numpy.eye(4) glass_pose = numpy.eye(4) fuze_pose[0:3, 3] = [ 0.8, -0.3, 0.4 ] glass_pose[0:3, 3] = [ 0.9, 0, 0.4 ] fuze.SetTransform(fuze_pose) glass.SetTransform(glass_pose) fuze.Enable(True) glass.Enable(True) robotLocation = numpy.array([[ 0. , 0. , 1. , 0.4], [ 1. , 0. , 0. , -0.1], [ 0. , 1. , 0. , 0.3], [ 0. , 0. , 0. , 1. ]]) robot.SetTransform(robotLocation) robot.right_arm.SetActive() #robot.right_arm.Grasp(fuze) raw_input("press enter to quit!")