examples for sir john

profileZeelex
lecture_13.pdf
Home>Mathematics homework help>examples for sir john

EET 477

Robotics

Lecture 13: 21Jul14

Objective:

 Review cyclic coordinate descent algorithm for inverse kinematics.

 Introduce trajectory planning

Discuss the speed of the CCD algorithm

 The following code represents the solution for Part one of homework 4.

 It generally takes between 8 to 30 seconds to determine a solution when the destination is not near the edge of the workspace.

%Author: Davis

%Date: 21Jul14

%Description: Homework 4, Problem 1

Destination_DH_Parms=Initial_DH_Parms;

Workspace = sum(Initial_DH_Parms(2,:));

cla;

hold on; %retains plots between plot statements

grid on;

axis([-1*Workspace,Workspace,-1*Workspace,Workspace,-1*Workspace,Workspace]); %defines

min_x, max_x, ...

view([0,0,15]); %defines point of view

Angle_Resolution = 4;

Frame_A=[0,1,0,0,0,0;

0,0,0,1,0,0;

0,0,0,0,0,1;

1,1,1,1,1,1];

Frame_B=Revolute_Transformation(Destination_DH_Parms(:,1))*Frame_A;

Frame_C=Revolute_Transformation(Destination_DH_Parms(:,1))*...

Revolute_Transformation(Destination_DH_Parms(:,2))*Frame_A;

Frame_D=Revolute_Transformation(Destination_DH_Parms(:,1))*...

Revolute_Transformation(Destination_DH_Parms(:,2))*...

Revolute_Transformation(Destination_DH_Parms(:,3))*Frame_A;

Frame_E=Revolute_Transformation(Destination_DH_Parms(:,1))*...

Revolute_Transformation(Destination_DH_Parms(:,2))*...

Revolute_Transformation(Destination_DH_Parms(:,3))*...

Revolute_Transformation(Destination_DH_Parms(:,4))*Frame_A;

Frame_F=Revolute_Transformation(Destination_DH_Parms(:,1))*...

Revolute_Transformation(Destination_DH_Parms(:,2))*...

Revolute_Transformation(Destination_DH_Parms(:,3))*...

Revolute_Transformation(Destination_DH_Parms(:,4))*...

Revolute_Transformation(Destination_DH_Parms(:,5))*Frame_A;

Frame_G=Revolute_Transformation(Destination_DH_Parms(:,1))*...

Revolute_Transformation(Destination_DH_Parms(:,2))*...

Revolute_Transformation(Destination_DH_Parms(:,3))*...

Revolute_Transformation(Destination_DH_Parms(:,4))*...

Revolute_Transformation(Destination_DH_Parms(:,5))*...

Revolute_Transformation(Destination_DH_Parms(:,6))*Frame_A;

%plot each displacement vector in green and P vector in cyan

plot3([Frame_A(1,1),Frame_B(1,1)],...

[Frame_A(2,1),Frame_B(2,1)],...

[Frame_A(3,1),Frame_B(3,1)],'blue','linewidth',3);

plot3([Frame_B(1,1),Frame_C(1,1)],...

[Frame_B(2,1),Frame_C(2,1)],...

[Frame_B(3,1),Frame_C(3,1)],'blue','linewidth',3);

plot3([Frame_C(1,1),Frame_D(1,1)],...

[Frame_C(2,1),Frame_D(2,1)],...

[Frame_C(3,1),Frame_D(3,1)],'blue','linewidth',3);

plot3([Frame_D(1,1),Frame_E(1,1)],...

[Frame_D(2,1),Frame_E(2,1)],...

[Frame_D(3,1),Frame_E(3,1)],'blue','linewidth',3);

plot3([Frame_E(1,1),Frame_F(1,1)],...

[Frame_E(2,1),Frame_F(2,1)],...

[Frame_E(3,1),Frame_F(3,1)],'blue','linewidth',3);

plot3([Frame_F(1,1),Frame_G(1,1)],...

[Frame_F(2,1),Frame_G(2,1)],...

[Frame_F(3,1),Frame_G(3,1)],'blue','linewidth',3);

%Select Destination

plot3([x_dest x_dest],[y_dest y_dest],[0 0],'r+')

drawnow;

start = clock();

Distance_to_Destination(1) = sqrt((x_dest-Frame_G(1,1))^2+(y_dest-Frame_G(2,1))^2)

if(sqrt(x_dest^2+y_dest^2)>Workspace)

text(0,0,'Outside of reachable space!!!!!!','FontSize',24,...

'FontWeight','bold','Color','r');

break;

end

