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
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)
#!/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!")
