Bogazici University

ME634 Robotics
Assignmet II
Kadir Cumali
2 December 2016

Q1. Cubic Trajectory Generation

a)

i) Joint variable space

Figure 1: Two-links manipulator

It is asked us to choose a via point to avoid the obstacle and compute a cubic polynomial trajectory for
joint space and end-effector space. In order to pass to joint variable space, we need to calculate the joint
variables from the given position of the end-effector. The knowns are the location of the inital position
P0

 r
10, 55s, position of the final point PF  r55, 50s, and the location of the obstacle PC  r30, 60s.

l1 and l2 are the links lengths.

2
1

 cos
1 p P0 p1q

P0 p2q2
l12
l22

q
2l1 l2

 atan2pP0 p2q, P0 p1qq
atan2pl2 sinp2 q, l1

(1)
l2 cosp2 qq

For two values of 2 , there are two 1 values. Let us call the joint variables p
polynomial is in the form,

q ptq  a0

a2 t2

a1 t

a 3 t3

(2)

 r1 , 2 s.

A cubic
(3)

The velocity and acceleration functions are thus,

q9ptq  a1

2a2 t

q:ptq  2a2

3a3 t2

(4)

6a3 t

When the trajectory need to avoid some obstacles, that means the trajectory should not pass through
some points. To make it simple, that means the trajectory should pass through some points that can
avoid hitting the obstacles. The most simple idea is to choose an appropriate velocity so that the
acceleration is continuous in this point. Since there is an obstacle, there must be two paths from inital
position to via point and from there to goal position.

Since there are 2 paths, that means there is 2 displacement equations.

q1 ptq  a0
q2 ptq  b0

a1 t

a2 t2

a3 t3

b1 t

b2 t2

b3 t 3

t P rt0 , tC s

(5)

t P rtC , tF s

In order to provide the continuos acceleration during the motion of the robotic arm, it is required
to solve 2 equations and equal the acceleration equations. To solve for the unknown coefficients, we
need to have the boundary condition for the first trajectory that is from initial position to via point.
Considering the start time t0 is zero, the boundary conditions:

$
,
'
q1 pt0 q  A1 /
'
/
'
& q1 ptC q  B1 /
.
'
q1 pt0 q  C1 /
'
/
'
'
/
% q1 ptC q  D1 /
   
A1
a0
0
0

 



B
a
t2C
t3C 
1
1



 
0
0 
 C1 
a2  

9


1

1
0

0
tC
1

0
a0
q91 pt0 q

 A1 ,

 0  C1

a0

a1 tC

2tC

3t2C

a3 t3C

 B1 ,

a2 t2C

a3
a1

D1

 C1 ,

a1

2a2 tC

3a3 t2C

 D1

is given. Assuming that we estimate the displacement time from initial position to

obstable, it is obtained three equations regarding the first trajectory.

 A1
 B1
3a3 t2C  D1
a0

a2 t2C

a0

2a2 tC

a3 t3C

(6)

For the second trajectory, lets constitute the equations. The trajectory starts from the obstacle to goal
position. The boundary conditions:

,
$
/
'
q
p
t
q

A
2
C
2
/
'
'
/
.
& q2 ptF q  B2 /
'
q2 ptC q  C2 /
'
/
'
/
'
% q2 ptF q  D2 /
   
t2C
t3C
b
A2
  0  


t2F
t3F 
b
B
1
2



 
2tC 3t2C 
 b2  
 C2 
9


1

1

0

tC
tF
1

0
q92 ptF q

 0  D2

2tF

3t2F

b3

D2

is given. Assuming that we know the displacement time from obstable to the goal

position, it is obtained four equations regarding the first trajectory.

b0
b0

b1 t C

b2 t2C

b1 tF

b2 t2F

b1

2b2 tC

b1

2b2 tF

 A2
 B2
3b3 t2C  C2
3b3 t2F  D2
b3 t3C
b3 t3F

(7)

For the continuos acceleration, the two accelerations at the via point must be equal. t0C represents
the time until the end-effector reaches the via point from the inital position. tCF is the time until the
2

