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 )