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"]
mp_values = [x0, g]
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 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_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)
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 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)
beh = IKCartesianDMPBehavior(ik, execution_time, dt)
return ik, beh, cart_mp_keys, cart_mp_values
P[:, 2] = 1.0
P[:, 3] = 1.0
def line2(P):
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
import sys
import pytransform.rotations as pyrot
from visualization import *
from convert import trajectory_ik, trajectory_fk, check_ik
from approxik import ApproxInvKin, ExactInvKin
if __name__ == "__main__":
filename = sys.argv[1]
print("= Approximate Inverse Kinematics =")
aik = ApproxInvKin(filename,
"kuka_lbr_l_link_0",
"kuka_lbr_l_link_7",
1.0,
0.001,
verbose=0)
check_ik(aik)
print("= Inverse Kinematics =")
eik = ExactInvKin(filename,
"kuka_lbr_l_link_0",
"kuka_lbr_l_link_7",
1e-4,
1000,
verbose=0)
check_ik(eik)
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]):