An Introduction to

Multibody System Dynamics

J. Ros September 21, 2021

Dept. Engineering. Public University of Navarre (UPNA)

Freshen Up Kinematics
 Freshen up Kinematics

We will review

1. Vector & 3-tuple

2. Coordinates,velocities generalized accelerations. Parameters.
3. Basis, Angular velocity, Simple rotation, Right hand rule, orientation
diagram. Derivation of vectors using mobile bases. Base change matrix. Euler
Parameters.
4. Position, velocity and acceleration of a point w.r.t. A given observer. Motion
composition, non-sliding conditions.
5. Constraints at the position velocity and acceleration level. Including
non-holonomic constraints.
 3D Vectors

Given orthonormal base That is a direct triedra

A vector can be expressed as

Using 3-tuples it can be expressed as

 3D Vector operations
Addition Product by an escalar

Scalar product
Vector product
Skew-symmetric form associated to Linear application
the cross product

Base change, is a identity tensor Base change, properties

Linear application, using two bases

Derivative of a vector: diff. geom. approach
Change of the vector w.r.t. the orientation + change due to orientation rotation

Derivative in different orientations (Frames)

Angular velocity properties follow Fig.: Differential geometry of the derivative of a rotating vector: Angular velocity

Angular velocity orientation diagram

Simple or Basic Rotation: Example
By definition

and

Therefore, projecting geometrically:

Base change matrix

Rotation tensor components matrix

Obviously, from motion geometry follows

Given the drawing of the arc, including the arrow, for

convenience we will use the following notation to denote
the corresponding angular velocity
Simple or Basic Rotations: Euler Angles
Derivative in mobile bases

Comparing both

Showing how to get ang. vel. components from the rotation or base change matrix components
Same idea using rotation matrix
Exercise
Exercise
Exercise
Analytical Kinematics - Problem 2

Multibody System Dynamics

Analytical Kinematics: Parameterization
Generalized coordinates, velocities and accelerations, and parameters.

Analyst work

Example: Problem #2

Abstraction
Set of generalized coordinates

Set of generalized velocities

Set of generalized accelerations

Set of parameters
Analytical Kinematics: Constraint equations
Coordinates, velocity and acceleration, level eqs.
Coordinate level eq
J in contact with the ground (height is null)

Velocity level eqs.

Velocity of J of the disc w.r.t. the ground is null in the contact plane

Velocity level eqs.

Velocity level eqs. as a single set

Example: Problem #2

Acceleration level eqs.

Abstraction

Coordinate level eqs

Velocity level eqs

Acceleration level eqs

Analytical Kinematics: Constraint equations
Coordinates, velocity and acceleration, level eqs.
1. Coordinate level eqs must be satisfied by , used to obtain a set of constrained coordinates

2. Velocity level eqs must be satisfied by , used to obtain a set of constrained velocities

3. Acceleration level eqs , used to obtain a set of constrained accelerations

Nonlinear problem in

Linear problem in

Linear problem

Example: Problem #2
Example: Problem #2
In the classroom, we’ve worked with Problem #2:
Parametrization, Bases, Points and Frame definition, and
setup of the constraint equations at the coordinate, velocity
and acceleration levels.
We’ve done this by hand and using the library.

Lib_3D_MEC_MATLAB

Lib_3D_MEC_MATLAB Manual:
The final slides in this set is a short manual on how to use the library, and are
based on this example.

For the different algorithms or problems in MBSD that we are going to present in these slides,
we will explain symbolic determination of required functions, export of these functions and
numerical implementation of the algorithms using MATLAB with Lib_3D_MEC_MATLAB
naming conventions.
We’ll do it in the slides using Problem #2, because it is general enough as to guide us
throughout the different algorithms that we are going to review.

Hopefully these slides will help you to go through MBSD problem implementation and
solution using MATLAB and Lib_3D_MEC_MATLAB within and beyond the scope of the
methods and algorithms that we are going to review.
Example: Problem #2 Parameterization
Do_you_want_to_run_the_script_again

%% Clean environment
close all
clear all Library Initialization
clc

%% Init lib_3D_MEC_MATLAB environment

init_lib_3D_MEC_MATLAB;
purgeMSDFunctions; purgeDraws;

