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