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.

Delete this entry (admin only).