end-effector reaches the goal from the point position selected.

2a2
To simplify the equations, we take t0C

6a3 t0C

 2b2

6b3 tCF

(8)

 tCF . Thus, a2  b2 , and a3  b3 . Moreover, the velocity must

 C2 and a1  b1  0. Lets call the

be a specific value for the continuos acceleration. Therefore, D1

q pt0 q, and q ptF q as q0 and qF respectively. From the equation (7),

b2 t2F

b3 t3F
3b3 t2F

2b2 tF

 qF
q0
0

(9)

We know the time of the movement, which is tF , and inital and the goal positions and also tC . Thus, it
can be solved for b2 , and b3 . The specific velocity at the via point equals to,
q9C

 2b2 tC

3b3 t2C

(10)

ii) End-Effector Cartesian Space

Lets define cosp1 q and sinp1 q as c1 and s1 . We can write the end-effector position associated with the

joint variables p  r1 , 2 s using the geometric approach.

X
Y

X9

Y:

l2 c12

 p
l1 s1
l2 s12 q1
l2 s12 2

 pl1 c1 l2 c12 q1 l2 c12 2

 p
l1 c1
l2 c12 q1
pl1 s1

 p
l1 s1
l2 s12 q1 pl1 c1
9

(11)

l2 s12

Y9

:
X

 l1 c1
 l1 s1

(12)

l2 s12 q:1
l2 c12 91
2l2 c12 92
l2 s12 :2

l2 c12 q:1
l2 s12 91
2l2 s12 92

l2 c12 :2

(13)

Q2. Potential Field Path Planning

In this question, potential field functions (attractive and repulsive ) and vectors are used for path
planning. The objective is to move an end effector of two link robot arm from start point to goal point
while avoiding from an obstacle. In order to apply this approach to robot motion planning, the arm
and the obstacle to be avoided are thought as an positive charge and the goal point is considered as a
negative charge. Hence, the manipulator will stay away from the obstacle and tend to move towards
goal.
Considering the approach of Khatib (1986), the conic and quadratic potentials are used. Conic potential
calculates the Euclidean distance between robot and goal point. This potential is used when robot is
far away from the goal, thus the attractive potential becomes large and robot moves faster. Quadratic
potential defined to overcome the discontinuities in the gradient vectors caused by world configuration. A
threshold dgoal is defined to switch between conic and quadratic potentials. The repulsive force, prevents
the collision. While far away from the obstacle, there will be no repulsive force affecting the robot.

Figure 2: Potential fields

Figure 3: Gradient of potential fields

Figure 4: Potential fields

Figure 5: Path planning

Appendices

Q.1.b.Contents

SET UP
inverse kinematics for initial position
inverse kinematics for via point
inverse kinematics for goal position
for theta1
for theta2
for end effector space

%This code models cubic trajectory for two-link manipulator when an

%obstacle exist in the path.

SET UP
clc
clear
close all
%student name
student_name=Kadir Cumali;

inverse kinematics for initial position

Px=-10;Py=55;a1=50;a2=25;
theta2=acos((Px^2+Py^2-a1^2-a2^2)/(2*a1*a2));
theta2_2=2*pi-theta2;
% theta1=atan2(Py,Px)-atan2(a2*sin(theta2),a1+a2*cos(theta2)); %for 1.theta2
theta1_2=atan2(Py,Px)-atan2(a2*sin(theta2_2),a1+a2*cos(theta2_2)); %for 2.theta2

inverse kinematics for via point

Pxv=30;Pyv=30;
theta2v=acos((Pxv^2+Pyv^2-a1^2-a2^2)/(2*a1*a2));
theta2_2v=2*pi-theta2v;
% theta1v=atan2(Pyv,Pxv)-atan2(a2*sin(theta2v),a1+a2*cos(theta2v)); %for 1.theta2
theta1_2v=atan2(Pyv,Pxv)-atan2(a2*sin(theta2_2v),a1+a2*cos(theta2_2v)); %for 2.theta2

inverse kinematics for goal position