%initialize distance to enter loop.

Tolerance = .5;

while(Distance_to_Destination(1) > Tolerance)

for m = 5:-1:1

First_Loop = 1;

for n=m:1:5

Distance_to_Destination(2) = 1000;

Destination_DH_Parms(3,n)=Destination_DH_Parms(3,n)+1;

Temp_Calc=Revolute_Transformation(Destination_DH_Parms(:,1))*...

Revolute_Transformation(Destination_DH_Parms(:,2))*...

Revolute_Transformation(Destination_DH_Parms(:,3))*...

Revolute_Transformation(Destination_DH_Parms(:,4))*...

Revolute_Transformation(Destination_DH_Parms(:,5))*...

Revolute_Transformation(Destination_DH_Parms(:,6))*Frame_A;

if(Distance_to_Destination(1)<...

(sqrt((Temp_Calc(1,1)-x_dest)^2+(Temp_Calc(2,1)-y_dest)^2)))

Rotation=-1;

else

Rotation=1;

end

Destination_DH_Parms(3,n)=Destination_DH_Parms(3,n)-1;

while((Distance_to_Destination(2)>Distance_to_Destination(1))&&...

(Distance_to_Destination(1)>Tolerance))

Destination_DH_Parms(3,n)=Destination_DH_Parms(3,n)+...

Angle_Resolution*Rotation;

Frame_B=Revolute_Transformation(Destination_DH_Parms(:,1))*Frame_A;

Frame_C=Revolute_Transformation(Destination_DH_Parms(:,1))*...

Revolute_Transformation(Destination_DH_Parms(:,2))*Frame_A;

Frame_D=Revolute_Transformation(Destination_DH_Parms(:,1))*...

Revolute_Transformation(Destination_DH_Parms(:,2))*...

Revolute_Transformation(Destination_DH_Parms(:,3))*Frame_A;

Frame_E=Revolute_Transformation(Destination_DH_Parms(:,1))*...

Revolute_Transformation(Destination_DH_Parms(:,2))*...

Revolute_Transformation(Destination_DH_Parms(:,3))*...

Revolute_Transformation(Destination_DH_Parms(:,4))*Frame_A;

Frame_F=Revolute_Transformation(Destination_DH_Parms(:,1))*...

Revolute_Transformation(Destination_DH_Parms(:,2))*...

Revolute_Transformation(Destination_DH_Parms(:,3))*...

Revolute_Transformation(Destination_DH_Parms(:,4))*...

Revolute_Transformation(Destination_DH_Parms(:,5))*Frame_A;

Frame_G=Revolute_Transformation(Destination_DH_Parms(:,1))*...

Revolute_Transformation(Destination_DH_Parms(:,2))*...

Revolute_Transformation(Destination_DH_Parms(:,3))*...

Revolute_Transformation(Destination_DH_Parms(:,4))*...

Revolute_Transformation(Destination_DH_Parms(:,5))*...

Revolute_Transformation(Destination_DH_Parms(:,6))*Frame_A;

Distance_to_Destination(2) = Distance_to_Destination(1);

Distance_to_Destination(1) = sqrt((x_dest-Frame_G(1,1))^2+(y_dest-

Frame_G(2,1))^2);

end

end

end

if(Distance_to_Destination(1)<10)

Angle_Resolution=1;

end

end

%plot each displacement vector in green and P vector in cyan

plot3([Frame_A(1,1),Frame_B(1,1)],...

[Frame_A(2,1),Frame_B(2,1)],...

[Frame_A(3,1),Frame_B(3,1)],'red','linewidth',3);

plot3([Frame_B(1,1),Frame_C(1,1)],...

[Frame_B(2,1),Frame_C(2,1)],...

[Frame_B(3,1),Frame_C(3,1)],'red','linewidth',3);

plot3([Frame_C(1,1),Frame_D(1,1)],...

[Frame_C(2,1),Frame_D(2,1)],...

[Frame_C(3,1),Frame_D(3,1)],'red','linewidth',3);

plot3([Frame_D(1,1),Frame_E(1,1)],...

[Frame_D(2,1),Frame_E(2,1)],...

[Frame_D(3,1),Frame_E(3,1)],'red','linewidth',3);

plot3([Frame_E(1,1),Frame_F(1,1)],...

[Frame_E(2,1),Frame_F(2,1)],...

[Frame_E(3,1),Frame_F(3,1)],'red','linewidth',3);

plot3([Frame_F(1,1),Frame_G(1,1)],...

[Frame_F(2,1),Frame_G(2,1)],...

[Frame_F(3,1),Frame_G(3,1)],'red','linewidth',3);

