#+TITLE: Gravity with Simscape Models
:DRAWER:
#+STARTUP: overview

#+LANGUAGE: en
#+EMAIL: dehaeze.thomas@gmail.com
#+AUTHOR: Dehaeze Thomas

#+HTML_LINK_HOME: ../index.html
#+HTML_LINK_UP:   ../index.html

#+HTML_HEAD: <link rel="stylesheet" type="text/css" href="https://research.tdehaeze.xyz/css/style.css"/>
#+HTML_HEAD: <script type="text/javascript" src="https://research.tdehaeze.xyz/js/script.js"></script>

#+HTML_MATHJAX: align: center tagside: right font: TeX

#+PROPERTY: header-args:matlab  :session *MATLAB*
#+PROPERTY: header-args:matlab+ :comments org
#+PROPERTY: header-args:matlab+ :results none
#+PROPERTY: header-args:matlab+ :exports org
#+PROPERTY: header-args:matlab+ :tangle matlab/simscape_gravity.m
#+PROPERTY: header-args:matlab+ :eval no-export
#+PROPERTY: header-args:matlab+ :output-dir figs
#+PROPERTY: header-args:matlab+ :mkdirp yes
:END:

* Introduction                                                        :ignore:
We would like to include gravity in a Simscape multi-body model while starting the simulation at the equilibrium.

The basic idea is to first perform a simulation of the system until it goes at it rest position and save the rest position forces and deflection in each joint.

We can then think of three solutions to start a new simulation directly at the steady state position:
- Change the equilibrium position $l_0$ of each joint such that at $t=0$, $k \times l_0$ is equal to the previously measured force in the joint.
  Here, it is suppose that we constrain the initial position of the joint to be equal to zero.
  Then, these forces cancel each other and the simulation starts at equilibrium.
- Add an External force (or maybe the "Internal Force") in the direction of the joint (the "resolution frame" is fixed to the solids) equal to the opposite of the previously measured force.
- After the first simulation, measure the final change of position of each joint, then set the initial position to be the measured static position.
  Then, the simulation will start at equilibrium, but all the elements will be "deflected".

The three solutions are tested below.

* Matlab Init                                                :noexport:ignore:
#+begin_src matlab :tangle no :exports none :results silent :noweb yes :var current_dir=(file-name-directory buffer-file-name)
  <<matlab-dir>>
#+end_src

#+begin_src matlab :exports none :results silent :noweb yes
  <<matlab-init>>
#+end_src

#+begin_src matlab :tangle no
  addpath('./matlab');
#+end_src

* Simulink
#+begin_src matlab
  open 'gravity_test.slx'
#+end_src

#+name: fig:simscape_model
#+caption: Simscape model used for the simulations
[[file:figs/simscape_model.png]]

* Initial Simulation
#+begin_src matlab
  g = -10; % [m/s^2]

  k = 1e3; % Stiffness [N/m]
  c = 10; % Damping [N/(m/s)]

  l0 = 0; % Initial wanted position [m]
  leq = 0; % equilibrium position [m]

  F_ext = 0; % External force [N]
  F_act = 0; % Actuator force [N]
#+end_src

#+begin_src matlab :exports none
  out_init = sim('gravity_test.slx')

  out_init.d.Name = 'Displacement';
  out_init.Fm.Name = 'Force Sensor';
#+end_src

#+begin_src matlab :exports none
  figure;
  subplot(1,2,1)
  plot(out_init.d)
  title('');
  subplot(1,2,2)
  plot(out_init.Fm)
  title('');
#+end_src

#+HEADER: :tangle no :exports results :results none :noweb yes
#+begin_src matlab :var filepath="figs/sim_init.pdf" :var figsize="full-tall" :post pdf2svg(file=*this*, ext="png")
<<plt-matlab>>
#+end_src

#+NAME: fig:sim_init
#+CAPTION: Initial Simulation ([[./figs/sim_init.png][png]], [[./figs/sim_init.pdf][pdf]])
[[file:figs/sim_init.png]]

