function workspaceTrajectory_3Darm() % This exercise refers to the robot described in file 4dof_robot_v2.png clc close all % Fixed parameters % ========================================================================= l1 = .2; l2 = .1; l3 = .4; l4 = .5; l5 = .2; % Normal operation similar to the example in jointSpace Trajectory % PE_start = [0.7, 0.0, 0.6, 0, +pi/2, 0]; % PE_end = [0.0, 0.5, 0.1, -pi/3, 0, 0]; % Singularity condition PE_start = [ 0.5, 0.0 , 1.0, +pi/2, 0, 0]; PE_end = [-0.2, 0.001, 1.0, +pi/2, 0, 0]; % Trajectory % ========================================================================= % Here we assume a linear trajectory in workspace, with a pose orientation % which changes smoothly between start and end orientations. nSteps = 100; % Simulation steps PE_trajectory(1,:) = linspace(PE_start(1), PE_end(1), nSteps); PE_trajectory(2,:) = linspace(PE_start(2), PE_end(2), nSteps); PE_trajectory(3,:) = linspace(PE_start(3), PE_end(3), nSteps); PE_trajectory(4,:) = linspace(PE_start(4), PE_end(4), nSteps); PE_trajectory(5,:) = linspace(PE_start(5), PE_end(5), nSteps); PE_trajectory(6,:) = linspace(PE_start(6), PE_end(6), nSteps); % Base point in homogenous coordinates P0 = [0;0;0;1]; % Calculate links % ========================================================================= % Here we define the links in their local frame of reference. Generally, we % consider the link to extend along the x axis, while we consider joints to % be aligned with the z axis. The location of the y axis generally follows. r1 = [ 0 0 l1]'; r2 = [l2 0 0]'; r3 = [l3 0 0]'; r4 = [l4 0 0]'; r5 = [l5 0 0]'; % Initialize arrays for speed of execution P1_trajectory = zeros(3,nSteps); P2_trajectory = zeros(3,nSteps); P3_trajectory = zeros(3,nSteps); P4_trajectory = zeros(3,nSteps); phi1_array = zeros(1,nSteps); phi2_array = zeros(1,nSteps); phi3_array = zeros(1,nSteps); phi4_array = zeros(1,nSteps); gamma_array = zeros(1,nSteps); for iStep = 1:nSteps % Perform analytical inverse kinematics % ===================================================================== % Here, we exploit the fact that the rotational motion around the world % z-axis can be easily decoupled from the rest of the robot. The rest % of the robot itself is a planar mechanism consisting of 3 rotational % joints. % The angle phi1 can be found by considering the orientation of the arm % with respect to the desired pose of the end-effector. phi1 = atan2(PE_trajectory(2,iStep), PE_trajectory(1,iStep,1)); % Gamma is the projected angle of the end-effector on the plane of the % planar part of the robot; it is more or less arbitrary; in this case % it's defined as the sum of the pose angles around the X and Y axes. gamma = (PE_trajectory(4,iStep) + PE_trajectory(5,iStep)); % Now we can compute the location of the known joints (P0, PE, P4 and % P2) P0_rotated = [0 ; 0]; PE_rotated = [sqrt(PE_trajectory(1,iStep)^2 + PE_trajectory(2,iStep)^2) ; PE_trajectory(3,iStep)]; P4_rotated = PE_rotated - l5*[cos(gamma) ; sin(gamma)]; P2_rotated = P0_rotated + [l2 ; l1]; % At this point we can use the cosine theorem to find one of the % possible solutions of the planar part of the robot. tau = acos( (l3^2 + l4^2 - norm(P4_rotated-P2_rotated)^2) / (2*l3*l4) ); phi3 = -(pi - tau); xi = atan2( P4_rotated(2)-P2_rotated(2) , P4_rotated(1)-P2_rotated(1) ); zeta = acos( (l3^2 + norm(P4_rotated-P2_rotated)^2 - l4^2) / (2*l3*norm(P4_rotated-P2_rotated)) ); phi2 = xi + zeta; phi4 = gamma - phi2 - phi3; % Display angles % [phi1 phi2 phi3 phi4]*180/pi % Calculate matrices % ===================================================================== % Here we can apply the relevant rotations. In particular, for this % robot there is a simple rotation around the z axis between the frame % and the 1st link; a fixed 90 degree rotation around the x-axis % between the 1st and 2nd link, and then 3 rotations around the z-axes % of the next joints. A1 = [ Rz(phi1) r1 ; 0 0 0 1]; A2 = [Rx(pi/2)*Rz(phi2) r2 ; 0 0 0 1]; A3 = [ Rz(phi3) r3 ; 0 0 0 1]; A4 = [ Rz(phi4) r4 ; 0 0 0 1]; A5 = [ Rz( 0 ) r5 ; 0 0 0 1]; % Calculate coordinates of points % ===================================================================== % Apply the homogeneous transformation matrices % Note that the vectors [0 0 0 1]' are relative to the origin in each % local frame of reference. This way we can calculate the chain points % P1, P2,..., PE easily. % Also note the composition of homogeneous matrices. P1 = A1*[0 0 0 1]'; P2 = A1*A2*[0 0 0 1]'; P3 = A1*A2*A3*[0 0 0 1]'; P4 = A1*A2*A3*A4*[0 0 0 1]'; PE = A1*A2*A3*A4*A5*[0 0 0 1]'; % Geometric coherence check % ===================================================================== % The length of the links is compared to the distance between % successive points in the kinematic chain % These should be close to zero. (Uncomment to verify) % norm(P0(1:3)-P1(1:3)) - l1 % norm(P1(1:3)-P2(1:3)) - l2 % norm(P2(1:3)-P3(1:3)) - l3 % norm(P3(1:3)-P4(1:3)) - l4 % norm(P4(1:3)-PE(1:3)) - l5 % Save in arrays P1_trajectory(:,iStep) = P1(1:3); P2_trajectory(:,iStep) = P2(1:3); P3_trajectory(:,iStep) = P3(1:3); P4_trajectory(:,iStep) = P4(1:3); phi1_array(iStep) = phi1; phi2_array(iStep) = phi2; phi3_array(iStep) = phi3; phi4_array(iStep) = phi4; gamma_array(iStep) = gamma; % Concatenate points for display purposes points = [P0 P1 P2 P3 P4 PE]; % Plot the configuration of the robot hold off plot3(points(1,:),points(2,:),points(3,:),'-o') hold on; plot3(P1_trajectory(1,1:iStep),P1_trajectory(2,1:iStep),P1_trajectory(3,1:iStep)) plot3(P2_trajectory(1,1:iStep),P2_trajectory(2,1:iStep),P2_trajectory(3,1:iStep)) plot3(P3_trajectory(1,1:iStep),P3_trajectory(2,1:iStep),P3_trajectory(3,1:iStep)) plot3(P4_trajectory(1,1:iStep),P4_trajectory(2,1:iStep),P4_trajectory(3,1:iStep)) plot3(PE_trajectory(1,1:iStep),PE_trajectory(2,1:iStep),PE_trajectory(3,1:iStep)) %hold on daspect([1 1 1]); grid on xlim([-.3 1.2]) ylim([-.3 1.2]) zlim([0 1]) view([116 24]) xlabel('X axis') ylabel('Y axis') drawnow end % Plot joint variables figure plot(phi1_array) hold on plot(phi2_array) plot(phi3_array) plot(phi4_array) xlabel('Steps') ylabel('Joint angles [rad]') legend('phi1','phi2','phi3','phi4') % % Plot gamma % figure % plot(gamma_array) % Rotation matrices function M = Rx(phi) % Rotation matrix around the x-axis of an angle phi M = [1 0 0 ; ... 0 cos(phi) -sin(phi) ; ... 0 sin(phi) cos(phi)]; function M = Ry(phi) % Rotation matrix around the y-axis of an angle phi M = [cos(phi) 0 sin(phi) ; ... 0 1 0 ; ... sin(phi) 0 cos(phi)]; function M = Rz(phi) % Rotation matrix around the z-axis of an angle phi M = [cos(phi) -sin(phi) 0 ; ... sin(phi) cos(phi) 0 ; ... 0 0 1];