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