Pxf=55;Pyf=50;
theta2f=acos((Pxf^2+Pyf^2-a1^2-a2^2)/(2*a1*a2));
theta2_2f=2*pi-theta2f;
% theta1v=atan2(Pyf,Pxf)-atan2(a2*sin(theta2f),a1+a2*cos(theta2f)); %for 1.theta2
7

theta1_2f=atan2(Pyf,Pxf)-atan2(a2*sin(theta2_2f),a1+a2*cos(theta2_2f)); %for 2.theta2

for theta1
tv=2;Vv=-0.518325 ; tf=4;Vf=0;
Cv=[1 0 0 0
1 tv tv^2 tv^3
0 1 0 0
0 1 2*tv 3*tv^2];
Rv=[theta1_2;theta1_2v;0;Vv];
Av=inv(Cv)*Rv;
t=0:0.02:tv;

%time interval

qv=Av(1)+Av(2)*t+Av(3)*t.^2+Av(4)*t.^3; %path for via

qv_dot=Av(2)+2*Av(3)*t+3*Av(4)*t.^2;

%velocity for theta1 until via point

qv_ddot=2*Av(3)+6*Av(4)*t;

%acc. for theta1 until via point

% from via point to final

Cf=[1 tv tv^2 tv^3
1 tf tf^2 tf^3
0 1 2*tv 3*tv^2
0 1 2*tf 3*tf^2];
Rf=[theta1_2v; theta1_2f;Vv;Vf];
Af=inv(Cf)*Rf;
tf=tv:0.02:4;
qf=Af(1)+Af(2)*tf+Af(3)*tf.^2+Af(4)*tf.^3; %path for final
qf_dot=Af(2)+2*Af(3)*tf+3*Af(4)*tf.^2;
qf_ddot=2*Af(3)+6*Af(4)*tf;
%plotting

%velocity for theta1 from via to final

%velocity for theta1 until via point

subplot(2,2,1);
plot(t,qv*180/pi,LineWidth,2);
hold on
plot(tf,qf*180/pi,LineWidth,2);
hl = legend(${\theta_1}$);
set(hl, Interpreter, latex);
subplot(2,2,2);
plot(t,qv_dot,LineWidth,2);
hold on
plot(tf,qf_dot,LineWidth,2);
hl = legend($\dot{\theta_1}$);
set(hl, Interpreter, latex);
subplot(2,2,[3,4]);
plot(t,qv_ddot,LineWidth,2);
hold on
plot(tf,qf_ddot,LineWidth,2);
hl = legend($\ddot{\theta_1}$);
set(hl, Interpreter, latex);

figure;
150

0.5
31

31

100

50

-0.5

-1
0

0.5
31

-0.5

-1
0

0.5

1.5

2.5

3.5

for theta2
tv=2; Vv2=0.482625; tf=4;Vf=0;
Cv_2=[1 0 0 0
1 tv tv^2 tv^3
0 1 0 0
0 1 2*tv 3*tv^2];
Rv_2=[theta2_2; theta2_2v;0;Vv2];
Av_2=inv(Cv_2)*Rv_2;
t=0:0.02:tv;
qv2=Av_2(1)+Av_2(2)*t+Av_2(3)*t.^2+Av_2(4)*t.^3; %path for via theta2
qv2_dot=Av_2(2)+2*Av_2(3)*t+3*Av_2(4)*t.^2;

%velocity for theta2 until via point

qv2_ddot=2*Av_2(3)+6*Av_2(4)*t;

%acc. for theta2 until via point

% from via point to final

Cf_2=[1 tv tv^2 tv^3
1 tf tf^2 tf^3
0 1 2*tv 3*tv^2
0 1 2*tf 3*tf^2];
Rf_2=[theta2_2v; theta2_2f;Vv2;Vf];
Af_2=inv(Cf_2)*Rf_2;
tf=tv:0.02:4;
qf2=Af_2(1)+Af_2(2)*tf+Af_2(3)*tf.^2+Af_2(4)*tf.^3;

%path for final

qf2_dot=Af_2(2)+2*Af_2(3)*tf+3*Af_2(4)*tf.^2;

