def transform_to( self, reference_pose ):
rel_vect, rel_theta = self.vunpack() # elements of this pose (the relative pose)
ref_vect, ref_theta = reference_pose.vunpack() # elements of the reference pose
# construct the elements of the transformed pose
result_vect_d = linalg.rotate_vector( rel_vect, ref_theta )
result_vect = linalg.add( ref_vect, result_vect_d )
result_theta = ref_theta + rel_theta
return Pose( result_vect, result_theta )
def transform_to(self, reference_pose):
rel_vect, rel_theta = self.vunpack(
) # elements of this pose (the relative pose)
ref_vect, ref_theta = reference_pose.vunpack(
) # elements of the reference pose
# construct the elements of the transformed pose
result_vect_d = linalg.rotate_vector(rel_vect, ref_theta)
result_vect = linalg.add(ref_vect, result_vect_d)
result_theta = ref_theta + rel_theta
return Pose(result_vect, result_theta)
def inverse(self):
result_theta = -self.theta
result_pos = linalg.rotate_vector([-self.x, -self.y], result_theta)
return Pose(result_pos, result_theta)
def inverse( self ):
result_theta = -self.theta
result_pos = linalg.rotate_vector( [ -self.x, -self.y ], result_theta )
return Pose( result_pos, result_theta )
