Elbow Manipulator Simulation

For the first term of ME 115 at Caltech (Introduction to Kinematics and Robotics), my final project was to develop a graphical simulation of the elbow manipulator. Given the trajectory of the joint angles, the end goal was the graphically display the movement of a simple physical model of an elbow manipulator.


It was shown in 1985 that the elbow manipulator is the “optimal” 6R manipulator. Because of this, the elbow manipulator has been employed often in usage scenarios all around the world. It is also one of the most recognized forms of a manipulator. Because of the importance of the elbow manipulator to the field of kinematics, it is useful to simulate such a manipulator and its motion. This project tackles the regional problem of the elbow manipulator; the simulation attempts to find the necessary angles to move the manipulator to the desired location from its current position. The simulation does not yet analyze any orientation issues that can be accomplished with the last three revolute joints in the standard 6R manipulator.

In order to demonstrate the motion of the elbow manipulator, a series of random points are chosen within the manipulator’s workspace. Then, starting from the “home” position of the manipulator, the joints are moved to have the tool frame at the end of the last link coincide with the desired location. Once the target location is reached, the target is then moved to the next location. Recalculation of the new path is done, and the manipulator once again moves to the desired location.

Special care was taken to account for a difference of a multiple of 2π in the angles. Despite being on different complex planes, the manipulator resides only in the real space, so care is taken to normalize the variables to find the shortest path. In addition, the manipulator simulation is realistic in that the links will not collide with each other. Though nearly physically impossible, the joints are given a full range of motion that is theoretically possible. This is, the manipulator in the simulation is not hampered by mechanical limitations, as a simulation should be.


Main method

clear all; close all; clc;

% Height of shoulder to base
H = 15;

% Arm lengths
L1 = 8; % Upper arm
L2 = 5; % Forearm

% Arm thickness (radius)
r  = 0.2;

% Desired position of tool frame
% Tool frame at end of forearm (where wrist would be)
p_d = [-5; -5; 19];

% Prompt for user given desired position
% p_d(1) = str2double(input('X-value: ', 's'));
% if isempty(p_d(1))
%     p_d(1) = (L1+L2)/2;
% end
% p_d(2) = str2double(input('Y-value: ', 's'));
% if isempty(p_d(2))
%     p_d(2) = (L1+L2)/2;
% end
% p_d(3) = str2double(input('Z-value: ', 's'));
% if isempty(p_d(3))
%     p_d(3) = H;
% end

% Initial joint angles
theta_current = [0; 0; 0];

% Initialize variables
fig = figure;
theta_d = zeros(3,1);
robot = zeros(4,1);
[sphere_x, sphere_y, sphere_z] = sphere(20);
desired_point = surf(1e-5*sphere_x, 1e-5*sphere_y, 1e-5*sphere_z);
set(desired_point, 'XData', 0.25*sphere_x + p_d(1), 'YData', 0.25*sphere_y + p_d(2), 'ZData', 0.25*sphere_z + p_d(3));
set(desired_point, 'EdgeColor', 'none', 'FaceColor', 'red');

% Set up figure
axis([-(L1+L2) (L1+L2) -(L1+L2) (L1+L2) 0 (L1+L2)+H]);
grid on;
% Display static text (for joint angles and target position)
angle_str = {{},{},{}};
for angle=1:3
    angle_str(angle) = {['\theta_' num2str(angle) ' = ']};
text(0,0, angle_str, 'Units', 'pixels');
point_str = {'x^D = ','y^D = ','z^D = '};
text(385,0, point_str, 'Units', 'pixels');

