Frames

module ModiaMath.Frames

This module contains functions for frames that is coordinate systems in 3D. The orientation of a frame is described either with a 3x3 rotation matrix or with a quaternion vector and its origin is described with a Vector3D:

• constModiaMath.RotationMatrix = SMatrix{3,3,Float64,9}`: Type of a Rotation matrix to rotate from a frame 1 into a frame 2.

• constModiaMath.Quaternion = SVector{4,Float64}`: Type of a Quaternion vector to rotate from a frame 1 into a frame 2.

• constModiaMath.Vector3D = SVector{3,Float64}`: Type of a vector in 3D space (e.g. position vector of the origin of a frame).

The following constants are defined

• constModiaMath.NullRotation: RotationMatrix with no rotation from a frame 1 into a frame 2.

• constModiaMath.NullQuaternion: Quaternion vector with no rotation from a frame 1 into a frame 2.

• constModiaMath.ZeroVector3D: Vector3D with only zero elements.

If an angle is given as an argument to one of the functions below, it might be a number (interpreted as having unit rad) or a number with a unit (for example: using Unitful; angle = 90u"°").

Constructors for a RotationMatrix R

The following functions return a ModiaMath.RotationMatrixR to rotate a frame 1 into a frame 2.

Constructors for a Quaternion q

The following functions return a ModiaMath.Quaternionq to rotate a frame 1 into a frame 2. Since q and -q define the same rotation the constructor functions have a keyword argument q_guess::Quaternion = NullQuaternion. From the two possible solutions q the one is returned that is closer to q_guess.

Operations on Frames

The following functions provide operations on frames. The orientation of a frame is defined with argument Rq meaning it can be either a ModiaMath.RotationMatrix R or a ModiaMath.Quaternion q (to rotate a frame 1 into a frame 2).

Interpolation of Frames

Given a set of frames by a vector r of position vectors to their origins and and an optional vector q of Quaternions of their absolute orientations, then the following functions interpolate linearly in these frames:

Examples

using ModiaMath
using Unitful

# R1,R2,R3 are the same RotationMatrices
R1 = ModiaMath.rot1(pi/2)
R2 = ModiaMath.rot1(90u"°")
R3 = ModiaMath.rot_e([1,0,0], pi/2)

Main developer

Martin Otter, DLR - Institute of System Dynamics and Control

The functions of this module are mostly a mapping of some of the functions of the Modelica Standard Library from Modelica (Modelica.Mechanics.MultiBody.Frames) to Julia (taking advantage of Julia features such as multiple dispatch and unit package Unitful).

const ModiaMath.NullQuaternion = Quaternion(0,0,0,1)

Constant Quaternion vector of a null rotation (= no rotation from frame 1 to frame 2)

Constant RotationMatrix that defines no rotation from frame 1 to frame 2.

const ModiaMath.ZeroVector3D = Vector3D(0.0, 0.0, 0.0)

Constant of a Vector3D where all elements are zero

path = ModiaMath.Path(r::Vector{Vector3D},
                      q::Vector{Quaternion} = Quaternion[];
                      v = ones(length(r)))

Return an instance of a new Path object. The Path object consists of n frames defined by the position vectors of their origins (r[i] for frame i) and optionally of their absolute rotation quaternions (q[i] for frame i) describing the rotation from the world frame to the respective frame.

A path parameter t is defined in the following way on these frames:

• t[1] = 0.0.
• t[i] = t[i-1] + pathLength_i/(v[i]+v[i-1])/2 if the origins of frames i-1 and i do not coincide.
• t[i] = t[i-1] + pathAngle_i/(v[i]+v[i-1])/2 if the origins of frames i-1 and i do coincide.

Hereby pathLength_i is the distance between the origins of frames i-1 and i in [m] and pathAngle_i is the planar rotation angle between frames i-1 and i in [rad].

If v[i] is the desired velocity or angular velocity at frame i, then path parameter t is approximately the time to move along the path. The time instant t_end of the last frame can be inquired with ModiaMath.t_pathEnd(path). For example, if a simulation shall be performed in such a way that the simulation should start with the first frame and end at stopTime at the last frame, then the path parameter should be selected as t = time*t_end/stopTime.

Given the actual path parameter, typically 0 <= t <= t_end (if t is outside of this interval, the frame at t is determined by extrapolation through the first two or the last two frames), the corresponding frame is determined by linear interpolation in the following way:

(rt, qt) = interpolate(  path,t)
 rt      = interpolate_r(path,t)

where rt is the position vector to the origin of the frame at path parameter t and qt is the absolute quaternion of the frame at path parameter t.

Example

import ModiaMath
using Unitful

r = [ ModiaMath.Vector3D(1,0,0),
      ModiaMath.Vector3D(0,1,0),
      ModiaMath.Vector3D(0,0,1) ]
q = [ ModiaMath.NullQuaternion,
      ModiaMath.qrot1(45u"°"),
      ModiaMath.qrot2(60u"°")]

path     = ModiaMath.Path(r,q)
t_end    = ModiaMath.t_pathEnd(path)
dt       = 0.1
stopTime = 2.0 
time     = 0.0

while time <= stopTime
   (rt, qt) = ModiaMath.interpolate(path, time*t_end/stopTime)
   time += dt
end

const ModiaMath.Quaternion = SVector{4,Float64}

Describes the rotation from a frame 1 into a frame 2 with a quaternion vector. If e is the (normalized) axis of rotation to rotate frame 1 into frame 2 (either resolved in frame 1 or frame 2) and angle is the rotation angle for this rotation then the quaternion vector q::ModiaMath.Quaternions is defined as:

q = [e*sin(angle/2),
       cos(angle/2]

const ModiaMath.RotationMatrix = SMatrix{3,3,Float64,9}

Describes the rotation from a frame 1 into a frame 2. An instance R of RotationMatrix has the following interpretation:

R::RotationMatrix = [ex ey ez]   

where ex, ey, ez are unit vectors in the direction of the x-axis, y-axis, and z-axis of frame 1, resolved in frame 2, respectively (for example ex=[1.0, 0.0, 0.0]) Therefore, if v1 is vector v resolved in frame 1 and v2 is vector v resolved in frame 2, the following relationship holds:

v2 = R*v1   
v1 = R'*v2

const ModiaMath.Vector3D = SVector{3,Float64}

Type of a vector in 3D space (e.g. position vector of the origin of a frame)

 R2 = ModiaMath.absoluteRotation(R1, R_rel) 
 q2 = ModiaMath.absoluteRotation(q1, q_rel)

Return ModiaMath.RotationMatrixR2 or ModiaMath.Quaternionq2 defining the rotation from frame 0 to frame 2 from RotationMatrix R1 or Quaternion q1that define the rotation from frame 0 to frame 1 and the relative RotationMatrix R_rel or the relative Quaternion q_rel that define the rotation from frame 1 to frame 2.

ModiaMath.assertQuaternion(q::AbstractVector)

Assert that vector q has the properties of a Quaternion vector (has 4 elements, norm(q) = 1)

ModiaMath.assertRotationMatrix(R::AbstractMatrix)

Assert that matrix R has the properties of a rotation matrix (is 3x3 and R'*R - eye(3) = zeros(3,3))

e = eAxis(axis::Int)

Return unit vector e in direction of axis axis (axis = 1,2,3 or -1,-2-,3).

Example

import ModiaMath

e1 = ModiMath.eAxis(1)    # e1 = Vector3D(1.0,  0.0, 0.0)
e2 = ModiMath.eAxis(-2)   # d2 = Vector3D(0.0, -1.0, 0.0)

q = ModiaMath.from_R(R::ModiaMath.RotationMatrix;
                     q_guess = NullQuaternion)

Return Quaternion q from RotationMatrix R.

From the two possible solutions q the one is returned that is closer to q_guess (note, q and -q define the same rotation).

R = ModiaMath.from_q(q::ModiaMath.Quaternion)

Return RotationMatrix R from Quaternion q.

(rt, qt) = ModiaMath.interpolate(path, t)

Return position rtand Quaternion qt of path::ModiaMath.Path at path parameter t::Number.

rt = ModiaMath.interpolate_r(path, t)

Return position r of path::ModiaMath.Path at path parameter t::Number.

 R_inv = ModiaMath.inverseRotation(R) 
 q_inv = ModiaMath.inverseRotation(q)

Return inverse ModiaMath.RotationMatrixR_inv or inverse ModiaMath.Quaternionq_inv defining the rotation from frame 1 to frame 0 from RotationMatrix R or Quaternion qthat define the rotation from frame 0 to frame 1.

angle = planarRotationAngle(e, v1, v2; angle_guess = 0.0)

Return angle of a planar rotation, given the normalized axis of rotation to rotate frame 1 around e into frame 2 (norm(e) == 1 required), and the representations of a vector in frame 1 (v1) and frame 2 (v2). Hereby, it is required that v1 is not parallel to e. The returned angle is in the range -pi <= angle - angle_guess <= pi (from the infinite many solutions, the one is returned that is closest to angle_guess).

Example

import ModiaMath
using Unitful

angle1 = 45u"°"
e      = normalize([1.0, 1.0, 1.0])
R      = ModiaMath.rot_e(e, angle1)

v1 = [1.0, 2.0, 3.0]
v2 = ModiaMath.resolve2(R, v1)

angle2 = planarRotationAngle(e, v1, v2)
isapprox(angle1, angle2)

q = ModiaMath.qrot1(angle; q_guess = NullQuaternion)

Return Quaternion q that rotates with angle angle along the x-axis of frame 1.

From the two possible solutions q the one is returned that is closer to q_guess (note, q and -q define the same rotation).

q = ModiaMath.qrot123(angle1, angle2, angle3)

Return Quaternion q by rotating with angle1 along the x-axis of frame 1, then with angle2 along the y-axis of this frame and then with angle3 along the z-axis of this frame.

From the two possible solutions q the one is returned that is closer to q_guess (note, q and -q define the same rotation).

q = ModiaMath.qrot2(angle; q_guess = NullQuaternion)

Return Quaternion q that rotates with angle angle along the y-axis of frame 1.

From the two possible solutions q the one is returned that is closer to q_guess (note, q and -q define the same rotation).

q = ModiaMath.qrot3(angle; q_guess = NullQuaternion)

Return Quaternion q that rotates with angle angle along the z-axis of frame 1.

From the two possible solutions q the one is returned that is closer to q_guess (note, q and -q define the same rotation).

q = ModiaMath.qrot_e(e, angle; q_guess = NullQuaternion)

Return Quaternion q that rotates with angle angle along unit axis e. This function assumes that norm(e) == 1.

From the two possible solutions q the one is returned that is closer to q_guess (note, q and -q define the same rotation).

q = ModiaMath.qrot_nxy(nx, ny)

It is assumed that the two input vectors nx and ny are resolved in frame 1 and are directed along the x and y axis of frame 2. The function returns the Quaternion q to rotate from frame 1 to frame 2.

The function is robust in the sense that it returns always a Quaternion q, even if ny is not orthogonal to nx or if one or both vectors have zero length. This is performed in the following way: If nx and ny are not orthogonal to each other, first a unit vector ey is determined that is orthogonal to nx and is lying in the plane spanned by nx and ny. If nx and ny are parallel or nearly parallel to each other or ny is a vector with zero or nearly zero length, a vector ey is selected arbitrarily such that ex and ey are orthogonal to each other. If both nx and ny are vectors with zero or nearly zero length, an arbitrary Quaternion q is returned.

Example

using Unitful
import ModiaMath

q1 = ModiaMath.qrot1(90u"°")
q2 = ModiaMath.qrot_nxy([1  , 0, 0], [0  , 0, 1  ])
q3 = ModiaMath.qrot_nxy([0.9, 0, 0], [1.1, 0, 1.1])
isapprox(q1,q2)   # returns true
isapprox(q1,q3)   # returns true

 R_rel = ModiaMath.relativeRotation(R1, R2) 
 q_rel = ModiaMath.relativeRotation(q1, q2)

Return relative ModiaMath.RotationMatrixR_rel or relative ModiaMath.Quaternionq_rel defining the rotation from frame 1 to frame 2 from absolute RotationMatrix R1 or absolute Quaternion q1that define the rotation from frame 0 to frame 1 and the absolute RotationMatrix R2 or the absolute Quaternion q2 that define the rotation from frame 0 to frame 2.

v1 = ModiaMath.resolve1([R|q], v2)

Transform vector v2 (v resolved in frame 2) to vector v1 (v resolved in frame 1) given either ModiaMath.RotationMatrix R or ModiaMath.Quaternion q (to rotate a frame 1 into a frame 2).

v2 = ModiaMath.resolve2([R|q], v1)

Transform vector v1 (v resolved in frame 1) to vector v2 (v resolved in frame 2) given either ModiaMath.RotationMatrix R or ModiaMath.Quaternion q (to rotate a frame 1 into a frame 2).

R = ModiaMath.rot1(angle)

Return RotationMatrix R that rotates with angle angle along the x-axis of frame 1.

R = ModiaMath.rot123(angle1, angle2, angle3)

Return RotationMatrix R by rotating with angle1 along the x-axis of frame 1, then with angle2 along the y-axis of this frame and then with angle3 along the z-axis of this frame.

R = ModiaMath.rot2(angle)

Return RotationMatrix R that rotates with angle angle in [radian] along the y-axis of frame 1.

R = ModiaMath.rot3(angle)

Return RotationMatrix R that rotates with angle angle in [radian] along the z-axis of frame 1.

R = ModiaMath.rot_e(e, angle)

Return RotationMatrix that rotates around angle angle along unit axis e. This function assumes that norm(e) == 1.

R = ModiaMath.rot_nxy(nx, ny)

It is assumed that the two input vectors nx and ny are resolved in frame 1 and are directed along the x and y axis of frame 2. The function returns the RotationMatrix R to rotate from frame 1 to frame 2.

The function is robust in the sense that it returns always a RotationMatrix R, even if ny is not orthogonal to nx or if one or both vectors have zero length. This is performed in the following way: If nx and ny are not orthogonal to each other, first a unit vector ey is determined that is orthogonal to nx and is lying in the plane spanned by nx and ny. If nx and ny are parallel or nearly parallel to each other or ny is a vector with zero or nearly zero length, a vector ey is selected arbitrarily such that ex and ey are orthogonal to each other. If both nx and ny are vectors with zero or nearly zero length, an arbitrary rotation matrix is returned.

Example

using Unitful
import ModiaMath

R1 = ModiaMath.rot1(90u"°")
R2 = ModiaMath.rot_nxy([1  , 0, 0], [0  , 0, 1  ])
R3 = ModiaMath.rot_nxy([0.9, 0, 0], [1.1, 0, 1.1])
isapprox(R1,R2)   # returns true
isapprox(R1,R3)   # returns true

M = ModiaMath.skew(e::AbstractVector)

Return the skew symmetric matrix M::SMatrix{3,3,Float64,9} of vector e (length(e) = 3)

t_end = ModiaMath.t_pathEnd(path::[`ModiaMath.Path`](@ref))

Return the final path parameter tof the last frame in path (path parameter of first frame = 0.0).