def update(self, inputVector):
q = inputVector[self.usedGeneralizedCoordinates]
Tx = Math.rotationMatrix3Dx(q[0])
Ty = Math.rotationMatrix3Dy(q[1])
Tz = Math.rotationMatrix3Dz(q[2])
self.childLink.relativeOrientation = np.array(Tz) @ np.array(
Ty) @ np.array(Tx)
self.childLink.absoluteBase = self.parentLink.absoluteFollower[
self.parentFollowerNumber] + np.array([q[3], q[4], q[5]])
self.childLink.absoluteOrientation = self.parentLink.absoluteOrientation @ self.childLink.relativeOrientation
rBaseToFollower = self.childLink.relativeFollower - np.matlib.repmat(
self.childLink.relativeBase,
self.childLink.relativeFollower.shape[0], 1)
rBaseToCoM = self.childLink.relativeCoM - self.childLink.relativeBase
self.childLink.absoluteFollower = np.matlib.repmat(
self.childLink.absoluteBase,
self.childLink.relativeFollower.shape[0],
1) + (self.childLink.absoluteOrientation @ rBaseToFollower.T).T
self.childLink.absoluteCoM = self.childLink.absoluteBase + self.childLink.absoluteOrientation @ rBaseToCoM
def update(self, inputVector):
q = inputVector[self.usedGeneralizedCoordinates]
Tx = Math.rotationMatrix3Dx(q[0])
Ty = Math.rotationMatrix3Dy(q[1])
self.childLink.relativeOrientation = self.defaultJointOrientation @ Ty @ Tx
self.forwardKinematicsJointUpdate()
def update(self, inputVector):
q = inputVector[self.usedGeneralizedCoordinates]
self.childLink.relativeOrientation = self.defaultJointOrientation @ Math.rotationMatrix3Dz(
q)
self.forwardKinematicsJointUpdate()