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_make_chains():
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)
chains = IK.make_chains(skeleton)
if len(chains) == 1:
print('success')
else:
print('failure')
if len(chains[0].bones) == 2:
print('success')
else:
print('failure')
if chains[0].bones[0] == 0:
print('success')
else:
print('failure')
if chains[0].bones[1] == 1:
print('success')
else:
print('failure')
IK.print_chains(chains)
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=B0.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=B2.idx, alpha=IK.degrees_to_radians(-90), beta=0.0, gamma=0.0, tx=1.0, ty=0.0, tz=0.0)
chains = IK.make_chains(skeleton)
if len(chains) == 2:
print('success')
else:
print('failure')
if len(chains[0].bones) == 2:
print('success')
else:
print('failure')
if chains[0].bones[0] == 0:
print('success')
else:
print('failure')
if chains[0].bones[1] == 1:
print('success')
else:
print('failure')
if len(chains[1].bones) == 3:
print('success')
else:
print('failure')
if chains[1].bones[0] == 0:
print('success')
else:
print('failure')
if chains[1].bones[1] == 2:
print('success')
else:
print('failure')
if chains[1].bones[2] == 3:
print('success')
else:
print('failure')
IK.print_chains(chains)