def append(self, other): """Appends other to self in space.""" my_dcm = self.__get_dcm(ARM_FRAME) rotated_vector = util.sequence_to_vector( ARM_FRAME, my_dcm * other.vector.to_matrix(ARM_FRAME)) new_direction = util.sequence_to_vector( ARM_FRAME, my_dcm * other.direction.to_matrix(ARM_FRAME)) return Position(self.vector + rotated_vector, new_direction, self.angle + other.angle)
def cm_at(self, theta): """Returns: A vector representing the location of the center of mass when the joint is at theta. """ delta, _ = self._direction(theta) if delta != ARM_FRAME.y: # Create a new RefernceFrame whose Y axis == delta. delta_frame_rotate = delta.cross(ARM_FRAME.y) angle = -sympy.acos(delta.dot(ARM_FRAME.y)) delta_frame = ARM_FRAME.orientnew('Na', 'Axis', [angle, delta_frame_rotate]) return util.sequence_to_vector( ARM_FRAME, ARM_FRAME.dcm(delta_frame) * self.cm.to_matrix(ARM_FRAME)) else: return self.cm
def append_vector(self, vector): """Appends vector to self in space.""" return self.vector + util.sequence_to_vector( ARM_FRAME, self.__get_dcm(ARM_FRAME) * vector.to_matrix(ARM_FRAME))