def draw_robot_at_conf(conf, transparency, name, robot, env, color=None):
held_obj = robot.GetGrabbed()
before = get_body_xytheta(robot)
newrobot = RaveCreateRobot(env, '')
newrobot.Clone(robot, 0)
newrobot.SetName(name)
env.Add(newrobot, True)
set_active_config(conf, newrobot)
newrobot.Enable(False)
if color is not None:
set_color(newrobot, color)
print get_body_xytheta(robot)
if len(held_obj) > 0:
print get_body_xytheta(held_obj[0])
set_robot_config(conf)
held_obj = robot.GetGrabbed()[0]
held_obj_trans = held_obj.GetTransform()
release_obj()
new_obj = RaveCreateKinBody(env, '')
new_obj.Clone(held_obj, 0)
new_obj.SetName(name + '_obj')
env.Add(new_obj, True)
new_obj.SetTransform(held_obj_trans)
for link in new_obj.GetLinks():
for geom in link.GetGeometries():
geom.SetTransparency(transparency)
print get_body_xytheta(robot)
grab_obj(held_obj)
set_robot_config(before)
for link in newrobot.GetLinks():
for geom in link.GetGeometries():
geom.SetTransparency(transparency)
def centered_box_body(env, dx, dy, dz, name=None, color=None, transparency=None): body = RaveCreateKinBody(env, '') body.InitFromBoxes(np.array([[0, 0, 0, .5*dx, .5*dy, .5*dz]]), draw=True) if name is not None: set_name(body, name) if color is not None: set_color(body, color) if transparency is not None: set_transparency(body, transparency) return body
def create_box(env, name, transform, dims, color=[0, 0, 1]):
infobox = OpenRAVEBody.create_body_info(dims, color, 0, True)
box = RaveCreateKinBody(env, '')
box.InitFromGeometries([infobox])
box.SetName(name)
box.SetTransform(transform)
return box
def _add_box(self, geom):
infobox = OpenRAVEBody.create_body_info(KinBody.Link.GeomType.Box,
geom.dim, [0.5, 0.2, 0.1])
self.env_body = RaveCreateKinBody(self._env, '')
self.env_body.InitFromGeometries([infobox])
self.env_body.SetName(self.name)
self._env.Add(self.env_body)
def __enter__(self):
if self.baked_kinbody is not None:
raise PrPyException(
'Another baked KinBody is available. Did you call __enter__'
' twice or forget to call __exit__?')
try:
kb_type = self.checker.SendCommand('BakeGetType')
except openrave_exception:
raise PrPyException('Collision checker does not support baking')
# TODO: How should we handle exceptions that are thrown below?
self.collision_saver.__enter__()
# This "bakes" the following Env and Self checks.
# (after the bake, the composite check is stored in self.baked_kinbody)
self.checker.SendCommand('BakeBegin')
self.env.CheckCollision(self.robot)
self.robot.CheckSelfCollision()
self.baked_kinbody = RaveCreateKinBody(self.env, kb_type)
if self.baked_kinbody is None:
raise PrPyException('Failed to create baked KinBody.')
self.checker.SendCommand('BakeEnd')
return self
def create_cylinder(env, body_name, t, dims, color=[0, 1, 1]):
infocylinder = OpenRAVEBody.create_body_info(GeometryType.Cylinder,
dims, color)
if type(env) != Environment:
import ipdb
ipdb.set_trace()
cylinder = RaveCreateKinBody(env, '')
cylinder.InitFromGeometries([infocylinder])
cylinder.SetName(body_name)
cylinder.SetTransform(t)
return cylinder
def draw_obj_at_conf(conf, transparency, name, obj, env, color=None):
before = get_body_xytheta(obj)
newobj = RaveCreateKinBody(env, '')
newobj.Clone(obj, 0)
newobj.SetName(name)
env.Add(newobj, True)
set_obj_xytheta(conf, newobj)
newobj.Enable(False)
if color is not None:
set_color(newobj, color)
set_body_transparency(newobj, transparency)
def main(env,options):
"Main example code."
if options.iktype is not None:
# cannot use .names due to python 2.5 (or is it boost version?)
for value,type in IkParameterization.Type.values.iteritems():
if type.name.lower() == options.iktype.lower():
iktype = type
break
else:
iktype = IkParameterizationType.Transform6D
env.Load(options.scene)
with env:
# load the Transform6D IK models
ikmodels = []
for robot in env.GetRobots():
for manip in robot.GetManipulators():
print manip
try:
robot.SetActiveManipulator(manip)
ikmodel = databases.inversekinematics.InverseKinematicsModel(robot, iktype=iktype)
if not ikmodel.load():
ikmodel.autogenerate()
if not ikmodel.has():
continue
ikmodels.append(ikmodel)
except Exception, e:
print 'failed manip %s'%manip, e
if len(ikmodels) == 0:
raveLogWarn('no manipulators found that can be loaded with iktype %s'%str(iktype))
# create the pickers
pickers = []
for ikmodel in ikmodels:
raveLogInfo('creating picker for %s'%str(ikmodel.manip))
picker = RaveCreateKinBody(env,'')
picker.InitFromBoxes(array([ [0.05,0,0,0.05,0.01,0.01], [0,0.05,0,0.01,0.05,0.01], [0,0,0.05,0.01,0.01,0.05] ]),True)
for igeom,geom in enumerate(picker.GetLinks()[0].GetGeometries()):
color = zeros(3)
color[igeom] = 1
geom.SetDiffuseColor(color)
picker.SetName(ikmodel.manip.GetName())
env.Add(picker,True)
# have to disable since not part of collision
picker.Enable(False)
picker.SetTransform(ikmodel.manip.GetTransform())
pickers.append([picker,ikmodel.manip])
def deserialize_kinbody(env, data, name=None, anonymous=False, state=True):
from openravepy import RaveCreateKinBody, RaveCreateRobot
deserialization_logger.debug('Deserializing %s "%s".',
'Robot' if data['is_robot'] else 'KinBody',
data['name']
)
link_infos = [
deserialize_link_info(link_data['info']) \
for link_data in data['links']
]
joint_infos = [
deserialize_joint_info(joint_data['info']) \
for joint_data in data['joints']
]
if data['is_robot']:
# TODO: Also load sensors.
manipulator_infos = [
deserialize_manipulator_info(manipulator_data['info']) \
for manipulator_data in data['manipulators']
]
sensor_infos = []
kinbody = RaveCreateRobot(env, '')
kinbody.Init(
link_infos, joint_infos,
manipulator_infos, sensor_infos,
data['uri']
)
else:
kinbody = RaveCreateKinBody(env, '')
kinbody.Init(link_infos, joint_infos, data['uri'])
kinbody.SetName(name or data['name'])
env.Add(kinbody, anonymous)
if state:
deserialize_kinbody_state(kinbody, data['kinbody_state'])
if kinbody.IsRobot():
deserialize_robot_state(kinbody, data['robot_state'])
return kinbody
def _add_obstacle(self, geom):
obstacles = np.matrix('-0.576036866359447, 0.918128654970760, 1;\
-0.806451612903226,-1.07017543859649, 1;\
1.01843317972350,-0.988304093567252, 1;\
0.640552995391705,0.906432748538011, 1;\
-0.576036866359447, 0.918128654970760, -1;\
-0.806451612903226,-1.07017543859649, -1;\
1.01843317972350,-0.988304093567252, -1;\
0.640552995391705,0.906432748538011, -1')
body = RaveCreateKinBody(self._env, '')
vertices = np.array(obstacles)
indices = np.array([[0, 1, 2], [2, 3, 0], [4, 5, 6], [6, 7, 4],
[0, 4, 5], [0, 1, 5], [1, 2, 5], [5, 6, 2],
[2, 3, 6], [6, 7, 3], [0, 3, 7], [0, 4, 7]])
body.InitFromTrimesh(trimesh=TriMesh(vertices, indices), draw=True)
body.SetName(self.name)
for link in body.GetLinks():
for geom in link.GetGeometries():
geom.SetDiffuseColor((.9, .9, .9))
self.env_body = body
self._env.AddKinBody(body)
def main(env,options):
"Main example code."
if options.iktype is not None:
# cannot use .names due to python 2.5 (or is it boost version?)
for value,type in IkParameterization.Type.values.items():
if type.name.lower() == options.iktype.lower():
iktype = type
break
else:
iktype = IkParameterizationType.Transform6D
env.Load(options.scene)
with env:
# load the Transform6D IK models
ikmodels = []
for robot in env.GetRobots():
for manip in robot.GetManipulators():
print(manip)
try:
robot.SetActiveManipulator(manip)
ikmodel = databases.inversekinematics.InverseKinematicsModel(robot, iktype=iktype)
if not ikmodel.load():
ikmodel.autogenerate()
if not ikmodel.has():
continue
ikmodels.append(ikmodel)
except Exception as e:
print('failed manip %s %r'%(manip, e))
if len(ikmodels) == 0:
raveLogWarn('no manipulators found that can be loaded with iktype %s'%str(iktype))
# create the pickers
pickers = []
for ikmodel in ikmodels:
raveLogInfo('creating picker for %s'%str(ikmodel.manip))
picker = RaveCreateKinBody(env,'')
picker.InitFromBoxes(array([ [0.05,0,0,0.05,0.01,0.01], [0,0.05,0,0.01,0.05,0.01], [0,0,0.05,0.01,0.01,0.05] ]),True)
for igeom,geom in enumerate(picker.GetLinks()[0].GetGeometries()):
color = zeros(3)
color[igeom] = 1
geom.SetDiffuseColor(color)
picker.SetName(ikmodel.manip.GetName())
env.Add(picker,True)
# have to disable since not part of collision
picker.Enable(False)
picker.SetTransform(ikmodel.manip.GetTransform())
pickers.append([picker,ikmodel.manip])
raveLogInfo('pickers loaded, try moving them')
def PickerThread(env,pickers,iktype):
"""this function runs in a separate thread monitoring each of the pickers
"""
Tpickers = [None]*len(pickers)
while True:
if env.GetViewer() is None:
break
with env:
for ipicker,(picker,manip) in enumerate(pickers):
T=picker.GetTransform()
if Tpickers[ipicker] is not None and sum(abs(Tpickers[ipicker]-T).flatten()) <= 1e-10:
continue
data = None
if iktype == IkParameterizationType.Direction3D:
data = T[0:3,2]
elif iktype == IkParameterizationType.Lookat3D:
data = T[0:3,3]
elif iktype == IkParameterizationType.Ray4D:
data = Ray(T[0:3,3],T[0:3,2])
elif iktype == IkParameterizationType.Rotation3D:
data = T[0:3,0:3]
elif iktype == IkParameterizationType.Transform6D:
data = T
elif iktype == IkParameterizationType.Translation3D:
data = T[0:3,3]
elif iktype == IkParameterizationType.TranslationDirection5D:
ikparam = Ray(T[0:3,3],T[0:3,2])
elif iktype == IkParameterizationType.TranslationLocalGlobal6D:
ikparam = [T[0:3,3],zeros(3)]
else:
raveLogWarn('iktype %s unsupported'%str(iktype))
continue
sol = manip.FindIKSolution(IkParameterization(data,iktype),IkFilterOptions.CheckEnvCollisions)
if sol is not None:
robot.SetDOFValues(sol,manip.GetArmIndices())
# have to update all other pickers since two manipulators can be connected to the same joints (torso)
for ipicker2, (picker2,manip2) in enumerate(pickers):
Tpickers[ipicker2] = manip2.GetTransform()
picker2.SetTransform(manip2.GetTransform())
# changing color produces bugs with qtcoin
# for igeom,geom in enumerate(picker.GetLinks()[0].GetGeometries()):
# color = zeros(3)
# color[igeom] = 0.4 if sol is None else 1.0
# geom.SetDiffuseColor(color)
Tpickers[ipicker] = T
# update rate of 0.05s
time.sleep(0.05)
# create the thread
t = threading.Thread(target=PickerThread,args=[env,pickers,iktype])
t.start()
while True:
t.join(1)
def deserialize_kinbody(env, data, name=None, anonymous=False, state=True,
urdf_module_getter=None):
from openravepy import RaveCreateKinBody, RaveCreateRobot
if urdf_module_getter is None:
urdf_module_getter = UnitaryMemoizer(lambda: openravepy.RaveCreateModule(env,'urdf'))
deserialization_logger.debug('Deserializing %s "%s".',
'Robot' if data['is_robot'] else 'KinBody',
data['name']
)
name_desired = name or data['name']
if data.get('uri_only', False):
if data['is_robot']:
parts = data['uri'].split()
if len(parts)==2 and parts[0].endswith('.urdf') and parts[1].endswith('.srdf'):
module_urdf = urdf_module_getter()
if module_urdf is None:
raise UnsupportedTypeDeserializationException('urdf srdf')
robot_name = module_urdf.SendCommand('Load {}'.format(data['uri']))
kinbody = env.GetRobot(robot_name)
if robot_name != name_desired:
env.Remove(kinbody)
kinbody.SetName(name_desired)
env.Add(kinbody, anonymous)
else:
kinbody = env.ReadRobotXMLFile(data['uri'])
kinbody.SetName(name_desired)
env.Add(kinbody, anonymous)
else:
if data['uri'].endswith('.urdf'):
module_urdf = urdf_module_getter()
if module_urdf is None:
raise UnsupportedTypeDeserializationException('urdf')
kinbody_name = module_urdf.SendCommand('Load {}'.format(data['uri']))
kinbody = env.GetKinBody(kinbody_name)
if kinbody_name != name_desired:
env.Remove(kinbody)
kinbody.SetName(name_desired)
env.Add(kinbody)
else:
kinbody = env.ReadKinBodyXMLFile(data['uri'])
kinbody.SetName(name_desired)
env.Add(kinbody)
else:
link_infos = [
deserialize_link_info(link_data['info']) \
for link_data in data['links']
]
joint_infos = [
deserialize_joint_info(joint_data['info']) \
for joint_data in data['joints']
]
if data['is_robot']:
# TODO: Also load sensors.
manipulator_infos = [
deserialize_manipulator_info(manipulator_data['info']) \
for manipulator_data in data['manipulators']
]
sensor_infos = []
kinbody = RaveCreateRobot(env, '')
kinbody.Init(
link_infos, joint_infos,
manipulator_infos, sensor_infos,
data['uri']
)
else:
kinbody = RaveCreateKinBody(env, '')
kinbody.Init(link_infos, joint_infos, data['uri'])
kinbody.SetName(name_desired)
env.Add(kinbody, anonymous)
if state:
deserialize_kinbody_state(kinbody, data['kinbody_state'])
if kinbody.IsRobot():
deserialize_robot_state(kinbody, data['robot_state'])
return kinbody
