def interp_hmats(newtimes, oldtimes, oldhmats): oldposes = openravepy.poseFromMatrices(oldhmats) newposes = np.empty((len(newtimes), 7)) newposes[:,4:7] = mu.interp2d(newtimes, oldtimes, oldposes[:,4:7]) newposes[:,0:4] = interp_quats(newtimes, oldtimes, oldposes[:,0:4]) newhmats = openravepy.matrixFromPoses(newposes) return newhmats
def interp_hmats(newtimes, oldtimes, oldhmats): oldposes = openravepy.poseFromMatrices(oldhmats) for i in xrange(1, len(oldposes)): qp = oldposes[i-1,0:4] qc = oldposes[i,0:4] if np.linalg.norm(qp + qc) < np.linalg.norm(qp-qc): oldposes[i,0:4] = -qc newposes = np.empty((len(newtimes), 7)) newposes[:,4:7] = mu.interp2d(newtimes, oldtimes, oldposes[:,4:7]) newposes[:,0:4] = interp_quats(newtimes, oldtimes, oldposes[:,0:4]) newhmats = openravepy.matrixFromPoses(newposes) return newhmats
def pose_traj_request(robot, hmats): n_steps = len(hmats) request = { "basic_info": { "n_steps": n_steps, "manip": "leftarm+rightarm+torso_lift_joint+base", "start_fixed": False }, "costs": [{ "type": "joint_vel", "params": { "coeffs": np.r_[np.ones(15), 10 * np.ones(3)].tolist() } }, { "type": "collision", "params": { "coeffs": [.1], "dist_pen": [0.01] } }], "constraints": [], "init_info": { "type": "stationary" } } poses = rave.poseFromMatrices(hmats) xyzs = poses[:, 4:7] quats = poses[:, 0:4] for i in xrange(1, n_steps - 1): print "waypoint xyz", xyzs[i] waypoint_cnt = { "type": "pose", "name": "waypoint_pose", "params": { "xyz": list(xyzs[i]), "wxyz": list(quats[i]), "link": "l_gripper_tool_frame", "pos_coeffs": [1, 1, 1], "rot_coeffs": [1, 1, 1], "timestep": i } } request["constraints"].append(waypoint_cnt) return request
def pose_traj_request(robot, hmats): n_steps = len(hmats) request = { "basic_info" : { "n_steps" : n_steps, "manip" : "leftarm+rightarm+torso_lift_joint+base", "start_fixed" : False }, "costs" : [ { "type" : "joint_vel", "params": {"coeffs" : np.r_[np.ones(15), 10*np.ones(3)].tolist()} }, { "type" : "collision", "params" : {"coeffs" : [.1],"dist_pen" : [0.01]} } ], "constraints" : [ ], "init_info" : { "type" : "stationary" } } poses = rave.poseFromMatrices(hmats) xyzs = poses[:,4:7] quats = poses[:,0:4] for i in xrange(1, n_steps-1): print "waypoint xyz", xyzs[i] waypoint_cnt = { "type" : "pose", "name" : "waypoint_pose", "params" : { "xyz" : list(xyzs[i]), "wxyz" : list(quats[i]), "link" : "l_gripper_tool_frame", "pos_coeffs" : [1,1,1], "rot_coeffs" : [1,1,1], "timestep" : i }} request["constraints"].append(waypoint_cnt) return request
def do_traj_ik_graph_search_opt(pr2, lr, gripper_poses): gripper_poses = [pose for i, pose in enumerate(gripper_poses) if i % 20 == 0] graph_search_res = do_traj_ik_graph_search(pr2, lr, gripper_poses) hmats = [juc.pose_to_hmat(pose) for pose in gripper_poses] poses = rave.poseFromMatrices(hmats) quats = poses[:,:4] xyzs = poses[:,4:7] manip_name = {"r":"rightarm", "l":"leftarm"}[lr] request = get_ik_request(manip_name, "%s_gripper_tool_frame"%lr, xyzs[-1], quats[-1], graph_search_res, xyzs, quats) s = json.dumps(request) print "REQUEST:", s pr2.robot.SetDOFValues(graph_search_res[0], pr2.robot.GetManipulator(manip_name).GetArmIndices()) trajoptpy.SetInteractive(True); prob = trajoptpy.ConstructProblem(s, pr2.env) result = trajoptpy.OptimizeProblem(prob)