%velocity for theta1 from via to final

qf2_ddot=2*Af_2(3)+6*Af_2(4)*tf;

%acc. for theta1 until via point

subplot(2,2,1);
plot(t,qv2*180/pi,LineWidth,2);
hold on
plot(tf,qf2*180/pi,LineWidth,2);
hl = legend(${\theta_2}$);
set(hl, Interpreter, latex);
subplot(2,2,2);
plot(t,qv2_dot,LineWidth,2);
hold on
plot(tf,qf2_dot,LineWidth,2);
hl = legend($\dot{\theta_2}$);
set(hl, Interpreter, latex);
subplot(2,2,[3,4]);
plot(t,qv2_ddot,LineWidth,2);
hold on
plot(tf,qf2_ddot,LineWidth,2);
hl = legend($\ddot{\theta_2}$);

10

set(hl, Interpreter, latex);

figure;
350

2
32

32

300

250

200

-1
0

2
32

-2

-4
0

0.5

1.5

2.5

11

3.5

for end effector space

Xv=a1*cos(qv)+a2*cos(qv+qv2);
Yv=a1*sin(qv)+a2*sin(qv+qv2);
Xf=a1*cos(qf)+a2*cos(qf+qf2);
Yf=a1*sin(qf)+a2*sin(qf+qf2);
subplot(2,1,1);
plot(t,Xv,LineWidth,2);
ylabel(X-position(mm));
hold on
grid on
plot(tf,Xf,LineWidth,2);
subplot(2,1,2);
plot(t,Yv,LineWidth,2);
hold on
plot(tf,Yf,LineWidth,2);
grid on
xlabel(time(s));
ylabel(Y-position(mm));
figure;
%X_dot until the via point
for i=1:length(qv)
Xv_dot(i)=(-a1*sin(qv(i))-a2*sin(qv(i)+qv2(i)))*qv_dot(i)-(a2*sin(qv(i)+qv2(i)))*qv2_dot(i);
Yv_dot(i)=(a1*cos(qv(i))+a2*cos(qv(i)+qv2(i)))*qv_dot(i)+(a2*sin(qv(i)+qv2(i)))*qv2_dot(i);
end
%X_dot until the goal from the via point
for i=1:length(qf)
Xf_dot(i)=(-a1*sin(qf(i))-a2*sin(qf(i)+qf2(i)))*qf_dot(i)-(a2*sin(qf(i)+qf2(i)))*qf2_dot(i);
Yf_dot(i)=(a1*cos(qf(i))+a2*cos(qf(i)+qf2(i)))*qf_dot(i)+(a2*sin(qf(i)+qf2(i)))*qf2_dot(i);
XYf_dot(i)=sqrt(Xf_dot(i)^2+Yf_dot(i)^2);
end
%plotting X_dot and Y_dot
subplot(2,1,1);
plot(t,Xv_dot,LineWidth,2);
hold on
plot(tf,Xf_dot,LineWidth,2);
hl = legend($\dot{X}$);
set(hl, Interpreter, latex);
subplot(2,1,2);
plot(t,Yv_dot,LineWidth,2);
hold on
plot(tf,Yf_dot,LineWidth,2);
hl = legend($\dot{Y}$);
set(hl, Interpreter, latex);
xlabel(time(s));
ylabel(velocity(mm/s));

12

figure;
%X_ddot and Y_ddot in the end-effector space
for i=1:length(qv)

Xv_ddot(i)=(-a1*cos(qv(i))-a2*cos(qv(i)+qv2(i)))*qv_dot(i)-(a1*sin(qv(i))+a2*sin(qv(i)+qv2(i)))*qv_dd
-2*a2*cos(qv(i)+qv2(i))*qv2_dot(i)-a2*cos(qv(i)+qv2(i))*qv_dot(i)-a2*sin(qv(i)+qv2(i))*qv2_ddot(i);

