def create_table(env, geom):
thickness = geom.thickness
leg_height = geom.leg_height
back = geom.back
dim1, dim2 = geom.table_dim
legdim1, legdim2 = geom.leg_dim
table_color = [0.5, 0.2, 0.1]
component_type = KinBody.Link.GeomType.Box
tabletop = OpenRAVEBody.create_body_info(
component_type, [dim1 / 2, dim2 / 2, thickness / 2], table_color)
leg1 = OpenRAVEBody.create_body_info(
component_type, [legdim1 / 2, legdim2 / 2, leg_height / 2],
table_color)
leg1._t[0, 3] = dim1 / 2 - legdim1 / 2
leg1._t[1, 3] = dim2 / 2 - legdim2 / 2
leg1._t[2, 3] = -leg_height / 2 - thickness / 2
leg2 = OpenRAVEBody.create_body_info(
component_type, [legdim1 / 2, legdim2 / 2, leg_height / 2],
table_color)
leg2._t[0, 3] = dim1 / 2 - legdim1 / 2
leg2._t[1, 3] = -dim2 / 2 + legdim2 / 2
leg2._t[2, 3] = -leg_height / 2 - thickness / 2
leg3 = OpenRAVEBody.create_body_info(
component_type, [legdim1 / 2, legdim2 / 2, leg_height / 2],
table_color)
leg3._t[0, 3] = -dim1 / 2 + legdim1 / 2
leg3._t[1, 3] = dim2 / 2 - legdim2 / 2
leg3._t[2, 3] = -leg_height / 2 - thickness / 2
leg4 = OpenRAVEBody.create_body_info(
component_type, [legdim1 / 2, legdim2 / 2, leg_height / 2],
table_color)
leg4._t[0, 3] = -dim1 / 2 + legdim1 / 2
leg4._t[1, 3] = -dim2 / 2 + legdim2 / 2
leg4._t[2, 3] = -leg_height / 2 - thickness / 2
if back:
back_plate = OpenRAVEBody.create_body_info(
component_type,
[legdim1 / 10, dim2 / 2, leg_height - thickness / 2],
table_color)
back_plate._t[0, 3] = dim1 / 2 - legdim1 / 10
back_plate._t[1, 3] = 0
back_plate._t[2, 3] = -leg_height / 2 - thickness / 4
table = RaveCreateRobot(env, '')
if not back:
table.InitFromGeometries([tabletop, leg1, leg2, leg3, leg4])
else:
table.InitFromGeometries(
[tabletop, leg1, leg2, leg3, leg4, back_plate])
return table
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 create_wall(env, wall_type):
component_type = KinBody.Link.GeomType.Box
wall_color = [0.5, 0.2, 0.1]
box_infos = []
if wall_type == 'closet':
wall_endpoints = [[-1.0, -3.0], [-1.0, 4.0], [1.9, 4.0],
[1.9, 8.0], [5.0, 8.0], [5.0, 4.0], [8.0, 4.0],
[8.0, -3.0], [-1.0, -3.0]]
else:
raise NotImplemented
for i, (start,
end) in enumerate(zip(wall_endpoints[0:-1],
wall_endpoints[1:])):
dim_x, dim_y = 0, 0
thickness = WALL_THICKNESS
if start[0] == end[0]:
ind_same, ind_diff = 0, 1
length = abs(start[ind_diff] - end[ind_diff])
dim_x, dim_y = thickness, length / 2 + thickness
elif start[1] == end[1]:
ind_same, ind_diff = 1, 0
length = abs(start[ind_diff] - end[ind_diff])
dim_x, dim_y = length / 2 + thickness, thickness
else:
raise NotImplemented, 'Can only create axis-aligned walls'
transform = np.eye(4)
transform[ind_same, 3] = start[ind_same]
if start[ind_diff] < end[ind_diff]:
transform[ind_diff, 3] = start[ind_diff] + length / 2
else:
transform[ind_diff, 3] = end[ind_diff] + length / 2
dims = [dim_x, dim_y, 1]
box_info = OpenRAVEBody.create_body_info(component_type, dims,
wall_color)
box_info._t = transform
box_infos.append(box_info)
wall = RaveCreateRobot(env, '')
wall.InitFromGeometries(box_infos)
return wall
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 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