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)]