Пример #1
0
	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)
Пример #2
0
	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)