MATLAB code

This MATLAB code is documented with Sphinx using the matlabdomain extension.

src


src module contains the following source code files:

src.main

main function

This code contains the numerical experiments with the step adaptive RKMK method based on Dormand-Prince pair (5,4)

Ref.: E. Celledoni, E. Çokaj, A. Leone, D. Murari, B. Owren. “Dynamics of the N-fold Pendulum in the framework of Lie Group Integrators”, arXiv.

Ref.: E. Celledoni, E. Çokaj, A. Leone, D. Murari, B. Owren. “Lie group integrators for mechanical systems”, International Journal of Computer Mathematics, 99:1, 58-88.


src/integrators


src.integrators.RKMK5(vecField, action, p, h)

Runge-Kutta-Munthe-Kaas time integrator order 5

Parameters:

  • vecField – right hand side of the ODE

  • action – Lie group action

  • p – solution at time t_n

  • h – time step size

Returns:

solution at time t_(n+1)


src.integrators.RKMK45(vecField, action, p, h)

RKMK(5,4) scheme

Parameters:

  • vecField – right hand side of the ODE

  • action – action on the Lie group

  • p – solution at time n

  • h – time stepsize

Returns:

numerical solution and local error


src.integrators.variableStepComparison(vecField, action, z0, T, tol)
Parameters:

  • vecField – right hand side of the ODE

  • action – Lie group action

  • z0 – initial value

  • T – time instant

  • tol – tolerance

Returns:

local error, time instant and new solution


src/Lie_group_functions


src.Lie_group_functions.expRodrigues(x)

Exponential map on SO(3)

Parameters:

input – element of the Lie algebra so(3), represented as a vector in R3

Returns:

element of the group SO(3)


src.Lie_group_functions.expSE3(input)

Exponential map on SE(3)

Parameters:

input – element of the Lie algebra se(3) represented as 6-component vector, i.e. as a pair (u,v) with with the 3-component vector u corresponding to a skew symmetric matrix hat(u) and the 3-component vector v corresponding to the translational part.

Returns:

element of the group SE(3), represented as a 3x4 matrix [A, b], with A 3x3 rotation matrix and b 3-component translation vector.


src.Lie_group_functions.exponentialSE3N(sigma)

Exponential map on SE(3)^N

Parameters:

sigma – element of se(3)^N

Returns:

element of SE(3)^N


src.Lie_group_functions.actionSE3(B, input)

action on the SE(3) group

Parameters:

  • B – element of the Lie group SE(3)

  • input – element of the Lie algebra se(3)

Returns:

element of the Lie algebra se(3)


src.Lie_group_functions.actionSE3N(B, input)

action on SE(3)^N

Parameters:

  • B – element of SE(3)^N

  • input – element of se(3)^N

Returns:

element of se(3)^N


src.Lie_group_functions.dexpinvSE3(sigma, input)

Inverse of the derivative of the exponential map on SE(3)

Parameters:

  • sigma – element in the Lie algebra se(3), represented as a 6x1 vector

  • input – element in the Lie algebra se(3), represented as a 6x1 vector

Returns:

inverse of dexp_sigma(input) as 6x1 vector


src.Lie_group_functions.dexpinvSE3N(sigma, input)

Inverse of the derivative of the exponential map on SE(3)^N

Parameters:

  • sigma – element of se(3)^N

  • input – element of se(3)^N

Returns:

inverse of dexp_sigma(input)


src/equations_of_motion


src.equations_of_motion.fManiToAlgebra(q, w, L, m)

RHS of the system

Parameters:

  • q – position vector [q1, …, qN] in S^2

  • w – angular velocities vector [w1, …, wN] in T_{qi}S^2

  • L – length of the pendulum

  • m – mass of the pendulum

Returns:

vector field of the system


src.equations_of_motion.assembleF(q, w, m, L)

This function defines the right hand side, precisely we need it to define the equations for the angular velocities, which now becomes R(q)w’ = F, and here we assemble this F vector.

Parameters:

  • q – position vector [q1, …, qN] in S^2

  • w – angular velocities vector [w1, …, wN] in T_{qi}S^2

  • L – length of the pendulum

  • m – mass of the pendulum

Returns:

equations for the angular velocities of the form R(q)w’ = F


src.equations_of_motion.assembleM(q, L, m)
Parameters:

  • q – position vector [q1, …, qN] in S^2

  • L – length of the pendulum

  • m – mass of the pendulum

Returns:

inertia mass matrix of the system


src.equations_of_motion.assembleR(q, L, m)
Parameters:

  • q – position vector [q1, …, qN] in S^2

  • L – length of the pendulum

  • m – mass of the pendulum

Returns:

symmetric block matrix used in the equations for the angular velocities


src.equations_of_motion.FuncQ(z)

This function is used to integrate with ODE45

Parameters:

z – is of the form z = [q1, q2, …, qN, w1, w2, …, wN]

Returns:

the part of the vector field for the dot{q}_i, so hat(w_i)*q_i


src.equations_of_motion.FuncW(z, L, m)

This function is used to integrate with ODE45

Parameters:

  • z – is of the form z = [q1, q2, …, qN, w1, w2, …, wN]

  • L – length of the pendulum

  • m – mass of the pendulum

Returns:

the part of the vector field for the dot{w}_i, so R(q)^{-1}*RHS of the ODE, defined by assembleF


src.equations_of_motion.initializeStat(N)

Initialization of the system

Parameters:

N – number of connected pendula

Returns:

initial positions for q and w of the N-fold pendulum


src/helpful_functions


src.helpful_functions.extractq(v)

extracts the q components from [q, w]

Parameters:

v – input vector v = [q1, w1, q2, w2, …, qN, wN]

Returns:

the q components of v, q = [q1,q2,…,qN]


src.helpful_functions.extractw(v)

extracts the w components from [q, w]

Parameters:

v – input vector v = [q1, w1, q2, w2, …, qN, wN]

Returns:

the w components of v, w = [w1, w2, …, wN]


src.helpful_functions.hat(v)

It returns the skew symmetric matrix A associated to the vector v

Parameters:

v – vector with 3 components

Returns:

element of the Lie algebra so(3) (skew symmetric 3x3 matrix)


src.helpful_functions.getVec(vec, i)
Parameters:

  • vec – vector

  • i – index

Returns:

i-th term of the input vector


src.helpful_functions.getBlock(Mat, i, j)
Parameters:

  • Mat – a matrix

  • i – index

  • j – index

Returns:

3x3 block in position i-j of a matrix Mat


src.helpful_functions.reorder(z)

This function reorders z to the format used in the Lie group integrators setting

Parameters:

z – [q1,q2,..,qN,w1,w2,…,wN]

Returns:

[q1,w1,q2,w2,…,qN,wN]