CCD Algorithm for Solving Inverse Kinematics Problem

The easiest way to do inverse kinematics is with CCD method (Cyclic Coordinate Descent). CCD algorithm was first propesed by Wang and Chen (A Combined Optimization Method for Solving the Inverse Kinematics Problem of Mechanical Manipulators. IEEE Tr. On Robotics and Automation, 7:489-498, 1991).


See on the picture: pc is the distance between joint position and end effector position; pe is the end effector position; and pt is the target position.
The idea is to rotate each joint - starting from joint close to the end effector to the immobile joint at the root - by θ. We do it repeatedly until we reach the target.

Now, how do we find θ? The answer is by using vector addition formula.


So, we solve those two equations for each joint.

Here is a Matlab code (m-file). Try change variable num_of_link for the desired number of links and pt for desired target point.



%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
clc;
clear;

pt = [3;5]; % target point (in column matrix)

figure(1)
title('Planar Robot','FontSize',12)
axis([-20 20 -20 20])       % axis limits
axis manual                 % set axis to exact manual value(
hold on                     % does not erase previous graphs
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%

%% initialization of data
num_of_link = 7; %number of link
xdata = (0:num_of_link);
for i = 1:num_of_link+1
    ydata(i) = 0;
end
for i = 1:num_of_link
    angledata(i) = 0;
end

hnd = plot(xdata,ydata,'-r', 'erasemode','xor');    % plot robot manipulator

error = dist([xdata(num_of_link+1) ydata(num_of_link+1)], pt);
while (error > 0.5)
    iteration = num_of_link + 1;
    while (iteration > 1)
        %% CCD Algorthm

        pe = [xdata(num_of_link+1); ydata(num_of_link+1)];
        pc = [xdata(iteration-1); ydata(iteration-1)];

        a = (pe - pc)/norm(pe-pc);
        b = (pt - pc)/norm(pt-pc);
        teta = acosd(dot(a, b));
       
        direction = cross([a(1) a(2) 0],[b(1) b(2) 0]);
        if direction(3) < 0
            teta = -teta;
        end
       
        % this part can be omitted, this purpose is to make the result
        % looks more natural
        if (teta > 30)
            teta = 30;
        elseif (teta < -30)
            teta = -30;
        end
       
        angledata(iteration-1) = teta;

        iteration = iteration - 1;

        %% let's do the rotation!
        for i = 1:num_of_link-1
            R = [cosd(angledata(i)) -sind(angledata(i)); sind(angledata(i)) cosd(angledata(i))]; % rotation matrix
            % p' = R * (p - c) + c
            temp = R * ([xdata(i+1); ydata(i+1)] - [xdata(i); ydata(i)]) + [xdata(i); ydata(i)];
            xdata(i+1) = temp(1);
            ydata(i+1) = temp(2);
            angledata(i+1) = angledata(i+1) + angledata(i);
            xdata(i+2) = xdata(i+1)+1;
            ydata(i+2) = ydata(i+1);
        end

        % end effector rotation
        i = i+1;
        R = [cosd(angledata(i)) -sind(angledata(i)); sind(angledata(i)) cosd(angledata(i))]; % rotation matrix
        % p' = R * (p - c) + c
        temp = R * ([xdata(i+1); ydata(i+1)] - [xdata(i); ydata(i)]) + [xdata(i); ydata(i)];
        xdata(i+1) = temp(1);
        ydata(i+1) = temp(2);

    end
    error = dist([xdata(num_of_link+1) ydata(num_of_link+1)], pt);
    disp(error);
end

for i = 1:1:size(xdata,2)
    plot(xdata(i), ydata(i), 'ob')
end

set(hnd,'xdata',xdata,'ydata',ydata); %update data to plot 

disp('done!');


Here are some results. We can see sometimes this method generates unnatural movement. To avoid this, joint limit constraints can be applied for each joint.
  • Number of links = 6; target point (x,y) = 3,5.


  • Number of links = 8; target point (x,y) = (4,5).



Comments