from convert import trajectory_fk, point_ik, timing_report, reaching_report
from approxik import ApproxInvKin, ExactInvKin
if __name__ == "__main__":
filename, base_link, ee_link = parse_args()
# Try setting the rotation penalty to 1.0
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)
lo = np.array([
-2.96706, -2.094395, -2.96706, -2.094395, -2.96706, -2.094395,
-3.054326
])
hi = np.array(
[2.96706, 2.094395, 2.96706, 2.094395, 2.96706, 2.094395, 3.054326])
rg = hi - lo
n_steps = 1000
random_state = np.random.RandomState(0)
Q = random_state.rand(n_steps, aik.get_n_joints())
Q = rg.reshape(1, -1) * Q + lo.reshape(1, -1)
P = trajectory_fk(Q, aik)
Qa, timings = point_ik(P, aik)
reaching_report(P, Qa, aik, label="Approximate")
timing_report(timings, "Approximate")
Qe, timings = point_ik(P, eik)
reaching_report(P, Qe, eik, label="Exact")
timing_report(timings, "Exact")
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"])
plt.show()
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()