Yv_ddot(i)=(-a1*sin(qv(i))-a2*sin(qv(i)+qv2(i)))*qv_dot(i)+(a1*cos(qv(i))+a2*cos(qv(i)+qv2(i)))*qv_dd
-2*a2*sin(qv(i)+qv2(i))*qv2_dot(i)-a2*sin(qv(i)+qv2(i))*qv_dot(i)+a2*cos(qv(i)+qv2(i))*qv2_ddot(i);
end
for i=1:length(qf)

Xf_ddot(i)=(-a1*cos(qf(i))-a2*cos(qf(i)+qf2(i)))*qf_dot(i)-(a1*sin(qf(i))+a2*sin(qf(i)+qf2(i)))*qf_dd
-2*a2*cos(qf(i)+qf2(i))*qf2_dot(i)-a2*cos(qf(i)+qf2(i))*qf_dot(i)-a2*sin(qf(i)+qf2(i))*qf2_ddot(i);

Yf_ddot(i)=(-a1*sin(qf(i))-a2*sin(qf(i)+qf2(i)))*qf_dot(i)+(a1*cos(qf(i))+a2*cos(qf(i)+qf2(i)))*qf_dd
-2*a2*sin(qf(i)+qf2(i))*qf2_dot(i)-a2*sin(qf(i)+qf2(i))*qf_dot(i)+a2*cos(qf(i)+qf2(i))*qf2_ddot(i);
end
%plotting X_ddot and Y_ddot
subplot(2,1,1);
plot(t,Xv_ddot,LineWidth,2);
hold on
plot(tf,Xf_ddot,LineWidth,2);
hl = legend($\ddot{X}$);
set(hl, Interpreter, latex);
subplot(2,1,2);
plot(t,Yv_ddot,LineWidth,2);
hold on
plot(tf,Yf_ddot,LineWidth,2);
hl = legend($\ddot{Y}$);
set(hl, Interpreter, latex);
xlabel(time(s));
ylabel(accelaration(mm/s^2));

X-position(mm)

60
40
20
0
-20
0

0.5

1.5

0.5

1.5

2.5

3.5

2.5

3.5

Y-position(mm)

60
50
40
30
20

time(s)

13

30
X

20
10
0
-10
0

0.5

1.5

2.5

3.5

20

velocity(mm/s)

-20

-40
0

0.5

1.5

2.5

3.5

time(s)
100
X

50
0
-50
-100

accelaration(mm/s 2 )

0.5

1.5

2.5

3.5

100
Y

50

-50
0

0.5

1.5

2.5

3.5

time(s)

Contents

SET UP
Q2.a creat a 2-DOF RR robot name of Kadir
joint space
cartesian space

%This code models Stanford robotic manipulator using the @Robotics

%Toolbox. It is for the question 2.a,b,c.

SET UP
clc
close all
%student name
14

student_name=Kadir Cumali;
%seting up the toolbox
startup_rvc;

Robotics, Vision & Control: (c) Peter Corke 1992-2011 http://www.petercorke.com

- Robotics Toolbox for Matlab (release 9.10)
- pHRIWARE (release 1.1): pHRIWARE is Copyrighted by Bryan Moutrie (2013-2016) (c)
Run rtbdemo to explore the toolbox

Q2.a creat a 2-DOF RR robot name of Kadir

L(1) = Link([0, 0, 50, 0,0]); %revolute 1
L(2) = Link([0, 0, 25, 0,0]);

%revolute 2

%linking the joints

robo = SerialLink(L, name, Kadir);
%displaying robot
robo.display;

robo =
Kadir (2 axis, RR, stdDH, fastRNE)
+---+-----------+-----------+-----------+-----------+-----------+
| j |

theta |

d |

a |

alpha |

offset |

+---+-----------+-----------+-----------+-----------+-----------+
|

1|

q1|

0|

50|

0|

0|

2|

q2|

0|

25|

0|

0|

+---+-----------+-----------+-----------+-----------+-----------+
grav =

base = 1

9.81
0

tool =
0

0
0

0
0

joint space
q_0=[2.2143 4.7124]; %initial point
q_p=[1.3086 4.1538]; %via point
q_f=[0.8321 5.9994]; %final point
%%pats
tempo=0:0.1:4;

%time
15

