P[:, 0] = 0.5 P[:, 1] = np.linspace(-0.8, 0.8, P.shape[0]) P[:, 2] = 0.1 P[:, 3] = 1.0 if __name__ == "__main__": filename, base_link, ee_link = parse_args() aik = ApproxInvKin(filename, base_link, ee_link, 1.0, 0.001, verbose=0) eik = ExactInvKin(filename, base_link, ee_link, 1e-4, 200, verbose=0) P = np.zeros((1000, 7)) line2(P) Qa, timings = trajectory_ik(P, aik) reaching_report(P, Qa, aik, label="Approximate IK") timing_report(timings, "Approximate IK") Pa = trajectory_fk(Qa, aik) Qe, timings, reachable = trajectory_ik(P, eik, return_reachable=True) reaching_report(P, Qe, eik, label="Exact IK") timing_report(timings, "Exact IK") Pe = trajectory_fk(Qe, eik) Pe[np.logical_not(reachable)] = np.nan Qe[np.logical_not(reachable)] = np.nan ax = plot_pose_trajectory(P, Pa) ax = plot_pose_trajectory(P, Pe) plot_joint_trajectories([Qa, Qe], labels=["Approximation", "Exact"])
if __name__ == "__main__": filename, base_link, ee_link = parse_args(base_link="link_0", ee_link="tcp") # Try setting the rotation penalty to 1.0 aik = ApproxInvKin(filename, base_link, ee_link, 1.0, 0.001, verbose=0) n_steps = 1000 Q = np.zeros((n_steps, aik.get_n_joints())) Q[:, 0] = np.linspace(-0.5 * np.pi, 0.5 * np.pi, n_steps) P = trajectory_fk(Q, aik) P_offset = P.copy() # Offset in (unchangeable) x-direction P_offset[:, 0] += 0.1 # Fix rotation P_offset[:, 3] = 1.0 P_offset[:, 4:] = 0.0 Qa, timings = trajectory_ik(P_offset, aik) Pa = trajectory_fk(Qa, aik) timing_report(timings) ax = plot_pose_trajectory(P_offset, Pa) plot_joint_trajectories([Q, Qa], labels=["Original", "Approximated Offset"]) plt.show()