%----Begin Edit----
g = newParam('g', 9.80665);
GravityVector = Vector3D([0, 0, - g]
','xyz');

%Integration step
Delta_t = 0.003;

%Graphical Output refresh freq (n_frames/n_steps)

Delta_t_refresh = 1 / 24;

% Assembly Init Problem solver parameters

Geom_Eq_init_tol = 1.0e - 10;
Geom_Eq_init_relax = 0.1;

% Assembly Problem solver parameters

Geom_Eq_tol = Delta_t ^ 2 * 10 ^ -3;
Geom_Eq_relax = 0.1;

% Equilibrium Problem solver parameters

Dyn_Eq_tol = 1.0e - 10;
Dyn_Eq_relax = 0.1;
Just copy and paste for your particular problem.
% Perturbed Dynamic State solver parameters This standard stuff that you can modify, but left as is for the
Per_Dyn_State_tol = 1.0e - 12;
Per_Dyn_State_relax = 0.1; moment
% pop_up_dialogs=false;
has_my_contact_forzes = false;
optimize_matlabFunction = false;
updateDraws_automatic = true;
equilibrium_problem_solved = false;
pop_up_dialogs = false;
Set_characteristic_size(1);
n_traj = 0;

simplify_expressions = true;
%----End Edit----

fig_dir = 'fig'; mkdir(fig_dir);

Example: Problem #2 Parameterization
%% Define generalized coordinates, velocities and accelerations %% Define Solids
%----Begin Edit---- Coord. def. %----Begin Edit---- Solid def.
Set_value(t, 0); rho = 7850;
newCoord('psi', pi / 6, 2.5); slider_height = 0.3; slider_rout = 0.1;
[h] = newCoord('h', 1.85); scad2stl('Slider.scad', 'Slider.stl', 'r_out', slider_rout, 'r_in', 0.06, 'height', slider_height);
[theta, dtheta] = newCoord(
'theta', pi / 6); Slider = newSolid('Slider', 'SliderF', 'Slider.stl', rho);
[l, dl, ddl] = newCoord(
'l', 2.1);
newCoord('phi', pi / 6, 10); scad2stl('Arm.scad', 'Arm.stl', 'rod_r', 0.05, 'r_in', 0.1, 'd', 0.2, 'l', 3.5);
%----End Edit---- Arm = newSolid('Arm', 'ArmF', 'Arm.stl', rho);
q, dq, ddq, n_q
scad2stl('Disc.scad', 'Disc.stl', 'r_out', Expr(R), 'r_in', 0.03, 'width', 0.025);
Disc = newSolid('Disc', 'DiscF', 'Disc.stl', rho);
%% Define Geometric Parameters
%----Begin Edit---- Param. def. scad2stl('gr.scad', 'gr.stl', 'l_x', 5, 'l_y', 5, 'l_z', 0.05) %Just aesthetical, not a part of the system
R = newParam('R', 0.5); newSolidExt('Gr', 'GrF', 'gr.stl');
%----End Edit----
param, n_param scad2stl('mast.scad', 'mast.stl', 'height', 2.3) %Just aesthetical, not a part of the system
newSolidExt('Mast', 'GrF', 'mast.stl');
%% Define Bases, Points and Frames
%----End Edit----
%----Begin Edit---- Base and Point def.
newBase('xyz', 'Bpsi', 3, psi)
GravityVector = Vector3D([0, 0, - g]
','Bpsi');%Gravity redefined
newBase('Bpsi', '123', 1, 0)
newBase('Bpsi', 'Btheta', 2, theta)
newBase('Btheta', '1''2''3''', 1, 0) % '' within a string is a
single ' %% Ask things related to Bases, Points and Frames
newBase('Btheta', 'Bphi', 1, - phi) %----Begin Edit---- Now you can ask things related to objects, do calculations...
newBase('Bphi', '1''''2''''3''''', 1, 0) OP = Pos('O', 'P')
OA = Vector3D([0, 0, h]','Bpsi'); timederivative(OP, 'GrF')
Vel_P_abs = Vel('GrF', 'P')
newPoint('O', 'A', OA) OP = Pos('O', 'J')
newPoint('A', 'C', Vector3D([l, 0, 0]','Btheta')) Vel_J_abs = Vel('GrF', 'J')
newPoint('C', 'J', Vector3D([0, 0, - R]','Btheta')) Vel_J_D_abs = Vel('GrF', 'J') + cross(Omega('GrF', 'DiscF'), Pos('C', 'J'))
newPoint('C', 'P', Vector3D([0, 0, R]','Bphi')) Vel_J_D_abs = Vel('GrF', 'J', 'DiscF')
%----End Edit----
%% Define Reference Frames Om_Disc_abs = Omega('GrF', 'DiscF')
%----Begin Edit---- Reference Frame def.
newFrame('O', 'xyz', 'GrF') One_xyz_Bpsi = BasMatr('xyz', 'Bpsi')
newFrame('O', 'xyz', 'IF') One_Bpsi_Btheta = BasMatr(
'Bpsi', 'Btheta')
newFrame('A', 'Btheta', 'ArmF') One_Btheta_Bphi = BasMatr(
'Btheta', 'Bphi')
newFrame('C', 'Bphi', 'DiscF') my_One_xyz_Bphi = One_xyz_Bpsi * One_Bpsi_Btheta * One_Btheta_Bphi
newFrame('A', 'Bpsi', 'SliderF') One_xyz_Bphi = BasMatr('xyz', 'Bphi')
%----End Edit---- %----End Edit----
Example: Problem #2 Parameterization
%% Draw Points, Frames, Bodies, ...
%----Begin Edit---- Draw Points, Reference
%Point of view, lighting, material Frames, Bodies,...
azimut = 107.2000; elevation = 27.2000;
view(azimut, elevation);

newFrameDraw('abs', 1);
newFrameDraw('Slider', 1);
newFrameDraw('Arm', 1);
newFrameDraw('Disc', 1);

newPointDraw('J', 1, [1, 1, 1]);

newPointDraw('P', 1, [1, 0, 0]);

newSolidDraw('Arm', [0.5, 0.5, 0]);

newSolidDraw('Slider', [0.8, 0.3, 0]);
newSolidDraw('Disc', [0.5, 0.5, 1]);

newSolidDraw('Gr', [0.5, 0.5, 0.5]);

newSolidDraw('Mast', [0.5, 0.5, 0.5]);

scad2stl('Spring', 'Spring', 'height', 1, 'Radius', slider_rout, 'Wire_radius', 0.1 * 0.1,

'Coils', 10)
spring_fv = stlRead('Spring.stl');
unit_vector = Vector3D([0, 0, 1]
','Bpsi');
OK = OA - (slider_height / 2) * unit_vector;
newFVVarLengthDraw('O', spring_fv, 'OK', [- 1, - 1, 1], [1, 1, 0]); clear spring_fv
%----End Edit----

camlight('headlight');
material('dull');
Example: Problem #2 Parameterization
%% Draw Sketch of Parametrization
if exist('./draw_dimensions.m', 'file') Draw Parameterization
%% Draw Dimensions
save_extended_state(
'extended_state_parametrization'
camproj('perspective');
);
Advanced!!
view([78.5263671875, 13.61328125]);
camtarget([- 0.328233934776823, 0.419858424492971, 1.673164190664704])
camva(8);
draw_dimensions;
setDrawTextSize(0.033)
load_extended_state(
'extended_state_parametrization'
);
Help(['Graphical Output shows the parametrization.'
, newline 'The situation is as-given (no Assembly or Initial Assembly
problem solved)']);
delete(dimensions_handle);
camproj('orthographic');
else
Warning('You need to provide dimension drawing instructions on ''./draw_dimensions.m'' file'
);
end
Exercise: Problem #1 Parameterization
Parameterize and determine constraint equations

The disk is in contact with the ground at

J. There is no sliding at J.

Shown with ground-disk contact splitted, to better illustrate the

parameterization meaning. J and J’ coincident when system is
assembled
Exercise: Problem 1 - scad files are given to make things easier
%% Define Solids
rho = 7850;

scad2stl('Mast', 'Mast', 'r_out', size_rod, 'height', 2.25, 'size_x', 1.5 * size_guide, 'size_y', size_guide, 'size_z', size_z);
Mast = newSolid('Mast', 'Mast', 'Mast.stl', rho);

scad2stl('Arm', 'Arm', 'rod_r', size_rod, 'r_in', size_rod, 'r_out', 2 * size_rod, 'height', 0.6, 'l', 5, 'd', 2.1);
Arm = newSolid('Arm', 'Arm', 'Arm.stl', rho);

scad2stl('Disc', 'Disc', 'r_out', Get_value(R), 'r_in', size_rod, 'width', size_rod);

Disc = newSolid('Disc', 'Disc', 'Disc.stl', rho);

scad2stl('Gr', 'Gr', 'size_x', Get_value(4 * l), 'size_y', Get_value(2 * l), 'size_z', size_z, 'size_guide', size_guide)
newSolidExt('Gr', 'abs', 'Gr.stl');

OpenScad screenshots show

default dimensions, that can
be adjusted with scad2stl
OpenScad: Drawing the Mast body in problem 1
Transformed solids are made a single one through union boolean operation (OR)

No cylinder translation
/rotation required because
cylinder is naturally drawn
from the origin in the +Z axis

Translated cube to place origin at

Plain cylinder Plain cube
the center of the lower face
Exercise
Algorithms in Kinematics

Multibody System Dynamics

J. Ros September 21, 2020

Dept. Engineering. Public University of Navarre (UPNA)

 Algorithms in Kinematics

We will review

1. We will review pseudoinverse and how it relates to generalized solutions of

linear equations.
2. Assembly problem @ the coordinate level, or minimum norm solution of the
geometric equations.
3. Assembly problem @ the velocity level, or minimum norm solution of the
kinematic equations.
4. We will see how to implement this in MATLAB using lib_3D_MEC_MATLAB
naming scheme of relevant functions, variable, exportation from symbolic
expressions, and implementation of algorithms
Monroe-Penrose Pseudoinverse

Annex
Assembly Problem @ coordinate level
Coordinate Level (geometric eqs.)

Minimum norm of (linearized solution)

First order Taylor approx Interpretation

Minimum norm Solution

Newton-Raphson iteration

Newton-Raphson iteration with relaxation

Fig.: Newton-Raphson iteration geometric interpretation in 1D

Assembly Problem @ coordinate level
Problem #2

e_Bpsi_3=Vector3D([0,0,1]','Bpsi');
expression Phi

% Phi, dPhiH
newPhi(dot(Pos('O','J'),e_Bpsi_3),true);

Generic code

% Coordinate Level Eqs. Export

PhiH=Phi; function Phi_(q,t,param)
Phi_q=jacobian(Phi,q);
matlabFunction(Phi,'File','Phi_','Vars',{q,t,param}); function Phi_q_(q,t,param)
matlabFunction(Phi_q,'File','Phi_q_','Vars',{q,t,param});

% Assembly Problem @ Coordinate Level (numeric sol)

Phi_num=Phi_(q_value,t_value,param_value);
q_value = q_value - Geom_Eq_relax *
pinv(Phi_q_(q_value,t_value,param_value) ) * Phi_num;
Phi_num = Phi_(q_value,t_value,param_value);
while norm(Phi_num) > Geom_Eq_tol
q_value = q_value - Geom_Eq_relax *
pinv( Phi_q_(q_value,t_value,param_value) ) * Phi_num;
Phi_num = Phi_(q_value,t_value,param_value);
end
Assembly Problem @ velocity level
Velocity Level (velocity eqs.) Minimum norm of Solution

They are linear in

Linear form around

where therefore

These are the so called naive/trivial solutions, faster/more-convenient solution

can be found in combination with the Udwadia & Kabala or the Independent
Coordinates solution procedures.

Note about structure: Linear w.r.t gen. velocities

Assembly Problem @ velocity level
Problem #2
e_Bpsi_1=Vector3D([1,0,0]','Bpsi');
e_Bpsi_2=Vector3D([0,1,0]','Bpsi');

Vel_J_Disc=Vel('abs','J','Disc')

% Phi, dPhiH
newdPhiNH([dot(Vel_J_Disc,e_Bpsi_1);... expresion dPhiNH
dot(Vel_J_Disc,e_Bpsi_2)],true);

Generic code

%Velocity Level Eqs. Export

dPhiH=timederivative(PhiH);
dPhi=[dPhiH;
dPhiNH];
dPhi_dq=jacobian(dPhi,dq);
beta=-subs(dPhi,dq,zeros(size(dq)));
matlabFunction(beta,'File','beta_','Vars',{q,t,param});
matlabFunction(dPhi_dq,'File','dPhi_dq_','Vars',{q,t,param});
function dPhi_dq_(q,t,param)
function beta_(q,t,param)
%Velocity Level Assembly Problem (numeric)
dPhi_dq_num=dPhi_dq_(q_value,t_value,param_value);
dq_value=dq_value + pinv( dPhi_dq_num ) *
(beta_(q_value,t_value,param_value) - dPhi_dq_num *
dq_value);
Assembly Problem @ acceleration level
Acceleration Level (acceleration eqs.)

They are linear in Solution

Usually it is not solved on itself but along

where with other problems: dynamic,
equilibrium,... But if required a minimum
norm solution analogous to the velocity
level problem can be used.
Note about structure:
That’s why we are not giving an algorithm.

Anyway, all the functions that have been

exported so far are used in dynamic
Generic code algorithms

%Acceleration Level Eqs. Export

ddPhiH=timederivative(dPhiH);
ddPhiNH=timederivative(dPhiNH);
ddPhi=[ddPhiH;
ddPhiNH]; function gamma_(q,dq,t,param)
gamma=-subs(ddPhi,ddq,zeros(size(ddq)));
matlabFunction(gamma,'File','gamma_','Vars',{q,dq,t,param});
Assembly Problem @ lib_3D_MEC_MATLAB
Script: assembly_problem.m, solves the coordinate & velocity level constraint eqs., in that order,
Requires Script: Export_Assembly_Problem.m, to export the required MATLAB functions
Phi_, Phi_q, dPhi_dq_, beta_, gamma_
Requires symbolic expression vectors: Phi, dPhiH, dPhiNH
Helper functions: newPhi(), newdPhiNH().
Requires symbolic and numeric vectors: q, dq, ddq, param
q_value, dq_value, ddq_value, param_value
Philosophy of lib_3D_MEC_MATLAB:
Each numerical algorithm script has an associated script that exports its required functions, that relies in some symbolic
expression vectors, that can be defined using associated helper functions.
Exercise: @ lib_3D_MEC_MATLAB open the scripts Numeric/assembly_problem.m and
Symbolic/Export_Assembly_Problem.m and check that they are consistent with the MATLAB listings given before.

Exercise: Execute Full_Problems/problem2/problem2_FM_FULL.m, up to and including Export_Assembly_Problem. Then

execute assembly_problem right in the console. Check the change in the value of the gen. coordinates and velocities, and
that the constraint equations are satisfied evaluating the constraint equations, and update the drawing:

%For instance run:

Get_value(q), Get_value(dq) %before assembly_problem
assembly_problem % will updateDraws automatically
Get_value(q), Get_value(dq) %after assembly_problem
Phi_(q_value,t_value,param_value) %geom. constr.
dPhi_dq_(q_value,t_value,param_value)*dq+beta_(q_value,t_value,param_value) %kin. constr.

Exercise: You cannot see graphically if velocity level constraints are satisfied. Unless you draw velocities. Drawing
before calling assembly_problem will help, as it will disappear ( ) as soon as the velocity
level eqs. get satisfied. If not done already, find the lines drawing that vector and execute them before solving
assembly problem. Have you seen how disappears?
Initial Assembly Problem @ coordinate level
Problem #2

psi0=newParam('psi0',0);
h0=newParam('h0',0.5);
expresion PhiInit
newPhiInit([psi-psi0;
h-h0],true);

Generic code

% Coordinate Level Initial Assembly Eqs. Export

PhiInit_q=jacobian(PhiInit, q); function PhiInit_(q,t,param)
matlabFunction(PhiInit,'File','PhiInit_','Vars',{q,t,param});
matlabFunction(PhiInit_q,'File','PhiInit_q_','Vars',{q,t,param}); function PhiInit_q_(q,t,param)

% Initial Assembly Problem @ Coordinate Level (numeric sol)

function_num=[Phi_(q_value,t_value,param_value);
PhiInit_(q_value,t_value,param_value)];
while norm(function_num)> Geom_Eq_init_tol
q_value = q_value - Geom_Eq_init_relax *
pinv([Phi_q_(q_value,t_value,param_value);
PhiInit_q_(q_value,t_value,param_value)]) *
function_num;
function_num=[Phi_(q_value,t_value,param_value);
PhiInit_(q_value,t_value,param_value)];
end
Initial Assembly Problem @ velocity level
Problem #2

dpsi0=newParam('dpsi0',3);
expression dPhiInit

newdPhiInit([dpsi-dpsi0],true);

Generic code

% Coordinate Level Initial Assembly Eqs. Export

dPhiInit_dq=jacobian(dPhiInit,dq);
betaInit=-subs(dPhiInit,dq,zeros(size(dq)));
matlabFunction(betaInit,'File','betaInit_','Vars',{q,t,param}); function betaInit_(q,t,param)
matlabFunction(dPhiInit_dq,'File','dPhiInit_dq_','Vars',{q,t,par function dPhiInit_q_(q,t,param)
am});

% Initial Assembly Problem @ Velocity Level (numeric sol)

jacobian_num=[dPhi_dq_(q_value,t_value,param_value);
dPhiInit_dq_(q_value,t_value,param_value);
dPhiAux_dq_(q_value,t_value,param_value)];
dq_value=dq_value + pinv( dPhiInit_dq_num ) *
([beta_(q_value,t_value,param_value);
betaInit_(q_value,t_value,param_value);
beta_aux_(q_value,t_value,param_value)] -
dPhiInit_dq_num * dq_value);
Initial Assembly Problem @ lib_3D_MEC_MATLAB
Script: initial_assembly_problem.m, solvesthe coordinate & velocity level constraint eqs. along with the respective
coordinate and velocity initial equations., in that order
Requires Script: Export_Initial_Assembly_Problem.m, to export the required MATLAB functions
PhiInit_, PhiInit_q, dPhiInit_dq_, betaInit_
Requires Script: Export_Assembly_Problem.m, to export the required MATLAB functions
Phi_, Phi_q, dPhi_dq_, beta_, gamma_
Requires symbolic expression vectors: PhiInit, dPhiInit
Helper functions: newPhiInit(), newdPhiInit()
Requires symbolic and numeric vectors: q, dq, ddq, param
q_value, dq_value, ddq_value, param_value
Philosophy of lib_3D_MEC_MATLAB:
Each numerical algorithm script has an associated script that exports its required functions, that relies in some symbolic
expression vectors, that can be defined using associated helper functions.
%For instance run:
Get_value(q), Get_value(dq) %before assembly_problem
initial_assembly_problem
Get_value(q), Get_value(dq) %after assembly_problem
Phi_(q_value,t_value,param_value) %geom. Constr.
PhiInit_(q_value,t_value,param_value) %coord level init. conds
dPhi_dq_(q_value,t_value,param_value)*dq+beta_(q_value,t_value,param_value) %kin. Constr.
dPhiInit_dq_(q_value,t_value,param_value)*dq+betaInit_(q_value,t_value,param_value) %vel. level init. conds
updateDraws
Exercise: Go to the lib_3D_MEC_MATLAB distribution, find the script initial_assembly_problem.m and identify the code
presented previously.

Exercise: Execute problem #2 script, problem2_FM_FULL.m, up to and including Export_Initial_Assembly_Problem. Then

execute initial_assembly_problem right in the console. Check the change in the value of the gen. coordinates and
velocities, that all the constraint equations and initial conditions are satisfied executing by evaluating the constraints, and
update the drawing:

Exercise: Can you see any other differences?. Clue, what in the hell are the _aux?
Initial Assembly Problem (another algorithm)
Think twice: It makes sense for the initial conditions to be satisfied with some error. Think, for instance, that the user of
your software is introducing too many conditions leading to an incompatible system of equations. But still you want for the
solution to satisfy exactly the constraint equations. How would you solve the problem?.

A possibility would be to use a minimization with constraints, such as the Lagrange multipliers minimization approach or
fmincon in MATLAB optimization toolbox. You will use the initial conditions to define the optimum function to be
minimized, while the constraints equations will be coincident with the constraints in this minimization setup.

Something on the line of

Advanced
Where

Is the so called weighting matrix (positive definite in general, but most frequently diagonal)

Lagrange minimization with constraints, define a Lagrangian:

where . The solution is obtained solving for the system

Initial Assembly Problem (another algorithm)
Left aside Lagrange minimization, specialized numerical procedures to do constrained optimization have been
developed. Even methods that allow inequality type constraints. This is the subject of optimization theory, also known as
numerical programing theory.

MATLAB’s Optimization Toolbox fmincon has a stack of algorithms that allows to do such a constrained minimization.
Even using inequality type constraints that can be helpful in a number of situations.

Advanced
Auxiliar Assembly Problem
Sometimes we have auxiliary constraints, constraints that are somehow different to standard constraints.

1. For example, in contact problems they can be used to locate the contact point and determine normals and
tangents to the contact plane. This may require the definition of additional coordinates.
2. Also, when using 4 parameters to parametrize a general tridimensional rotation, such as Euler parameters,

Advanced
quaternions, and related rotation parametrizations. A constraint must be satisfied among the 4 parameters,
because a general rotation has only 3 degrees of freedom.
Generally, these auxiliary constraints need to be solved only @ the position and velocity levels. Also generally,
acceleration level constraints do not need to be considered at the dynamics solution procedure.

This is an advanced topic, and we will treat it only if time allows.

% Auxiliar Assembly Problem (Coordinate Level) % Auxiliar Assembly Problem (Velocity Level)
PhiAux_num = PhiAux_(q_value,t_value,param_value); dPhiAux_dq_num=dPhiAux_dq_(q_value,t_value,param_value);
while norm(PhiAux_num)> Geom_Eq_tol dq_value=dq_value + pinv(
q_value = q_value - Geom_Eq_relax * pinv( dPhiAux_dq_num ) * (beta_aux_(q_value,t_value,param_value) -
dPhiAux_dq_num * dq_value);
PhiAux_q_(q_value,t_value,param_value) ) * PhiAux_num;
PhiAux_num = PhiAux_(q_value,t_value,param_value);
end Auxiliar Assembly problem is solved before and not together
with Assembly Problem (it could be possible but slower)

% Initial Assembly Problem (Coordinate Level) % Initial Assembly Problem (Velocity Level)
function_num=[Phi_(q_value,t_value,param_value); jacobian_num=[dPhi_dq_(q_value,t_value,param_value);
PhiInit_(q_value,t_value,param_value); dPhiInit_dq_(q_value,t_value,param_value);
PhiAux_(q_value,t_value,param_value)]; dPhiAux_dq_(q_value,t_value,param_value)];
while norm(function_num)> Geom_Eq_init_tol dq_value=dq_value + pinv( jacobian_num ) *
q_value = q_value - Geom_Eq_init_relax * pinv( ([beta_(q_value,t_value,param_value);
[Phi_q_(q_value,t_value,param_value); betaInit_(q_value,t_value,param_value);
PhiInit_q_(q_value,t_value,param_value); beta_aux_(q_value,t_value,param_value)] -
PhiAux_q_(q_value,t_value,param_value)] ) * jacobian_num * dq_value);
function_num;
function_num=[Phi_(q_value,t_value,param_value); Initial Auxiliar Assembly problem, assembled with constraint
PhiInit_(q_value,t_value,param_value); and initial condition equations, and solved together
PhiAux_(q_value,t_value,param_value)];
end
Symbolic Modeling. Recursivity.

%% Define Solids
%----Begin Edit----
rho = 7850;
newSolid('Sol1', 'O1B1', 'Sol1.stl', rho);
newSolid(...);

%----End Edit----

%% Define Bases, Points and Frames

%----Begin Edit----
newBase('B0', 'B1', n_axis, angle)
newBase(...)

newPoint('OO', 'O1', Vector3D(...))

newPoint(...)

newFrame('O1', 'B1', 'O1B1')

newFrame(...)
%----End Edit----
Euler-Rodrigues Formula

Rotated vector

Rotation Tensor
Alternate expression
Angle-Axis rotation specification
The 3 components of the vector (axis) The constraint between parameters becomes

and the angle

From Euler-Rodrigues formula

*
Alternate expression

not a good option in a symbolic setup

preferred in a symbolic setup

**still better in a symbolic setup

% Base defined using Rotation + Angle * & **, used for

implementation, even for
newBase('xyz','BBody',[n1,n2,n3]',phi)
basic/simple rotations
Rodrigues Vector

Advanced
Exponential map
The rotation matrix can also be expressed as:

Advanced
For a constant rot axis

Note that the argument of the exponential is a matrix, and therefore this is a
matrix exponential. This is the, so called, Exponential Map:

This exponential can be computed in a variety of forms. For the skew-symmetric

matrices, perhaps, the Euler-Rodrigues formula is (numerically/symbolically) the
best option:

SO(3), is the so-called 3-dimensional special orthogonal group, is a collection of matrices. The word orthogonal
means that the columns of the matrix are orthogonal to one another, and the word special means the matrices have
to have determinant 1.
The eigenvector of the rotation matrix corresponding to the eigenvalue 1, is the
rotation direction

Special Orthogonal Group SO(3), Euler Angles, Angle-axis, Rodriguez Vector and Unit-Quaternion: Overview, Mapping and Challenges

Hashim A. Hashim

arXiv:1909.06669v2 [math.OC]
Euler Rodrigues formula from the exponential map

Advanced
Example: Simple or Basic Rotation revisited
Based on

We can project geometrically to obtain

But in a computer, symbolically, is more convenient to use

the Euler-Rodrigues formula for the rotation tensor

And the Euler-Rodrigues formula for the angular velocity

1 2
Making one obtains the same results :
1.- Note that:
and

and
2.- Note that also:

Obviously, base change matrix can be computed using the same formula:

gives the same result (i.e. MATLAB symbolic Toolbox)

Euler Parameters Relation to Vector + Angle parameters

The constraint between parameters becomes

The Euler-Rodrigues formula then becomes

*

The angular velocity can be obtained as

not a good option in a symbolic setup

Advanced
preferred in a symbolic setup

It takes some sweat, but it can be demonstrated that

** still better in a symbolic setup

where
* & **, used for of rotations
% Base defined using Euler parameters defined in terms of Euler
newBase('xyz','BBody',[e0,e1,e2,e3]') parameters
Euler Parameters: Quaternions
Note that Euler parameters are the equivalent to the components of unit quaternions frequently used to
describe rotations.
So, the only important think about quaternions is not the parametrization, because is the same than for
Euler parameters, but the special algebra that they offer: composition of rotations using quaternions,...
Lib_3d_MEC Does not implement quaternion algebra, but any rotation elsewhere defined using
quaternions can implemented defining the rotations by the way of Euler parameters, that are
implemented in Lib_3D_MEC

Advanced
Freshen Up Dynamics

Multibody System Dynamics

J. Ros September 21, 2020

Dept. Engineering. Public University of Navarre (UPNA)

Freshen Up Dynamics: Newton-Euler Equations

Multibody System Dynamics

J. Ros September 21, 2020

Dept. Engineering. Public University of Navarre (UPNA)

Newton Euler Eqs.
What we call an Action: Is a set of forces at the particle level.

Action enter the dynamics of solids through their resultant forces and
moments

Note: The pair force/moment is frequently referred as Wrench

Vector Theorems - Newton Euler Eqs.

Linear momentum theorem

Angular momentum theorem

Force/Moment (Wrench) characterization

Inertia: Gravity, gravitational

Constitutive: viscous friction, spring, damper, dry-friction

Constraint: Forces or moments for each translation or rotation avoided.

Characterization for translations avoided for a point of the body and rotations
avoided around axes going through it. You choose the point, although there are
interesting options
Result from model
External: Forces not in the previous characterization simplification - ideality
of joints/constraints
Inertia force and moment characterization
Defined as:

Usually, is taken as a fixed point in

Note: These are frequently referred as D’Alembert inertia force and moment
Constraint force and moment characterization
First notice that, for a body, the pair angular velocity + velocity of one of its points (so called Twist)
completely determine its motion.

Second, that the Twist can be defined w.r.t. any of the points of the body.

Constraints exactly remove a number of d.o.f. of a body w.r.t. the body to which it’s joined, by the
way of unknown forces and moments.

“If for a given point and orientation, fixed to one of the bodies, the twist is so that the
non-zero components are independent translational and rotational velocities, then:
For each null translational or rotational component of the twist, there is a
corresponding force moment component for a constraint wrench referred to the same
point.”

In strict mathematical terms: if a twist is expressed in terms of a

independent set of velocities, the constraint wrench is defined in terms of
a set of independent variables (joint unknowns) so that it expands the null
space of the twist. This general procedure can be implemented
symbolically.
Example: Problem 2. Newton-Euler Dynamics using Forces and Moments

Multibody System Dynamics

J. Ros September 21, 2020

Dept. Engineering. Public University of Navarre (UPNA)

% Constraint Wrenches (Definition using helper function newConstraint_Force_Moment)
%----Begin Edit----
% Ground->Slider
[FC_Gr_Slider,MC_Gr_Slider_A]=newConstraint_Force_Moment([1,1,0],[1,1,0],'A','Bpsi','Slider','Gr')

% Slider->Arm
[FC_Slider_Arm,
MC_Slider_Arm_A]=newConstraint_Force_Moment([1,1,1],[1,0,1],'A','Bpsi','Arm','Slider')

% Arm->Disc
[FC_Arm_Disc, MC_Arm_Disc_C]=newConstraint_Force_Moment([0,1,1],[0,1,1],'C','Btheta','Disc','Arm') epsilon =

% Ground->Disc FC_Gr_Slider_1
[FC_Gr_Disc, MC_Gr_Disc_J]=newConstraint_Force_Moment([1,1,1],[0,0,0],'J','Bpsi','Disc','Gr') FC_Gr_Slider_2
%----End Edit---- MC_Gr_Slider_1
epsilon, n_epsilon MC_Gr_Slider_2
FC_Slider_Arm_1
Output Output FC_Slider_Arm_2
FC_Slider_Arm_3
MC_Slider_Arm_1
FC_Gr_Slider = MC_Slider_Arm_3
FC_Arm_Disc_2
FC_Gr_Slider_1
FC_Arm_Disc_3
FC_Gr_Slider_2
0 MC_Arm_Disc_2
'Bpsi' Helper functions automatically define joint MC_Arm_Disc_3
FC_Gr_Disc_1
MC_Gr_Slider_A = constraint variables FC_Gr_Disc_2
FC_Gr_Disc_3
MC_Gr_Slider_1
MC_Gr_Slider_2 n_epsilon =
0
'Bpsi'
16
 FV_Gr_Disc =

0
0
0
% Constitutive 'Bpsi'
%----Begin Edit---- Output
% Visc Ground-ous>Disc (rolling) MV_Gr_Disc_J =
c_Gr_Disc=newParam('c_Gr_Disc',10.0);
c_Gr_Disc*(dtheta*sin(psi) + dphi*cos(psi)*cos(theta))
FV_Gr_Disc=Vector3D([0,0,0]','Bpsi'); -c_Gr_Disc*(dtheta*cos(psi) - dphi*cos(theta)*sin(psi))
MV_Gr_Disc_J=-Omega('abs','Disc')*c_Gr_Disc; -c_Gr_Disc*(dpsi + dphi*sin(theta))
_'xyz'

% Spring-Damper Ground->Slider
k_Gr_Slider =newParam('k_Gr_Slider',900);
rho0=newParam('rho0',1); Output FSD_Gr_Slider =
c_Gr_Slider=newParam('c_Gr_Slider',30);
0
FSD_Gr_Slider=Vector3D([0,0,-k_Gr_Slider*(h-rho0)-c_Gr_Slider*dh]','Bpsi'); 0
MSD_Gr_Slider_A=Vector3D([0,0,0]','Bpsi'); - c_Gr_Slider*dh - k_Gr_Slider*(h -
%----End Edit---- rho0)
'Bpsi'

MSD_Gr_Slider_A =

0
0
0
'Bpsi'
% Gravity
%----Begin Edit----
% Slider
FG_Slider=Vector3D([0,0,-m_Slider*g]','Bpsi');
MG_Slider_G_Slider=Vector3D([0,0,0]','Bpsi');

% This can be automated using a function

%[FG_Slider,
%MG_Slider_G_Slider]=newGravity_Force_Moment('Slider','G_Slider');

% Arm
[FG_Arm, MG_Arm_G_Arm]=newGravity_Force_Moment('Arm','G_Arm');
% Note that moment is defined in the solids frame origin

% Disc
[FG_Disc, MG_Disc_G_Disc]=newGravity_Force_Moment('Disc','G_Disc');
%----End Edit----

% Inertia
%----Begin Edit----
% Slider
FI_Slider=-m_Slider*Accel('IF','G_Slider');

h_Slider_A=I_Slider_A*Omega('IF','Slider');

MI_Slider_A=-(timederivative(h_Slider_A,'IF')+m_Slider*cross(Pos('A','G_Slider'),Accel('IF','A')));

% This can be automated using a function

% [FI_Slider,MI_Slider_A]=newInertia_Force_Moment('Slider');

% Arm
[FI_Arm,MI_Arm_A]=newInertia_Force_Moment('Arm')

% Disc
[FI_Disc,MI_Disc_C]=newInertia_Force_Moment('Disc')
%----End Edit----
%% Newton-Euler Dynamic Equations
% using Forces and Moments
%----Begin Edit----
% Slider
Sum_Forces_Slider= FI_Slider+FG_Slider+FC_Gr_Slider-FC_Slider_Arm+FSD_Gr_Slider;
Sum_Moments_Slider_A= MI_Slider_A+(MG_Slider_G_Slider+cross( Pos('A','G_Slider') ,
FG_Slider))+MC_Gr_Slider_A-MC_Slider_Arm_A+MSD_Gr_Slider_A;

% Arm
AC=Pos('A','C');
AG_Arm=Pos('A','G_Arm');
Sum_Forces_Arm= FI_Arm+FG_Arm+FC_Slider_Arm-FC_Arm_Disc;
Sum_Moments_Arm_A= MI_Arm_A+(MG_Arm_G_Arm+cross(AG_Arm,FG_Arm))+MC_Slider_Arm_A-(MC_Arm_Disc_C + cross ( AC ,FC_Arm_Disc) );

% Disc
CG_Disc=Pos('C','G_Disc');
CJ=Pos('C','J');
Sum_Forces_Disc= FI_Disc+simplify(FG_Disc)+FC_Gr_Disc+FV_Gr_Disc+FC_Arm_Disc;
Sum_Moments_Disc_C= MI_Disc_C+(MG_Disc_G_Disc+cross(CG_Disc,FG_Disc))+(MC_Gr_Disc_J+cross( CJ , FC_Gr_Disc)) + (MV_Gr_Disc_J +
cross ( CJ , FV_Gr_Disc) ) + MC_Arm_Disc_C;

%----End Edit----
%% Newton-Euler
%----Begin Edit----
newDyn_eq_NE([Sum_Forces_Slider;
Sum_Moments_Slider_A;
Sum_Forces_Arm;
Sum_Moments_Arm_A;
Sum_Forces_Disc;
Sum_Moments_Disc_C]);
%----End Edit----
%So mass matrix is positive definite
Dyn_eq_NE=-Dyn_eq_NE; %
%Dyn_eq_NE=simplify(Dyn_eq_NE);

Helper function: newDyn_eq_NE([eqs_1;

eqs_2;
...
]);
Exercise: Four-Bar. Newton-Euler Dynamics using Forces and Moments

Multibody System Dynamics

J. Ros September 21, 2020

Dept. Engineering. Public University of Navarre (UPNA)

Continue your work in lib_3D_MEC_MATLAB on
the Four-Bar mechanism, by introducing the NE
Dynamic equations
Screws: Twist and Wrenches

Multibody System Dynamics

J. Ros September 21, 2020

Dept. Engineering. Public University of Navarre (UPNA)

Screws: Twist and Wrenches
Twist: The couple, angular velocity of a body, and velocity of one of its points. It is a 6D
vector.

Wrench: The couple, force/moment acting on a body w.r.t. one of its points. It is a 6D
vector.

Power of a force moment pair:

Where the scalar (inner) product is defined to produce power. With components

Sum of wrenches: The sum of two wrenches defined w.r.t. the same point, represent, the wrench
corresponding to the set of forces associated to both wrenches. Multiplying a set of forces by a constant has
associated the wrench of the original set times the constant. Wrenches are a vector space.

Sum of twists: The sum of two twist defined w.r.t. the same point, represent, the twist of a body undergoing
the sum of both translational and both rotational velocities. Multiplying the velocities of the particles of a
rigid body by a constant the twist becomes that the initial twist times the constant. Twists are a vector
space.
Screws: Twist and Wrenches
A twist fully characterizes the the motion of a rigid body. Using velocity composition

So, if the point B used to define the twist is changed to B’, we can define the new twist in
terms of the old one one. Therefore, to refer a twist to a new point:

note: and

A wrench fully characterizes the effect of a system of forces on the dynamics of a rigid
body. We know that the moment of a system of forces can be “translated” as:

So, if the point B used to define the wrench is changed to B’, we can define the new wrench
in terms of the old one. Therefore, to refer a wrench to a new point:

note: and

So, the change of point of reference is accomplished in the same way for any screw, for
either twist or wrenches

We can do addition of two screws, product by an scalar, and scalar product. In order to do these operations both
screws must be referred to the same point, and the previous translation tensor (in 6D Vector Space) becomes
handy. A class of 6D Vectors (screws) can be defined to work following the rules defined. Just in the same way that
a class of 3D Vectors was defined in order to follow the rules of the 3D euclidean space vectors in mechanics.
Screws: Kinetic Energy
Given the Twist:

The Kinetic Energy is defined as:

Where

is the generalized momenta of the solid, for the given twist velocity components. It
takes a specific form a soon as bases are chosen to express twist as a 6-tuple, Given
bases we define

Then kinetic energy of a set of solids is defined as

Defining, the generalized twists velocity tuple as the -tuple , the system kinetic energy

is defined as where is the so called mass matrix w.r.t. the generalized

twists velocity tuple.

Analogously, the kinetic energy w.r.t. the generalized velocity pair , with

Finally, the kinetic energy w.r.t. the generalized velocity couple , with
Screws: Twist and Wrenches. Implementation
We can do addition of two screws, product by an scalar, an scalar product. In order to do these operation both
screws must be referred to the same point, and the previous translation tensor (in 6D Vector space) becomes
handy. A class of 6D Vectors (screws) can be defined to work following the rules defined. Just in the same way
that a class of 3D Vectors was defined in order to follow the rules of the 3D euclidean space in mechanics.
% Vector6D Class definition
classdef (InferiorClasses = {?sym}) Vector6D
properties
FW %Force or Omega: a vector 3D
Class Def. MV %Moment or Velocity: a vector 3D
Point %Reference Point: name or point number
Action %Solid over which twist or wrench is defined: name or solid number
Reaction %Solid from which the action is coming or frame w.r.t. which the twist is defined
end
Methods
...
% Basic construction of objects
% Twist
v_B_SolB=Vector3D(...
omega_SolB=Vector3D(...
twist_SB=Vector6D(omega_SolB, v_B_SolB, 'B','SolB')
Examples
% Wrench
F_SolA_SolB=Vector3D(...
M_SolA_SolB_B=Vector3D(...
wrench_SA_SB=Vector6D(F_SolA_SolB, M_SolA_SolB_B, 'B','SolB')

%Helper functions
Twist_SolB = Twist('SolB');
WrenchIG_SolB=newInertia_and_Gravity_Wrench('SolB');

% Operations
dW_wrench_SA_SB_dtheta=dot( jacobian(twist_SB, dtheta), wrench_SA_SB )
...
Vector Theorems or Newton-Euler Eqs.
Linear momentum theorem

Angular momentum theorem

Wrench form is much easier if using a computer.

Note that wrench translation and base change
happen behind the scene. Note that corresponding
point and bases used to do the sum are chosen
automatically.
Example: Problem 2. Newton-Euler Dynamics using Wrenches

Multibody System Dynamics

J. Ros September 21, 2020

Dept. Engineering. Public University of Navarre (UPNA)

Characterization of Actions (Wrench)

Constraint Actions
%% Wrench Definition
%----Begin Edit----
% Constraint Wrenches (Definition using helper function newConstraint_Wrench)

% Ground->Slider
Wrench_Gr_Slider=newConstraint_Wrench([1,1,0],[1,1,0],'A','Bpsi','Slider','Gr');

% Slider->Arm
Wrench_SliderArm=newConstraint_Wrench([1,1,1],[1,0,1],'A','Bpsi','Arm','Slider');

epsilon =
% Arm->Disc
Wrench_ArmDisc=newConstraint_Wrench([0,1,1],[0,1,1],'C','Btheta','Disc','Arm');
FC_Gr_Slider_1
FC_Gr_Slider_2
% Ground->Disc
MC_Gr_Slider_1
Wrench_Gr_Disc=newConstraint_Wrench([1,1,1],[0,0,0],'J','Bpsi','Disc','Gr');
MC_Gr_Slider_2
%----End Edit----
FC_Slider_Arm_1
epsilon, n_epsilon
FC_Slider_Arm_2
FC_Slider_Arm_3
Output Output MC_Slider_Arm_1
MC_Slider_Arm_3
Important things to note: they are defined in terms of constraint FC_Arm_Disc_2
Wrench_Gr_Slider =
variables/unknowns FC_Arm_Disc_3
MC_Arm_Disc_2
FGrSlider1 MC_Arm_Disc_3
FGrSlider2 These variables are unknowns along with the generalized
FC_Gr_Disc_1
0 coordinates in the direct dynamics problem FC_Gr_Disc_2
'Bpsi' FC_Gr_Disc_3
MGrSlider1
Helper functions automatically define joint constraint
MGrSlider2 n_epsilon =
0
variables/unknowns
'Bpsi' 16
'A'
Characterization of Actions (Wrench)

Constitutive Actions

% Constitutive
%----Begin Edit----
% Viscous Ground->Disc (rolling)
c_Gr_Disc=newParam('c_Gr_Disc',10.0);
Output WrenchV_GrDi =
FV_Gr_Disc=Vector3D([0,0,0]','Bpsi');
0
MV_Gr_Disc_J=-Omega('abs','Disc')*c_Gr_Disc;
0
WrenchV_GrDisc=newWrench(Vector6D(FV_Gr_Disc,MV_Gr_Disc_J,'J'),'Dis,’Gr’c')
0
'Bpsi'
c_Gr_Disc*(dtheta*sin(psi) +dphi*cos(psi)*cos(theta))
-c_Gr_Disc*(dtheta*cos(psi) -dphi*cos(theta)*sin(psi))
% Spring-Damper Ground->Slider
-c_Gr_Disc*(dpsi + dphi*sin(theta))
k_Gr_Slider =newParam('k_Gr_Slider',900);
'xyz'
rho0=newParam('rho0',1);
'J'
c_Gr_Slider=newParam('c_Gr_Slider',30);

FSD_Gr_Slider=Vector3D([0,0,-k_Gr_Slider*(h-rho0)-c_Gr_Slider*dh]','Bpsi');
MSD_Gr_Slider_A=Vector3D([0,0,0]','Bpsi');
WrenchSD_Gr_Slider=newWrench(Vector6D(FSD_Gr_Slider,MSD_Gr_Slider_A,'A'),'Slider','Gr') WrenchSD_Gr_Slider =
%----End Edit----
0
Output 0
-c_Gr_Slider*dh -k_Gr_Slider*(h -rho0)
'Bpsi'
0
0
0
'Bpsi'
'A'
Characterization of Actions (Wrench)
% Gravity + Inertia
%----Begin Edit----
% Gravity Gravity Actions
% Slider
% WrenchG_Chassis=newGravity_Wrench('Slider');

% Arm
% WrenchG_Wheel_L=newGravity_Wrench('Arm');

% Disc
% WrenchG_Wheel_R=newGravity_Wrench('Disc');

% Previous wrench definitions commented because they are defined along with inertia forces.

% Inertia Inertia Actions

% Slider
% WrenchI_Chassis=newInertia_Wrench('Slider');

% Arm
% WrenchI_Wheel_L=newInertia_Wrench('Arm');

% Disc
% WrenchI_Wheel_R=newInertia_Wrench('Disc');

% Gravity + Inertia Gravity + Inertia Actions

% Slider
WrenchIG_Slider=newInertia_and_Gravity_Wrench('Slider');

% Arm
WrenchIG_Arm=newInertia_and_Gravity_Wrench('Arm');

% Disc
WrenchIG_Disc=newInertia_and_Gravity_Wrench('Disc');
%----End Edit----
Dynamic Equations
Newton-Euler Formalism (Wrench)
%% Newton-Euler Dynamic Equations

Automatizable
% using Forces and Moments
%----Begin Edit----
% Slider
Sum_Wrenches_Slider_NE= WrenchIG_Slider+Wrench_Gr_Slider-Wrench_SliderArm+WrenchSD_Gr_Slider;

% Arm
Sum_Wrenches_Arm_NE= WrenchIG_Arm+Wrench_SliderArm-Wrench_ArmDisc;

% Disc
Sum_Wrenches_Disc_NE= WrenchIG_Disc+Wrench_Gr_Disc+WrenchV_GrDi+Wrench_ArmDisc;
%----End Edit----

Newton-Euler Formalism (Force-Moment)

%% Newton-Euler Dynamic Equations
% using Forces and Moments
%----Begin Edit----
% Slider
Sum_Forces_Slider= FI_Slider+FG_Slider+FC_Gr_Slider-FC_Slider_Arm+FSD_Gr_Slider;
Sum_Moments_Slider_A= MI_Slider_A+(MG_Slider_G_Slider+cross( Pos('A','G_Slider') ,
FG_Slider))+MC_Gr_Slider_A-MC_Slider_Arm_A+MSD_Gr_Slider_A;

% Arm
AC=Pos('A','C'); Prone to Error
AG_Arm=Pos('A','G_Arm');
Sum_Forces_Arm= FI_Arm+FG_Arm+FC_Slider_Arm-FC_Arm_Disc;
Sum_Moments_Arm_A= MI_Arm_A+(MG_Arm_G_Arm+cross(AG_Arm,FG_Arm))+MC_Slider_Arm_A-(MC_Arm_Disc_C + cross ( AC
,FC_Arm_Disc) );

% Disc
CG_Disc=Pos('C','G_Disc');
Sum_Forces_Disc= FI_Disc+simplify(FG_Disc)+FC_Gr_Disc+FV_Gr_Disc+FC_Arm_Disc;
Sum_Moments_Disc_C= MI_Disc_C+(MG_Disc_G_Disc+cross(CG_Disc,FG_Disc,'Bphi'))+(MC_Gr_Disc_J+cross( CJ ,
FC_Gr_Disc)) + (MV_Gr_Disc_J + cross ( CJ , FV_Gr_Disc) ) + MC_Arm_Disc_C;
%----End Edit----
Dynamic Equations
Newton-Euler Formalism (Wrench)

%% Newton-Euler
%----Begin Edit----
newDyn_eq_NE([Sum_Wrenches_Slider_NE;
Sum_Wrenches_Arm_NE;
Sum_Wrenches_Disc_NE])
%----End Edit----
%So mass matrix is positive definite
Dyn_eq_NE=-Dyn_eq_NE; % Dyn_eq_NE=simplify(Dyn_eq_NE);

Helper function:
newDyn_eq_NE([eqs_1;
eqs_2;
...
]);
Direct Dynamics Problem with Newton Euler Equations

Multibody System Dynamics

J. Ros September 21, 2020

Dept. Engineering. Public University of Navarre (UPNA)

Dynamic Equations structure:
Newton-Euler formalism

The dynamic equations in the NE formalism have the form

Contribution to independent term on direct dynamic problem

Factors of unknowns on direct dynamic problem

Where, obviously as they are linear in joint unknowns and accelerations

Mass matrix
(not square in NE formalism)

%% Export Direct Dynamics Problem Newton-Euler

M_vq=jacobian(Dyn_eq_NE,ddq);

delta_v=subs(Dyn_eq_NE,epsilon,sym(zeros(size(epsilon))));
delta_v=-subs(delta_v,ddq,zeros(size(ddq)));
FC_vepsilon=-jacobian(Dyn_eq_NE,epsilon);

matlabFunction(FC_vepsilon,'File','FC_vepsilon_','Vars',{q,t,param});
matlabFunction(M_vq,'File','M_vq_','Vars',{q,t,param});
matlabFunction(delta_v,'File','delta_v_','Vars',{q,dq,t,param});
Direct Dynamics Problem:
Newton-Euler formalism

Set of equations Unknowns

Joint unknowns

Generalized accel.

Estructure (both equations are linear in the unknowns):

Constraint Force Jacobian
Mass matrix

Can appear when using

auxiliary constraints

Velocity Constraint Jacobian Pseudoinverse use allows redundant joint unknowns,

Solution: and degenerate mass matrices and vel. Jacobians

We call this solution

Naive NE
Only if there are no redundant joint
unknowns, and mass matrix and velocity
jacobian are not degenerate
Direct Dynamics Problem:
Newton-Euler formalism Naive Solution

For generality we implement a

solution that allows for
auxiliary constraints
Naive NE Solution

Generic code

%% Naive NE Direct Dynamics Problem solution (with auxiliary

constraints)
dPhiAux_dq_value=dPhiAux_dq_(q_value,t_value,param_value);
FC_vepsilon_value=FC_vepsilon_(q_value,t_value,param_value);

coeff_matrix_A=...
[M_vq_(q_value,t_value,param_value), -FC_vepsilon_value;
dPhiAux_dq_value, zeros(n_ddPhiAux,n_epsilon);
dPhi_dq_(q_value,t_value,param_value),zeros(n_dPhi,n_epsilon)];

indep_term_vector_b=
[delta_v_(q_value,dq_value,t_value,param_value);
gammaAux_(q_value,dq_value,t_value,param_value); Trick: we always export auxiliary functions
gamma_(q_value,dq_value,t_value,param_value)];

unknown_vector_x=pinv(coeff_matrix_A)*indep_term_vector_b; Just, they return empty matrices if there are

no auxiliary constraints.
ddq_value=unknown_vector_x(1:n_ddq,1);
epsilon_value=unknown_vector_x(n_ddq+1:n_ddq+n_epsilon,1);
Example: Problem 2 Direct Dynamics Problem

Multibody System Dynamics

J. Ros September 21, 2020

Dept. Engineering. Public University of Navarre (UPNA)

Direct Dynamics Problem:
Newton-Euler formalism

Prerequisite: execute problem2_W.m up to, and including, the introduction of the

dynamic equations: newDyn_eq_NE(...)

Export NE Dynamic Equations: @ the console execute

%% Export Direct Dynamics Problem Newton-Euler

M_vq=jacobian(Dyn_eq_NE,ddq);
Solve NE Dynamic Equations. @ the console execute
delta_v=subs(Dyn_eq_NE,epsilon,sym(zeros(size(epsilon))));
delta_v=-subs(delta_v,ddq,zeros(size(ddq))); %% Naive Direct Dynamics Problem solution (with auxiliary
FC_vepsilon=-jacobian(Dyn_eq_NE,epsilon); constraints)
dPhiAux_dq_value=dPhiAux_dq_(q_value,t_value,param_value);
matlabFunction(FC_vepsilon,'File','FC_vepsilon_','Vars',{q,t,para FC_vepsilon_value=FC_vepsilon_(q_value,t_value,param_value);
m});
matlabFunction(M_vq,'File','M_vq_','Vars',{q,t,param}); coeff_matrix_A=...
matlabFunction(delta_v,'File','delta_v_','Vars',{q,dq,t,param}); [M_vq_(q_value,t_value,param_value), -FC_vepsilon_value;
dPhiAux_dq_value,
zeros(n_ddPhiAux,n_epsilon);

dPhi_dq_(q_value,t_value,param_value),zeros(n_dPhi,n_epsilon)]
;

indep_term_vector_b=
Don’t forget to actualize the Drawing: [delta_v_(q_value,dq_value,t_value,param_value);
gammaAux_(q_value,dq_value,t_value,param_value);
updateDraws;
gamma_(q_value,dq_value,t_value,param_value)];

unknown_vector_x=pinv(coeff_matrix_A)*indep_term_vector_b;

ddq_value=unknown_vector_x(1:n_ddq,1);
epsilon_value=unknown_vector_x(n_ddq+1:n_ddq+n_epsilon,1);
Direct Dynamics Problem. Exercise:
Newton-Euler formalism

Prerequisite: execute problem_name_W.m up to, and including, the introduction of the %% Export Direct Dynamics Problem Newton-Euler
M_vq=jacobian(Dyn_eq_NE,ddq);
dynamic equations
delta_v=subs(Dyn_eq_NE,epsilon,sym(zeros(size(epsilon))));
newDyn_eq_NE(...) delta_v=-subs(delta_v,ddq,zeros(size(ddq)));
FC_vepsilon=-jacobian(Dyn_eq_NE,epsilon);
Then, proceed as in the previous example…
matlabFunction(FC_vepsilon,'File','FC_vepsilon_','Vars',{q,t,param}
);
matlabFunction(M_vq,'File','M_vq_','Vars',{q,t,param});

The presented code to export the dynamics equations, as is general, is packaged into the matlabFunction(delta_v,'File','delta_v_','Vars',{q,dq,t,param});

script
Export_Direct_Dynamics_Problem
The presented code to solve the dynamics equations, as is general, is packaged into script
direct_dynamics_problem
In order for this script to choose the Naive NE solver the global variable
%% Naive NE Direct Dynamics Problem solution (with auxiliary
constraints)
dyn_equations_type='Newton-Euler'; dPhiAux_dq_value=dPhiAux_dq_(q_value,t_value,param_value);

must be set in advance. FC_vepsilon_value=FC_vepsilon_(q_value,t_value,param_value);

coeff_matrix_A=...
So you can export and solve the the direct dynamics using the Naive NE problem as follows [M_vq_(q_value,t_value,param_value), -FC_vepsilon_value;
dPhiAux_dq_value,
Export_Direct_Dynamics_Problem zeros(n_ddPhiAux,n_epsilon);
dPhi_dq_(q_value,t_value,param_value),zeros(n_dPhi,n_epsilon)];
dyn_equations_type='Newton-Euler';
indep_term_vector_b=
direct_dynamics_problem [delta_v_(q_value,dq_value,t_value,param_value);
gammaAux_(q_value,dq_value,t_value,param_value);
gamma_(q_value,dq_value,t_value,param_value)];

unknown_vector_x=pinv(coeff_matrix_A)*indep_term_vector_b;

ddq_value=unknown_vector_x(1:n_ddq,1);
epsilon_value=unknown_vector_x(n_ddq+1:n_ddq+n_epsilon,1);
Exercise: Four-Bar Direct Dynamics Problem

Multibody System Dynamics

J. Ros September 21, 2020

Dept. Engineering. Public University of Navarre (UPNA)

Continue your work in lib_3D_MEC_MATLAB on
the Four-Bar mechanism, by introducing the NE
Dynamic equations
Direct Dynamic Simulation

Multibody System Dynamics

J. Ros September 21, 2020

Dept. Engineering. Public University of Navarre (UPNA)

Direct Dynamic Simulation:

If assembly problem is removed the

Integration Stability is lost:
The error in the constraints at position
and velocity level drift away.
If we introduce these error variables in
the set of differential equations we can
see zero eigenvalues associated with
... them. This explains the drift on a
mathematical ground.
Direct Dynamic Simulation: %% RK4 improved integration
t_value_0=t_value;
q_value_0=q_value;
% Computing k3
t_value=t_value_0+Delta_t/2;
q_value=q_value_0+q_k2/2;
dq_value_0=dq_value; dq_value=dq_value_0+dq_k2/2;

% Computing k1 direct_dynamics_problem;
direct_dynamics_problem;
q_k1=Delta_t*dq_value; q_k3=Delta_t*dq_value;
dq_k1=Delta_t*ddq_value; dq_k3=Delta_t*ddq_value;
%% Dynamic Simulation
% Computing k2 % Computing k4
% Setup time step and integration span
t_value=t_value_0+Delta_t/2; t_value=t_value_0+Delta_t;
% Each time you run this section you get T_span additional time q_value=q_value_0+q_k1/2; q_value=q_value_0+q_k3;
% of simulation dq_value=dq_value_0+dq_k1/2; dq_value=dq_value_0+dq_k3;

T_span=6 direct_dynamics_problem; q_k4=Delta_t*dq_value;

dq_k4=Delta_t*ddq_value;
t_end=Get_value(t)+T_span; q_k2=Delta_t*dq_value;
dq_k2=Delta_t*ddq_value; % Position Actualization
t_refresh=Get_value(t); q_value=q_value_0+1/6*(q_k1+2*q_k2+2*q_k3+q_k4);

% Velocity Actualization
dq_value=dq_value_0+1/6*(dq_k1+2*dq_k2+2*dq_k3+dq_k4);
% Open Extended State file for writing
fid = fopen('dynamics_extended_state_series.txt','w');

% Write Extended State

printf_extended_state(fid); while t_value <=t_end
if strcmp(integrator_name,'Euler')
integrator_type=’Euler’; %% Euler improved integration
% direct_dynamics_problem;
dynamic_simulation(fid); q_value=q_value+Delta_t*(dq_value+0.5*Delta_t*ddq_value);
dq_value=dq_value+Delta_t*ddq_value;
% Close Extended State file t_value=t_value+Delta_t;
fclose(fid); end
%% Assembly Problem, used for constraint projection
updateDraws_automatic=true; assembly_problem;

%% Direct Dynamics
direct_dynamics_problem;

Have a look to dynamic_simulation.m to see a % Update Drawing with Delta_t_refresh period

implementation of a different integrators. With if t_value-t_value_ant > Delta_t_refresh
updateDraws;
integrator_type=’RK4’; t_value_ant=t_value_ant+Delta_t_refresh;
end
An order 4 Runge-Kutta method is used case.
% Write Extended State
You can try it! printf_extended_state(fid);
end
Example: Problem 2 Direct Dynamics Simulation

Multibody System Dynamics

J. Ros September 21, 2020

Dept. Engineering. Public University of Navarre (UPNA)

Direct Dynamic Simulation:
Newton-Euler formalism
Prerequisite: execute problem2_W.m up to, and including, the introduction of the
dynamic equations: newDyn_eq_NE(...), and export the dynamic equations:
Export_Direct_Dynamics_Problem

Perform a Direct Dynamics Simulation: @ the console execute

initial_assembly_problem;

direct_dynamics problem;

%% Dynamic Simulation
% Setup time step and integration span
% Each time you run this section you get T_span additional time of
% simulation
T_span=6
t_end=Get_value(t)+T_span;
t_refresh=Get_value(t);

% Open Extended State file for writing

fid = fopen('dynamics_extended_state_series.txt','w');

% Write Extended State

printf_extended_state(fid);

integrator_type=’Euler’;

dynamic_simulation(fid);

% Close Extended State file

fclose(fid);

updateDraws_automatic=true;
Exercise: Four-Bar Direct Dynamics Simulation

Multibody System Dynamics

J. Ros September 21, 2020

Dept. Engineering. Public University of Navarre (UPNA)

Direct Dynamics Problem. Exercise:
Newton-Euler formalism
up to, and including, the introduction of the dynamic equations:
newDyn_eq_NE(...), and export the dynamic equations:
Export_Direct_Dynamics_Problem

Then, proceed as in the previous example…

Freshen Up Dynamics: Virtual Power Principle

Multibody System Dynamics

J. Ros September 21, 2020

Dept. Engineering. Public University of Navarre (UPNA)

Recommended material

Chapter 3 in
Virt. Power principle for a System of Particles
Principle

Systematization

Generalized forze components

Generalized forze
Virt. Power principle for a System of Bodies

Principle

Systematization using vectors

Systematization using screws

Generalized forze components

Generalized forze
Example: Problem 2 (Virtual Power Principle Dynamics)

Multibody System Dynamics

J. Ros September 21, 2020

Dept. Engineering. Public University of Navarre (UPNA)

%----Begin Edit----
Twist_Slider = Twist('Slider');
Twist_Arm = Twist('Arm');
Twist_Disc = Twist('Disc');

for i=1:n_q
newDyn_eq_VP( dot( Sum_Wrenches_Slider_NE , diff( Twist_Slider , dq(i) ) )+...
dot( Sum_Wrenches_Arm_NE , diff( Twist_Arm , dq(i) ) )+...
dot( Sum_Wrenches_Disc_NE , diff( Twist_Disc , dq(i) ) ) );
end
%----End Edit----

%% Export Direct Dynamics Problem "a la" Virtual Power or Lagrange

% So mass matrix is positive definite
Dyn_eq_VP=-Dyn_eq_VP;
Export_Direct_Dynamics_Problem_VP

Helper function: newDyn_eq_VP([eqs_1;

eqs_2;
...
]);
Direct Dynamics Problem:
Virtual Power formalism
Prerequisite: execute problem2_W.m up to, and including, the introduction of the
dynamic equations: newDyn_eq_VP(...)

Export NE Dynamic Equations: @ the console execute

%% Export Direct Dynamics Problem "a la" Virtual Power or Lagrange

Solve VP Dynamic Equations. @ the console execute

M_qq=jacobian(Dyn_eq_VP,ddq);
delta_q=subs(Dyn_eq_VP,epsilon,sym(zeros(size(epsilon))));
delta_q=subs(delta_q,lambda,sym(zeros(size(lambda))));
delta_q=-subs(delta_q,ddq,zeros(size(ddq))); %% Naive Direct Dynamics Problem solution (with auxiliary constraints)
%delta=simplify(delta_q) dPhi_dq_value=dPhi_dq_(q_value,t_value,param_value);
FC_qepsilon=-jacobian(Dyn_eq_VP,epsilon); dPhiAux_dq_value=dPhiAux_dq_(q_value,t_value,param_value);
%FC_vepsilon=simplify(FC_vepsilon); coeff_matrix_A=[M_qq_(q_value,t_value,param_value),dPhi_dq_value',dPhiAux_dq_value';
dPhi_dq_value,zeros(n_ddPhi,n_lambda+n_lambda_aux)
matlabFunction(FC_qepsilon,'File','FC_qepsilon_','Vars',{q,t,param}); dPhiAux_dq_value,zeros(n_ddPhiAux,n_lambda+n_lambda_aux)];
matlabFunction(M_qq,'File','M_qq_','Vars',{q,t,param});
matlabFunction(delta_q,'File','delta_q_','Vars',{q,dq,t,param}); indep_term_vector_b=[delta_q_(q_value,dq_value,t_value,param_value);
gamma_(q_value,dq_value,t_value,param_value)
gammaAux_(q_value,dq_value,t_value,param_value)];

@ Export_Direct_Dynamics_Problem_NE.m unknown_vector_x=pinv(coeff_matrix_A)*indep_term_vector_b;

ddq_value=unknown_vector_x(1:n_ddq,1);
lambda_value=unknown_vector_x(n_ddq+1:n_ddq+n_lambda,1);
lambda_aux_value=unknown_vector_x(n_ddq+n_lambda+1:n_ddq+n_lambda+n_lambda_aux,1);

epsilon_value=pinv(-FC_qepsilon_(q_value,t_value,param_value))*...
Don’t forget to actualize the Drawing: updateDraws; dPhi_dq_value'*lambda_value;

@ ddq_lambda_epsilon_from_Lagrange_Naive.m
Exercise: Problem Four-Bar (Virtual Power Principle Dynamics)

Multibody System Dynamics

J. Ros September 21, 2020

Dept. Engineering. Public University of Navarre (UPNA)

Direct Dynamics Problem:
Virtual-Power formalism

%----Begin Edit----
Twist_... = Twist('...');
...

for i=1:n_q
newDyn_eq_VP( dot( ... , diff( .. , dq(i) ) ) +...
+...
+... );
end
%----End Edit----

%So mass matrix is positive definite

Dyn_eq_VP=-Dyn_eq_VP;

%% Define Lagrange multipliers

newLambdas();
lambda, n_lambda
Direct Dynamics Problem with Virtual Power Principle

Multibody System Dynamics

J. Ros September 21, 2020

Dept. Engineering. Public University of Navarre (UPNA)

 Dynamic Equations. Virt. Power and Lagrange formalism
The dynamic equations in the VP formulation have the form

Where, obviously as they are linear in joint unknowns and accelerations

%% Export Direct Dynamics Problem "a la" Virtual Power or Lagrange

M_qq=jacobian(Dyn_eq_VP,ddq);
delta_q=subs(Dyn_eq_VP,epsilon,sym(zeros(size(epsilon))));
delta_q=subs(delta_q,lambda,sym(zeros(size(lambda))));
delta_q=-subs(delta_q,ddq,zeros(size(ddq)));
FC_qepsilon=-jacobian(Dyn_eq_VP,epsilon);

matlabFunction(FC_qepsilon,'File','FC_qepsilon_','Vars',{q,t,param});
matlabFunction(M_qq,'File','M_qq_','Vars',{q,t,param});
matlabFunction(delta_q,'File','delta_q_','Vars',{q,dq,t,param});
Direct Dynamics Problem:
Virtual Power formalism
Set of equations

Generalized accel.

Joint unknowns
Linear in the unknowns
Mass matrix

Velocity Jacobian

Solution: Naive Solution

We consider all joint unknowns, so even if there are not
redundancies we have an underdetermined set of equations.
Pseudoinverse will offer minimum norm solution, that makes
joint unknowns 0, unless they are associated to joints broken by
the defined parametrization.
Dynamic Equations: Virt. Power vs Lagrange formalism
The dynamic equations in the Lagrange formulation, along with the
constraint equation at the acceleration level, are

These can be obtained from the VP Dynamic Eqs, substituting by

All the required functions are exported when exporting the Constraint Equations and the
VP Dynamic Eqs. Then

Lib_3D_MEC_MATLAB uses this approach when

solving the dynamic eqs using both VP or
Lagrange. Thus these approaches are equivalent
in the library implementation.
Direct Dynamic Equations: NE vs VP comparison
Newton-Euler Lagrange
Solve
Solve

for for

These are the so called naive/trivial solutions, much faster solution can be obtained using the Udwadia & Kabala or even faster using Independent Coordinates solution
procedures.
Equilibrium Problem

Multibody System Dynamics

J. Ros September 21, 2020

Dept. Engineering. Public University of Navarre (UPNA)

Static Equilibrium and its generalization
To begin with, the most basic picture of a state of equilibrium can be obtained from the idea of static
equilibrium. Find, if any, a estate of the system so that its derivative is zero. If the system is in such an state
,the system will keep its state.

Well, we will see, that an equilibrium state can be stable, marginally stable and unstable. Meaning that if a
small perturbation is introduced to the state, the perturbation will vanish, will keep the amplitude of some
of its components, and some of its components will increase amplitude exponentially with time.

Sometimes, some coordinates / velocities do not appear in the

formulation of the dynamic problem. These are called ignorable
coordinates / velocities. In these circumstances we can find an
equilibrium form a restricted state that does not contains the
aforementioned coordinates / velocities.

Such equilibriums can be attained even with ignorable coordinates /

velocities not being constant.

In this respect, we say that the equilibrium is dynamic, in comparison

with the previously defined static equilibrium.

We allow to formulate general equilibrium problems in which the user

seeks static equilibrium for a set of coordinates, velocities or
accelerations, by the way of a definition of an Extra set of equations to be
satisfied at such positions.

Dynamic equilibrium is parametrization dependent!!!!

Static Equilibrium and its generalization
Dynamic equilibrium is parametrization dependent, so if our physical intuition tells us
that it can exit a motion of the system for which a feature is constant, we will try to use a
parametrization in which the parameters or velocities relate directly to such feature, so
the equilibrium problem can be formulated, by enforcing the constancy of such
coordinate or velocity (even acceleration).
 Equilibrium Equations (Dynamic)
%% Write Dynamic Equilibrium Equations
%----Begin Edit----
... is a extra set of conditions to be met @ equilibrium
Extra_Dyn_Eq_eq=[...
];
%----End Edit----

%% Equilibrium problem "a la" Newton-Euler

Dyn_Eq_eq_NE=[Dyn_eq_NE;
ddPhi;
dPhi;
Phi;
Extra_Dyn_Eq_eq];

%% Equilibrium problem "a la" Virtual Power or

Lagrange
Dyn_Eq_eq_L=[Dyn_eq_L;
ddPhi;
dPhi;
Phi;
Extra_Dyn_Eq_eq];

%% Export Equilibrium problem "a la" Lagrange

%% Export Equilibrium problem "a la" Newton-Euler unk_eq_VP=[q;
unk_eq_NE=[q; dq;
dq; ddq;
ddq; lambda];
epsilon]; J_Dyn_Eq_eq_VP=jacobian(Dyn_Eq_eq_VP,unk_eq_VP);
J_Dyn_Eq_eq_NE=jacobian(Dyn_Eq_eq_NE,unk_eq_NE); matlabFunction(Dyn_Eq_eq_VP,'File','Dyn_Eq_eq_VP_','Vars',{q,dq,ddq,t,lambd
matlabFunction(Dyn_Eq_eq_NE,'File','Dyn_Eq_eq_NE_','Vars',{q,dq,ddq,t,epsi a,param});
lon,param}); matlabFunction(J_Dyn_Eq_eq_VP,'File','J_Dyn_Eq_eq_VP_','Vars',{q,dq,ddq,t,l
matlabFunction(J_Dyn_Eq_eq_NE,'File','J_Dyn_Eq_eq_NE_','Vars',{q,dq,ddq,t, ambda,param});
epsilon,param});
 Equilibrium Equations (Dynamic)
Solution

A possibility is to use Newton-Raphson as it is a nonlinear problem. This requires the determination

of the Jacobian of the equation w.r.t. The unknowns

Dyn_Eq_eq_L_num=Dyn_Eq_eq_VP_(q_value,dq_value,ddq_value,t_value,lambda_value,param_value);
x_value=[q_value;
dq_value;
ddq_value;
lambda_value];
while norm(Dyn_Eq_eq_L_num)> Dyn_Eq_tol
x_value=x_value- Dyn_Eq_relax * pinv(J_Dyn_Eq_eq_L_(q_value,dq_value,ddq_value,t_value,lambda_value,param_value) ) * Dyn_Eq_eq_L_num;
q_value=x_value(1:n_q,1);
dq_value=x_value(n_q+1:2*n_q,1);
ddq_value=x_value(2*n_q+1:3*n_q,1);
lambda_value=x_value(3*n_q+1:end,1);
Dyn_Eq_eq_VP_num=Dyn_Eq_eq_L_(q_value,dq_value,ddq_value,t_value,lambda_value,param_value);
end
dPhi_dq_num=dPhi_dq_(q_value,t_value,param_value)
epsilon_value=pinv(-FC_qepsilon_(q_value,t_value,param_value))*dPhi_dq_num'*lambda_value;
Direct Dynamics Problem: Solution on Independent Coordinates

Multibody System Dynamics

J. Ros September 21, 2020

Dept. Engineering. Public University of Navarre (UPNA)

Direct Dynamics Problem
Solution on Independent Coordinates
Partitioning coordinates as

That can be reordered in a computationally efficient way as

with
Direct Dynamics Problem
[ddq_value,epsilon_value,lambda_value]=ddq_lambda_epsilon_from_Lagrange
_dz(q_value, dq_value, t_value)

Solution on Independent Coordinates

with

Still, performance can be improved by:

● Use Cholesky decomposition to obtain the inverse of the coefficient matrix
● Use LU (with column pivoting) instead of QR to obtain coordinate partitioning, and use this
factorization when computing the effect of the inverse.
%Determine a permutation perm, so that dq(permperm_dd_dz)=[dd;dz] delta_q_num=delta_q_(q_value,dq_value,t_value,param_value);
dPhi_dq_num=dPhi_dq_(q_value,t_value,param_value); delta_dd_num=delta_q_num(perm_dd,1);
[L,U,perm_dd_dz] = lu(dPhi_dq_num','vector') %[Q,R,perm_dd_dz] = delta_dz_num=delta_q_num(perm_dz,1);
qr(dPhi_dq_num,'vector');
%Assemble coeff_matrix and indep_term_vector
%permutations for dependent and independent velocities ATM_dddd=(M_dddd_num*A)';
n_dd=rank(U); %n_dd=rank(R); M_dzddA=M_dzdd_num*A;
perm_dd=perm_dd_dz(1:n_dd);
perm_dz=perm_dd_dz(n_dd+1:end); coeff_matrix=ATM_dddd*A+M_dzddA+M_dzddA'+M_dzdz_num;
indep_term_vector_b=A'*delta_dd_num+delta_dz_num-(ATM_dddd+M_dzdd_num)*b;
%Determine the dependent dd and independent dz jacobians
dPhi_dd_num=dPhi_dq_num(:,perm_dd); %Solve
dPhi_dz_num=dPhi_dq_num(:,perm_dz); ddz_value=pinv(coeff_matrix)*indep_term_vector_b;
ddd_value=A*ddz_value+b;
%Determine the A and b matrices
inv_dPhi_dd_num=pinv(dPhi_dd_num); ddq_value(perm_dd_dz,1)=[ddd_value;
A=-inv_dPhi_dd_num * dPhi_dz_num; ddz_value];
b=inv_dPhi_dd_num*gamma_(q_value,dq_value,t_value,param_value);
lambda_value=inv_dPhi_dd_num'*(delta_dd_num-(M_dddd_num*ddd_value+M_dzdd_num'*ddz_value));
%Determine submatrices
M_dqdq_num=M_qq_(q_value,t_value,param_value); epsilon_value=pinv(-FC_qepsilon_(q_value,t_value,param_value))*dPhi_dq_num'*lambda_value;
M_dddd_num=M_dqdq_num(perm_dd,perm_dd);
M_dzdd_num=M_dqdq_num(perm_dz,perm_dd);
M_dzdz_num=M_dqdq_num(perm_dz,perm_dz);
Coordinate Partitioning
In general, it is not operative to change the order in the components of and
What it is done is to access these vectors, and associated columns in jacobians indexed by a permutation
vector. This permutation is the one done in the jacobian columns
when a set of dependent velocities is obtained in terms of a set of independent ones , by doing the
forward elimination in the Gauss method to solve a system of linear equations.
Let be this permutation, then

%Determine a permutation perm, so that dq(permperm_dd_dz)=[dd;dz]

dPhi_dq_num=dPhi_dq_(q_value,t_value,param_value);
[L,U,perm_dd_dz] = lu(dPhi_dq_num','vector') %[Q,R,perm_dd_dz] =
qr(dPhi_dq_num,'vector');

%permutations for dependent and independent velocities

n_dd=rank(U); %n_dd=rank(R);

%Determine the dependent dd and independent dz jacobians using perm

perm_dd=perm_dd_dz(1:n_dd);
perm_dz=perm_dd_dz(n_dd+1:end);

%Determine the dependent dd and independent dz jacobians

dPhi_dd_num=dPhi_dq_num(:,perm_dd);
dPhi_dz_num=dPhi_dq_num(:,perm_dz);
Direct Dynamics Problem
Solution on Independent Coordinates

with

Using LU with full pivoting

[L,U,row_perm,perm_dddz]=lucp(dPhi_dq,1e-10,’vector’); %L*U=[L*U_dd,L*U_dz]= dPhi_dq(row_perm,perm_dddz)
perm_dd=perm_dddz(1:n_dd); perm_dz=perm_dddz(n_dd+1:n_q)

Advanced
dPhi_dz=dPhi_dq(row_perm,perm_dd); dPhi_dz=dPhi_dq(row_perm,perm_dz);
U_dd=U(:,perm_dd); U_dz=U(:,perm_dz)

Solving
% L*U(:,perm_dd) * dd = (beta(row_perm,:) - dPhi_dz *dz);
y=L\(beta(row_perm,:)- dPhi_dz*dz); % L * y = beta(row_perm,:) - dPhi_dz*dz;
dd=U(:,perm_dd)\y % U(:,perm_dd) * dd = y;

Computing is like solving

% L*U_dd * A = -dPhi_dz;
y=-L\dPhi_dz; % L*y = -dPhi_dz;
A=U_dd\y; % U_dd * A = y;

Computing is like solving

% L*U_dd * b = gamma(row_perm,:);
y=L\gamma(row_perm,:); % L*y=gamma(row_perm,:);
b=U(:,perm_dd)\y; % U(:,perm_dd)*b= y;

This is the trick for really fast implementations. ddq_lambda_epsilon_from_Lagrange_dz uses inverse because of readability/simplicity. But a
really quick implementation cannot be done using MATLAB. A C or C++ LU based (along the previous lines) implementation is recommended.
Direct Dynamics Problem: Linearization on independent coordinates

Multibody System Dynamics

J. Ros September 21, 2020

Dept. Engineering. Public University of Navarre (UPNA)

Linearization on Indep. Coord. (lin. point @ dynamics manifold)

Valid for holonomic constraints.

I believe it is also valid for
non-holonomic constraints.

% Export Eigenvalue Problem

Dyn_eq_VP_open=subs(Dyn_eq_VP,epsilon,sym(zeros(size(epsilon)))); Rheonomic constraint linearization require constraint
Dyn_eq_L=Dyn_eq_VP_open+dPhi_dq'*lambda; linearization to be done wrt time also. This leads to time
K_qq=jacobian(Dyn_eq_L,q)+... dependent terms in constraint equations @ the level of gen.
C_qq=jacobian(Dyn_eq_L,dq)+... coord., vel. and accel. But it does not alter the Mass Damping
matlabFunction(K_qq,'File','K_qq_','Vars',{q,dq,ddq,lambda,t,param}); and Inertia matrices obtained here. See next slide...
matlabFunction(C_qq,'File','C_qq_','Vars',{q,dq,t,param});
Rheonomic Constraint Linearization on Indep. Coord. (lin. point @
dynamics manifold) -Time dependent linearization -

Valid for holonomic constraints.

I believe it is also valid for
non-holonomic constraints.

Repeating the algebraic manipulations (explained but not developed in full) shown in the previous slide, but with this
generalized linearizations instead, the linearization procedure obtained would give

Same Mass, Damping and Stiffness matrices,

and same stability. It will become obvious
that the right hand side (time) term does not
affect stability.
Linearization on Indep. Coord. (@point in the dynamics manifold)
%Determine the dependent dd and independent dz
jacobians using perm
dPhi_dd_num=dPhi_dq_num(:,perm_dd_dz(1:n_dd));
dPhi_dz_num=dPhi_dq_num(:,perm_dd_dz(n_dd+1:end));

%Determine the so called R matrix

%(different from previous R -qr algorithm- this is
a coincidence)
R=[-inv(dPhi_dd_num)*dPhi_dz_num;
eye(length(q_value)-n_dd)];
Dynamic equations can be expressed as first order equations as follows:
%Determine the Indep. Vel. Mass Damping and
stiffness matrices
M_dqdq_num=M_qq_(q_value,t_value,param_value);
M_dddzdddz_num=M_dqdq_num(perm_dd_dz,perm_dd_dz);
C_dqdq_num=C_qq_(q_value,dq_value,t_value,param_val
ue)
C_dddzdddz_num=C_dqdq_num(perm_dd_dz,perm_dd_dz);
K_dqdq_num=K_qq_(q_value,dq_value,t_value,param_val
ue)
K_dddzdddz_num=K_dqdq_num(perm_dd_dz,perm_dd_dz);

M_dzdz=R'*M_dddzdddz_num*R+...
C_dzdz=R'*C_dddzdddz_num*R+...
K_dzdz=R'*K_dddzdddz_num*R+...
Therefore:
%Assembling the eigenproblem matrix (state space
form)
A=[zeros(n_dz,n_dz),eye(n_dz);
-M_dzdz^-1*K_dzdz,-M_dzdz^-1*C_dzdz];
From which we determine stability
[V,D]=eig(A)
Direct Dynamics Problem: Stability

Multibody System Dynamics

J. Ros September 21, 2020

Dept. Engineering. Public University of Navarre (UPNA)

Stability (@dynamic equilibrium point)

Don’t confuse these with the ones related to

constraints appearing in Lagrange equations
Stability: a curious example
The figure shows a disc in a dynamic equilibrium configuration (no friction
present). The disc rotates without sliding while the plane containing the
disc keeps a constant angle with the ground. As a consequence the disc
rotates around the vertical axis. The green vector is the velocity of its
center (cst. modulus), and the red path its trajectory. Apparently the
equilibrium is stable. See video

But with our parametrization if we ask to get the eigenvalues at

this equilibrium point we obtain:

That somehow, tells us that the equilibrium is not stable.

A closer look,shows that the set of coordinates used to compute the eigenvalues are
automatically partitioned to

Meaning that the linearized motion is described in terms of . The time evolution clearly shows that x and y
evolution deviates completely from the linearization point.
We conclude that we are obtaining the eigenvalues of a dynamic system that w.r.t. the independent coordinate parameterization is
not at equilibrium. And therefore those eigenvalues do not speak about stability.
Stability: a curious example
I instead we choose the partition of coordinates, so that the angular
coordinates are the independent coordinate set,

the following eigenvalues are obtained

As expected, because there is no friction, with this parameterization the system is marginally stable.
The video and figures show a simulation with a small perturbation on the angle w.r.t the plane.

The complex part of the eigenvalues can be estimated from the periodic plots
of any of the independent velocities. It can be observed a perfect match with
the imaginary part of the first eigenvalue.
Stability: a curious example
I instead we choose the partition of coordinates, so that the angular
coordinates are the independent coordinate set,

the following eigenvalues are obtained

General conclusion:
To determine the stability we require to do linearization w.r.t a set of independent coordinates that do not change while at the
equilibrium point used for the linearization.
 Animating a mode shape
on Indep. Coord.

Don’t confuse these with the ones related to

constraints appearing in Lagrange equations

It’s rare to compute the matrix function using this formula. For different reasons,
one of them being that the matrix can become singular. Usually a Padé
approximant of the function is used to implement directly this matrix function

Valid only in no repeated eigenvalues. For the most general

case use, for example, the matrix functions:

Note that, generalized accelerations at a dynamic equilibrium point are almost

always 0, and at static equilibrium point generalized velocities are always 0. This
last condition frequently makes the last term in the equation to vanish.
Final note
This is the bare minimum you have to know.

Due to time limitations we are not presenting in these slides the basic ideas of
mechanics: forces, moments, wrenches, derivative of vectors or computations of
velocity accelerations, twists angular velocity and acceleration, base frame
reference and base change matrixes,...

These concepts and it use should be mastered by the student.

Also, we are not covering the initial assembly problem, not the perturbed
equilibrium problem, as these are trivially problems.

Most of the algorithms/ideas can be obtained also by looking at the MATLAB code
in lib_3D_MEC_MATLAB.
Numerical Linearization
END OF MATERIAL

Nonsense notes intended for teacher only use may follow...

Rotation specification in terms of the rotation
or base change matrixes

Given a desired parametrization, and a expression for the

rotation matrix in terms of it

The angular velocity can be obtained as

not a good option in a symbolic setup

preferred in a symbolic setup *

* used for of rotations defined

% Base defined using a rotation matrix in the non-rotated base in terms of general
parametrization of the rotation
newBase('xyz','BBody',R_xyz_BBody__xyz)
or base change matrix