qa=jtraj(q_0,q_p,tempo);
qb=jtraj(q_p,q_f,tempo);
W = [-60 60 0 100 0 60];

%workspace for the robot

C=[30,60];
circle(C,10);

%obstacle

hold on
robo.plot(qa,workspace, W);
robo.plot(qb,workspace, W);
title(Joint space);
figure;

16

cartesian space
T0=robo.fkine(q_0);

% forward kinemetics for the inital position

Tp=robo.fkine(q_p);

% forward kinemetics for via point

Tf=robo.fkine(q_f);

% forward kinemetics for goal position

q1=ctraj(T0,Tp,50);
q2=ctraj(Tp,Tf,50);

%N=50 samples

circle(C,10);
hold on
grid on
tranimate(q1);
tranimate(q2);
title(End-effector space);

17

End-effector space

70
65
60
55

YZ X

50
45
40
35
Y
ZX

30
25
20

25

30

35

40

45

50

55

Q1.c.Contents

SET UP
Q2.a creat a 2-DOF RR robot name of Kadir
joint space
cartesian space

SET UP
clc
close all
%student name
student_name=Kadir Cumali;
%seting up the toolbox
startup_rvc;

Robotics, Vision & Control: (c) Peter Corke 1992-2011

Q2.a creat a 2-DOF RR robot name of Kadir

L(1) = Link([0, 0, 50, 0,0]); %revolute 1
L(2) = Link([0, 0, 25, 0,0]);

%revolute 2

%linking the joints

robo = SerialLink(L, name, Kadir);

18

60

%displaying robot
robo.display;

robo =
Kadir (2 axis, RR, stdDH, fastRNE)
+---+-----------+-----------+-----------+-----------+-----------+
| j |

theta |

d |

a |

alpha |

offset |

+---+-----------+-----------+-----------+-----------+-----------+
|

1|

q1|

0|

50|

0|

0|

2|

q2|

0|

25|

0|

0|

+---+-----------+-----------+-----------+-----------+-----------+
grav =

base = 1

9.81
0

tool =
0

0
0

joint space
q_0=[2.2143 4.7124]; %initial point
q_p=[1.3086 4.1538]; %via point
q_f=[0.8321 5.9994]; %final point
%%pats
tempo=0:0.1:4;

%time

qa=jtraj(q_0,q_p,tempo);
qb=jtraj(q_p,q_f,tempo);
W = [-60 60 0 100 0 60];

%workspace for the robot

C=[30,60];
circle(C,10);

%obstacle

hold on
robo.plot(qa,workspace, W);
robo.plot(qb,workspace, W);
title(Joint space);
figure;

19

cartesian space
T0=robo.fkine(q_0);

% forward kinemetics for the inital position

Tp=robo.fkine(q_p);

% forward kinemetics for via point

Tf=robo.fkine(q_f);

% forward kinemetics for goal position

q1=ctraj(T0,Tp,50);
q2=ctraj(Tp,Tf,50);

%N=50 samples

circle(C,10);
hold on
grid on
tranimate(q1);
tranimate(q2);
title(End-effector space);

20

End-effector space

70
65
60
55

YZ X

50
45
40
35
Y
ZX

30
25
20

25

30

35

40

45

50

55

Q2.a.Contents
SET UP
map

SET UP
clc
clear
close all
%student name
student_name=Kadir Cumali;

map
pos = [-60 0 120 100];
%initial and the final position of the end-efefctor
x0=-10;
xf=55;
y0=55;
yf=50;
rectangle(Position,pos);
hold on
plot(x0,y0,O,LineWidth,2);
hold on
plot(xf,yf,X,LineWidth,2);
hold on
%obstacle
Obs_pos=[20,50,20,20];
r=10;
21

60

