def add_bone(skeleton,
parent_idx,
alpha,
beta,
gamma,
tx,
ty,
tz,
euler_code='ZYZ'):
if not skeleton.has_bone(parent_idx):
raise RuntimeError('add_bone(): Internal error, parent did not exist')
if not is_valid_euler_code(euler_code):
raise RuntimeError(
'create_root(): Internal error, invalid Euler angle code given')
end_effector = Bone()
end_effector.idx = len(skeleton.bones)
end_effector.parent = parent_idx
end_effector.t = V3.make(tx, ty, tz)
end_effector.alpha = alpha
end_effector.beta = beta
end_effector.gamma = gamma
end_effector.euler_code = euler_code
skeleton.bones[parent_idx].children.append(end_effector.idx)
skeleton.bones.append(end_effector)
return end_effector
def __print_bone(indent, bone, skeleton):
print(indent, 'Bone idx :', bone.idx, 'Parent idx :',
bone.parent, 'Euler angles :',
V3.make(bone.alpha, bone.beta, bone.gamma), 'Euler code :',
bone.euler_code, ' Origin :', bone.t_wcs)
for idx in bone.children:
__print_bone(indent + '\t', skeleton.bones[idx], skeleton)
def test_vector3_make():
xex = torch.tensor([1.0, 2.0, 3.0])
x = V3.make(1, 2, 3)
assert (torch.norm(xex - x) == 0)
print("vector 3 make test success")
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')
def create_root(skeleton, alpha, beta, gamma, tx, ty, tz, euler_code='ZYZ'):
if skeleton.has_root():
raise RuntimeError(
'create_root(): Internal error, root already existed')
if not is_valid_euler_code(euler_code):
raise RuntimeError(
'create_root(): Internal error, invalid Euler angle code given')
root = Bone()
root.t = V3.make(tx, ty, tz)
root.alpha = alpha
root.beta = beta
root.gamma = gamma
root.euler_code = euler_code
skeleton.bones.append(root)
return root
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 __init__(self):
self.bones = [] # Indices of all bones that are part of the chain.
self.skeleton = None # Reference to skeleton holding the bones
self.goal = V3.make(10, 0, 0) # A default goal position in world coordinates
self.tool = V3.zero() # A tool vector in end-effector coordinates