# Demo entry 6720466

**Theta_2**

Submitted by **anonymous**
on Mar 16, 2018 at 16:13

Language: Matlab. Code size: 1.1 kB.

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% % solve for second joint theta2, two different % solutions are returned, corresponding % to elbow up and down solution %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% function q2 = Solve_for_theta2(robot, q, Pm) %Evaluate the parameters %theta = eval(robot.DH.theta); d = eval(robot.DH.d); a = eval(robot.DH.a); %alpha = eval(robot.DH.alpha); %See geometry L2=a(2); L3=d(4); A2=a(3); %See geometry of the robot %compute L4 L4 = sqrt(A2^2 + L3^2); %The inverse kinematic problem can be solved as in the IRB 140 (for example) %given q1 is known, compute first DH transformation T01=DH_Compute(robot, q, 1); %Express Pm in the reference system 1, for convenience p1 = inv(T01)*[Pm; 1]; r = sqrt(p1(1)^2 + p1(2)^2); beta = atan2(-p1(2), p1(1)); gamma = real(acos((L2^2+r^2-L4^2)/(2*r*L2))); %return two possible solutions %elbow up and elbow down %the order here is important and is coordinated with the function %solve_for_theta3 q2(1) = pi/2 - beta - gamma; %elbow up q2(2) = pi/2 - beta + gamma; %elbow down

This snippet took 0.01 seconds to highlight.

Back to the Entry List or Home.