Basic math usage

Basic math usage#

This notebook demonstrates how to work with basic math objects using kdFlex

import Karana.Math as km
import numpy as np
import math

Vectors and Matrices#

kdFlex natively supports numpy arrays at the Python level for vectors and matrices. The Python bindings seamless convert the numpy arrays to the C++ Vec and Mat class. Here’s an example of creating a simple three-vector using numpy:

vec3 = np.array([1.0, 2.0, 3.0])
vec3

array([1., 2., 3.])

Similarly, here is the \(3 \times 3\) identity matrix:

eye33 = np.array([[1.0, 0.0, 0.0], [0.0, 1.0, 0.0], [0.0, 0.0, 1.0]])
eye33

array([[1., 0., 0.],
       [0., 1., 0.],
       [0., 0., 1.]])

numpy has rich API, which you are encouraged to explore. For example, the above could also be achieved with:

eye33 = np.eye(3)
eye33

array([[1., 0., 0.],
       [0., 1., 0.],
       [0., 0., 1.]])

Spatial vectors#

kdFlex uses spatial notation which involves working with 6-dimension spatial vector quantities. For example, spatial velocity is a spatial vector consisting of both the \(3\)-dimensional angular and linear velocities. A SpatialVector is a generic container for these \(3 + 3\) dimensional vectors, where \(w\) is the angular part and \(v\) is the linear part:

vel = km.SpatialVector(w=[1, 2, 3], v=[4, 5, 6])
vel

Karana.Math.SpatialVector(
	array([1., 2., 3.]),
	array([4., 5., 6.])
)

For commonly used quantities, there are specialized, unit-aware SpatialVectors. Here we create a SpatialVelocity with nonstandard units:

from Karana.Math.Kquantities import ureg

vel = km.SpatialVelocity([1, 2, 3] * ureg.deg / ureg.s, [4, 5, 6] * ureg.km / ureg.s)
vel

Karana.Math.SpatialVelocity(
	<Quantity([0.01745329 0.03490659 0.05235988], '1 / second')>,
	<Quantity([4000. 5000. 6000.], 'meter / second')>
)

Rotations#

kdFlex primarily represents rotations using unit quaternions, consisting of four components. Unlike three-vector representations such as Euler angles, unit quaternions are free of singularities (often seen as gimbal lock), making them more suitable for general rotations.

There are a number of ways to construct a UnitQuaternion. For example, this rotates by \(\pi /2 \) about the \(y\)-axis:

z_vec = np.array([0.0, 0.0, 1.0])
y90 = km.UnitQuaternion(math.pi / 2, np.array([0.0, 1.0, 0.0]))
y90 * z_vec

array([1.00000000e+00, 0.00000000e+00, 2.22044605e-16])

For convenience and brevity, you can typically use native python lists in place of numpy arrays in most cases, with the vectors automatically being converted behind the scenes:

z_vec = [0.0, 0.0, 1.0]
y90 = km.UnitQuaternion(math.pi / 2, [0.0, 1.0, 0.0])
y90 * z_vec

array([1.00000000e+00, 0.00000000e+00, 2.22044605e-16])

Here’s an example of converting \(ZXZ\) Euler angles to a UnitQuaternion:

angles = km.EulerAngles(math.pi / 2, math.pi / 2, -math.pi / 2, km.EulerSystem.ZXZ)
quat = km.UnitQuaternion(angles)
quat

Karana.Math.UnitQuaternion(1.1102230246251565e-16, 0.7071067811865475, 0, 0.7071067811865476)

Converting a UnitQuaternion to a direction cosine matrix:

quat = km.UnitQuaternion(math.pi / 2, [0.0, 1.0, 0.0])
quat.toRotationMatrix()

Karana.Math.RotationMatrix(
array([[ 2.22044605e-16,  0.00000000e+00,  1.00000000e+00],
       [ 0.00000000e+00,  1.00000000e+00,  0.00000000e+00],
       [-1.00000000e+00,  0.00000000e+00,  2.22044605e-16]])
)

Homogeneous Transforms#

In kdFlex, a homogeneous transform, or HomTran, consists of a translation together with a rotation. Here we build the transform which translates all components by \(100\) and rotates the \(y\)-axis to the \(z\)-axis:

transform = km.HomTran(
    vec=[100, 100, 100],
    q=km.UnitQuaternion([0, 1, 0], [0, 0, 1]),  # rotates to send y-component to z-axis
)

transform * [1, 2, 3]

array([101.,  97., 102.])

Applying the inverse transform:

transform.inverse() * [1, 2, 3]

array([-99., -97.,  98.])

Transforms can also be composed:

transform * transform.inverse()

Karana.Math.HomTran(
	Karana.Math.UnitQuaternion(0, 0, 0, 1),
	array([0.00000000e+00, 4.26325641e-14, 0.00000000e+00])
)

Transforming spatial vectors#

HomTran also provides a rich set of SOA operations. For example, we can use the phiStar operator to transform a SpatialVelocity to another frame on a rigid body:

T_ab = km.HomTran(
    vec=[100, 100, 100],
    q=km.UnitQuaternion([0, 1, 0], [0, 0, 1]),  # rotates to send y-component to z-axis
)
vel_a = km.SpatialVelocity([1, 2, 3], [4, 5, 6])
vel_a

Karana.Math.SpatialVelocity(
	<Quantity([1. 2. 3.], '1 / second')>,
	<Quantity([4. 5. 6.], 'meter / second')>
)

vel_b = T_ab.phiStar(vel_a)
vel_b

Karana.Math.SpatialVector(
	array([ 1.,  3., -2.]),
	array([ -96.,  -94., -205.])
)

The phi operator can also be used to apply parallel axis transformation to spatial inertias as shown below.

i = np.eye(3)
i[0, 0] = 5.0
M_a = km.SpatialInertia(0.7, np.array([0.2, 0.3, 0.4]), i)
M_a

Karana.Math.SpatialInertia(
<Quantity(0.7, 'kilogram')>,
<Quantity([0.2 0.3 0.4], 'meter')>,
<Quantity([[5. 0. 0.]
 [0. 1. 0.]
 [0. 0. 1.]], 'kilogram * meter ** 2')>
)

# apply parallel axis theorem to shift M_a from 'a' to 'b'
M_b = M_a.parallelAxis(T_ab).matrix()

# use the phi() operator to do this transformation
M_b_alt = T_ab.phiMatrix() @ M_a.matrix() @ T_ab.phiMatrix().transpose()

# verify that the two paths give the same result
assert np.allclose(M_b - M_b_alt, 0, atol=1e-11)
Contents