Homework #0 Solution: 2R Serial-chain Robot: Forward and Inverse Kinematics

Problem 1: Write a Matlab function

RR_ForwardPosKin(a,theta) that returns disp. Test this by inputting 1 =t , 2 = 20 sin(3t+/3) and plot the resulting Cartesian trajectory x vs t, y vs t (as t varies smoothly from 0 to 360 degrees).

MAE 593: Math Methods in Robotics, Fall 2011

Figure 1

Solution:
We have x = L1 cos 1+L2 cos (1+2) y = L1 sin 1+L2 sin (1+2) where L1 and L2 are the Length of the links 1 and 2 are the angles as shown in fig 1

So we define a function that returns the value of X and Y using the above formula taking link lengths and angles as input and give us the required position o. Testing the Solution: We test our code by passing an array of inputs following the relation 1 =t , 2 = 20 sin(3t+/3) for a particular link length and then plot the position vs time for each coordinates. Implementation:
% Description : This function returns end effector position of a 2 link % robot arm once link length (matrix a) and joint angle(relative) (matrix theta) % are specified. function [x,y] = RR_ForwardPosKin(a,theta) x = a(1)*cos(theta(1)*pi/180)+a(2)*cos((theta(1)+theta(2))*pi/180); % calculates x position of end effector y = a(1)*sin(theta(1)*pi/180)+a(2)*sin((theta(1)+theta(2))*pi/180);% calculates y position of end effector end

Matlab expects all its angles to be in radians for input to sin and cos function. Hence all the degree values have been converted to radians.

% Description : This m file plots xe and ye position of end effector for % different values of t with a 0.5 variation in values of T from 0 to 360 a = [3,2]; % Link Lengths xe = null(1); % initialize array xe ye = null(1); % initialize array ye for t = 0:0.5:360 % varying t in steps of 0.5 to generate smooth curve theta = [t,20*sin(((3*t*pi)/180)+pi/3)]; [p,q] = RR_ForwardPosKin(a,theta); % Calls forwardposkin function to return values xe = [xe p]; ye = [ye q]; end t = 0:0.5:360; subplot(2,1,1); plot(t,xe) xlabel('Variation of T') ylabel('X location of End Effector') ylim([-6 6]) xlim([0 360]) title('Plot of Xe location and t variation') subplot(2,1,2); plot(t,ye) ylim([-6 6]) xlim([0 360]) xlabel('Variation of T') ylabel('Y location of end effector') xlim([0 360]) xlabel('Variation of T') ylabel('Y location of end effector') title('Plot of Ye location and t variation')

Results:

Problem 2: Write the Matlab function RR_InvPosKin(a,disp,sigma) that returns theta Test this routine by creating a set of test data for [xe, ye] as follows. Find the x and y coordinates of 360 data points lying in a circle of radius 4 cm centered around the (1,2) with data points selected at every degree increment (If you are ambitious, convert this to an ellipse or a square of roughly the same size). Plot the resulting computed joint angles plotted against the polar angle of the data point. Solution: We know x = L1 cos 1+L2 cos 2 y = L1 sin 1+L2 sin 2

--------------(1) where L1 and L2 are the Length of the links 1 and 2 are the absolute angles

Taking the term with L1 on to the Left hand side for both equations and squaring and adding we have x+y+(L1)-2L1 xcos 1-2L1 ysin 1 = (L2) Comparing this equation with Acos +Bsin +C = 0 ---------------(3) -------------(2)

We have A = -2L1 x , B = -2L1 y , C = x+y+(L1)-(L2) Representing cos and sin in half angle form and taking tan /2 = t , we have A(1-t)/(1+t) + B(2t)/ (1+t) + C = 0 Simplifying we have (C-A)t + 2Bt + (A+C) = 0 Solution of this quadratic equation for t = -(B (B - C + A))/(C-A) Now we have two values of 1/2. We get 2 from equation 1 For a particular value of x,y,L1,L2 we get two sets of valves for 1 and 2.Now putting the condition from sigma we get the values of 1 and 2 Testing the solution: Generate an array of values for a circle with center (1,2) and radius 4 from the equation X = 1 + 4*cos r Y = 2 + 4*sin r (r varies from 0 to 360 on a degree interval)

