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 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)
