Two-link robot: traveling in shortest path between two configurations (no obstacle)

clear all

fig_samples=figure;

n_samples=100; %specify this value

W={[-pi pi],[-pi pi]}; %specify this value

samples(:,1) = W{1}(1,1) + (W{1}(1,2)-W{1}(1,1))*rand(n_samples,1); % theta_1 componenet

samples(:,2) = W{2}(1,1) + (W{2}(1,2)-W{2}(1,1))*rand(n_samples,1); % theta_2 componenet

plot(samples(:,1),samples(:,2),'o')

xlim([W{1}(1,1),W{1}(1,2)])

ylim([W{2}(1,1),W{2}(1,2)])

axis square

Some background information

Circular distance between two angles

and

Circular distance between two configurations of a two link robot

and

Back to our problem

Let's consider the case that there is no obstacles in the space.

The code below finds the distance between the samples and remembers the rotation directions for each angle in going from one configuration to another.

The cc rotation is noted by 1 and clockwise rotation is 0.

%cc  1
%c   0
sample_set=(1:1:n_samples);
for i=1:n_samples
    for j=setdiff(sample_set,i)
        d_cc_theta_1(i,j)=mod((samples(j,1)-samples(i,1)),2*pi);%cc distance
        d_c_theta_1(i,j)=mod((samples(i,1)-samples(j,1)),2*pi); %c distance
        d_circular_theta_1(i,j)=min(d_cc_theta_1(i,j),d_cc_theta_1(i,j));
        if d_cc_theta_1(i,j)<=d_c_theta_1(i,j)
            rotation_theta_1(i,j)=1; %cc
        else
            rotation_theta_1(i,j)=0; %c
        end
        d_cc_theta_2(i,j)=mod((samples(j,2)-samples(i,2)),2*pi); %cc distance
        d_c_theta_2(i,j)=mod((samples(i,2)-samples(j,2)),2*pi); %c distance
        d_circular_theta_2(i,j)=min(d_cc_theta_2(i,j),d_cc_theta_2(i,j));
        if d_cc_theta_2(i,j)<=d_c_theta_2(i,j)
            rotation_theta_2(i,j)=1; %cc
        else
            rotation_theta_2(i,j)=0; %c
        end    
    end
end

Now let's pick any two samples and based on the distance and rotation directions plot the motion for going from one sample to the other.

%the samples we choose
s=10;
g=4;

We set minimum stepsize of angular movement between two consecutive displacement by `step_size'.

%stepsize of the angular distance
step_size=10*pi/180;
%here I am using a large angle so you can see how the motion is taking place from a static plot.
%in you project I want everyone to use step_size=2.5*pi/180;
if d_circular_theta_1(s,g)>=d_circular_theta_2(s,g)
    N_angluar_steps=floor(d_circular_theta_1(s,g)/step_size);
else
    N_angluar_steps=floor(d_circular_theta_2(s,g)/step_size);
end

Now we get that angular location as we travel from configuration s to configuration g (s and g are the chosen samples). Notice that here we are calling our information encoder matrix rotation_theta_1 and rotation_theta_2 to figure out the rotation direction between the two configuration for each arm, so we can travel from s to g in shortest path.

if rotation_theta_1(s,g)==0 %clockwise
    theta1_profile=c_motion(samples(s,1),samples(g,1),d_circular_theta_1(s,g)/N_angluar_steps,N_angluar_steps+1);
else
    theta1_profile=cc_motion(samples(s,1),samples(g,1),d_circular_theta_1(s,g)/N_angluar_steps,N_angluar_steps+1);
end
if rotation_theta_2(s,g)==0 %clockwise
    theta2_profile=c_motion(samples(s,2),samples(g,2),d_circular_theta_2(s,g)/N_angluar_steps,N_angluar_steps+1);
else
    theta2_profile=cc_motion(samples(s,2),samples(g,2),d_circular_theta_2(s,g)/N_angluar_steps,N_angluar_steps+1);
end

Now let us plot the motion (this is the part that you should animate)

l1=1; %length of arm 1

l2=0.5; %length of arm 2

figure

for i=1:length(theta1_profile)

plot_robot_config(theta1_profile(i),theta2_profile(i),l1,l2,'c',num2str(i))

hold on

end

%plot_robot_config(theta1(1),theta2(1),l1,l2,'b','start')

%plot_robot_config(theta1(2),theta2(2),l1,l2,'g','goal')

axis square

xlim([-(l1+l2),(l1+l2)])

ylim([-(l1+l2),(l1+l2)])

The followings are functions we use in this demo (comes from the two_links_motion.mlx). These functions can come handy in your project.

Forward Kinematics to plot a configuration

function plot_robot_config(theta1,theta2,l1,l2,robot_color,config_label)
x_m=l1*cos(theta1);
y_m=l1*sin(theta1);
x_e=x_m+l2*cos(theta1+theta2);
y_e=y_m+l2*sin(theta1+theta2);
line([0,x_m],[0,y_m],'LineWidth',3,'Color',robot_color);
hold on
plot(x_m,y_m,'o','MarkerSize',8,'Color',robot_color);
line([x_m,x_e],[y_m,y_e],'LineWidth',3,'Color',robot_color);
plot(x_e,y_e,'o','MarkerSize',8,'Color',robot_color);
if isempty(config_label)==0
    text(0.9*x_e,0.9*y_e,config_label);
end
end

Function for counterclockwise motion

function theta=cc_motion(theta_s,theta_g,step_theta,N)
theta_s_360=mod(theta_s,2*pi);
theta_g_360=mod(theta_g,2*pi);
delta_theta=abs(theta_g_360-theta_s_360);
%counter clockwise motion
if theta_s_360>=theta_g_360
    for t=1:N
        theta(t)=theta_s_360+step_theta*(t-1);
     if theta(t)>2*pi
            theta(t)=mod(theta(t),2*pi);
        end
    end
else
    for t=1:N
        theta(t)=theta_s_360+step_theta*(t-1);
        if theta(t)>2*pi
            theta(t)=mod(theta(t),2*pi);
        end
    end
end
end

Function for clockwise motion

%clockwise motion
function theta=c_motion(theta_s,theta_g,step_theta,N)
theta_s_360=mod(theta_s,2*pi);
theta_g_360=mod(theta_g,2*pi);
delta_theta=abs(theta_g_360-theta_s_360);
if theta_s_360>=theta_g_360
    for t=1:N
        theta(t)=theta_s_360-step_theta*(t-1);
        if theta(t)>2*pi
            theta(t)=mod(theta(t),2*pi);
        end
    end
else
    for t=1:N
        theta(t)=theta_s_360-step_theta*(t-1);
        if theta(t)>2*pi
            theta(t)=mod(theta(t),2*pi);
        end
    end
end
end