35 lines
840 B
Mathematica
35 lines
840 B
Mathematica
|
function [J] = getJacobianNanoHexapod(Hbm)
|
||
|
|
% getJacobianNanoHexapod -
|
||
|
|
%
|
||
|
|
% Syntax: [J] = getJacobianNanoHexapod(Hbm)
|
||
|
|
%
|
||
|
|
% Inputs:
|
||
|
|
% - Hbm - Height of {B} w.r.t. {M} [m]
|
||
|
|
%
|
||
|
|
% Outputs:
|
||
|
|
% - J - Jacobian Matrix
|
||
|
|
|
||
|
|
Fa = [[-86.05, -74.78, 22.49],
|
||
|
|
[ 86.05, -74.78, 22.49],
|
||
|
|
[ 107.79, -37.13, 22.49],
|
||
|
|
[ 21.74, 111.91, 22.49],
|
||
|
|
[-21.74, 111.91, 22.49],
|
||
|
|
[-107.79, -37.13, 22.49]]'*1e-3; % Ai w.r.t. {F} [m]
|
||
|
|
|
||
|
|
Mb = [[-28.47, -106.25, -22.50],
|
||
|
|
[ 28.47, -106.25, -22.50],
|
||
|
|
[ 106.25, 28.47, -22.50],
|
||
|
|
[ 77.78, 77.78, -22.50],
|
||
|
|
[-77.78, 77.78, -22.50],
|
||
|
|
[-106.25, 28.47, -22.50]]'*1e-3; % Bi w.r.t. {M} [m]
|
||
|
|
|
||
|
|
H = 95e-3; % Stewart platform height [m]
|
||
|
|
Fb = Mb + [0; 0; H]; % Bi w.r.t. {F} [m]
|
||
|
|
|
||
|
|
si = Fb - Fa;
|
||
|
|
si = si./vecnorm(si); % Normalize
|
||
|
|
|
||
|
|
Bb = Mb - [0; 0; Hbm];
|
||
|
|
|
||
|
|
J = [si', cross(Bb, si)'];
|