21 lines
932 B
Mathematica
21 lines
932 B
Mathematica
|
function [L] = inverseKinematicsHexapod(hexapod, AP, ARB)
|
||
|
|
% inverseKinematicsHexapod - Compute the initial position of each leg to have the wanted Hexapod's position
|
||
|
|
%
|
||
|
|
% Syntax: inverseKinematicsHexapod(hexapod, AP, ARB)
|
||
|
|
%
|
||
|
|
% Inputs:
|
||
|
|
% - hexapod - Hexapod object containing the geometry of the hexapod
|
||
|
|
% - AP - Position vector of point OB expressed in frame {A} in [m]
|
||
|
|
% - ARB - Rotation Matrix expressed in frame {A}
|
||
|
|
|
||
|
|
% Wanted Length of the hexapod's legs [m]
|
||
|
|
L = zeros(6, 1);
|
||
|
|
|
||
|
|
for i = 1:length(L)
|
||
|
|
Bbi = hexapod.pos_top_tranform(i, :)' - 1e-3*[0 ; 0 ; hexapod.TP.thickness+hexapod.Leg.sphere.top+hexapod.SP.thickness.top+hexapod.jacobian]; % [m]
|
||
|
|
Aai = hexapod.pos_base(i, :)' + 1e-3*[0 ; 0 ; hexapod.BP.thickness+hexapod.Leg.sphere.bottom+hexapod.SP.thickness.bottom-hexapod.h-hexapod.jacobian]; % [m]
|
||
|
|
|
||
|
|
L(i) = sqrt(AP'*AP + Bbi'*Bbi + Aai'*Aai - 2*AP'*Aai + 2*AP'*(ARB*Bbi) - 2*(ARB*Bbi)'*Aai);
|
||
|
|
end
|
||
|
|
end
|