finish=clock()-start

The following code represents the same implementation of the CCD algorithm with the following differences:

 It uses the same initial joint parameters and destination as the first script. o The first script must be executed first to generate these parameters

 It only draws the initial and final solutions, with no intermediate steps.

 Generally arrives at the solutions in less than 1 second when destination is not near edge of workspace %Author: Davis

%Date: 21Jul14

%Description: Homework 4, problem 1 Solution only

Destination_DH_Parms=Initial_DH_Parms;

Workspace = sum(Initial_DH_Parms(2,:));

cla;

hold on; %retains plots between plot statements

grid on;

axis([-1*Workspace,Workspace,-1*Workspace,Workspace,-1*Workspace,Workspace]); %defines

min_x, max_x, ...

view([0,0,15]); %defines point of view

Angle_Resolution = 1;

Frame_A=[0,1,0,0,0,0;

0,0,0,1,0,0;

0,0,0,0,0,1;

1,1,1,1,1,1];

Frame_B=Revolute_Transformation(Destination_DH_Parms(:,1))*Frame_A;

Frame_C=Revolute_Transformation(Destination_DH_Parms(:,1))*...

Revolute_Transformation(Destination_DH_Parms(:,2))*Frame_A;

Frame_D=Revolute_Transformation(Destination_DH_Parms(:,1))*...

Revolute_Transformation(Destination_DH_Parms(:,2))*...

Revolute_Transformation(Destination_DH_Parms(:,3))*Frame_A;

Frame_E=Revolute_Transformation(Destination_DH_Parms(:,1))*...

Revolute_Transformation(Destination_DH_Parms(:,2))*...

Revolute_Transformation(Destination_DH_Parms(:,3))*...

Revolute_Transformation(Destination_DH_Parms(:,4))*Frame_A;

Frame_F=Revolute_Transformation(Destination_DH_Parms(:,1))*...

Revolute_Transformation(Destination_DH_Parms(:,2))*...

Revolute_Transformation(Destination_DH_Parms(:,3))*...

Revolute_Transformation(Destination_DH_Parms(:,4))*...

Revolute_Transformation(Destination_DH_Parms(:,5))*Frame_A;

Frame_G=Revolute_Transformation(Destination_DH_Parms(:,1))*...

Revolute_Transformation(Destination_DH_Parms(:,2))*...

Revolute_Transformation(Destination_DH_Parms(:,3))*...

Revolute_Transformation(Destination_DH_Parms(:,4))*...

Revolute_Transformation(Destination_DH_Parms(:,5))*...

Revolute_Transformation(Destination_DH_Parms(:,6))*Frame_A;

%plot each displacement vector in green and P vector in cyan

plot3([Frame_A(1,1),Frame_B(1,1)],...

[Frame_A(2,1),Frame_B(2,1)],...

[Frame_A(3,1),Frame_B(3,1)],'blue','linewidth',3);

plot3([Frame_B(1,1),Frame_C(1,1)],...

[Frame_B(2,1),Frame_C(2,1)],...

[Frame_B(3,1),Frame_C(3,1)],'blue','linewidth',3);

plot3([Frame_C(1,1),Frame_D(1,1)],...

[Frame_C(2,1),Frame_D(2,1)],...

[Frame_C(3,1),Frame_D(3,1)],'blue','linewidth',3);

plot3([Frame_D(1,1),Frame_E(1,1)],...

[Frame_D(2,1),Frame_E(2,1)],...

[Frame_D(3,1),Frame_E(3,1)],'blue','linewidth',3);

plot3([Frame_E(1,1),Frame_F(1,1)],...

[Frame_E(2,1),Frame_F(2,1)],...

[Frame_E(3,1),Frame_F(3,1)],'blue','linewidth',3);

plot3([Frame_F(1,1),Frame_G(1,1)],...

[Frame_F(2,1),Frame_G(2,1)],...

[Frame_F(3,1),Frame_G(3,1)],'blue','linewidth',3);

%Select Destination

plot3([x_dest x_dest],[y_dest y_dest],[0 0],'r+')

drawnow;

start = clock();

Distance_to_Destination(1) = sqrt((x_dest-Frame_G(1,1))^2+(y_dest-Frame_G(2,1))^2)

if(sqrt(x_dest^2+y_dest^2)>Workspace)

text(0,0,'Outside of reachable space!!!!!!','FontSize',24,...

'FontWeight','bold','Color','r');

break;

end

%initialize distance to enter loop.

Tolerance = .5;

