def set_rotation(joint):
if "root" in joint.name:
joint.matrix = joint.C.dot(conversions.E2R(joint.degree)).dot(
joint.Cinv)
else:
joint.matrix = joint.C.dot(conversions.E2R(joint.degree)).dot(
joint.Cinv)
joint.coordinate = joint.length * joint.matrix.dot(joint.direction)
for child in joint.child_joints:
set_rotation(child)
def __init__(
self,
name=None,
dof=3,
xform_from_parent_joint=constants.eye_T(),
parent_joint=None,
limits=None,
direction=None,
length=None,
axis=None,
):
self.name = name if name else f"joint_{random.getrandbits(32)}"
self.child_joints = []
self.index_child_joint = {}
self.xform_global = constants.eye_T()
self.xform_from_parent_joint = xform_from_parent_joint
self.parent_joint = self.set_parent_joint(parent_joint)
self.info = {"dof": dof} # set ball joint by default
self.length = length
if axis is not None:
axis = np.deg2rad(axis)
self.C = conversions.E2R(axis)
self.Cinv = np.linalg.inv(self.C)
self.matrix = None
self.degree = np.zeros(3)
self.coordinate = None
if direction is not None:
self.direction = direction.squeeze()
if limits is not None:
self.limits = np.zeros([3, 2])
for lm, nm in zip(limits, dof):
if nm == "rx":
self.limits[0] = lm
elif nm == "ry":
self.limits[1] = lm
else:
self.limits[2] = lm