def joint_positions(self):
# displacements from the root, with default initial position (0,0,0)
orienter = self.init_position.abs_orientations[-1]
if len(self.positions) > 0:
init_position = self.positions[0] # if root is defined, keep it where it is
else:
if self.init_position is not None:
init_position = self.init_position.get_joint(
self.labels[0].name
) # initialize root position to end of previous
else:
init_position = np.array((0, 0, 0)) # or just put it at origin if no previous
positions = [init_position]
# print init_position
self.abs_orientations = [orienter]
for i in range(1, len(self.labels)):
label = self.labels[i]
self.abs_orientations.append(q.prod(self.abs_orientations[i - 1], label.transform()))
new_end = positions[i - 1] + self.lengths[i - 1] * q.act(self.abs_orientations[i], np.array((0, 0, 1.0)))
positions.append(new_end)
# print positions
self.positions = positions
def reorient(self, quat):
for u in self.units:
self.units[u].abs_orientations = [q.prod(a, quat) for a in self.units[u].abs_orientations]
self.set_position()
def transform(self):
# quaternion realizing transformation
q1 = np.array((np.cos(0.5 * self.axial), 0, 0, np.sin(0.5 * self.axial)))
q2 = q.axis_mover(self.direction)
return q.invert(q.prod(q1, q2))