def test_jacobian_prismatic():
chain = pk.build_serial_chain_from_urdf(
open("prismatic_robot.urdf").read(), "link4")
th = torch.zeros(3)
tg = chain.forward_kinematics(th)
m = tg.get_matrix()
pos = m[0, :3, 3]
assert torch.allclose(pos, torch.tensor([0, 0, 1.]))
th = torch.tensor([0, 0.1, 0])
tg = chain.forward_kinematics(th)
m = tg.get_matrix()
pos = m[0, :3, 3]
assert torch.allclose(pos, torch.tensor([0, -0.1, 1.]))
th = torch.tensor([0.1, 0.1, 0])
tg = chain.forward_kinematics(th)
m = tg.get_matrix()
pos = m[0, :3, 3]
assert torch.allclose(pos, torch.tensor([0, -0.1, 1.1]))
th = torch.tensor([0.1, 0.1, 0.1])
tg = chain.forward_kinematics(th)
m = tg.get_matrix()
pos = m[0, :3, 3]
assert torch.allclose(pos, torch.tensor([0.1, -0.1, 1.1]))
J = chain.jacobian(th)
assert torch.allclose(
J,
torch.tensor([[[0., 0., 1.], [0., -1., 0.], [1., 0., 0.], [0., 0., 0.],
[0., 0., 0.], [0., 0., 0.]]]))
def test_jacobian_at_different_loc_than_ee():
chain = pk.build_serial_chain_from_urdf(open("kuka_iiwa.urdf").read(), "lbr_iiwa_link_7")
th = torch.tensor([0.0, -math.pi / 4.0, 0.0, math.pi / 2.0, 0.0, math.pi / 4.0, 0.0])
loc = torch.tensor([0.1, 0, 0])
J = chain.jacobian(th, locations=loc)
J_c1 = torch.tensor([[[-0., 0.11414214, -0., 0.18284271, 0., 0.1, 0.],
[-0.66082756, -0., -0.38656497, -0., 0.12798633, -0., 0.1],
[-0., 0.66082756, -0., -0.36384271, 0., 0.081, -0.],
[-0., -0., -0.70710678, -0., -0.70710678, 0., -1.],
[0., 1., 0., -1., 0., 1., 0.],
[1., 0., 0.70710678, 0., -0.70710678, -0., 0.]]])
assert torch.allclose(J, J_c1)
loc = torch.tensor([-0.1, 0.05, 0])
J = chain.jacobian(th, locations=loc)
J_c2 = torch.tensor([[[-0.05, -0.08585786, -0.03535534, 0.38284271, 0.03535534, -0.1, -0.],
[-0.66082756, -0., -0.52798633, -0., -0.01343503, 0., -0.1],
[-0., 0.66082756, -0.03535534, -0.36384271, -0.03535534, 0.081, -0.05],
[-0., -0., -0.70710678, -0., -0.70710678, 0., -1.],
[0., 1., 0., -1., 0., 1., 0.],
[1., 0., 0.70710678, 0., -0.70710678, -0., 0.]]])
assert torch.allclose(J, J_c2)
# check that batching the location is fine
th = th.repeat(2, 1)
loc = torch.tensor([[0.1, 0, 0], [-0.1, 0.05, 0]])
J = chain.jacobian(th, locations=loc)
assert torch.allclose(J, torch.cat((J_c1, J_c2)))
def test_parallel():
N = 100
chain = pk.build_serial_chain_from_urdf(open("kuka_iiwa.urdf").read(), "lbr_iiwa_link_7")
th = torch.cat(
(torch.tensor([[0.0, -math.pi / 4.0, 0.0, math.pi / 2.0, 0.0, math.pi / 4.0, 0.0]]), torch.rand(N, 7)))
J = chain.jacobian(th)
for i in range(N):
J_i = chain.jacobian(th[i])
assert torch.allclose(J[i], J_i)
def test_dtype_device():
N = 1000
d = "cuda" if torch.cuda.is_available() else "cpu"
dtype = torch.float64
chain = pk.build_serial_chain_from_urdf(open("kuka_iiwa.urdf").read(), "lbr_iiwa_link_7")
chain = chain.to(dtype=dtype, device=d)
th = torch.rand(N, 7, dtype=dtype, device=d)
J = chain.jacobian(th)
assert J.dtype is dtype
def test_gradient():
N = 10
d = "cuda" if torch.cuda.is_available() else "cpu"
dtype = torch.float64
chain = pk.build_serial_chain_from_urdf(open("kuka_iiwa.urdf").read(), "lbr_iiwa_link_7")
chain = chain.to(dtype=dtype, device=d)
th = torch.rand(N, 7, dtype=dtype, device=d, requires_grad=True)
J = chain.jacobian(th)
assert th.grad is None
J.norm().backward()
assert th.grad is not None
def test_correctness():
chain = pk.build_serial_chain_from_urdf(
open("kuka_iiwa.urdf").read(), "lbr_iiwa_link_7")
th = torch.tensor(
[0.0, -math.pi / 4.0, 0.0, math.pi / 2.0, 0.0, math.pi / 4.0, 0.0])
J = chain.jacobian(th)
assert torch.allclose(
J,
torch.tensor([[[
-2.47727592e-12, 1.41421356e-02, -2.44155677e-12, 2.82842712e-01,
3.93156995e-13, 1.67535985e-14, 0.00000000e+00
],
[
-6.60827561e-01, -3.23289325e-12, -4.57275649e-01,
-2.63812394e-12, 5.72756493e-02, 1.76135096e-13,
0.00000000e+00
],
[
4.75722920e-17, 6.60827561e-01, -1.60872783e-12,
-3.63842712e-01, 2.72085294e-13, 8.10000000e-02,
0.00000000e+00
],
[
-9.71829137e-17, -2.06797669e-13, -7.07106781e-01,
-6.71813437e-12, -7.07106781e-01, 6.86429574e-12,
-1.00000000e+00
],
[
-3.06245972e-16, 1.00000000e+00, 1.28796020e-12,
-1.00000000e+00, 8.21292608e-12, 1.00000000e+00,
1.96776811e-12
],
[
1.00000000e+00, 4.89681088e-12, 7.07106781e-01,
2.02812117e-12, -7.07106781e-01, -2.17450736e-12,
2.06834549e-13
]]]))
chain = pk.build_chain_from_sdf(open("simple_arm.sdf").read())
chain = pk.SerialChain(chain, "arm_wrist_roll_frame")
th = torch.tensor([0.8, 0.2, -0.5, -0.3])
J = chain.jacobian(th)
torch.allclose(
J,
torch.tensor([[[0., -1.51017878, -0.46280904, 0.],
[0., 0.37144033, 0.29716627, 0.], [0., 0., 0., 0.],
[0., 0., 0., 0.], [0., 0., 0., 0.], [0., 1., 1., 1.]]]))
def test_fkik():
data = '<robot name="test_robot">' \
'<link name="link1" />' \
'<link name="link2" />' \
'<link name="link3" />' \
'<joint name="joint1" type="revolute">' \
'<origin xyz="1.0 0.0 0.0"/>' \
'<parent link="link1"/>' \
'<child link="link2"/>' \
'</joint>' \
'<joint name="joint2" type="revolute">' \
'<origin xyz="1.0 0.0 0.0"/>' \
'<parent link="link2"/>' \
'<child link="link3"/>' \
'</joint>' \
'</robot>'
chain = pk.build_serial_chain_from_urdf(data, 'link3')
th1 = torch.tensor([0.42553542, 0.17529176])
tg = chain.forward_kinematics(th1)
pos, rot = quat_pos_from_transform3d(tg)
assert torch.allclose(pos, torch.tensor([[1.91081784, 0.41280851,
0.0000]]))
assert quaternion_equality(
rot, torch.tensor([[0.95521418, 0.0000, 0.0000, 0.2959153]]))
print(tg)
# TODO implement and test inverse kinematics
# th2 = chain.inverse_kinematics(tg)
# self.assertTrue(np.allclose(th1, th2, atol=1.0e-6))
# test batch kinematics
N = 20
th_batch = torch.rand(N, 2)
tg_batch = chain.forward_kinematics(th_batch)
m = tg_batch.get_matrix()
for i in range(N):
tg = chain.forward_kinematics(th_batch[i])
assert torch.allclose(tg.get_matrix().view(4, 4), m[i])
# check that gradients are passed through
th2 = torch.tensor([0.42553542, 0.17529176], requires_grad=True)
tg = chain.forward_kinematics(th2)
pos, rot = quat_pos_from_transform3d(tg)
# note that since we are using existing operations we are not checking grad calculation correctness
assert th2.grad is None
pos.norm().backward()
assert th2.grad is not None
def test_cuda():
if torch.cuda.is_available():
d = "cuda"
dtype = torch.float64
chain = pk.build_chain_from_sdf(open("simple_arm.sdf").read())
chain = chain.to(dtype=dtype, device=d)
ret = chain.forward_kinematics({
'arm_elbow_pan_joint': math.pi / 2.0,
'arm_wrist_lift_joint': -0.5
})
tg = ret['arm_wrist_roll']
pos, rot = quat_pos_from_transform3d(tg)
assert quaternion_equality(
rot,
torch.tensor([0.70710678, 0., 0., 0.70710678],
dtype=dtype,
device=d))
assert torch.allclose(
pos, torch.tensor([1.05, 0.55, 0.5], dtype=dtype, device=d))
data = '<robot name="test_robot">' \
'<link name="link1" />' \
'<link name="link2" />' \
'<link name="link3" />' \
'<joint name="joint1" type="revolute">' \
'<origin xyz="1.0 0.0 0.0"/>' \
'<parent link="link1"/>' \
'<child link="link2"/>' \
'</joint>' \
'<joint name="joint2" type="revolute">' \
'<origin xyz="1.0 0.0 0.0"/>' \
'<parent link="link2"/>' \
'<child link="link3"/>' \
'</joint>' \
'</robot>'
chain = pk.build_serial_chain_from_urdf(data, 'link3')
chain = chain.to(dtype=dtype, device=d)
N = 20
th_batch = torch.rand(N, 2).to(device=d, dtype=dtype)
tg_batch = chain.forward_kinematics(th_batch)
m = tg_batch.get_matrix()
for i in range(N):
tg = chain.forward_kinematics(th_batch[i])
assert torch.allclose(tg.get_matrix().view(4, 4), m[i])
def test_urdf():
chain = pk.build_serial_chain_from_urdf(
open("kuka_iiwa.urdf").read(), "lbr_iiwa_link_7")
print(chain)
print(chain.get_joint_parameter_names())
th = [0.0, -math.pi / 4.0, 0.0, math.pi / 2.0, 0.0, math.pi / 4.0, 0.0]
ret = chain.forward_kinematics(th, end_only=False)
tg = ret['lbr_iiwa_link_7']
pos, rot = quat_pos_from_transform3d(tg)
assert quaternion_equality(
rot, torch.tensor([7.07106781e-01, 0, -7.07106781e-01, 0]))
assert torch.allclose(pos,
torch.tensor([-6.60827561e-01, 0, 3.74142136e-01]))
N = 1000
d = "cuda" if torch.cuda.is_available() else "cpu"
dtype = torch.float64
th_batch = torch.rand(N,
len(chain.get_joint_parameter_names()),
dtype=dtype,
device=d)
chain = chain.to(dtype=dtype, device=d)
import time
start = time.time()
tg_batch = chain.forward_kinematics(th_batch)
m = tg_batch.get_matrix()
elapsed = time.time() - start
print("elapsed {}s for N={} when parallel".format(elapsed, N))
start = time.time()
elapsed = 0
for i in range(N):
tg = chain.forward_kinematics(th_batch[i])
elapsed += time.time() - start
start = time.time()
assert torch.allclose(tg.get_matrix().view(4, 4), m[i])
print("elapsed {}s for N={} when serial".format(elapsed, N))
def test_correctness():
chain = pk.build_serial_chain_from_urdf(open("kuka_iiwa.urdf").read(), "lbr_iiwa_link_7")
th = torch.tensor([0.0, -math.pi / 4.0, 0.0, math.pi / 2.0, 0.0, math.pi / 4.0, 0.0])
J = chain.jacobian(th)
assert torch.allclose(J, torch.tensor([[[0, 1.41421356e-02, 0, 2.82842712e-01, 0, 0, 0],
[-6.60827561e-01, 0, -4.57275649e-01, 0, 5.72756493e-02, 0, 0],
[0, 6.60827561e-01, 0, -3.63842712e-01, 0, 8.10000000e-02, 0],
[0, 0, -7.07106781e-01, 0, -7.07106781e-01, 0, -1],
[0, 1, 0, -1, 0, 1, 0],
[1, 0, 7.07106781e-01, 0, -7.07106781e-01, 0, 0]]]))
chain = pk.build_chain_from_sdf(open("simple_arm.sdf").read())
chain = pk.SerialChain(chain, "arm_wrist_roll_frame")
th = torch.tensor([0.8, 0.2, -0.5, -0.3])
J = chain.jacobian(th)
torch.allclose(J, torch.tensor([[[0., -1.51017878, -0.46280904, 0.],
[0., 0.37144033, 0.29716627, 0.],
[0., 0., 0., 0.],
[0., 0., 0., 0.],
[0., 0., 0., 0.],
[0., 1., 1., 1.]]]))
