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)