1-Solve using matlab codes Figure 1 shows a schematic model of the human arm. a

Question

1-Solve using matlab codes Figure 1 shows a schematic model of the human arm. a is the angle between the upper arm and the shoulder. and is the elbow angle. We define q1 and q2 from a and Beta: Assuming that the origin of our coordinate system is fixed on the shoulder, and the movement of the arm is always limited to the x - y plane (i.e. the arm movement is two-dimensional), we can calculate the position of the elbow and hand: Xelbow = l1cos(q1) Yelhmv = l1 sin(q1) Xhand = Xelbow + 12 cos(q 1 + q2) Yhand = Yelbow + 12 sin(q 1 + q2) Or = X hand = l1 cos(q1) + l2 cos(q1 + q2) = Y hand = l1 sin(q1) + l2 sin(q1 + q2) where l1 is the length of the upper arm, and l2 is the length of lower arm. We can measure a and beta by putting sensors on the shoulder and elbow. Write a function that receives a, beta, I1 and 12 and returns X hand and Y hand

Explanation / Answer

%% Program: arm.m clc; clear all; close all; syms q1 q2 x y; % robot link lengths l1 = 0.17914; % upperarm in meters l2 = 0.18159; % forearm in meters %% Transformation from shoulder to wrist % shoulder to elbow transformation (0 to 1, T_01) A1 = [cos(q1), -sin(q1), 0, 0; sin(q1), cos(q1), 0, 0; 0, 0, 1, 0; 0, 0, 0, 1]; A2 = [1, 0, 0, l1; 0, 1, 0, 0; 0, 0, 1, 0; 0, 0, 0, 1]; T_01 = A1*A2; % elbow to wrist transformation (1 to 2, T_12) A1 = [cos(q2), -sin(q2), 0, 0; sin(q2), cos(q2), 0, 0; 0, 0, 1, 0; 0, 0, 0, 1]; A2 = [1, 0, 0, l2; 0, 1, 0, 0; 0, 0, 1, 0; 0, 0, 0, 1]; T_12 = A1*A2; % shoulder to wrist transformation (0 to 2, T_02) T_02 = simplify(T_01 * T_12); disp('Transformation Matrix from Shoulder to Wrist:'); disp(T_02); %% Given position vector position_ref and orientation % matrix orientation_ref determine q1, q2 syms px py pz; % desired (reference) position px = 0.25; py = 0.1; pz = 0; position_ref = sprintf('px: %f py: %f pz: %f ', px, py, pz); disp(position_ref); % desired (reference) orientation orientation_ref = [1, 0, 0; 0, 1, 0; 0, 0, 1]; % desired (reference) position and orientation ref = [1, 0, 0, px; 0, 1, 0, py; 0, 0, 1, pz; 0, 0, 0, 1]; disp('Reference Orientation & Position of End-Effector:'); disp(ref); % solve for cos(q2), which here equals x % px = l2*cos(q1+q2) + l1*cos(q1) % py = l2*sin(q1+q2) + l1*sin(q1) % px^2 + py^2 = [l2*cos(q1+q2) + l1*cos(q1)]^2 + % [l2*sin(q1+q2) + l1*sin(q1)]^2 % This reduces to: % px^2 + py^2 = l2^2*(cos(q1+q2)^2 + sin(q1+q2)^2) + % l1^2*(cos(q1)^2 + sin(q1)^2) + % 2*l1*l2*(cos(q1)*cos(q1+q2) + sin(q1)*sin(q1+q2)) % This reduces to: % px^2 + py^2 = l1^2 + % l2^2 + % 2*l1*l2*(c1(c1c2 - s1s2) + s1(c1s2 + s1c2)) % This reduces to: % px^2 + py^2 = l1^2 + l2^2 + 2*l1*l2*cos(q2) % so, cos(q2) = [px^2 + py^2 - l1^2 - l2^2] / (2*l1*l2) cos2 = ((ref(1,4))^2 + (ref(2,4))^2 - l1^2 - l2^2) / (2*l1*l2); if (cos2 >= -1 && cos2

Related Questions

Navigate

• Browse (All)
• Browse 1
• Subjects

---

---

Integrity-first tutoring: explanations and feedback only — we do not complete graded work. Learn more.