% dk=[n sa p; 0 0 0 1] % n, s, a: They are 3 vector for the end-effector's orientation dk=A06; % Position and orientation of end-effector n=dk(1:3,1); s=dk(1:3,2); a=dk(1:3,3); R=[n s a]; dk=A06; % Direct kinematics matrix % Inverse Kinematic p_ot=dk(1:3,4); % End-effector's position pw=p_ot-d1*a; % Wrist's position pw_x=dk(1,4); % Vector's components that representes the wrist's position pw_y=dk(2,4); pw_z=dk(3,4); c3=(pw_x^2+pw_y^2+pw_z^2-a2^2-a3^2)/(2*a2*a3); % cos(teta3) s3=-sqrt(1-c3^2); % sin(teta3) teta3-atan2(real(s3), real(c3)); c2=(sqrt(pw_x^2+pw_y^2)*(a2+a3*c3)+pw_z*a3*s3)/(a2^2+a3^2+2*a2*a3*c3); % cos(teta2) s2=(pw_z* (a2+a3*c3)-sqrt(pw_x^2+pw_y^2)*a3*s3)/(a2^2+a3^2+2*a2*a3*c3); % sin(teta2) teta2-atan2(real((a2+a3*c3)*pw_z-a3*53*sqrt(pw_x^2+pw_y^2)),real((a2+a3*c3)*sqrt(pw_x^2+pw_y^2)+a3*s3*pw_z)); teta1-atan2(pw_y, pw_x); R3_0=[cos(tetal) *cos(teta2+teta3) -cos(tetal) *sin(teta2+teta3) sin(tetal); sin(tetal)*cos(teta2+teta3) -sin(tetal)*sin(teta2+teta3) -cos(tetal); sin(teta2+teta3) cos(teta2+teta3) 0]; R6_3=R3_0*R; % Matrix for the Euler's angle of spherical wrist % Inverse kinematic for the spherical wrist teta4-atan2(R6_3(2,3),R6_3(1,3)); teta5-atan2(sqrt((R6_3(1,3))^2+(R6_3(2,3))^2),R6_3(3,3)); teta6=atan2(R6_3(3,2),R6_3(3,1)); q=[tetal teta2 teta3 teta4 teta5 teta6]
% dk=[n sa p; 0 0 0 1] % n, s, a: They are 3 vector for the end-effector's orientation dk=A06; % Position and orientation of end-effector n=dk(1:3,1); s=dk(1:3,2); a=dk(1:3,3); R=[n s a]; dk=A06; % Direct kinematics matrix % Inverse Kinematic p_ot=dk(1:3,4); % End-effector's position pw=p_ot-d1*a; % Wrist's position pw_x=dk(1,4); % Vector's components that representes the wrist's position pw_y=dk(2,4); pw_z=dk(3,4); c3=(pw_x^2+pw_y^2+pw_z^2-a2^2-a3^2)/(2*a2*a3); % cos(teta3) s3=-sqrt(1-c3^2); % sin(teta3) teta3-atan2(real(s3), real(c3)); c2=(sqrt(pw_x^2+pw_y^2)*(a2+a3*c3)+pw_z*a3*s3)/(a2^2+a3^2+2*a2*a3*c3); % cos(teta2) s2=(pw_z* (a2+a3*c3)-sqrt(pw_x^2+pw_y^2)*a3*s3)/(a2^2+a3^2+2*a2*a3*c3); % sin(teta2) teta2-atan2(real((a2+a3*c3)*pw_z-a3*53*sqrt(pw_x^2+pw_y^2)),real((a2+a3*c3)*sqrt(pw_x^2+pw_y^2)+a3*s3*pw_z)); teta1-atan2(pw_y, pw_x); R3_0=[cos(tetal) *cos(teta2+teta3) -cos(tetal) *sin(teta2+teta3) sin(tetal); sin(tetal)*cos(teta2+teta3) -sin(tetal)*sin(teta2+teta3) -cos(tetal); sin(teta2+teta3) cos(teta2+teta3) 0]; R6_3=R3_0*R; % Matrix for the Euler's angle of spherical wrist % Inverse kinematic for the spherical wrist teta4-atan2(R6_3(2,3),R6_3(1,3)); teta5-atan2(sqrt((R6_3(1,3))^2+(R6_3(2,3))^2),R6_3(3,3)); teta6=atan2(R6_3(3,2),R6_3(3,1)); q=[tetal teta2 teta3 teta4 teta5 teta6]
Elements Of Electromagnetics
7th Edition
ISBN:9780190698614
Author:Sadiku, Matthew N. O.
Publisher:Sadiku, Matthew N. O.
ChapterMA: Math Assessment
Section: Chapter Questions
Problem 1.1MA
Related questions
Question
Can anybody help me, the underlying reason behind the equations of teta 1 up until 6. These equations should be the representation of inverse matrix of 6R Manipulators
![% dk=[n sa p; 0 0 0 1]
% n, s, a: They are 3 vector for the end-effector's orientation
dk=A06; % Position and orientation of end-effector
n=dk(1:3,1);
s=dk(1:3,2);
a=dk(1:3,3);
R=[n s a];
dk=A06; % Direct kinematics matrix
% Inverse Kinematic
p_ot=dk(1:3,4); % End-effector's position
pw=p_ot-d1*a; % Wrist's position
pw_x=dk(1,4); % Vector's components that representes the wrist's position
pw_y=dk(2,4);
pw_z=dk(3,4);
c3=(pw_x^2+pw_y^2+pw_z^2-a2^2-a3^2)/(2*a2*a3); % cos(teta3)
s3=-sqrt(1-c3^2); % sin(teta3)
teta3-atan2(real(s3), real(c3));
c2=(sqrt(pw_x^2+pw_y^2)*(a2+a3*c3)+pw_z*a3*s3)/(a2^2+a3^2+2*a2*a3*c3); % cos(teta2)
s2=(pw_z* (a2+a3*c3)-sqrt(pw_x^2+pw_y^2)*a3*s3)/(a2^2+a3^2+2*a2*a3*c3); % sin(teta2)
teta2-atan2(real((a2+a3*c3)*pw_z-a3*53*sqrt(pw_x^2+pw_y^2)),real((a2+a3*c3)*sqrt(pw_x^2+pw_y^2)+a3*s3*pw_z));
teta1-atan2(pw_y, pw_x);
R3_0=[cos(tetal) *cos(teta2+teta3) -cos(tetal) *sin(teta2+teta3) sin(tetal);
sin(tetal)*cos(teta2+teta3) -sin(tetal)*sin(teta2+teta3) -cos(tetal);
sin(teta2+teta3) cos(teta2+teta3) 0];
R6_3=R3_0*R; % Matrix for the Euler's angle of spherical wrist
% Inverse kinematic for the spherical wrist
teta4-atan2(R6_3(2,3),R6_3(1,3));
teta5-atan2(sqrt((R6_3(1,3))^2+(R6_3(2,3))^2),R6_3(3,3));
teta6=atan2(R6_3(3,2),R6_3(3,1));
q=[tetal teta2 teta3 teta4 teta5 teta6]](/v2/_next/image?url=https%3A%2F%2Fcontent.bartleby.com%2Fqna-images%2Fquestion%2F5263f929-975c-4434-ab99-1f9c7031aa3f%2F353ba663-f92d-460f-89f5-3e84c7d39ad9%2Fxjw9p6n_processed.png&w=3840&q=75)
Transcribed Image Text:% dk=[n sa p; 0 0 0 1]
% n, s, a: They are 3 vector for the end-effector's orientation
dk=A06; % Position and orientation of end-effector
n=dk(1:3,1);
s=dk(1:3,2);
a=dk(1:3,3);
R=[n s a];
dk=A06; % Direct kinematics matrix
% Inverse Kinematic
p_ot=dk(1:3,4); % End-effector's position
pw=p_ot-d1*a; % Wrist's position
pw_x=dk(1,4); % Vector's components that representes the wrist's position
pw_y=dk(2,4);
pw_z=dk(3,4);
c3=(pw_x^2+pw_y^2+pw_z^2-a2^2-a3^2)/(2*a2*a3); % cos(teta3)
s3=-sqrt(1-c3^2); % sin(teta3)
teta3-atan2(real(s3), real(c3));
c2=(sqrt(pw_x^2+pw_y^2)*(a2+a3*c3)+pw_z*a3*s3)/(a2^2+a3^2+2*a2*a3*c3); % cos(teta2)
s2=(pw_z* (a2+a3*c3)-sqrt(pw_x^2+pw_y^2)*a3*s3)/(a2^2+a3^2+2*a2*a3*c3); % sin(teta2)
teta2-atan2(real((a2+a3*c3)*pw_z-a3*53*sqrt(pw_x^2+pw_y^2)),real((a2+a3*c3)*sqrt(pw_x^2+pw_y^2)+a3*s3*pw_z));
teta1-atan2(pw_y, pw_x);
R3_0=[cos(tetal) *cos(teta2+teta3) -cos(tetal) *sin(teta2+teta3) sin(tetal);
sin(tetal)*cos(teta2+teta3) -sin(tetal)*sin(teta2+teta3) -cos(tetal);
sin(teta2+teta3) cos(teta2+teta3) 0];
R6_3=R3_0*R; % Matrix for the Euler's angle of spherical wrist
% Inverse kinematic for the spherical wrist
teta4-atan2(R6_3(2,3),R6_3(1,3));
teta5-atan2(sqrt((R6_3(1,3))^2+(R6_3(2,3))^2),R6_3(3,3));
teta6=atan2(R6_3(3,2),R6_3(3,1));
q=[tetal teta2 teta3 teta4 teta5 teta6]
AI-Generated Solution
AI-generated content may present inaccurate or offensive content that does not represent bartleby’s views.
Unlock instant AI solutions
Tap the button
to generate a solution
Recommended textbooks for you
Elements Of Electromagnetics
Mechanical Engineering
ISBN:
9780190698614
Author:
Sadiku, Matthew N. O.
Publisher:
Oxford University Press
Mechanics of Materials (10th Edition)
Mechanical Engineering
ISBN:
9780134319650
Author:
Russell C. Hibbeler
Publisher:
PEARSON
Thermodynamics: An Engineering Approach
Mechanical Engineering
ISBN:
9781259822674
Author:
Yunus A. Cengel Dr., Michael A. Boles
Publisher:
McGraw-Hill Education
Elements Of Electromagnetics
Mechanical Engineering
ISBN:
9780190698614
Author:
Sadiku, Matthew N. O.
Publisher:
Oxford University Press
Mechanics of Materials (10th Edition)
Mechanical Engineering
ISBN:
9780134319650
Author:
Russell C. Hibbeler
Publisher:
PEARSON
Thermodynamics: An Engineering Approach
Mechanical Engineering
ISBN:
9781259822674
Author:
Yunus A. Cengel Dr., Michael A. Boles
Publisher:
McGraw-Hill Education
Control Systems Engineering
Mechanical Engineering
ISBN:
9781118170519
Author:
Norman S. Nise
Publisher:
WILEY
Mechanics of Materials (MindTap Course List)
Mechanical Engineering
ISBN:
9781337093347
Author:
Barry J. Goodno, James M. Gere
Publisher:
Cengage Learning
Engineering Mechanics: Statics
Mechanical Engineering
ISBN:
9781118807330
Author:
James L. Meriam, L. G. Kraige, J. N. Bolton
Publisher:
WILEY