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 make_exact_cart_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)
p = np.empty(7)
ik.jnt_to_cart(x0, p)
x0_pos = p[:3].copy()
x0_rot = p[3:].copy()
ik.jnt_to_cart(g, p)
g_pos = p[:3].copy()
g_rot = p[3:].copy()
cart_mp_values = [x0_pos, x0_rot, g_pos, g_rot]
beh = IKCartesianDMPBehavior(ik, execution_time, dt)
beh.csdmp.set_params(_initial_cart_params(ik))
return ik, beh, cart_mp_keys, cart_mp_values
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)
import numpy as np
from approxik import ApproxInvKin, ExactInvKin
from ik_cartesian_dmp_behavior import (IKCartesianDMPBehavior,
IKCartesianDMPBehaviorWithGoalParams)
from dmp_behavior import DMPBehavior, DMPBehaviorWithGoalParams
from bolero.representation import CartesianDMPBehavior
x0j = np.array([0.0, -0.5, -0.5, 0.0, -0.5, 0.0])
gj = np.array([0.0, 0.5, 0.5, 0.0, 0.5, 0.0])
ik = ExactInvKin("../data/compi.urdf", "linkmount", "linkkelle", 1e-5, 150)
x0 = np.zeros(7)
ik.jnt_to_cart(x0j, x0)
g = np.zeros(7)
ik.jnt_to_cart(gj, g)
execution_time = 0.5
dt = 0.001
cart_mp_keys = ["x0", "q0", "g", "qg"]
cart_mp_values = [x0[:3], x0[3:], g[:3], g[3:]]
def _initial_cart_params(ik):
joint_dmp = DMPBehavior(execution_time, dt)
n_joints = ik.get_n_joints()
joint_dmp.init(n_joints, n_joints)
joint_dmp.set_meta_parameters(["x0", "g"], [x0j, gj])
Q = joint_dmp.trajectory()[0]
P = np.empty((Q.shape[0], 7))
for t in range(Q.shape[0]):