Jacobian Matrix done

This commit is contained in:
Unknown 2018-05-03 14:01:42 +02:00
parent 4e04250feb
commit 20d3abe320
4 changed files with 41 additions and 10 deletions

View File

@ -1,5 +1,7 @@
%% Nass height %% Nass height
Nass.h = 90 ; %mm Nass = struct();
Nass.h = 90; %mm
Nass.jacobian = 174.5; %mm
%% Bottom Plate %% Bottom Plate
BP = struct(); BP = struct();

View File

@ -1,7 +1,6 @@
%% %%
clear; clear;
clc; clc;
%% %%
run Design_Nass.m run Design_Nass.m
@ -20,7 +19,7 @@ alpha_t = TP.leg.ang*deg2rad; % +- offset angle from 120 degree spacing on top
height = (Nass.h-BP.thickness-TP.thickness-Leg.sphere.bottom-Leg.sphere.top-SP.thickness.bottom-SP.thickness.top)*0.001 ; % 2 meter height in home configuration height = (Nass.h-BP.thickness-TP.thickness-Leg.sphere.bottom-Leg.sphere.top-SP.thickness.bottom-SP.thickness.top)*0.001 ; % 2 meter height in home configuration
radius_b = BP.leg.rad*0.001; % rayon emplacement support base radius_b = BP.leg.rad*0.001; % rayon emplacement support base
radius_t = TP.leg.rad*0.001; % top radius in meters radius_t = TP.leg.rad*0.001; % top radius in meters
for i = 1:3, for i = 1:3
% base points % base points
angle_m_b = (2*pi/3)* (i-1) - alpha_b; angle_m_b = (2*pi/3)* (i-1) - alpha_b;
angle_p_b = (2*pi/3)* (i-1) + alpha_b; angle_p_b = (2*pi/3)* (i-1) + alpha_b;
@ -44,7 +43,7 @@ body_pts = pos_top' - height*[zeros(2,6);ones(1,6)];
legs = pos_top - pos_base; legs = pos_top - pos_base;
leg_length = [ ]; leg_length = [ ];
leg_vectors = [ ]; leg_vectors = [ ];
for i = 1:6, for i = 1:6
leg_length(i) = norm(legs(i,:)); leg_length(i) = norm(legs(i,:));
leg_vectors(i,:) = legs(i,:) / leg_length(i); leg_vectors(i,:) = legs(i,:) / leg_length(i);
end end
@ -52,7 +51,7 @@ end
Leg.lenght = 1000*leg_length(1)/1.5; Leg.lenght = 1000*leg_length(1)/1.5;
% Calculate revolute and cylindrical axes % Calculate revolute and cylindrical axes
for i = 1:6, for i = 1:6
rev1(i,:) = cross(leg_vectors(i,:), z_axis); rev1(i,:) = cross(leg_vectors(i,:), z_axis);
rev1(i,:) = rev1(i,:) / norm(rev1(i,:)); rev1(i,:) = rev1(i,:) / norm(rev1(i,:));
rev2(i,:) = - cross(rev1(i,:), leg_vectors(i,:)); rev2(i,:) = - cross(rev1(i,:), leg_vectors(i,:));
@ -67,7 +66,7 @@ end
lower_leg = struct('origin', [0 0 0], 'rotation', eye(3), 'end_point', [0 0 0]); lower_leg = struct('origin', [0 0 0], 'rotation', eye(3), 'end_point', [0 0 0]);
upper_leg = struct('origin', [0 0 0], 'rotation', eye(3), 'end_point', [0 0 0]); upper_leg = struct('origin', [0 0 0], 'rotation', eye(3), 'end_point', [0 0 0]);
for i = 1:6, for i = 1:6
lower_leg(i).origin = pos_base(i,:) + (3/8)*legs(i,:); lower_leg(i).origin = pos_base(i,:) + (3/8)*legs(i,:);
lower_leg(i).end_point = pos_base(i,:) + (3/4)*legs(i,:); lower_leg(i).end_point = pos_base(i,:) + (3/4)*legs(i,:);
lower_leg(i).rotation = [rev1(i,:)', rev2(i,:)', cyl1(i,:)']; lower_leg(i).rotation = [rev1(i,:)', rev2(i,:)', cyl1(i,:)'];
@ -75,3 +74,6 @@ for i = 1:6,
upper_leg(i).end_point = pos_base(i,:) + (1/4)*legs(i,:); upper_leg(i).end_point = pos_base(i,:) + (1/4)*legs(i,:);
upper_leg(i).rotation = [rev1(i,:)', rev2(i,:)', cyl1(i,:)']; upper_leg(i).rotation = [rev1(i,:)', rev2(i,:)', cyl1(i,:)'];
end end
%%
run JacobianMatrix.m

27
JacobianMatrix.m Normal file
View File

@ -0,0 +1,27 @@
%% Position Matrix
M_pos_base = pos_base + (height+(TP.thickness+Leg.sphere.top+SP.thickness.top+Nass.jacobian)*1e-3)*[zeros(6, 2),ones(6, 1)];
%% Rotation Matrix
RM = leg_vectors;
%%
J = computeJacobian(RM, M_pos_base);
%% Jacobian Matrix
function J = computeJacobian(RM,M_pos_base)
J = zeros(6);
J(:, 1:3) = RM;
for i = 1:6
J(i, 4:6) = -RM(i, :)*getCrossProductMatrix(M_pos_base(i, :));
end
function M = getCrossProductMatrix(v)
M = zeros(3);
M(1, 2) = -v(3);
M(1, 3) = v(2);
M(2, 3) = -v(1);
M(2, 1) = -M(1, 2);
M(3, 1) = -M(1, 3);
M(3, 2) = -M(2, 3);
end
end

Binary file not shown.