def DistanceFromFace(face_point): """ Calculates the distance projected in the ground from a face. """ vector = copy.deepcopy(face_point) vector.z = 0.0 return vector_magnitude(vector)
def pose_at_distance(pose,distance): """ Returns a pose that has the same orientation as the original but the position is at a distance from the original. Very usefull when you want to mantain a distance from an object. """ unit_vector = normalize_vector(pose.position) k = vector_magnitude(pose.position) distance_des = k - distance dist_vector = multiply_vector(unit_vector, distance_des) new_pose = Pose() new_pose.position = dist_vector new_pose.orientation = pose.orientation return new_pose