def __init__(self):
self.idx = 0 # Bone index
self.euler_code = 'ZYZ' # Euler angle convention
self.alpha = 0.0 # Euler angles of bone
self.beta = 0.0
self.gamma = 0.0
self.q_wcs = Q.identity() # Joint frame orientation in WCS
self.t_wcs = V3.zero() # Joint origin in WCS
self.t = V3.zero() # Joint vector in parent frame
self.parent = -1 # Index to parent bone or -1 if bone is root
self.children = [] # Indices to children bones
def test_vector3_zero():
xex = torch.zeros((3, ))
x = V3.zero()
assert (torch.norm(xex - x) == 0)
print("vector 3 zero 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 __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