Implementation: Particular considerations taken for this routine include: - No Solution : A quadratic equation will have no roots when its discriminant is zero or in physical terms it means the desired end effector is out of operating space of robotic manipulator. - Same Roots : When the roots of Frendensteins equation are same , that implies there is only one configuration to reach the end goal point possible irrespective of value of sigma. In physical terms it mean theta2 is 0,pi/2 or pi. - Atan2 has range between [-pi, pi]. To see whether theta2 has elbow up or elbow down configuration as sigma is defined only for positive values of theta2. Another way of doing the same thing could be redefining the sigma = +1 for theta range from pi<theta2<0. But to be concurrent with the values mentioned in problem, I decided to add 2*pi whenever the value was negative. Case 1: Assuming the origin of robot link is at center on circle. I am assuming this so that all the points on the circle lie in the operating space of the robot. To check whether the joint angle solution is correct, I took these values and passed them to forwardPosKin program and plotted the resulting xe,ye values from which I got a perfect circle. Case 2: Assuming the origin of robot link is at origin(0,0). Some points on circle lie outside the operating space of the robot. Whenever there is no joint configuration possible, I have assigned theta1 and theta2 as zero just to maintain continuity in the plot. To check whether the joint angle solution is correct, I took these values and passed them to forwardPosKin program and plotted the resulting xe,ye values.
% Description : This function returns joint angles(relative) of a 2 link % robot arm once link length (matrix a), end effector location in X-Y Plane % and sigma (-1 for elbow down and +1 for elbow up)are specified. function [theta1, theta2] = RR_InvPosKin(a,disp,sigma) A = -2*a(1)*disp(1); % computing standard A,B,C of Frendenstein's Equation B = -2*a(1)*disp(2); C = (disp(1)^2+disp(2)^2+a(1)^2-a(2)^2); if ((B^2+A^2-C^2) < 0) % verifying if solution exists fprintf('No valid solutions possible for this configuration \n'); theta1 = 0; theta2 = 0; else t1 = (-B+sqrt(B^2+A^2-C^2))/(C-A); % if the solution exist, solutions are evaulated t2 = (-B-sqrt(B^2+A^2-C^2))/(C-A); thetasol1 = 2*atan(t1); % This gives one solution of theta1 thetasol2 = 2*atan(t2); % This gives another solution of theta1 theta2sol1 = atan2((disp(2)-a(1)*sin(thetasol1)),(disp(1) a(1)*cos(thetasol1))); % this calculates theta2 (absolute) for first solution of theta1

theta2sol2 = atan2((disp(2)-a(1)*sin(thetasol2)),(disp(1) a(1)*cos(thetasol2))); % this calculates theta2 (absolute) for second solution of theta1 if theta2sol1 < 0 %atan2 has range [-pi,pi] theta2sol1 = 2*pi+theta2sol1; % this notation would help in evaluation of elbow up and down computation end if theta2sol2 < 0 theta2sol2 = 2*pi+theta2sol2; end theta2sol1 = theta2sol1 - thetasol1; % Relative value of joint angle 2 theta2sol2 = theta2sol2 - thetasol2; % Relative value of joint angle 1 thetamatrix = [theta2sol1 thetasol1; theta2sol2 thetasol2]; % created this matrix to resolve elbow up and down configuration if thetasol1 == thetasol2 % For both roots same theta1 = (thetasol1*180)/pi ; theta2 = (theta2sol1*180)/pi; else thetamatrix = sortrows(thetamatrix); % Lower value of joint angle 2 will the first value in matrix if sigma == -1 theta1 = (thetamatrix(3)*180)/pi; theta2 = (thetamatrix(1)*180)/pi; % pick upper row in sorted thetamatrix else theta1 = (thetamatrix(4)*180)/pi; theta2 = (thetamatrix(2)*180)/pi;% pick lower or the bigger value in sorted thetamatrix end end end

x and y coordinates of 360 data points lying in a circle of radius 4 cm centered around the (1,2) with data points selected at every degree increment. To get x coordinate = 1+4*cos(theta) where theta is polar angle To get y coordinate = 2+4*sin(theta) where theta is polar angle
% Description : This is for plotting polar angle versus joint angle 1 and % joint angle 2. a = [3,2]; j1 = null(1); % initialize array j1 j2 = null(1); % initialize array j2 xe = null(1); % initialize array xe ye = null(1); % initialize array ye for pangle = 0:1:360 p = 1+4*cos(pangle*pi/180); % r*cos(theta) to evaluate x q = 2+4*sin(pangle*pi/180); % r*sin(theta) to evaluate y [theta1 theta2] = RR_InvPosKin(a,[p,q],1); % theta 1 and theta2 give the value of joint angles j1 = [j1 theta1]; % array of various joint angle 1 values j2 = [j2 theta2]; % array of various joint angle 2 values [x,y] = RR_ForwardPosKin(a,[theta1,theta2]); % theta 1 and theta2 are passed to forwardPosKin to verify if the solution is correct xe = [xe x]; ye = [ye y]; end

pangle = 0:1:360; subplot(2,2,2); plot(pangle,j1); xlabel('Variation of polar angle') ylabel('Value of Joint angle 1') xlim([0 360]); title('Plot of Joint angle 1 versus polar angle') subplot(2,2,4); plot(pangle,j2); xlabel('Variation of Polar Angle') ylabel('Value of Joint Angle 2') xlim([0 360]); title('Plot of Joint angle 2 versus polar angle') subplot(2,2,[1 3]);plot(xe,ye); title('Plot of xe,ye verified using ForwardPosKin') xlim([-4 4]); ylim([-4 4]);

Results:

Case 1

Case 2