L2 norm of the quaternion 4-vector. This should be 1.0 for a unit quaternion (versor) Slow but accurate. If speed is a concern, consi

Move the robot hand dynamically to a given target pose Args: target_ee_pose: (4, 4) transformation of robot hand in robot

Validate a sequence to be of a certain length and ensure it's a numpy array of floats. Raises: ValueError: Invalid length or non-

Generate a random unit quaternion. Uniformly distributed across the rotation space As per: http://planning.cs.uiuc.edu/node198.html

Normalise the object to a unit quaternion using a fast approximation method if appropriate. Object is guaranteed to be a quaternion of approx

Object is guaranteed to be a unit quaternion after calling this operation UNLESS the object is equivalent to Quaternion(0)

Rotate vector(s) v about the rotation described by quaternion(s) q. Expects a tensor of shape (*, 4) for q and a tensor of shape (*, 3) for v

Matrix representation of quaternion for multiplication purposes.

Rotate a quaternion vector using the stored rotation. Params: q: The vector to be rotated, in quaternion form (0 + xi + yj + kz)

Quaternion Exponential. Find the exponential of a quaternion amount. Params: q: the input quaternion/argument as a Quat

This function is provided only to show how to convert camera observation to world space coordinates. It can be removed if not needed.

Determine whether the quaternion is of unit length to within a specified tolerance value. Params: tolerance: [optional] maximum a

Quaternion Logarithm. Find the logarithm of a quaternion amount. Params: q: the input quaternion/argument as a Quaterni

Convert quaternion(s) q to Euler angles. Expects a tensor of shape (*, 4), where * denotes any number of dimensions. Returns a tensor of

Initialise from axis and angle representation Create a Quaternion by specifying the 3-vector rotation axis and rotation angle (in rad

Initialise from matrix representation Create a Quaternion by specifying the 3x3 rotation or 4x4 transformation matrix (as a numpy arr

Matrix representation of quaternion for multiplication purposes.

Helper method: Wrap any angle to lie between -pi and pi Odd multiples of pi are wrapped to +pi (as opposed to -pi)

This function is a kinematic-level calculation which do not consider dynamics. Pay attention to the frame of twist, is it spatial twi

Get the axis or vector about which the quaternion rotation occurs For a null rotation (a purely real quaternion), the rotation angle will

Control the robot dynamically to execute the given twist for one time step This method will try to execute the joint velocity using the inter

Quaternion logarithm map. Find the logarithm map on the quaternion Riemannian manifold. Params: q: the base point at wh

Compute the exponential coordinate corresponding to the given SE(3) matrix Note: unit twist is not a unit vector Args: pose

Multiply quaternion(s) q with quaternion(s) r. Expects two equally-sized tensors of shape (*, 4), where * denotes any number of dimensions.

Spherical Linear Interpolation between quaternions. Implemented as described in https://en.wikipedia.org/wiki/Slerp Find a valid quat

Quaternion symmetrized logarithm map. Find the symmetrized logarithm map on the quaternion Riemannian manifold. Params: