Package `dynkin`

A toolkit for 3D dynamics and kinematics of rigid bodies using the YPR euler angle convention.

pydynkin - A python version of `dynkin`

A toolkit for 3D dynamics and kinematics of rigid bodies using the YPR euler angle convention.

Note: dynkin is also available in a C++ version, available here: https://github.com/freol35241/dynkin

General

dynkin is a set of tools for handling the dynamics and kinematics of rigid bodies in 3 dimensions (6DOFs). Some features:

Homogenous transformation matrices
Chained reference frames
Idealized rigid body implementation

The fundamentals of reference frames and the kinematic relations of these are based on Theory of Applied Robotics (Reza N. Jazar) , the idealized rigid body implementation follows the outline suggested in the lectures by Fossen.

Installation

pip install dynkin

Theory intro

Some basic notions:

A reference Frame is defined in dynkin as an object which positions, vectors, velocities, accelerations etc can be decomposed in. dynkin represents reference Frames by (4x4) homogenous transformation matrices. A Frame is aligned (positioned, rotated) and moves (linear and angular velocity) in relation to another Frame or the inertial frame (represented by None).
The pose of a Frame is its generalized position and the twist of a Frame is its generalized velocity, both in relation to the inertial frame.
All rotations in dynkin are internally represented by rotation matrices but the external API, so far, deals only with Euler angles of the YPR (Yaw-Pitch-Roll) convention.
A kinematic chain is a single-linked list of Frames, where each Frame holds a reference to its closest parent. Any number of Frames may be attached into such a chain and the chain may also have any number of branches, it is however the user´s responsibility to ensure no kinematic loops occur.
A transform() is an object relating two Frames enabling transformation of positions, vectors, velocities etc from one Frame to the other. The Frames do not need to be part of the same kinematic chain.
A RigidBody is a 3D body with arbitrary extent that may be described by a generalized inertia matrix (6x6). It accelerates when subject to generalized external forces (wrenches) and rotational velocities give rise to inertial forces (coriolis and centripetal contributions).

Examples

Single frame

from dynkin import Frame, transform

frame1 = Frame(position=[1, 2, 3], attitude=[0, 0, 90], degrees=True)

# Find transformation from the inertial frame to frame1
ti1 = transform(None, frame1)

# Transformation of vector
v1_decomposed_in_frame1 = ti1.apply_vector(v1_decomposed_in_inertial_frame)

# Transformation of position
p1_decomposed_in_frame1 = ti1.apply_position(p1_decomposed_in_inertial_frame)

# Transformation of wrench
w1_decomposed_in_frame1 = ti1.apply_wrench(w1_decomposed_in_inertial_frame)

# Find the inverse transformation
t1i = ti1.inv()

# Pose of this frame, decomposed in inertial frame
frame1.get_pose()

# Twist of this frame, decomposed in inertial frame
frame.get_twist()

Two frames

from dynkin import Frame, transform

frame1 = Frame(position=[1, 2, 3], attitude=[0, 0, 90], degrees=True)
frame2 = Frame(position=[3, 2, 1], attitude=[0, 0, -90], degrees=True)

# Find transformation from frame1 to frame2
t12 = transform(frame1, frame2)

# Transformation of vector
v1_decomposed_in_frame2 = t12.apply_vector(v1_decomposed_in_frame1)

# Transformation of position
p1_decomposed_in_frame2 = t12.apply_position(p1_decomposed_in_frame1)

# Transformation of wrench
w1_decomposed_in_frame2 = t12.apply_wrench(w1_decomposed_in_frame1)

# Find the inverse transformation
t21 = t12.inv()

Kinematic chains

from dynkin import Frame, transform

frame1 = Frame(position=[1, 2, 3], attitude=[0, 0, 90], degrees=True)
frame2 = frame1.align_child(position=[3, 2, 1], attitude=[0, 0, -90], degrees=True)
frame3 = frame2.align_child(position=[1, 1, 1], attitude=[0, 0, 0], degrees=True)

# Find transformation from inertial frame to frame3
ti3 = transform(None, frame3)

# Transformation from frame3 and frame1
t31 = transform(frame3, frame1)

...

TODO: RigidBody example

License

Distributed under the terms of the MIT license, dynkin is free and open source software

Expand source code

"""
A toolkit for 3D dynamics and kinematics of rigid bodies using the
 YPR euler angle convention.

.. include:: ../README.md
"""

__pdoc__ = {
    '': False,
    'euler': False,
    'frame': False,
    'rigid_body': False,
}

import numpy as np

def assert_3D_vector(v):
    """
    Make sure v is a 3x1 numpy array with dtype==float
    """
    out = np.array(v, dtype=float)
    if not out.size == 3:
        raise AssertionError('Must be a np.ndarray of size 3!')
    return out

def assert_6D_vector(v):
    """
    Make sure v is a 6x1 numpy array with dtype==float
    """
    out = np.array(v, dtype=float)
    if not out.size == 6:
        raise AssertionError('Must be a np.ndarray of size 6!')
    return out

def assert_6D_matrix(m):
    """
    Make sure m is a 6x6 numpy array with dtype==float
    """
    out = np.array(m, dtype=float)
    if not out.shape == (6, 6):
        raise ValueError('Must be a np.ndarray of shape (6,6)!')
    return out


from .frame import Frame, Transform, transform
from .rigid_body import RigidBody

__all__ = [
    'RigidBody',
    'Frame',
    'Transform',
    'transform'
]

Functions

def transform(zeroth_frame, end_frame)

Finds a Transform between two frames

Expand source code

def transform(zeroth_frame, end_frame):
    """
    Finds a Transform between two frames
    """    
    HTM = np.eye(4)
    f = end_frame
    
    while f:
        if f == zeroth_frame:
            return Transform(HTM)
        
        HTM = f.HTM @ HTM
        f = f.parent
        
    # If we get to here, the least common base fram is the inertial frame (None)
    T = Transform(HTM)
    
    if zeroth_frame is None:
        return T
    
    T_ = transform(None, zeroth_frame).inv()
    
    return Transform(T_.HTM @ T.HTM)

Classes

class Frame (position=None, attitude=None, linear_velocity=None, angular_velocity=None, degrees=False, parent=None)

A coordinate reference frame

Expand source code

class Frame:
    """
    A coordinate reference frame
    """    
    def __init__(self, position=None, attitude=None, linear_velocity=None, angular_velocity=None, degrees=False, parent=None):
        self._parent = parent
        self._HTM = np.eye(4)
        
        self.set_position(position or [0, 0, 0])
        self.set_attitude(attitude or [0, 0, 0,], degrees=degrees)
        self.set_linear_velocity(linear_velocity or [0, 0, 0])
        self.set_angular_velocity(angular_velocity or [0, 0, 0], degrees=degrees)
        
    @property
    def parent(self):
        """
        Reference to parent Frame
        """
        return self._parent
        
    @property
    def HTM(self):
        """
        Homogenous Transformation Matrix (4x4) defining this Frame
        """
        return self._HTM
    
    @property
    def position(self):
        """
        Position (3x1) of this Frame in relation to parent Frame
        """
        return self._HTM[:3, -1]
        
    def set_position(self, position):
        """
        Set position (3x1) of this Frame in relation to parent Frame
        """
        self._HTM[:3, -1] = assert_3D_vector(position)
        
    @property
    def rotation(self):
        """
        Rotation (3x3) of this Frame in relation to parent Frame
        """
        return self.HTM[:3, :3]
    
    def set_rotation(self, rotation):
        """
        Set rotation (3x3) of this Frame in relation to parent Frame
        """
        self._HTM[:3, :3] = rotation
        
    @property
    def attitude(self):
        """
        Attitude (euler angles)(3x1) of this Frame in relation to parent Frame
        """
        return rotation_to_euler(self.rotation)
        
    def set_attitude(self, attitude, degrees=False):
        """
        Set attitude (euler angles)(3x1) of this Frame in relation to parent Frame
        """
        self._HTM[:3, :3] = euler_to_rotation(
            assert_3D_vector(np.deg2rad(attitude) if degrees else attitude)
        )
            
    @property
    def linear_velocity(self):
        """
        Linear velocity (3x1) of this Frame in relation to parent Frame, decomposed in this Frame
        """
        return self._linear_velocity
    
    def set_linear_velocity(self, velocity):
        """
        Set linear velocity (3x1) of this Frame in relation to parent Frame, decomposed in this Frame
        """
        self._linear_velocity = assert_3D_vector(velocity)
            
    @property
    def angular_velocity(self):
        """
        Angular velocity (3x1) of this Frame in relation to parent Frame, decomposed in this Frame
        """
        return self._omega
    
    def set_angular_velocity(self, omega, degrees=False):
        """
        Set angular velocity (3x1) of this Frame in relation to parent Frame, decomposed in this Frame
        """
        self._omega = assert_3D_vector(np.deg2rad(omega) if degrees else omega)
        
    def get_pose(self):
        """
        Pose (6x1) of this Frame in relation to the inertial Frame
        """
        t = transform(None, self)
        return np.array([
            *t.HTM[:3, -1],
            *rotation_to_euler(t.HTM[:3, :3])
        ])
        
    def get_twist(self):
        """
        Twist (6x1) of this Frame in relation to the inertial Frame
        """
        v = self.linear_velocity
        w = self.angular_velocity
        
        if self.parent is not None:
            twist_p = self.parent.get_twist()
            t = transform(self, self.parent)
            v += t.apply_vector(twist_p[:3] + np.cross(twist_p[3:], self.position))
            w += t.apply_vector(twist_p[3:])
            
        return np.array([*v, *w])            
        
    def align_child(self, *args, **kwargs):
        """
        Align a child frame in relation to this Frame, accepts same args and kwargs as Frame()
        """
        kwargs.update({
            'parent': self
        })
        return Frame(*args, **kwargs)

Subclasses

dynkin.rigid_body.RigidBody

Instance variables

var HTM

Homogenous Transformation Matrix (4x4) defining this Frame

Expand source code

@property
def HTM(self):
    """
    Homogenous Transformation Matrix (4x4) defining this Frame
    """
    return self._HTM

var angular_velocity

Angular velocity (3x1) of this Frame in relation to parent Frame, decomposed in this Frame

Expand source code

@property
def angular_velocity(self):
    """
    Angular velocity (3x1) of this Frame in relation to parent Frame, decomposed in this Frame
    """
    return self._omega

var attitude

Attitude (euler angles)(3x1) of this Frame in relation to parent Frame

Expand source code

@property
def attitude(self):
    """
    Attitude (euler angles)(3x1) of this Frame in relation to parent Frame
    """
    return rotation_to_euler(self.rotation)

var linear_velocity

Linear velocity (3x1) of this Frame in relation to parent Frame, decomposed in this Frame

Expand source code

@property
def linear_velocity(self):
    """
    Linear velocity (3x1) of this Frame in relation to parent Frame, decomposed in this Frame
    """
    return self._linear_velocity

var parent

Reference to parent Frame

Expand source code

@property
def parent(self):
    """
    Reference to parent Frame
    """
    return self._parent

var position

Position (3x1) of this Frame in relation to parent Frame

Expand source code

@property
def position(self):
    """
    Position (3x1) of this Frame in relation to parent Frame
    """
    return self._HTM[:3, -1]

var rotation

Rotation (3x3) of this Frame in relation to parent Frame

Expand source code

@property
def rotation(self):
    """
    Rotation (3x3) of this Frame in relation to parent Frame
    """
    return self.HTM[:3, :3]

Methods

def align_child(self, *args, **kwargs)

Align a child frame in relation to this Frame, accepts same args and kwargs as Frame()

Expand source code

def align_child(self, *args, **kwargs):
    """
    Align a child frame in relation to this Frame, accepts same args and kwargs as Frame()
    """
    kwargs.update({
        'parent': self
    })
    return Frame(*args, **kwargs)

def get_pose(self)

Pose (6x1) of this Frame in relation to the inertial Frame

Expand source code

def get_pose(self):
    """
    Pose (6x1) of this Frame in relation to the inertial Frame
    """
    t = transform(None, self)
    return np.array([
        *t.HTM[:3, -1],
        *rotation_to_euler(t.HTM[:3, :3])
    ])

def get_twist(self)

Twist (6x1) of this Frame in relation to the inertial Frame

Expand source code

def get_twist(self):
    """
    Twist (6x1) of this Frame in relation to the inertial Frame
    """
    v = self.linear_velocity
    w = self.angular_velocity
    
    if self.parent is not None:
        twist_p = self.parent.get_twist()
        t = transform(self, self.parent)
        v += t.apply_vector(twist_p[:3] + np.cross(twist_p[3:], self.position))
        w += t.apply_vector(twist_p[3:])
        
    return np.array([*v, *w])

def set_angular_velocity(self, omega, degrees=False)

Set angular velocity (3x1) of this Frame in relation to parent Frame, decomposed in this Frame

Expand source code

def set_angular_velocity(self, omega, degrees=False):
    """
    Set angular velocity (3x1) of this Frame in relation to parent Frame, decomposed in this Frame
    """
    self._omega = assert_3D_vector(np.deg2rad(omega) if degrees else omega)

def set_attitude(self, attitude, degrees=False)

Set attitude (euler angles)(3x1) of this Frame in relation to parent Frame

Expand source code

def set_attitude(self, attitude, degrees=False):
    """
    Set attitude (euler angles)(3x1) of this Frame in relation to parent Frame
    """
    self._HTM[:3, :3] = euler_to_rotation(
        assert_3D_vector(np.deg2rad(attitude) if degrees else attitude)
    )

def set_linear_velocity(self, velocity)

Set linear velocity (3x1) of this Frame in relation to parent Frame, decomposed in this Frame

Expand source code

def set_linear_velocity(self, velocity):
    """
    Set linear velocity (3x1) of this Frame in relation to parent Frame, decomposed in this Frame
    """
    self._linear_velocity = assert_3D_vector(velocity)

def set_position(self, position)

Set position (3x1) of this Frame in relation to parent Frame

Expand source code

def set_position(self, position):
    """
    Set position (3x1) of this Frame in relation to parent Frame
    """
    self._HTM[:3, -1] = assert_3D_vector(position)

def set_rotation(self, rotation)

Set rotation (3x3) of this Frame in relation to parent Frame

Expand source code

def set_rotation(self, rotation):
    """
    Set rotation (3x3) of this Frame in relation to parent Frame
    """
    self._HTM[:3, :3] = rotation

class RigidBody (inertia, cog=None)

Idealized rigid body implementation

Expand source code

class RigidBody(Frame):
    """
    Idealized rigid body implementation
    """
    def __init__(self, inertia, cog=None):
        super().__init__()
        
        self._inertia = assert_6D_matrix(inertia)
        
        self._cog = self.align_child(
            position=cog
        )
        
    @classmethod
    def from_mass_gyradii(cls, mass, gyradii, cog=None):
        """
        Instantiate from mass and gyradii
        """
        return cls(
            generalized_inertia_matrix(mass, gyradii),
            cog=cog
            )
        
    @property
    def mass(self):
        """
        Mass of this RigidBody
        """
        return self._inertia[0, 0]
    
    @property
    def CoG(self):
        """
        Reference to Centre of Gravity Frame of this RigidBody
        """
        return self._cog
    
    def coriolis_centripetal_matrix(self, inertia, twist):
        """
        Returns Coriolis-Centripetal matrix (6x6) of this RigidBody
        """
        inertia = assert_6D_matrix(inertia)
        twist = assert_6D_vector(twist)
        C = np.zeros((6,6))
        C[:3, :3] = self.mass*skew(twist[3:])
        C[3:, 3:] = -skew(inertia[3:, 3:] @ twist[3:])
        return C
        
    def generalized_coordinates(self):
        """
        Returns the generalized coordinates of this RigidBody,
        [x, y, z, fi, theta, psi]
        """
        return self.get_pose()
    
    def generalized_velocities(self):
        """
        Returns the generalized velocities of this RigidBody,
        [dx, dy, dz, dfi, dtheta, dpsi]
        """
        twist = self.get_twist()
        t = transform(None, self)
        return np.array([
            *t.apply_vector(twist[:3]),
            *angular_velocity_to_deuler(self.attitude, twist[3:])
        ])

    def acceleration(self, wrench, additional_inertia=None):
        """
        Returns the resulting acceleration from the given wrench
        """
        f = assert_6D_vector(wrench)
        additional_inertia = assert_6D_matrix(additional_inertia or np.zeros((6, 6)))
        twist = self.get_twist()
        H = motion_transformation_matrix(self.CoG.position)
        I_cg = self._inertia + additional_inertia
        C_cg = self.coriolis_centripetal_matrix(I_cg, twist)
        I_b = H.T @ I_cg @ H
        C_b = H.T @ C_cg @ H
        f_cc = C_b @ twist
        f -= f_cc
        
        return np.linalg.solve(I_b, f)
    
    def wrench(self, acceleration, additional_inertia=None):
        """
        Returns the required wrench to obtain the given acceleration
        """
        a = assert_6D_vector(acceleration)
        additional_inertia = assert_6D_matrix(additional_inertia or np.zeros((6, 6)))
        twist = self.get_twist()
        H = motion_transformation_matrix(self.CoG.position)
        I_cg = self._inertia + additional_inertia
        C_cg = self.coriolis_centripetal_matrix(I_cg, twist)
        I_b = H.T @ I_cg @ H
        C_b = H.T @ C_cg @ H
        f_cc = C_b @ twist
        
        f = I_b @ a
        f += f_cc
        
        return f

Ancestors

dynkin.frame.Frame

Static methods

def from_mass_gyradii(mass, gyradii, cog=None)

Instantiate from mass and gyradii

Expand source code

@classmethod
def from_mass_gyradii(cls, mass, gyradii, cog=None):
    """
    Instantiate from mass and gyradii
    """
    return cls(
        generalized_inertia_matrix(mass, gyradii),
        cog=cog
        )

Instance variables

var CoG

Reference to Centre of Gravity Frame of this RigidBody

Expand source code

@property
def CoG(self):
    """
    Reference to Centre of Gravity Frame of this RigidBody
    """
    return self._cog

var mass

Mass of this RigidBody

Expand source code

@property
def mass(self):
    """
    Mass of this RigidBody
    """
    return self._inertia[0, 0]

Methods

def acceleration(self, wrench, additional_inertia=None)

Returns the resulting acceleration from the given wrench

Expand source code

def acceleration(self, wrench, additional_inertia=None):
    """
    Returns the resulting acceleration from the given wrench
    """
    f = assert_6D_vector(wrench)
    additional_inertia = assert_6D_matrix(additional_inertia or np.zeros((6, 6)))
    twist = self.get_twist()
    H = motion_transformation_matrix(self.CoG.position)
    I_cg = self._inertia + additional_inertia
    C_cg = self.coriolis_centripetal_matrix(I_cg, twist)
    I_b = H.T @ I_cg @ H
    C_b = H.T @ C_cg @ H
    f_cc = C_b @ twist
    f -= f_cc
    
    return np.linalg.solve(I_b, f)

def coriolis_centripetal_matrix(self, inertia, twist)

Returns Coriolis-Centripetal matrix (6x6) of this RigidBody

Expand source code

def coriolis_centripetal_matrix(self, inertia, twist):
    """
    Returns Coriolis-Centripetal matrix (6x6) of this RigidBody
    """
    inertia = assert_6D_matrix(inertia)
    twist = assert_6D_vector(twist)
    C = np.zeros((6,6))
    C[:3, :3] = self.mass*skew(twist[3:])
    C[3:, 3:] = -skew(inertia[3:, 3:] @ twist[3:])
    return C

def generalized_coordinates(self)

Returns the generalized coordinates of this RigidBody, [x, y, z, fi, theta, psi]

Expand source code

def generalized_coordinates(self):
    """
    Returns the generalized coordinates of this RigidBody,
    [x, y, z, fi, theta, psi]
    """
    return self.get_pose()

def generalized_velocities(self)

Returns the generalized velocities of this RigidBody, [dx, dy, dz, dfi, dtheta, dpsi]

Expand source code

def generalized_velocities(self):
    """
    Returns the generalized velocities of this RigidBody,
    [dx, dy, dz, dfi, dtheta, dpsi]
    """
    twist = self.get_twist()
    t = transform(None, self)
    return np.array([
        *t.apply_vector(twist[:3]),
        *angular_velocity_to_deuler(self.attitude, twist[3:])
    ])

def wrench(self, acceleration, additional_inertia=None)

Returns the required wrench to obtain the given acceleration

Expand source code

def wrench(self, acceleration, additional_inertia=None):
    """
    Returns the required wrench to obtain the given acceleration
    """
    a = assert_6D_vector(acceleration)
    additional_inertia = assert_6D_matrix(additional_inertia or np.zeros((6, 6)))
    twist = self.get_twist()
    H = motion_transformation_matrix(self.CoG.position)
    I_cg = self._inertia + additional_inertia
    C_cg = self.coriolis_centripetal_matrix(I_cg, twist)
    I_b = H.T @ I_cg @ H
    C_b = H.T @ C_cg @ H
    f_cc = C_b @ twist
    
    f = I_b @ a
    f += f_cc
    
    return f

class Transform (HTM)

A Transform to be applied to positions, vectors, wrenches

Expand source code

class Transform:
    """
    A Transform to be applied to positions, vectors, wrenches
    """    
    def __init__(self, HTM):
        self._HTM = HTM
        
    @property
    def translation(self):
        """
        Translation part (3x1) of Transform
        """
        return self._HTM[:3, -1]
    
    @property
    def HTM(self):
        """
        Homogenous Transformation Matrix (4x4) of Transform
        """
        return self._HTM
    
    def apply_vector(self, vector):
        """
        Applies this Transform to vector
        """
        vector = assert_3D_vector(vector)
        return self._HTM[:3, :3] @ vector
    
    def apply_position(self, position):
        """
        Applies this Transform to position
        """
        tmp = np.ones(4)
        tmp[:3] = assert_3D_vector(position)
        return (self._HTM @ tmp)[:3]
    
    def apply_wrench(self, wrench):
        """
        Applies this Transform to wrench
        """
        wrench = assert_6D_vector(wrench)
        out = wrench[:]
        out[:3] = self.apply_vector(wrench[:3])
        out[3:] = self.apply_vector(wrench[3:]) + np.cross(self.translation, out[:3])
        return out
        
    def inv(self):
        """
        Returns inverse of this Transform
        """
        HTM_ = np.linalg.inv(self._HTM)
        return Transform(HTM_)

Instance variables

var HTM

Homogenous Transformation Matrix (4x4) of Transform

Expand source code

@property
def HTM(self):
    """
    Homogenous Transformation Matrix (4x4) of Transform
    """
    return self._HTM

var translation

Translation part (3x1) of Transform

Expand source code

@property
def translation(self):
    """
    Translation part (3x1) of Transform
    """
    return self._HTM[:3, -1]

Methods

def apply_position(self, position)

Applies this Transform to position

Expand source code

def apply_position(self, position):
    """
    Applies this Transform to position
    """
    tmp = np.ones(4)
    tmp[:3] = assert_3D_vector(position)
    return (self._HTM @ tmp)[:3]

def apply_vector(self, vector)

Applies this Transform to vector

Expand source code

def apply_vector(self, vector):
    """
    Applies this Transform to vector
    """
    vector = assert_3D_vector(vector)
    return self._HTM[:3, :3] @ vector

def apply_wrench(self, wrench)

Applies this Transform to wrench

Expand source code

def apply_wrench(self, wrench):
    """
    Applies this Transform to wrench
    """
    wrench = assert_6D_vector(wrench)
    out = wrench[:]
    out[:3] = self.apply_vector(wrench[:3])
    out[3:] = self.apply_vector(wrench[3:]) + np.cross(self.translation, out[:3])
    return out

def inv(self)

Returns inverse of this Transform

Expand source code

def inv(self):
    """
    Returns inverse of this Transform
    """
    HTM_ = np.linalg.inv(self._HTM)
    return Transform(HTM_)

pydynkin - A python version of dynkin

General

Installation

Theory intro

Examples

Single frame

Two frames

Kinematic chains

License

Functions

Classes

Subclasses

Instance variables

Methods

Ancestors

Static methods

Instance variables

Methods

Instance variables

Methods

pydynkin - A python version of `dynkin`