for run=1:100
    % Calculate a random desired point
    p_d = rand(3,1);
    a = ((L1+L2) - abs(L2-L1)) * p_d(1) + abs(L2-L1);
    b = p_d(2) * 2*pi;
    c = p_d(3) * pi;
    p_d(1) = a * cos(b) * sin(c);
    p_d(2) = a * sin(b) * sin(c);
    p_d(3) = max(0, a * cos(c) + H);
    % Plot desired point as sphere
    if desired_point == 0
        desired_point = surf(0.25*sphere_x + p_d(1), 0.25*sphere_y + p_d(2), 0.25*sphere_z + p_d(3));
        set(desired_point, 'XData', 0.25*sphere_x + p_d(1), 'YData', 0.25*sphere_y + p_d(2), 'ZData', 0.25*sphere_z + p_d(3));
    set(desired_point, 'EdgeColor', 'none', 'FaceColor', 'red');
    % Calculate desired end theta
    % Check that the desired position is reachable
    if norm(p_d - [0; 0; H]) <= (L1 + L2) && norm(p_d - [0; 0; H]) >= (abs(L1 - L2))
        % Solve for 2 solutions for theta_3
        temp = acos((norm(p_d - [0;0;H])^2 - L1^2 - L2^2)/(2*L1*L2));
        theta_3 = [temp; -temp];

        % Solve for 2 solutions for theta_2 for EACH theta_3
        % theta_3(1) -> theta_2(1,:)
        theta_2 = zeros(2,2);
        for i=1:2
            r_H = norm([p_d(1); p_d(2)]);
            a = r_H + (L1 + L2*cos(theta_3(i)));
            b = 2 * L2 * sin(theta_3(i));
            c = r_H - (L1 + L2 * cos(theta_3(i)));
            theta_2(i,1) = 2 * atan(real((-b + sqrt(b^2 - 4*a*c)) / (2 * a)));
            theta_2(i,2) = 2 * atan(real((-b - sqrt(b^2 - 4*a*c)) / (2 * a)));

        % Solve for theta_1 for each pair of theta_2 and theta_3
        theta_1 = zeros(2,2);
        for i=1:2
            for j=1:2
                temp = L1 * cos(theta_2(i,j)) + L2 * cos(theta_2(i,j) + theta_3(i));
                theta_1(i,j) = atan2(p_d(2) / temp, p_d(1) / temp);

        % Gather all pairs of thetas
        theta = zeros(4,3);
        for i=1:4
            theta(i,1) = theta_1(floor((i-1)/2)+1, mod(i-1,2)+1);
            theta(i,2) = theta_2(floor((i-1)/2)+1, mod(i-1,2)+1);
            theta(i,3) = theta_3(floor((i-1)/2)+1);
            g_base_wrist = g_transform(0,0,H,0) * g_transform(0,0,0,theta(i,1)) * g_transform(0,-pi/2,0,theta(i,2)) * g_transform(L1,0,0,theta(i,3)) * g_transform(L2,0,0,0);
            [~,d] = g_to_vector(g_base_wrist);
            % Use correction to accommodate flipping over itself
            if norm(d - p_d) > 1e-4
                theta(i,1) = theta(i,1) + pi;
                theta(i,2) = theta(i,2) + pi;
        % Change joint angles to be on same revolution as current angles
        for i=1:4
            if abs(theta(i,1) - theta_current(1)) > pi
                if theta_current(1) > 0
                    theta(i,1) = theta(i,1) + 2*pi;
                    theta(i,1) = theta(i,1) - 2*pi;
            if theta(i,2) > pi/2
                theta(i,2) = theta(i,2) - 2*pi;
        % Check for collisions
        % Once joint angles are normalized, only collision could be between
        % last link and base link
        for i=4:-1:1
            [~,d_base_wrist] = g_to_vector(g_transform(0,0,H,0) * g_transform(0,0,0,theta(i,1)) * g_transform(0,-pi/2,0,theta(i,2)) * g_transform(L1,0,0,theta(i,3)) * g_transform(L2,0,0,0));
            [~,d_base_elbow] = g_to_vector(g_transform(0,0,H,0) * g_transform(0,0,0,theta(i,1)) * g_transform(0,-pi/2,0,theta(i,2)) * g_transform(L1,0,0,theta(i,3)));
            d_base_wrist = d_base_wrist';
            d_base_elbow = d_base_elbow';
            if d_base_wrist(3) < H
                if sign(d_base_wrist(1)) ~= sign(d_base_elbow(1)) || sign(d_base_wrist(2)) ~= sign(d_base_elbow(2))
                    theta(i,:) = [];

        % Determine closest set of desired theta
        delta_theta = zeros(size(theta,1),1);
        for sol=1:size(theta,1)
            delta_theta(sol) = norm(theta(sol,:)' - theta_current(:))^2;
        [~, I] = min(delta_theta);
        theta_d = theta(I,:)';

    % Plot the robot
    theta_start = theta_current;
    T = 1;
    dt = 0.025; % keep above about 0.02 to keep animation in real time
    robot = plot_elbow(H, L1, L2, r, theta_current, robot);
    % Draw the values of the joint angles and target coordinates to the plot
    angle_str = {{},{},{}};
    text_angles = zeros(3,1);
    for angle=1:3
        angle_str(angle) = {num2str(round(rad2deg(theta_current(angle))))};
        text_angles(angle) = text(55,43-21*angle, angle_str(angle), 'Units', 'pixels', 'HorizontalAlignment', 'Right');
    point_str = {{};{};{}};
    text_point = zeros(3,1);
    for coord=1:3
        point_str(coord) = {num2str(p_d(coord))};
        text_point(coord) = text(475,43-21*coord, point_str(coord), 'Units', 'pixels', 'HorizontalAlignment', 'Right');
    % Clear current text of joint angles (since it changes with each timestep)
    for t=0:dt:T
        theta_current = (theta_d - theta_start) * t/T + theta_start;
        robot = plot_elbow(H, L1, L2, r, theta_current, robot);
        % Draw the values of the joint angles to the plot
        angle_str = {{},{},{}};
        text_angles = zeros(3,1);
        for angle=1:3
            angle_str(angle) = {num2str(round(rad2deg(theta_current(angle))))};
            text_angles(angle) = text(55,43-21*angle, angle_str(angle), 'Units', 'pixels', 'HorizontalAlignment', 'Right');
        % Wait rest of timestep to keep as close to realtime as possible
        elapsed_time = toc;
        pause(dt - elapsed_time);
        % Clear current text displaying joint angles
    % Clear the current text of target coordinates
    % Normalize joint angles
    % theta_1 = [-pi, pi]
    % theta_2 = [-3*pi/2, pi/2]
    % theta_3 = [-pi, pi]
    while theta_current(1) < -pi || theta_current(1) > pi
        theta_current(1) = theta_current(1) + (-sign(theta_current(1))) * 2*pi;
    while theta_current(2) < -3*pi/2 || theta_current(2) > pi/2
        theta_current(2) = theta_current(2) + (-sign(theta_current(2))) * 2*pi;
    while theta_current(3) < -pi || theta_current(3) > pi
        theta_current(3) = theta_current(3) + (-sign(theta_current(3))) * 2*pi;

Plotting function

function [robot] = plot_elbow(H, L1, L2, r, theta_current, robot)

    g_base_shoulder = g_transform(0,0,H,0);
    g_shoulder_elbow = g_transform(0,0,0,theta_current(1)) * g_transform(0,-pi/2,0,theta_current(2)) * g_transform(L1,0,0,theta_current(3));
    g_elbow_wrist =  g_transform(L2,0,0,0);

    % Find displacement relative to base of each joint
    D = zeros(4,3);
    [~,d] = g_to_vector(g_base_shoulder);
    D(2,:) = d';
    [~,d] = g_to_vector(g_base_shoulder * g_shoulder_elbow);
    D(3,:) = d';
    [~,d] = g_to_vector(g_base_shoulder * g_shoulder_elbow * g_elbow_wrist);
    D(4,:) = d';

    % Plot the arm
    for i=1:size(D,1)-1
        % Create unit vector of link
        vector = D(i+1,:) - D(i,:);
        vector = vector ./ norm(vector);

        % Find w vector and angle to rotate circle about
        % so circle will be perpendicular to link vector
        w = cross([0,0,1], vector);
        phi = atan2(norm(cross([0,0,1],vector)),dot([0,0,1],vector));

        % Create rotation vector to rotate circle of link
        w_hat = [0,-w(3),w(2);w(3),0,-w(1);-w(2),w(1),0];
        R = expm(phi*w_hat);

        % Define points of circle for arm vertices

        % resolution is number of points for the circle on ends of links
        resolution = 12;
        vertices = zeros(resolution*2,3);
        vertices(1:size(vertices,1)/2,1) = r*cos(0:2*pi/(size(vertices,1)/2):2*pi-2*pi/(size(vertices,1)/2));
        vertices(1:size(vertices,1)/2,2) = r*sin(0:2*pi/(size(vertices,1)/2):2*pi-2*pi/(size(vertices,1)/2));
        vertices(1:size(vertices,1)/2,3) = zeros(size(vertices,1)/2,1);
        vertices(size(vertices,1)/2+1:end,1) = r*cos(0:2*pi/(size(vertices,1)/2):2*pi-2*pi/(size(vertices,1)/2));
        vertices(size(vertices,1)/2+1:end,2) = r*sin(0:2*pi/(size(vertices,1)/2):2*pi-2*pi/(size(vertices,1)/2));
        vertices(size(vertices,1)/2+1:end,3) = zeros(size(vertices,1)/2,1);

        % Rotate vertices to match direction of link
        for j=1:size(vertices,1)
            vertices(j,:) = (R * vertices(j,:)')';

        % Move vertices to be at placement of link
        vertices(1:size(vertices,1)/2,1) = vertices(1:size(vertices,1)/2,1) + D(i,1);
        vertices(1:size(vertices,1)/2,2) = vertices(1:size(vertices,1)/2,2) + D(i,2);
        vertices(1:size(vertices,1)/2,3) = vertices(1:size(vertices,1)/2,3) + D(i,3);
        vertices(size(vertices,1)/2+1:end,1) = vertices(size(vertices,1)/2+1:end,1) + D(i+1,1);
        vertices(size(vertices,1)/2+1:end,2) = vertices(size(vertices,1)/2+1:end,2) + D(i+1,2);
        vertices(size(vertices,1)/2+1:end,3) = vertices(size(vertices,1)/2+1:end,3) + D(i+1,3);

        % Create array of faces that define the link
        faces = zeros(size(vertices,1)/2+2, max(size(vertices,1)/2, 4));
        for j=1:size(vertices,1)/2
            faces(j,1:4) = [j, mod(j, size(vertices,1)/2)+1, mod(j, size(vertices,1)/2)+1+size(vertices,1)/2, j+size(vertices,1)/2];
            if size(vertices,1)/2 > 4
                faces(j,5:end) = NaN(1, size(faces,2)-4);
        faces(size(vertices,1)/2+1, 1:size(vertices,1)/2) = 1:size(vertices,1)/2;
        faces(size(vertices,1)/2+2, 1:size(vertices,1)/2) = size(vertices,1)/2+1:size(vertices,1);
        if size(vertices,1)/2 < size(faces,2)
            faces(size(faces,1)-1:end, size(vertices,1)/2+1:end) = NaN(2, size(faces,2)-size(vertices,1)/2);

        % Draw 3D link
        color = [0.5, 0.5, 1.0];
        if robot(i) == 0
            robot(i) = patch('Vertices',vertices,'Faces',faces,'FaceColor',[0.5,0.5,1.0],'EdgeColor',0.5*color);
            set(robot(i), 'Vertices',vertices,'Faces',faces,'FaceColor',[0.5,0.5,1.0],'EdgeColor',0.5*color);

        hold on;

Helper functions

function [R, d] = g_to_vector(g)
    d = g(1:3,4);
    R = g(1:3,1:3);

function [g] = g_transform(a, alpha, d, theta)
    g = [cos(theta),            -sin(theta),           0,           a;
         sin(theta)*cos(alpha), cos(theta)*cos(alpha), -sin(alpha), -d*sin(alpha);
         sin(theta)*sin(alpha), cos(theta)*sin(alpha), cos(alpha),  d*cos(alpha);
         0,                     0,                     0,           1           ];

Simulation Screenshots

Movement to Reach One Target

The following are screenshots taken at a set interval as the manipulator moves to a specific target.


The project has several shortcomings. First, the manipulator moves linearly between the current location and the final desired location. A much better solution would be to use splines to have a more organic movement that more closely mimics the movement of human limbs. Also, the software does not take into account any possible obstructions (besides itself) that could lie in the path of the manipulator. This can be fixed but requires a far more complex motion planning algorithm beyond the scope of this class.

Additionally, the software is currently written to have a constant time between all pairs of target points. This means that the manipulator will move slowly to a target that is near, while it will race to reach a target on the opposite side of its workspace. This is unrealistic as the joints usually move at or near their maximum speed so as to minimize the total time spent during travel of the manipulator. This can be further improved through the use of the Jacobian matrix to either maximize the overall speed of the tool frame (end of the manipulator) or to maximize the joint rotation speeds. In both cases, the total time to reach the desired location would be dependent on the distance the manipulator needs to travel, as opposed to the current constant time.

Lastly, the graphics are very low quality and each link is represented only by a simple cylinder. While the resolution of the cylinder can be increased, the joints suffer a lack of quality as there becomes space between the cylinders as the joint angles grow. While not important to the final result, this shortcoming affects the overall quality of the simulation. This can be improved with better rendering of the links through a more complicated algorithm that takes into the account the joint angles to match the links to each other and set them flush. Alternatively, an object to represent the joint could be constructed to simulate the physical joint, such as another (perpendicular) cylinder to represent a hinge.

In the end, the project does accomplish its goals. The simulation is easy to run (self-contained MATLAB files) and illustrates how an elbow manipulator moves. Additionally, the randomness of the targets means that the algorithm to both move the robot as well as select the desired joint angles is robust enough to cover the entire workspace. An array of target points could just as easily have been specified instead of a set of random points. With the random points, the code illustrates the workspace of the manipulator as well as shows the robustness of the simulation code.