AOX distance function

* Constuct a cuboid given its: - Center coordinate - Orientation about that center given in Euler XYZ angles - Half-e

Return length, i.e. eucledian norm, of ndarray along axis. >>> v = numpy.random.random(3) >>> n = vector_norm(v) >>> numpy.allclose(n, nu

Return ndarray normalized by length, i.e. eucledian norm, along axis. >>> v0 = numpy.random.random(3) >>> v1 = unit_vector(v0) >>> numpy.

Return homogeneous rotation matrix from quaternion. >>> R = quaternion_matrix([0.06146124, 0, 0, 0.99810947]) >>> numpy.allclose(R, rotation_

Basic union-find TODO: We may be able to improve these implementations

Return true iff data has been marked for removal.

Return homogeneous rotation matrix.

Return sphere point perpendicular to axis.

Because ceil isn't constexpr until C++23 for some reason.....

Return the nearest neighbors within distance \c radius in sorted order

Convert an OMPL state into a VAMP vector

Ported from OMPL: src/ompl/util/src/GeometricEquations.cpp

Add spheres to the env/ Sphere positions are (N,3) and sphere radii are (N)

Adds a slider to step through waypoints of a trajectory also allows for auto step through using play/pause button Args: server (

Return unit sphere coordinates from window coordinates.

Return Euler angles from rotation matrix for specified axis sequence. axes : One of 24 axis sequences as string or encoded tuple Note that m

Return Euler angles from quaternion for specified axis sequence. >>> angles = euler_from_quaternion([0.06146124, 0, 0, 0.99810947]) >>> numpy

Return the k nearest neighbors in sorted order

Return true iff the node needs to be split into child nodes.