def make_joint_dmp(x0, g, execution_time, dt):
ik = ExactInvKin("../data/kuka_lbr.urdf", "kuka_lbr_l_link_0",
"kuka_lbr_l_link_7", 1e-5, 150)
beh = DMPBehavior(execution_time, dt)
mp_keys = ["x0", "g"]
joints_start = np.zeros(ik.get_n_joints())
ik.cart_to_jnt(x0, joints_start)
joints_goal = joints_start.copy()
ik.cart_to_jnt(g, joints_goal)
mp_values = [joints_start, joints_goal]
return ik, beh, mp_keys, mp_values
def test_unreachable_pose():
urdf_path = os.path.join(DATA_PATH, "kuka_lbr.urdf")
base_link = "kuka_lbr_l_link_0"
ee_link = "kuka_lbr_l_link_7"
aik = ApproxInvKin(urdf_path, base_link, ee_link, 1.0, 1.0, verbose=0)
eik = ExactInvKin(urdf_path, base_link, ee_link, 1e-6, 1000, verbose=0)
n_joints = aik.get_n_joints()
q = 0.3 * np.ones(n_joints)
p = np.empty(7)
p[:] = np.nan
eik.jnt_to_cart(q, p)
p[2] += 0.1
q_result = np.zeros(n_joints)
p_result_exact = np.zeros(7)
success = eik.cart_to_jnt(p, q_result)
assert_false(success)
eik.jnt_to_cart(q_result, p_result_exact)
q_result = np.zeros(n_joints)
p_result_approx = np.zeros(7)
success = aik.cart_to_jnt(p, q_result)
assert_false(success)
aik.jnt_to_cart(q_result, p_result_approx)
exact_dist = np.linalg.norm(p - p_result_exact)
approx_dist = np.linalg.norm(p - p_result_approx)
assert_less(approx_dist, exact_dist)
def test_reachable_path():
urdf_path = os.path.join(DATA_PATH, "kuka_lbr.urdf")
base_link = "kuka_lbr_l_link_0"
ee_link = "kuka_lbr_l_link_7"
aik = ApproxInvKin(urdf_path, base_link, ee_link, 1.0, 1.0, verbose=0)
eik = ExactInvKin(urdf_path, base_link, ee_link, 1e-10, 1000, verbose=0)
n_joints = aik.get_n_joints()
n_steps = 101
Q = np.empty((n_steps, n_joints))
for i in range(n_joints):
Q[:, i] = np.linspace(-0.5, 0.5, n_steps)
p1 = np.empty(7)
p1[:] = np.nan
p2 = np.empty(7)
p2[:] = np.nan
qe = np.empty(n_joints)
qe[:] = Q[0]
qa = np.empty(n_joints)
qa[:] = Q[0]
for t in range(n_steps):
if t > 0:
qe[:] = Q[t - 1]
qa[:] = Q[t - 1]
# Exact IK seems to be closer in Cartesian space
eik.jnt_to_cart(Q[t], p1)
eik.cart_to_jnt(p1, qe)
eik.jnt_to_cart(qe, p2)
assert_array_almost_equal(p1, p2, decimal=6)
assert_array_almost_equal(qe, Q[t], decimal=0)
# Approximate IK seems to be closer in joint space
aik.jnt_to_cart(Q[t], p1)
aik.cart_to_jnt(p1, qa)
aik.jnt_to_cart(qa, p2)
assert_array_almost_equal(p1, p2, decimal=3)
assert_array_almost_equal(qa, Q[t], decimal=2)