From cf3b729ba7df918c6fffc512f4900e3e45dfccc3 Mon Sep 17 00:00:00 2001
From: Thomas Dehaeze <dehaeze.thomas@gmail.com>
Date: Wed, 22 Jan 2020 13:26:58 +0100
Subject: [PATCH] Add conf_simscape loading and tangled one function

---
 control-study.org             |  9 ++++++++-
 src/forwardKinematicsApprox.m | 31 +++++++++++++++++++++++++++++++
 2 files changed, 39 insertions(+), 1 deletion(-)
 create mode 100644 src/forwardKinematicsApprox.m

diff --git a/control-study.org b/control-study.org
index 26082da..4001a58 100644
--- a/control-study.org
+++ b/control-study.org
@@ -44,6 +44,7 @@
 
 #+begin_src matlab
   addpath('./src/')
+  addpath('./simulink/')
 #+end_src
 
 ** Control Schematic
@@ -70,12 +71,18 @@
 ** Initialize the Stewart platform
 #+begin_src matlab
   stewart = initializeFramesPositions('H', 90e-3, 'MO_B', 45e-3);
-  stewart = generateCubicConfiguration(stewart, 'Hc', 60e-3, 'FOc', 45e-3, 'FHa', 5e-3, 'MHb', 5e-3);
+  % stewart = generateCubicConfiguration(stewart, 'Hc', 60e-3, 'FOc', 45e-3, 'FHa', 5e-3, 'MHb', 5e-3);
+  stewart = generateGeneralConfiguration(stewart);
   stewart = computeJointsPose(stewart);
   stewart = initializeStrutDynamics(stewart, 'Ki', 1e6*ones(6,1), 'Ci', 1e2*ones(6,1));
   stewart = computeJacobian(stewart);
 #+end_src
 
+** Initialize the Simulation
+#+begin_src matlab
+  load('mat/conf_simscape.mat');
+#+end_src
+
 ** Identification of the plant
 Let's identify the transfer function from $\bm{\tau}}$ to $\bm{L}$.
 #+begin_src matlab
diff --git a/src/forwardKinematicsApprox.m b/src/forwardKinematicsApprox.m
new file mode 100644
index 0000000..942b92a
--- /dev/null
+++ b/src/forwardKinematicsApprox.m
@@ -0,0 +1,31 @@
+function [P, R] = forwardKinematicsApprox(stewart, args)
+% forwardKinematicsApprox - Computed the approximate pose of {B} with respect to {A} from the length of each strut and using
+%                           the Jacobian Matrix
+%
+% Syntax: [P, R] = forwardKinematicsApprox(stewart, args)
+%
+% Inputs:
+%    - stewart - A structure with the following fields
+%        - J  [6x6] - The Jacobian Matrix
+%    - args - Can have the following fields:
+%        - dL [6x1] - Displacement of each strut [m]
+%
+% Outputs:
+%    - P  [3x1] - The estimated position of {B} with respect to {A}
+%    - R  [3x3] - The estimated rotation matrix that gives the orientation of {B} with respect to {A}
+
+arguments
+    stewart
+    args.dL (6,1) double {mustBeNumeric} = zeros(6,1)
+end
+
+X = stewart.J\args.dL;
+
+P = X(1:3);
+
+theta = norm(X(4:6));
+s = X(4:6)/theta;
+
+R = [s(1)^2*(1-cos(theta)) + cos(theta) ,        s(1)*s(2)*(1-cos(theta)) - s(3)*sin(theta), s(1)*s(3)*(1-cos(theta)) + s(2)*sin(theta);
+     s(2)*s(1)*(1-cos(theta)) + s(3)*sin(theta), s(2)^2*(1-cos(theta)) + cos(theta),         s(2)*s(3)*(1-cos(theta)) - s(1)*sin(theta);
+     s(3)*s(1)*(1-cos(theta)) - s(2)*sin(theta), s(3)*s(2)*(1-cos(theta)) + s(1)*sin(theta), s(3)^2*(1-cos(theta)) + cos(theta)];