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 joint_positions(self):
self.positions[0] = self.HipCenter
self.positions[1] = q.act(self.orientation, self.lengths[0] * self.HipLeft) + self.HipCenter
self.positions[2] = q.act(self.orientation, self.lengths[0] * self.HipRight) + self.HipCenter
self.positions[3] = q.act(self.orientation, self.lengths[1] * self.Spine) + self.HipCenter
self.abs_orientations = [self.orientation for i in range(3)]
