def _get_facing_angle(self, origin_frame, target_frame): ray = origin_frame.p - target_frame.p ray.Normalize() target_normal = target_frame.M * PyKDL.Vector(1, 0, 0) return angles.normalize_angle(math.acos(PyKDL.dot(target_normal, ray)) - math.pi)
def _get_viewing_angle(self, origin_frame, target_frame): origin_normal = origin_frame.M * PyKDL.Vector(1, 0, 0) target_normal = target_frame.M * PyKDL.Vector(1, 0, 0) return angles.normalize_angle(math.acos(PyKDL.dot(target_normal, origin_normal)) - math.pi)