MATLAB code

src

src module contains an example script and related functions:

src.main

Damped Spherical Pendulum solved with Lie group time integrator

By calling the main script one will generate parameters and solution files. The solutions generated by the main script can later be used for the post-processing analysis.

src.fManiToAlgebra(y, damp, k)

Right hand side of the spherical pendulum in the corresponding Lie algebra.

Parameters:

  • y – generalized coordinates

  • damp – damping parameter

  • k – parameter

Returns:

vector field

src.initializeSE3()

Initialization in SE3

Returns:

normalized vector in R3

respective orthogonal vector (angular velocity) concatenation of the two

src.residualSE3(v0, v, h, f, action, method)

RHS of the system

Parameters:

  • v0 – initial condition

  • v – current configuration

  • h – time step size

  • f – vector field

  • action – action on the Lie group

  • method – numerical method in use

Returns:

RHS of the system

src.jacobianSE3(v0, v, h, f, action, method)

Jacobian of the system

Parameters:

  • v0 – initial condition

  • v – current configuration

  • h – time step size

  • f – vector field

  • action – action on the Lie group

  • method – numerical method in use

Returns:

Jacobian of the system

src/integrator

The folder integrator contains some functions implementing Lie group time integrators.

For example, the Lie forward Euler:

src.integrator.LieEuler(vector_field, action, expMap, initial_condition, h)

Lie Forward Euler time integrator

Parameters:

  • vector_field – right hand side of the ODE

  • action – action on the Lie group

  • expMap – exponential map from the Lie algebra to the Lie group

  • initial_condition – initial condition

  • h – time step size

Returns:

discrete trajectory

The Newton-Raphson algorithm for implicit methods:

src.integrator.NewtonRaphson(residual, jacobian, initial_condition, h, max_it, atol, rtol)

Newton-Raphson scheme to solve the differential equations

Parameters:

  • residual – function to evaluate the RHS of the system

  • jacobian – iteration matrix

  • initial_condition – initial condition

  • h – time step size

  • max_it – maximum number of iterations

  • atol – absolute tolerance

  • rtol – relative tolerance

Returns:

solution of the system of nonlinear equations

Some useful function for the operation in the Lie group:

src.integrator.actionSE3(A, v)

action on the SE3 group

Parameters:

  • A – element of the Lie group SE3

  • v – element of the Lie algebra se3

Returns:

element of the Lie algebra se3

src.integrator.expSE3(input)

exponential map on the SE3 group

Parameters:

input – element of the Lie algebra se3

Returns:

element of the Lie group SE3

src/post_processing

src.post_processing.readAll(many)

reading all solution files

Parameters:

many – how many solutions to read

Returns:

cells of solution and relative parameters

src.post_processing.evalErr(sols, params)

Evaluates the absolute and relative error w.r.t. a reference solution

Parameters:

  • sols – solutions given in a cell struct

  • params – parameters of each solution given in a cell struct

Results:

absolute and relative error at the end point

src.post_processing.plotErr(error)

plotting error

Params error:

absolute and relative error

over position and angular velocity

src.post_processing.potential(q, L, m)

Potential energy of a pendulum subject to Earth gravity [g = 9.81 m/s^2] with fix end in [0, 0, 0]

Parameters:

  • q – position vector

  • L – lenght of the pendulum

  • m – mass of the pendulum

Results:

potential energy

src.post_processing.plotEnergy(sols, params)

plotting energy

Parameters:

  • sols – solutions given in a cell struct

  • params – parameters of each solution given in a cell struct

Among the post-processing functions, we are interested in evaluating the distance between two solutions of the problem:

src.post_processing.riemannianDistance(sols, params)

Evaluates the distance using the norm of the Sasaki Logarithmic map

Parameters:

  • sols – solutions given in a cell struct

  • params – parameters of each solution given in a cell struct

Returns:

Riemannian distance between the first two given solutions