Obs_x=30;
Obs_y=60;
rectangle(Position,Obs_pos,Curvature,[1 1],Facecolor,r)
%attractive function
d=10;
p0=2;
[X,Y]=meshgrid(-100:1:100);
ksi=1;
eta=12;
for i=1:length(X)
for j=1:length(Y)
dist=sqrt((X(i,j)-xf)^2+(Y(i,j)-yf)^2);
if dist<d
U_att(i,j)=0.5*ksi*((X(i,j)-xf)^2+(Y(i,j)-yf)^2);
else
U_att(i,j)=d*ksi*sqrt((X(i,j)-xf)^2+(Y(i,j)-yf)^2)-0.5*ksi*d^2;
end
end
end
% repulsive function
for i=1:length(X)
for j=1:length(Y)
if (sqrt((X(i,j)-Obs_x)^2+(Y(i,j)-Obs_y)^2)-r) <= p0
U_rep(i,j)=0.5*eta*(1/(sqrt((X(i,j)-Obs_x)^2+(Y(i,j)-Obs_y)^2)-r)-1/p0)^2;
else
U_rep(i,j)=0;
end
end
end
for i=1:length(X)
for j=1:length(Y)
if sqrt((X(i,j)-Obs_x)^2+(Y(i,j)-Obs_y)^2)>=1.5*r
U_rep(i,j)=U_rep(i,j);
elseif sqrt((X(i,j)-Obs_x)^2+(Y(i,j)-Obs_y)^2)<1.5*r && sqrt((X(i,j)-Obs_x)^2+(Y(i,j)-Obs_y)^2)>r
U_rep(i,j)=U_rep(i,j);
else
U_rep(i,j)=2500;
end
if U_rep(i,j)~=2500 && U_rep(i,j)~=0
U_rep(i,j)=500;
U(i,j)=U_rep(i,j)+U_att(i,j);
else
U(i,j)=U_rep(i,j)+U_att(i,j);
22

end
end
end
meshc(X,Y,U)
title(Contour lines of potential surfaces (goal + obstacle));

Q2.b.Contents
fields
clc
clear
close all
pos = [-60 0 120 100];
%initial position and goal
x_0=-10;
x_f=55;
y_0=55;
y_f=50;
rectangle(Position,pos,Curvature,[1 1]);
hold on
plot(x_0,y_0,O,LineWidth,2);
hold on
plot(x_f,y_f,X,LineWidth,2);
%obstacle
r=10;
x_Obs=30;
y_Obs=60;
Obs=[x_Obs,y_Obs];

23

hold on
rectangle(Position,[x_Obs-r,y_Obs-r,2*r,2*r],Curvature,[1,1], FaceColor,r);
hold on
100
90
80
70
60
50
40
30
20
10
0
-60

-40

-20

20

40

60

fields
q0=[x_0;y_0];
qf=[x_f;y_f];
q_0=q0;
safetydistance=2;
d_star=sqrt((q_0(1)-qf(1))^2+(q_0(2)-qf(2))^2)*safetydistance;
speed=5;
time=0;
time_inc=0.001;
ksi=1; eta=12; %parameters for the potential functions
dis_inc=0.5;
while (q_0(1)>qf(1)+dis_inc || q_0(1)<qf(1)-dis_inc || q_0(2)>qf(2)+dis_inc || q_0(2)<qf(2)-dis_inc)
%attraction field
temp=q_0(end,:);
gradU_att=[0;0];
gradU_rep=[0;0];
d=sqrt((temp(1)-qf(1))^2+(temp(2)-qf(2))^2);
if d<=d_star
gradU_att=ksi*(temp-qf);
else
gradU_att=d_star*(temp-qf)/d;
end
%repulsion field
d_obst=sqrt((temp(1)-x_Obs)^2+(temp(2)-y_Obs)^2)-r;

24

qstar=r*2;
gradd_obst=(temp-Obs)/d_obst;
if d_obst<qstar
gradU_rep=gradU_rep+eta*((1/qstar-1/d_obst)*(1/d_obst^2))*gradd_obst;
end
gradU=-(gradU_att+gradU_rep);
temp=temp+gradU*time_inc;
q_0=[q_0;temp];
speed=sqrt(gradU(1)^2+gradU(2)^2);
time=time+time_inc;
end
plot(q_0(:,1),q_0(:,2),b,LineWidth,2);
100
90
80
70
60
50
40
30
20
10
0
-60

-40

-20

20

40

25

60