%FORWARDKINEMATICS Forward Kinematics % Function for calculating forward kinematics of a 2 link planar arm % % Syntax: [J1, J2] = ForwardKinematics(J0, l1, l2, theta1, theta2) % % Inputs: % J0 - 2-vector giving x y coords of the robot base % l1 - length of link 1 % l2 - length of link 2 % theta1 - angle of first joint % theta2 - angle of second joint relative to first link % % Outputs: % J1 - x y coords of the end of link 1 % J2 - x y coords of the end of link 2 (end-effector) % Author: Travis Hydzik % Last revision: 20 October 2004 function [J1, J2] = ForwardKinematics(J0, l1, l2, theta1, theta2) theta2 = theta1 + theta2; J1 = J0 + [l1*cos(theta1); l1*sin(theta1)]; J2 = J1 + [l2*cos(theta2); l2*sin(theta2)];