def do_traj_ik_graph_search(pr2, lr, gripper_poses):
manip = get_manipulator(pr2, lr)
hmats = [juc.pose_to_hmat(pose) for pose in gripper_poses]
def ikfunc(hmat):
return traj_ik_graph_search.ik_for_link(hmat, manip, "%s_gripper_tool_frame"%lr, return_all_solns=True)
pr2.update_rave()
start_joints = pr2.robot.GetDOFValues(manip.GetArmJoints())
robot = manip.GetRobot()
env = robot.GetEnv()
report = rave.CollisionReport()
link_info = []
def nodecost(joints):
robot.SetDOFValues(joints, manip.GetArmJoints())
return 100*env.CheckCollision(robot)
paths, costs, timesteps = traj_ik_graph_search.traj_cart2joint(hmats, ikfunc, start_joints=start_joints, nodecost=nodecost)
i_best = np.argmin(costs)
print "lowest cost of initial trajs:", costs[i_best]
best_path = paths[i_best]
return best_path
def do_traj_ik_graph_search(lr, gripper_poses):
hmats = [conversions.pose_to_hmat(pose) for pose in gripper_poses]
def ikfunc(hmat):
return traj_ik_graph_search.ik_for_link(hmat, manip, "%s_gripper_tool_frame"%lr, return_all_solns=True)
manip = get_manipulator(lr)
start_joints = Globals.pr2.robot.GetDOFValues(manip.GetArmJoints())
def nodecost(joints):
robot = manip.GetRobot()
robot.SetDOFValues(joints, manip.GetArmJoints())
return 100*robot.GetEnv().CheckCollision(robot)
paths, costs, timesteps = traj_ik_graph_search.traj_cart2joint(hmats, ikfunc, start_joints=start_joints, nodecost=nodecost)
i_best = np.argmin(costs)
print "lowest cost of initial trajs:", costs[i_best]
path_init = paths[i_best]
return path_init
