Beispiel #1
0
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