while(Distance_to_Destination(1) > Tolerance)

for m = 5:-1:1

First_Loop = 1;

for n=m:1:5

Distance_to_Destination(2) = 1000;

Destination_DH_Parms(3,n)=Destination_DH_Parms(3,n)+1;

Temp_Calc=Revolute_Transformation(Destination_DH_Parms(:,1))*...

Revolute_Transformation(Destination_DH_Parms(:,2))*...

Revolute_Transformation(Destination_DH_Parms(:,3))*...

Revolute_Transformation(Destination_DH_Parms(:,4))*...

Revolute_Transformation(Destination_DH_Parms(:,5))*...

Revolute_Transformation(Destination_DH_Parms(:,6))*Frame_A;

if(Distance_to_Destination(1)<...

(sqrt((Temp_Calc(1,1)-x_dest)^2+(Temp_Calc(2,1)-y_dest)^2)))

Rotation=-1;

else

Rotation=1;

end

Destination_DH_Parms(3,n)=Destination_DH_Parms(3,n)-1;

while((Distance_to_Destination(2)>Distance_to_Destination(1))&&...

(Distance_to_Destination(1)>Tolerance))

Destination_DH_Parms(3,n)=Destination_DH_Parms(3,n)+...

Angle_Resolution*Rotation;

Frame_B=Revolute_Transformation(Destination_DH_Parms(:,1))*Frame_A;

Frame_C=Revolute_Transformation(Destination_DH_Parms(:,1))*...

Revolute_Transformation(Destination_DH_Parms(:,2))*Frame_A;

Frame_D=Revolute_Transformation(Destination_DH_Parms(:,1))*...

Revolute_Transformation(Destination_DH_Parms(:,2))*...

Revolute_Transformation(Destination_DH_Parms(:,3))*Frame_A;

Frame_E=Revolute_Transformation(Destination_DH_Parms(:,1))*...

Revolute_Transformation(Destination_DH_Parms(:,2))*...

Revolute_Transformation(Destination_DH_Parms(:,3))*...

Revolute_Transformation(Destination_DH_Parms(:,4))*Frame_A;

Frame_F=Revolute_Transformation(Destination_DH_Parms(:,1))*...

Revolute_Transformation(Destination_DH_Parms(:,2))*...

Revolute_Transformation(Destination_DH_Parms(:,3))*...

Revolute_Transformation(Destination_DH_Parms(:,4))*...

Revolute_Transformation(Destination_DH_Parms(:,5))*Frame_A;

Frame_G=Revolute_Transformation(Destination_DH_Parms(:,1))*...

Revolute_Transformation(Destination_DH_Parms(:,2))*...

Revolute_Transformation(Destination_DH_Parms(:,3))*...

Revolute_Transformation(Destination_DH_Parms(:,4))*...

Revolute_Transformation(Destination_DH_Parms(:,5))*...

Revolute_Transformation(Destination_DH_Parms(:,6))*Frame_A;

Distance_to_Destination(2) = Distance_to_Destination(1);

Distance_to_Destination(1) = sqrt((x_dest-Frame_G(1,1))^2+(y_dest-

Frame_G(2,1))^2);

end

end

end

if(Distance_to_Destination(1)<10)

Angle_Resolution=.1;

end

end

%plot each displacement vector in green and P vector in cyan

plot3([Frame_A(1,1),Frame_B(1,1)],...

[Frame_A(2,1),Frame_B(2,1)],...

[Frame_A(3,1),Frame_B(3,1)],'red','linewidth',3);

plot3([Frame_B(1,1),Frame_C(1,1)],...

[Frame_B(2,1),Frame_C(2,1)],...

[Frame_B(3,1),Frame_C(3,1)],'red','linewidth',3);

plot3([Frame_C(1,1),Frame_D(1,1)],...

[Frame_C(2,1),Frame_D(2,1)],...

[Frame_C(3,1),Frame_D(3,1)],'red','linewidth',3);

plot3([Frame_D(1,1),Frame_E(1,1)],...

[Frame_D(2,1),Frame_E(2,1)],...

[Frame_D(3,1),Frame_E(3,1)],'red','linewidth',3);

plot3([Frame_E(1,1),Frame_F(1,1)],...

[Frame_E(2,1),Frame_F(2,1)],...

[Frame_E(3,1),Frame_F(3,1)],'red','linewidth',3);

plot3([Frame_F(1,1),Frame_G(1,1)],...

[Frame_F(2,1),Frame_G(2,1)],...

[Frame_F(3,1),Frame_G(3,1)],'red','linewidth',3);

finish=clock()-start