Equilibrium
===========
.. container:: python
.. highlight:: python
Introduction
------------
This tutorial introduces the equilibrium module in Robotran. You should
already be comfortable with the basics modelling features of the
`Getting started tutorial `__
of Robotran.
.. figure:: figure/AnalysisModules_equilibrium.png
:alt: Summary of the tutorial contents
Features summary
.. container:: python
The programming language in this tutorial is **Python**. The same
tutorial for **C** is available, but not for **Matlab** at the
moment.
Equilibrium principle
~~~~~~~~~~~~~~~~~~~~~
In the case of a closed-loop multibody system, the equilibrium is
computed based on the reduced equations of motions (see Robotran
Basics). Generalized accelerations
\ :math:`\ddot{\mathbf{q}}_\mathbf{u}`\ are set to zero, the remaining
reduced force term is then a set of non-linear equations which can be
solved numerically to obtain the equilibrium configuration:
\ :math:`\mathbf{F_r}(\mathbf{q_u}, \dot{\mathbf{q}}_\mathbf{u}) = \mathbf{0}`\
This system of equations can be solved with the help of a Newton-Raphson
method. If \ :math:`\mathbf{F}`\ denotes the equations to cancel, the
Newton-Raphson algorithm iterates on the equilibrium variables
\ :math:`\mathbf{x}`\ until the equilibrium state is obtained.
\ :math:`\mathbf{x}_{k+1} = \mathbf{x}_{k} - \left(\dfrac{\partial \mathbf{F} }{ \partial \mathbf{x} } \left( \mathbf{x}_{k} \right)\right)^{-1} \mathbf{F}\left( \mathbf{x}_{k} \right)`\
The equilibrium can either be static or quasistatic depending on the
equilibrium variables considered. During an equilibrium process
variables can be ignored, exchanged or added.
Example - the cart pendulum with spring
~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~
For this tutorial, let us consider the pendulum from the “modelling
features” tutorial. The complex pendulum (with its four bar mechanism
and its spring) is mounted on a cart. The cart slides horizontally on
the ground and the pendulum rotates with respect to the cart.
.. figure:: figure/initial_2.png
:alt: Illustration for the four bar pendulum mounted on a cart
Illustration of the modified example with the four-bar pendulum
mounted on a cart
.. container:: python
To create the model use the *simpleProject-Python* template. Build
your model by first adding a prismatic joint (along the first
direction) between the base and the cart body (mass 5kg) and set it
to independent. Use the data given in the “modelling features”
tutorial to model the rest of the pendulum-spring mechanism. Finally,
add a *F sensor* on the slider (i.e. the hanging mass).
The system contains a kinematic loop implying independent and dependent
variables. There are three 3 independent variables:
\ :math:`\mathbf{q_u} = \left[ q_1,q_2,q_3 \right]`\ and 2 dependent
variables: \ :math:`\mathbf{q_v} = \left[ q_4,q_5 \right]`\
.. figure:: figure/robotran.png
:alt: *MBsysPad* Diagram for the four bar pendulum mounted on a cart
MBSysPad diagram of the four-bar pendulum mounted on a cart
..
REMARK:
- Do not forget to implement the constitutive law (spring-damper)
for the joint force (between the pendulum and the slider) but also
for the spring force (between the cart and the pendulum).
- It is possible to reuse your previous model (from the “modelling
features”) by adding the cart and its translation d.o.f. (see
section “MBsysPad 2D diagram manipulation” in “Tips and tricks”)
Static Equilibrium
------------------
For static equilibrium, the generalized velocities
\ :math:`\dot{\mathbf{q}}_{\mathbf{u}}`\ are set to zero. In that case,
the equilibrium variables are the 3 independent coordinates,
\ :math:`\mathbf{x} = \mathbf{q_u}`\ and the equilibrium equations are
the set of reduced equations,
\ :math:`\mathbf{F} = \mathbf{F}_\mathbf{r} = \mathbf{0}`\ .
REMARK:
By default, an equilibrium will be static with the free coordinates
as equilibrium variables.
Non-sensitive variables
~~~~~~~~~~~~~~~~~~~~~~~
An equilibrium variable \ :math:`x_i`\ can be non-sensitive with
respect to the equilibrium equations \ :math:`\mathbf{F}`\ . This means
that the variable \ :math:`x_i`\ does not influence the equilibrium
equations, hence its gradients is:
\ :math:`\dfrac{\partial \mathbf{F} }{ \partial x_i } = \mathbf{0}`\
The variable \ :math:`x_i`\ can be ignored in the equilibrium iteration
process. Its value can then be chosen arbitrarily by the user before the
*equilibrium* process and will not evolve.
Often the non-sensitive variables are absolute positions and/or
orientations. For example, the positions and orientation of a car in the
horizontal plane are non-sensitive and can be chosen arbitrary.
.. _example---the-cart-pendulum-with-spring-1:
Example - the cart pendulum with spring
~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~
Among the 3 free variables of the multibody system, the translation of
the cart \ :math:`q_1`\ is non-sensitive: none of the equations
\ :math:`\mathbf{F_r}=\mathbf{0}`\ are influenced by the cart position
. Its value does not depend on the equilibrium process and can then be
chosen arbitrary. However, the equilibrium state for the two remaining
variables \ :math:`q_2`\ ,\ :math:`q_3`\ has to be determined.
.. container:: python
The *equilibrium* module is based on a similar infrastructure to the
*direct dynamic* module.
- Just after the partitioning in the *main* function, execute the
static equilibrium by writing the following lines.
- Option *senstol* allows to adjust the criteria of sensitivity
(default is 1e-6). A higher value (e.g., 1e-2) might lead to
the detection of more non-sensitive variables.
- Option *verbose* displays the steps of the Newton-Raphson
algorithm (values of \ :math:`\mathbf{F}`\ and
\ :math:`\mathbf{x}`\ ).
- Results are available through the function *print_equil*.
- For more details of the parameters, we refer to the
`Documentation `__.
.. code:: python
#...
#==============================================================================
# Packages loading
#==============================================================================
try:
import MBsysPy as Robotran
except:
raise ImportError("MBsysPy not found/installed."
"See: https://www.robotran.be/download/how-to-install/"
)
#...
#==============================================================================
# Static Equilibrium
#==============================================================================
mbs_data.process = 2
mbs_data.q[mbs_data.joint_id["R2_pendulum"]] = 0.2
mbs_equil = Robotran.MbsEquil(mbs_data)
mbs_equil.set_options(save_anim = 1, senstol = 1e-6, compute_uxd = 0, resfilename = "equil1")
mbs_equil.run()
mbs_equil.print_equil()
#...
..
There you can see the printout of the equilibrium process and
results. The cart translation is detected as non-sensitive and is
kept to its zero initial value. The pendulum is angled at an angle of
about 10 degrees (0.1642 radian) due to the upper spring and the mass
suspended from the pendulum.
.. figure:: figure/static_printout.png
:alt: Printout of the computation for a static equilibrium
Printout for a static equilibrium
..
In some cases, multiple equilibria are possible. The starting
configuration provided by the user will decide which equilibrium is
reached. For example, setting the starting value of the pendulum R2
rotation to 1.0 radian will end up with a different equilibrium
configuration.
| |Eq|
| |te|
..
Remarks :
- The equilibrium process is computed based on the *mbs_data*
structure.
- If not changed after loading, the initial values for
\ :math:`\mathbf{q_u}`\ are the ones written in the *.mbs*
from *MBsysPad*.
- After an equilibrium, the structure *mbs_data* is modified
and contains the equilibrium configuration.
- For a static equilibrium performed on the generalized
coordinates (i.e. the default setting), it is possible to
visualize the iteration process of the equilibrium by loading
the file equil_q.anim with MBSysPad. Each second corresponds to
one iteration step of the Newton-Raphson Algorithm.
Exchange of equilibrium variables
---------------------------------
It is possible to replace some of the variables while performing an
equilibrium. One of the free coordinates \ :math:`q_i`\ can be replaced
by another quantity of the system denoted by \ :math:`x_{sub}`\ (e.g. a
mass, a stiffness, a length, …). The equilibrium equations remain the
reduced force term equalling zero,
\ :math:`\mathbf{F_r} = \mathbf{0}`\ . In that case, the replaced free
coordinates \ :math:`q_i`\ are kept constant during the equilibrium
process. It is important that the exchange makes sense and that the
exchange variable (i.e. the new one) is sensitive to the equilibrium
equations.
\ :math:`\mathbf{x} = \left[ \mathbf{q_u}\setminus \{ q_i \} , x_{sub} \right]`\
\ :math:`\mathbf{F} = \mathbf{F_r} = \mathbf{0}`\ such that the
pendulum is vertical
Remarks:
The exchange is not limited to one variable but can be applied to
several variables at the same time
.. _example---the-cart-pendulum-with-spring-2:
Example - the cart pendulum with spring
~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~
If we come back to the cart pendulum model, different variable exchanges
can be performed. Here are some examples:
- The elongation of the lower spring \ :math:`q_3`\ can be set to a
desired length and replaced by the spring stiffness \ :math:`k`\ or
its free length \ :math:`z_0`\ .
- The pendulum angular position can be set and replaced by
- the upper spring stiffness \ :math:`K`\ or its free length
\ :math:`Z_0`\
- the joint torque applied along the pendulum \ :math:`Q_2`\ or the
crank rotational axis \ :math:`Q_4`\ .
- The crank angular position can be set and replaced by
- the length of the rod
- the distance between the pendulum the crank attachment point with
the cart.
..
Remarks:
The exchange and the desired equilibrium configuration has to make
sense otherwise the equilibrium results could be inconsistent from a
physical point of view (negative mass or negative free length for a
spring)
In this tutorial, The exchange consists of setting the angular position
of the pendulum to 0 radian and replacing it by the upper spring free
length. The algorithm will then determine the free length and the other
variables values (i.e. cart position \ :math:`q_1`\ and the lower
spring elongation \ :math:`q_3`\ ) such that the pendulum is vertical.
..
.. container:: python
- Some additional lines of code are necessary in the *equilibrium*
process:
- The pendulum rotation is set to zero in the *mbs_data*
structure.
- Function *add_exchange_um* adds a user model as a new
equilibrium variable that replaces the specified default one.
- For more details of the parameters, we refer to the
`Documentation `__.
.. code:: python
#...
#==============================================================================
# Static Equilibrium
#==============================================================================
mbs_data.reset()
mbs_data.set_qa(mbs_data.joint_id["R2_pendulum"])
mbs_data.q[mbs_data.joint_id["R2_pendulum"]] = 0.0
mbs_equil = Robotran.MbsEquil(mbs_data)
mbs_equil.add_exchange_um("mylink","Z0",mbs_data.joint_id["R2_pendulum"])
mbs_equil.set_options(senstol = 1e-6, compute_uxd = 0, resfilename = "equil3")
mbs_equil.run()
mbs_equil.print_equil()
mbs_data.unset_qa(mbs_data.joint_id["R2_pendulum"])
#...
There you can see the printout of the equilibrium results. The cart
position is again detected as non-sensitive and is kept to its zero
initial value. The pendulum is angled at 0 radian (print mbs_data to
verify) and the free length of the upper spring \ :math:`x_{2}`\ is
found to be much longer it was than initially (the spring is now working
in compression while working in extension initially). The slider
elongation is slightly larger than initially.
.. figure:: figure/exchange_printout.png
:alt: Printout of the equilibrium computation with a variable exchange
Printout for an equilibrium with exchange
Extra Equilibrium variables
---------------------------
Another feature of the *equilibrium* module is the addition of
equilibrium variables and equations. One equation of equilibrium
\ :math:`F_{e} = 0`\ can be added to the initial equations
\ :math:`\mathbf{F_r} = \mathbf{0}`\ as far as one equilibrium variable
\ :math:`x_{e}`\ is also added. This added variable has to be sensitive
with respect to the added equation.
\ :math:`\mathbf{x} = \left[ \mathbf{q_u} , x_{e} \right]`\
\ :math:`\mathbf{F} = \left[ \mathbf{F_r} , F_e \right] = \mathbf{0}`\
Remarks:
The addition is not limited to one equation and one variable, several
pairs of those can be added at the same time.
Such features can be practical for adjustments of any kind (geometry, …
) which would imply complex relationship between the variables. For
example, the front wheels alignement for a car can be adjusted by
varying the steering rod length. The 2 added equations are used to
express that the toe of the wheels has to be equal to a given value. The
2 added variables are the steering rods lengths.
.. _example---the-cart-pendulum-with-spring-3:
Example - the cart pendulum with spring
~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~
If we come back to the cart pendulum example, several equation additions
can be performed. Let us assume that the slider needs to be horizontally
offset of 0.2 meter from the pendulum anchor point on the cart. This
condition can be written as an additional equilibrium equation.
.. container:: python
- The equation has to be written by the user in the function
*user_equil_fxe* (in *user_equil.py*, the dedicated user file for
equilibrium).
.. code:: python
#...
def user_equil_fxe(mbs_data, f):
f[1] = np.sin(mbs_data.q[mbs_data.joint_id["R2_pendulum"]])*(mbs_data.dpt[3][3]+ mbs_data.q[mbs_data.joint_id["T3_slider"]]) - 0.4
return
..
Remarks:
The anchor point of the lower spring is hardcoded (0.5) but it
could be retrieved from the *mbs_data* structure using the anchor
point ID.
There are several ways to ensure a given horizontal position of the
slider. One possible way is to apply a joint torque \ :math:`Q_{2}`\
between the cart and the pendulum.
- Some additional lines of code are necessary in the *equilibrium*
process:
- Function *add_extra_variable* adds a new equilibrium variable
which is defined the variable contained in the array *ptr* at
index *index* and renamed by the user if wanted.
- For more details of the parameters, we refer to the
`Documentation `__.
.. code:: python
#...
#==============================================================================
# Static Equilibrium
#==============================================================================
mbs_data.reset()
mbs_data.set_qa(mbs_data.joint_id["R2_pendulum"])
mbs_equil = Robotran.MbsEquil(mbs_data)
mbs_equil.add_extra_variable(mbs_data.Qq,mbs_data.joint_id["R2_pendulum"],"Qa")
mbs_equil.set_options(senstol = 1e-6, compute_uxd = 0)
mbs_equil.set_options(grad_lpk = 1)
mbs_equil.run()
mbs_equil.print_equil()
mbs_data.unset_qa(mbs_data.joint_id["R2_pendulum"])
#...
..
Remarks:
- Each added equilibrium equation corresponds to an added
equilibrium variable.
- It is also possible to replace an existing variable by the new
equilibrium variable with the function
*replace_extra_variable*.
There you can see the printout of the equilibrium results. There are
4 equilibrium variables. The pendulum orientation \ :math:`x_{2}`\
is positive such that it is angled to the right. The 4th equilibrium
variable \ :math:`x_{4}`\ corresponds to a positive joint torque
\ :math:`Q_{2}`\ which acts against gravity and the upper spring to
position the slider to the given horizontal position. From the
*mbs_data* structure, you can verify the vertical position of the
slider (combining \ :math:`q_{2}`\ and \ :math:`q_{3}`\ as in the
user function)
.. figure:: figure/extra_printout.png
:alt: Printout of the equilibrium computation with a variable addition
Printout for a static equilibrium
Quasistatic Equilibrium
-----------------------
A quasistatic equilibrium is a special type of equilibrium such that
there is one joint *i* with a non-zero velocity \ :math:`\dot{q}_i`\ .
Forces (or torques) \ :math:`F_{qstc}`\ act against resistance forces
(often friction) to maintain the vehicle in a continuous constant
motion. There are two ways to compute the equilibrium:
- Fix the speed and determine the forces (or torques) necessary to
maintain this speed.
- Fix the forces (or torques) applied on the system and determine the
resulting speed.
Computing those equilibrium utilizes the notion of non-sensitive
variable and exchange of variables. In quasistatic equilibrium, the
position \ :math:`q_i`\ associated with the non-zero speed is not
relevant and can then be considered non-sensitive. This variable can be
removed from the set of equilibrium variables and exchanged for a
sensitive variable. Depending on which approach is chosen, the exchange
variable is the force or the speed \ :math:`\dot{q}_i`\
- Fixed speed
\ :math:`\dot{q}_i \rightarrow \mathbf{x} = \left[ \mathbf{q_u}\setminus \{ q_i \} , F_{qstc} \right] \phantom{.....} \mathbf{F}(\mathbf{x})=\mathbf{0}`\
- Fixed force
\ :math:`F_{qstc} \rightarrow \mathbf{x} = \left[ \mathbf{q_u}\setminus \{ q_i \} , \dot{q}_i \right] \phantom{.....} \mathbf{F}(\mathbf{x})=\mathbf{0}`\
It is important to notice that the exchange only concerns the joint *i*
and that the other free independent variables from
\ :math:`\mathbf{q_u}`\ are still part of the equilibrium process.
Remarks:
The quasistatic force \ :math:`F_{qstc}`\ can be of different
natures, it can either be a joint force or an external force.
The main application of quasistatic equilibrium is in vehicule dynamics
where equilibrium can be performed in straight line and in curves.
Multiple wheels does however require good understanding of the module
features: for a fixed speed, both forces and speeds need to be exchanged
in the process.
.. _example---the-cart-pendulum-with-spring-4:
Example - the cart pendulum with spring
~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~
So far, the cart translation has not been subjected to any calculation
as it was non-sensitive. A quasistatic equilibrium can be performed for
the cart translation speed but it will make sense only if there is a
resistive force (i.e. aerodynamic drag) . In this example, the
quasistatic force \ :math:`F_{qstc}`\ can simply be a joint force
\ :math:`Q_{1}`\ applied between the ground and the cart (i.e. a linear
actuator).
- The first step is to implement an aerodynamic drag force on the
slider.
- Add a *Force sensor* on the slider, name it and regenerate the
equations with *MBSysPad*
- Implement the aerodynamic force as a function of the slider angle
\ :math:`\theta`\ of attack and its horizontal velocity
\ :math:`v`\ :
\ :math:`F_{aero} = - a v^2 \left(b - \sin(2\theta)(b-c) \right)`\
.. container:: python
.. code:: python
import numpy as np
def user_ExtForces(PxF, RxF, VxF, OMxF, AxF, OMPxF, mbs_data, tsim, ixF):
#...
#==============================================================================
# BEGIN OF USER CODE
#==============================================================================
# get the force id
F1 = mbs_data.extforce_id['aero_force']
if ixF == F1:
id = mbs_data.joint_id['R2_pendulum']
a = 0.5
b = 1.05
c = 0.80
v = VxF[1]
Fx = -a*v*v*(b-np.sin(mbs_data.q[id]*2)*(b-c))
# if PxF[1] > gap:
# Fx = -10000*(PxF[1]-gap)
#...
#==============================================================================
# END OF USER CODE
#==============================================================================
Swr=np.zeros(9+1)
Swr[1:]=np.r_[Fx,Fy,Fz,Mx,My,Mz,dxF] #concatenation...
return Swr
..
Remarks:
The external force is placed on the slider body rather than on the
pendulum (as it was the case for the modelling features tutorial)
..
- To perform the quasistatic equilibrium, an exchange must be
performed in the *main.py* file
- Option *mode* has to be set on 2 (or 3 if you also have
non-zero accelerations).
- The desired speed for the cart is set to 5 m/s.
- Function *add_exchange* adds a new equilibrium variable that
replaces the specified one.
- For more details of the parameters, we refer to the
`Documentation `__.
.. code:: c
..
#...
#==============================================================================
# Static Equilibrium
#==============================================================================
#.. a static equilibrium to perform beforehand and get good initial conditions for the qstc equilibrium
#==============================================================================
# Quasistatic Equilibrium
#==============================================================================
mbs_data.reset()
mbs_data.set_qa(mbs_data.joint_id["T1_cart"])
mbs_equil = Robotran.MbsEquil(mbs_data)
mbs_data.qd[mbs_data.joint_id["T1_cart"]] = 5
mbs_equil.set_options(senstol = 1e-6, compute_uxd = 0, resfilename = "equil7")
mbs_equil.set_options(verbose = 1, grad_lpk = 1, mode = 2)
mbs_equil.add_exchange(mbs_data.Qa,mbs_data.joint_id["T1_cart"], mbs_data.joint_id["T1_cart"],"Qa")
mbs_equil.run()
mbs_equil.print_equil()
mbs_data.unset_qa(mbs_data.joint_id["T1_cart"])
#...
..
There you can see the printout of the quasistatic equilibrium. The
non-sensitive position of the cart \ :math:`x_{1}`\ is replaced by
the active joint force \ :math:`Q_{1}`\ on the cart necessary to
balance the aerodynamic drag. Note that if you are not comfortable
with using the variable Qa, the active joint force can also be
defined in the *user_JointForces* function with the help of a user
model variable. The pendulum is slightly angled on the left with a
negative value for \ :math:`x_{2}`\ . The lower spring elongation is
about 5 cm larger than initially because of the aerodynamic drag.
Starting a direct dynamic simulation will correspond to a constant
speed displacement of the overall system.
.. figure:: figure/qstc_printout.png
:alt: Printout of the computation for a quasistatic equilibrium
Printout for a quasistatic equilibrium
..
Remarks:
Limit yourself to speed less than 10 m/s for the cart. Otherwise,
equilibrium might not converge or encounter undesired solution
(reverse four-bar configuration)
Qa stands for actuated torques/forces, while Qq is the joint
torques/forces.
.. |Eq| image:: figure/static_conf_1.png
.. |te| image:: figure/static_conf_2.png