def test_jacobian():
skeleton = IK.create_skeleton()
B0 = IK.create_root(skeleton,
alpha=IK.degrees_to_radians(0),
beta=0.0,
gamma=0.0,
tx=1.0,
ty=0.0,
tz=0.0)
B1 = IK.add_bone(skeleton,
parent_idx=B0.idx,
alpha=IK.degrees_to_radians(0),
beta=0.0,
gamma=0.0,
tx=1.0,
ty=0.0,
tz=0.0)
B2 = IK.add_bone(skeleton,
parent_idx=B1.idx,
alpha=IK.degrees_to_radians(0),
beta=0.0,
gamma=0.0,
tx=1.0,
ty=0.0,
tz=0.0)
IK.update_skeleton(skeleton)
chains = IK.make_chains(skeleton)
Jtst = IK.compute_jacobian(chains, skeleton)
J = torch.tensor([[0., 0., 0., 0., 0., 0., 0., 0., 0.],
[2., 0., 2., 1., 0., 1., 0., 0., 0.],
[0., -2., 0., 0., -1., 0., 0., 0., 0.]])
if (torch.max(torch.abs(J - Jtst))) > 0.0:
print('failure')
else:
print('success')
def test_gradient():
skeleton = IK.create_skeleton()
B0 = IK.create_root(skeleton, alpha=IK.degrees_to_radians(0), beta=0.0, gamma=0.0, tx=1.0, ty=0.0, tz=0.0)
B1 = IK.add_bone(skeleton, parent_idx=B0.idx, alpha=IK.degrees_to_radians(0), beta=0.0, gamma=0.0, tx=1.0, ty=0.0, tz=0.0)
B2 = IK.add_bone(skeleton, parent_idx=B1.idx, alpha=IK.degrees_to_radians(0), beta=0.0, gamma=0.0, tx=1.0, ty=0.0, tz=0.0)
B3 = IK.add_bone(skeleton, parent_idx=B2.idx, alpha=IK.degrees_to_radians(0), beta=0.0, gamma=0.0, tx=1.0, ty=0.0, tz=0.0)
IK.update_skeleton(skeleton)
chains = IK.make_chains(skeleton)
chains[0].goal = V3.make(10, 0, 0)
J = IK.compute_jacobian(chains, skeleton)
g = IK.compute_gradient(chains, skeleton, J)
print(g)
g_app = IK.finite_difference_gradient(chains, skeleton, h=0.00001)
print(g_app)
def test_update_skeleton():
skeleton = IK.create_skeleton()
B0 = IK.create_root(skeleton, alpha=IK.degrees_to_radians(90), beta=0.0, gamma=0.0, tx=1.0, ty=0.0, tz=0.0)
B1 = IK.add_bone(skeleton, parent_idx=B0.idx, alpha=IK.degrees_to_radians(90), beta=0.0, gamma=0.0, tx=1.0, ty=0.0, tz=0.0)
B2 = IK.add_bone(skeleton, parent_idx=B1.idx, alpha=IK.degrees_to_radians(-90), beta=0.0, gamma=0.0, tx=1.0, ty=0.0, tz=0.0)
B3 = IK.add_bone(skeleton, parent_idx=B0.idx, alpha=IK.degrees_to_radians(-90), beta=0.0, gamma=0.0, tx=-1.0, ty=0.0, tz=0.0)
IK.update_skeleton(skeleton)
if V3.norm(B0.t_wcs - V3.make(1.0, 0.0, 0.0)) < 0.001:
print('success')
else:
print('failure')
if V3.norm(B1.t_wcs - V3.make(1.0, 1.0, 0.0)) < 0.001:
print('success')
else:
print('failure')
if V3.norm(B2.t_wcs - V3.make(0.0, 1.0, 0.0)) < 0.001:
print('success')
else:
print('failure')
if V3.norm(B3.t_wcs - V3.make(1.0, -1.0, 0.0)) < 0.001:
print('success')
else:
print('failure')
IK.print_skeleton(skeleton)
skeleton = IK.create_skeleton()
B0 = IK.create_root(skeleton, alpha=IK.degrees_to_radians(0), beta=0.0, gamma=0.0, tx=1.0, ty=0.0, tz=0.0)
B1 = IK.add_bone(skeleton, parent_idx=B0.idx, alpha=IK.degrees_to_radians(0), beta=0.0, gamma=0.0, tx=1.0, ty=0.0, tz=0.0)
IK.update_skeleton(skeleton)
if V3.norm(B0.t_wcs - V3.make(1.0, 0.0, 0.0)) < 0.001:
print('success')
else:
print('failure')
if V3.norm(B1.t_wcs - V3.make(2.0, 0.0, 0.0)) < 0.001:
print('success')
else:
print('failure')
B0.alpha = IK.degrees_to_radians(90)
IK.update_skeleton(skeleton)
if V3.norm(B0.t_wcs - V3.make(1.0, 0.0, 0.0)) < 0.001:
print('success')
else:
print('failure')
if V3.norm(B1.t_wcs - V3.make(1.0, 1.0, 0.0)) < 0.001:
print('success')
else:
print('failure')