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