27 lines
878 B
Matlab
27 lines
878 B
Matlab
function [x, y] = MyFK(L1, L2, Q1, Q2)
|
|
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
|
|
%function [q1 q2 q1_ q2_] = MyFK(L1, L2, Q1, Q2)
|
|
%
|
|
% Task: returns the x y coordinates in the cartesian space
|
|
% from a given point in the q1 q2 joint space
|
|
% using geometry rules since we know that we have two links only of length L1 and L2
|
|
% computation way faster than the method with matrices
|
|
%
|
|
% Inputs:
|
|
%-L1: length of the first link of the 2d planar 2 link robot arm
|
|
%-L2: length of the second link of the 2d planar 2 link robot arm
|
|
%-q1: q1 coordinate of the given point
|
|
%-q2: q2 coordinate of the given point
|
|
%
|
|
% Outputs:
|
|
%-x: x coordinate of the given point
|
|
%-y: x coordinate of the given point
|
|
%
|
|
%
|
|
% Thomas OLIVE (thomas.olive@ecam.fr)
|
|
% 18/12/2021
|
|
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
|
|
|
|
x = L1*cosd(Q1) + L2*cosd(Q1+Q2);
|
|
y = L1*sind(Q1) + L2*sind(Q1+Q2);
|
|
endfunction |