def compute_jacobian(chains, skeleton):
num_angles = len(skeleton.bones) * 3
num_coords = len(chains) * 3
J = np.zeros((num_coords, num_angles), dtype=np.float64)
row_offset = 0
for chain in chains:
e = get_end_effector(chain, skeleton)
for idx in chain.bones:
bone = skeleton.bones[idx]
q_parent = Q.identity()
if bone.has_parent():
q_parent = skeleton.bones[bone.parent].q_wcs
# In the 5 lines of code below the ZYZ Euler angles are hardwired into the code.
q_alpha = bone.get_rotation_alpha()
q_alpha_beta = Q.prod(q_alpha, bone.get_rotation_beta())
u = Q.rotate(q_parent, bone.get_axis_alpha())
v = Q.rotate(q_parent, Q.rotate(q_alpha, bone.get_axis_beta()))
w = Q.rotate(q_parent, Q.rotate(q_alpha_beta,
bone.get_axis_gamma()))
delta_p = e - bone.t_wcs
J_alpha = V3.cross(u, delta_p)
J_beta = V3.cross(v, delta_p)
J_gamma = V3.cross(w, delta_p)
J[row_offset:row_offset + 3, idx * 3 + 0] = J_alpha
J[row_offset:row_offset + 3, idx * 3 + 1] = J_beta
J[row_offset:row_offset + 3, idx * 3 + 2] = J_gamma
row_offset += 3
return J
def compute_hessian(chains, skeleton, J):
H = np.dot(np.transpose(J), J)
row_offset = 0
for chain in chains:
e = get_end_effector(chain, skeleton)
r = chain.goal - e
for k in chain.bones:
for h in chain.bones:
if h > k:
continue
Bh = skeleton.bones[h]
q_parent = Q.identity()
if Bh.has_parent():
q_parent = skeleton.bones[Bh.parent].q_wcs
# In the 7 lines of code below the ZYZ Euler angles are hardwired into the code.
q_alpha = Bh.get_rotation_alpha()
q_beta = Bh.get_rotation_beta()
q_gamma = Bh.get_rotation_gamma()
q_alpha_beta = Q.prod(q_alpha, q_beta)
u = Q.rotate(q_parent, Bh.get_axis_alpha())
v = Q.rotate(q_parent, Q.rotate(q_alpha, Bh.get_axis_beta()))
w = Q.rotate(q_parent,
Q.rotate(q_alpha_beta, Bh.get_axis_gamma()))
k_offset = k * 3
h_offset = h * 3
J_a = J[row_offset:row_offset + 3, k_offset]
J_b = J[row_offset:row_offset + 3, k_offset + 1]
J_c = J[row_offset:row_offset + 3, k_offset + 2]
ua = np.dot(V3.cross(u, J_a), r)
va = np.dot(V3.cross(v, J_a), r)
wa = np.dot(V3.cross(w, J_a), r)
ub = np.dot(V3.cross(u, J_b), r)
vb = np.dot(V3.cross(v, J_b), r)
wb = np.dot(V3.cross(w, J_b), r)
uc = np.dot(V3.cross(u, J_c), r)
vc = np.dot(V3.cross(v, J_c), r)
wc = np.dot(V3.cross(w, J_c), r)
dH = np.array([[ua, va, wa], [ub, vb, wb], [uc, vc, wc]])
H[h_offset:h_offset + 3, k_offset:k_offset + 3] -= dH
if h != k:
H[k_offset:k_offset + 3,
h_offset:h_offset + 3] -= np.transpose(dH)
row_offset += 3
return H
def test_quaternion_rotate():
q = torch.tensor([1.0, 0.0, 1.0, 0.0])
r = pi / 4.0
r_ = torch.tensor([r, 0.0, 0.0])
qr = Q.rotate(q, r_)
# Numpy implementation
import numpy as np
def prod(qa, qb):
a = qa[0]
b = qb[0]
A = qa[1:]
B = qb[1:]
qs = a * b - np.dot(A, B)
qv = a * B + A * b + np.cross(A, B, axis=0)
return np.array([qs, qv[0], qv[1], qv[2]])
anp = np.array([1.0, 0.0, 1.0, 0.0])
qrnp = np.array([0.0, r, 0.0, 0.0])
qrex = prod(prod(anp, qrnp), np.array([anp[0], -anp[1], -anp[2],
-anp[3]]))[1:]
assert (torch.norm(torch.from_numpy(qrex) - qr) < 1e-4)
print("quat rotate test success")
def __update_bone(bone, skeleton):
q_alpha = bone.get_rotation_alpha()
q_beta = bone.get_rotation_beta()
q_gamma = bone.get_rotation_gamma()
q_bone = Q.prod(q_alpha, Q.prod(q_beta, q_gamma))
t_parent = V3.zero()
q_parent = Q.identity()
if bone.has_parent():
t_parent = skeleton.bones[bone.parent].t_wcs
q_parent = skeleton.bones[bone.parent].q_wcs
bone.t_wcs = t_parent + Q.rotate(q_parent, bone.t)
bone.q_wcs = Q.prod(q_parent, q_bone)
for idx in bone.children:
__update_bone(skeleton.bones[idx], skeleton)
def get_end_effector(chain, skeleton):
t_wcs = skeleton.bones[chain.bones[-1]].t_wcs
q_wcs = skeleton.bones[chain.bones[-1]].q_wcs
tool = chain.tool
return t_wcs + Q.rotate(q_wcs, tool)