Add conf_simscape loading and tangled one function
This commit is contained in:
parent
de76cc5812
commit
cf3b729ba7
@ -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
|
||||
|
31
src/forwardKinematicsApprox.m
Normal file
31
src/forwardKinematicsApprox.m
Normal file
@ -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)];
|
Loading…
Reference in New Issue
Block a user