* Change the equilibrium position
#+begin_src matlab
  l0 = 0; % Initial wanted position [m]
  leq = -out_init.Fm.Data(end)/k; % equilibrium position [m]

  F_ext = 0; % External force [N]
  F_act = 0; % Actuator force [N]
#+end_src

#+begin_src matlab :exports none
  out = sim('gravity_test.slx');

  out.d.Name = 'Displacement';
  out.Fm.Name = 'Force Sensor';
#+end_src

#+begin_src matlab :exports none
  figure;
  subplot(1,2,1)
  plot(out.d)
  title('');
  subplot(1,2,2)
  plot(out.Fm)
  title('');
#+end_src

#+HEADER: :tangle no :exports results :results none :noweb yes
#+begin_src matlab :var filepath="figs/sim_change_eq_position.pdf" :var figsize="full-tall" :post pdf2svg(file=*this*, ext="png")
<<plt-matlab>>
#+end_src

#+NAME: fig:sim_change_eq_position
#+CAPTION: Simulation after changing the equilibrium position of the joint ([[./figs/sim_change_eq_position.png][png]], [[./figs/sim_change_eq_position.pdf][pdf]])
[[file:figs/sim_change_eq_position.png]]

* Add external force
#+begin_src matlab
  l0 = 0; % Initial wanted position [m]
  leq = 0; % equilibrium position [m]

  F_ext = -out_init.Fm.Data(end); % External force [N]
  F_act = 0; % Actuator force [N]
#+end_src

#+begin_src matlab :exports none
  out = sim('gravity_test.slx');

  out.d.Name = 'Displacement';
  out.Fm.Name = 'Force Sensor';
#+end_src

#+begin_src matlab :exports none
  figure;
  subplot(1,2,1)
  plot(out.d)
  title('');
  subplot(1,2,2)
  plot(out.Fm)
  title('');
#+end_src

#+HEADER: :tangle no :exports results :results none :noweb yes
#+begin_src matlab :var filepath="figs/sim_add_external_force.pdf" :var figsize="full-tall" :post pdf2svg(file=*this*, ext="png")
<<plt-matlab>>
#+end_src

#+NAME: fig:sim_add_external_force
#+CAPTION: Simulater after adding an external force applied to the solid ([[./figs/sim_add_external_force.png][png]], [[./figs/sim_add_external_force.pdf][pdf]])
[[file:figs/sim_add_external_force.png]]
* Change initial position
#+begin_src matlab
  l0 = out_init.d.Data(end); % Initial wanted position [m]
  leq = 0; % equilibrium position [m]

  F_ext = 0; % External force [N]
  F_act = 0; % Actuator force [N]
#+end_src

#+begin_src matlab :exports none
  out = sim('gravity_test.slx');

  out.d.Name = 'Displacement';
  out.Fm.Name = 'Force Sensor';
#+end_src

#+begin_src matlab :exports none
  figure;
  subplot(1,2,1)
  plot(out.d)
  title('');
  subplot(1,2,2)
  plot(out.Fm)
  title('');
#+end_src

#+HEADER: :tangle no :exports results :results none :noweb yes
#+begin_src matlab :var filepath="figs/sim_change_initial_position.pdf" :var figsize="full-tall" :post pdf2svg(file=*this*, ext="png")
<<plt-matlab>>
#+end_src

#+NAME: fig:sim_change_initial_position
#+CAPTION: Simulation after changing the initial position of the joint ([[./figs/sim_change_initial_position.png][png]], [[./figs/sim_change_initial_position.pdf][pdf]])
[[file:figs/sim_change_initial_position.png]]
* Conclusion
Three techniques:
- Change the equilibrium position
  - This does not change the equilibrium position of the system
- Add external force
  - May not be physical
- Change the initial position
  - The static position is not the same as the static position without gravity
  - Very easy to implement

#+begin_important
  Changing the equilibrium position of each joint seem the most practical solution.
#+end_important