ROBOTIC CONTROL SYSTEMS

UZB 438E

PROBLEM SET 9
By
Enes Sagnak 110200063

Course given by:

Murad Abu-Khalaf

Department of Aeronautics and Astronautics

Aeronautics Engineering
CONTENTS

1 Question 1 2
1.1 Plots ......................................................................................................................... 11
1.2 Summary................................................................................................................... 11

2 Question 2 14
2.1 Summary................................................................................................................... 26

3 REFERENCES 27

1
1. QUESTION 1

Figure 1.1: Question 1

The code is based on the source code from Lecture 10 slides and originally demonstrated the use of
Probabilistic Roadmaps (PRM) for motion planning and control of a two-link elbow arm planar
manipulator. The environment was initially defined with a static obstacle and fixed starting and goal
positions. The code is modified for the environment to improve collision detection and tailor the planner to
suit their specific problem. This included adjustments to visibility conditions and the configuration of the
adjacency matrix to enhance collision avoidance.

Inverse kinematics adjustments were also made to improve the precision of the solutions. The step
size in inverse kinematics was made dynamic, adapting to the manipulatorâĂŹs configuration. This
change aimed to refine the path planning process and produce more accurate joint angle solutions.

The trajectory generation was modified to include path smoothing techniques using cubic polynomial
interpolation. These techniques help in creating smoother, more stable paths, which is crucial for
real-time applications where sudden changes in joint angles can lead to instability.

Additionally, for the kinematic planning, the proportional gain (Kp) was set to 50, and the velocity gain
(Kv) was tuned to 5, based on several tested values. The best results were achieved at these specific
settings, balancing path smoothness and stability.

2
 Enes SAGNAK UZB438E Homework3

The solution of the question is given below as a MATLAB code.

2 % Planning and Control for a Two Link Elbow Arm Planar Manipulator .
3 clc ; clear ; close all ;
4 rng ( ’ default ’) ; % Set the seed for the random number generator
5

6 % Create the environment map and the PRM ( Probabilistic Roadmap )

7 envMap = c r e a t e E n v i r o n m e n t M a p () ;
8 roadmap = createPRM ( envMap ) ;
9

10 % Define start and goal positions

11 startPosition = [0.9000; 1.3000]; % Starting position of the end - effector
12 goalPosition = [ -0.9000; -1.3000]; % Goal position of the end - effector
13

14 % Obtain a collision - free configuration for the End - Effector ’ s START position
15 for i = 1:5
16 StartConfig = g e t I n v e r s e K i n e m a t i c s ( startPosition , 2 * ( rand (2 , 1) - 0.5) * pi ) ;
17 [ jointX1 , jointY1 , endEffectorX , endEffectorY ] =
g e t F o r w a r d K i n e m a t i c s ( StartConfig (1) , StartConfig (2) ) ;
18 isCollision = d et ec t Co ll i si on ( envMap , jointX1 , endEffectorX , jointY1 , endEffectorY ,
10) ;
19 if ~ isCollision && sqrt (( endEffectorX - startPosition (1) ) ^2 + ( endEffectorY -
startPosition (2) ) ^2) < 0.1
20 break ; % Exit the loop if there is no collision and the end - effector is close to
the start position
21 end
22 end
23

24 % Add StartConfig to the PRM

25 n N o d e s B e f o r e A d d i n g = roadmap . numnodes ; % Get the current number of nodes
26 roadmap = roadmap . addnode (1) ; % Add a new node to the roadmap
27 roadmap . Nodes . sampl edConf igs ( end , :) = StartConfig ’; % Store the configuration in the
roadmap
28 roadmap . Nodes . s a m p l e d P o s i t i o n s ( end , :) = [ endEffectorX , endEffectorY ]; % Store the
end - effector position
29

30 % Find the nearest node in the PRM

31 s h o r t e s t D i s t a n c e = inf ; n e a r e s t N o d e I n d e x = 0;
32 for i = 1: n N o d e s B e f o r e A d d i n g
33 % Input must be row vectors
34 d = v i s i b l e C o n f i g u r a t i o n ( envMap , StartConfig ’ , roadmap . Nodes . sampl edConf igs (i ,:) , 5) ;
35 if d < s h o r t e s t D i s t a n c e
36 n e a r e s t N o d e I n d e x = i ; % Update the index of the nearest node
37 s h o r t e s t D i s t a n c e = d ; % Update the shortest distance
38 end
39 end
40

41 if isfinite ( s h o r t e s t D i s t a n c e )
42 roadmap = addedge ( roadmap , roadmap . numnodes , nearestNodeIndex , d ) ; % Add the new

3
 Enes SAGNAK UZB438E Homework3

node to the roadmap

43 end
44

45 % Obtain a collision - free configuration for the End - Effector ’ s GOAL position
46 for i = 1:5
47 GoalConfig = g e t I n v e r s e K i n e m a t i c s ( goalPosition , 2 * ( rand (2 , 1) - 0.5) * pi ) ;
48 [ jointX1 , jointY1 , endEffectorX , endEffectorY ] = g e t F o r w a r d K i n e m a t i c s ( GoalConfig (1) ,
GoalConfig (2) ) ;
49 isCollision = d et ec t Co ll i si on ( envMap , jointX1 , endEffectorX , jointY1 , endEffectorY ,
10) ;
50 if ~ isCollision && sqrt (( endEffectorX - goalPosition (1) ) ^2 + ( endEffectorY -
goalPosition (2) ) ^2) < 0.1
51 break ; % Exit the loop if there is no collision and the end - effector is close to
the goal position
52 end
53 end
54

55 % Add GoalConfig to the PRM

56 n N o d e s B e f o r e A d d i n g = roadmap . numnodes ; % Get the number of nodes before adding the goal
57 roadmap = roadmap . addnode (1) ; % Add a new node for the goal configuration
58 roadmap . Nodes . sampl edConf igs ( end , :) = GoalConfig ’; % Store the goal configuration
59 roadmap . Nodes . s a m p l e d P o s i t i o n s ( end , :) = [ endEffectorX , endEffectorY ]; % Store the
end - effector position
60

61 % Find the nearest node in the PRM for the goal configuration
62 s h o r t e s t D i s t a n c e = inf ; n e a r e s t N o d e I n d e x = 0;
63 for i = 1: n N o d e s B e f o r e A d d i n g
64 % Input must be row vectors
65 d = v i s i b l e C o n f i g u r a t i o n ( envMap , GoalConfig ’ , roadmap . Nodes . s ampled Config s (i ,:) , 5) ;
66 if d < s h o r t e s t D i s t a n c e
67 n e a r e s t N o d e I n d e x = i ; % Update the index of the nearest node
68 s h o r t e s t D i s t a n c e = d ; % Update the shortest distance
69 end
70 end
71

72 if isfinite ( s h o r t e s t D i s t a n c e )
73 roadmap = addedge ( roadmap , roadmap . numnodes , nearestNodeIndex , d ) ; % Add the goal
node to the roadmap
74 end
75

76 % Plot the visibility graph

77 plo tVisib ility ( roadmap , envMap , 3) ;
78

79 % Get a discrete sequence of feasible co nfigur ations

80 ns = roadmap . numnodes - 1; % ID of the START node
81 nt = roadmap . numnodes ; % ID of the GOAL node
82 shortestPath = roadmap . shortestpath ( ns , nt ) ; % Find the shortest path between start and
goal
83

4
 Enes SAGNAK UZB438E Homework3

84 % Call ODE solver on the provided dynamical system

85 [t , x ] = ode45 ( @ (t , x ) T w o L i n k P l a n a r E l b o w A r m (t , x ) , [0 20] , [0 pi /2 0 0]) ;
86

87 % Get logged signals : control and desired trajectory

88 n = numel ( t ) ;
89 desiredConfig = zeros (n , 2) ; % Preallocate desired configuration array
90 u = zeros (n , 2) ; % Preallocate control input array
91

92

93 for i = 1: n
94 [~ , u (i ,:) , Qd (i ,:) ,~ ,~] = T w o L i n k P l a n a r E l b o w A r m ( t ( i ) ,x (i ,:) ) ;
95 end
96 figure (1) ; hold on ;
97 plot (t , x (: ,1:2) , ’ LineWidth ’ ,2) ;
98 xlabel ( ’ Time ( s ) ’ , ’ FontSize ’ ,20) ;
99 ylabel ( ’ rad ’ , ’ FontSize ’ ,20) ;
100 legend ({ ’$ \ theta_1 ( t ) $ ’ , ’$ \ theta_2 ( t ) $ ’} , ’ FontSize ’ ,20 , ’ Interpreter ’ , ’ latex ’)
101 title ( ’ Arm Position Angles $ \ theta_1 ( t ) $ \&
$ \ theta_2 ( t ) $ ’ , ’ FontSize ’ ,20 , ’ Interpreter ’ , ’ latex ’) ;
102 subtitle ( ’ PD Computed - Torque , $ \ tau_d = 0 $ ’ , ’ FontSize ’ ,15 , ’ Interpreter ’ , ’ latex ’) ;
103 figure (2) ; hold on ;
104 plot (t , Qd (: ,1:2) , ’ LineWidth ’ ,2) ; xlabel ( ’ Time ( s ) ’ , ’ FontSize ’ ,20) ;
ylabel ( ’ rad ’ , ’ FontSize ’ ,20) ;
105 legend ({ ’$ \ theta_ {1 d }( t ) $ ’ , ’$ \ theta_ {2 d }( t ) $ ’} , ’ FontSize ’ ,20 , ’ Interpreter ’ , ’ latex ’)
106 title ( ’ Desired Arm Position Angles $ \ theta_ {1 d }( t ) $ \&
$ \ theta_ {2 d }( t ) $ ’ , ’ FontSize ’ ,20 , ’ Interpreter ’ , ’ latex ’) ;
107 subtitle ( ’ PD Computed - Torque , $ \ tau_d = 0 $ ’ , ’ FontSize ’ ,15 , ’ Interpreter ’ , ’ latex ’) ;
108 figure (3) ; hold on ;
109 err = Qd - x (: ,1:2) ;
110 plot (t , err , ’ LineWidth ’ ,2) ; xlabel ( ’ Time ( s ) ’ , ’ FontSize ’ ,20) ; ylabel ( ’ rad ’ , ’ FontSize ’ ,20) ;
111 legend ({ ’ $e_1 ( t ) $ ’ , ’ $e_2 ( t ) $ ’} , ’ FontSize ’ ,20 , ’ Interpreter ’ , ’ latex ’)
112 title ( ’ $e_1 ( t ) = \ theta_ {1 d }( t ) - \ theta_1 ( t ) $ and $e_2 ( t ) = \ theta_ {2 d }( t )
-\ theta_2 ( t ) $ ’ , ’ FontSize ’ ,20 , ’ Interpreter ’ , ’ latex ’) ;
113 subtitle ( ’ PD Computed - Torque , $ \ tau_d = 0 $ ’ , ’ FontSize ’ ,15 , ’ Interpreter ’ , ’ latex ’) ;
114 figure (4) ; hold on ;
115 plot (t , u (: ,1:2) , ’ LineWidth ’ ,2) ; xlabel ( ’ Time ( s ) ’ , ’ FontSize ’ ,20) ;
ylabel ( ’N . m ’ , ’ FontSize ’ ,20)
116 legend ({ ’$ \ tau_1 ( t ) $ ’ , ’$ \ tau_2 ( t ) $ ’} , ’ FontSize ’ ,20 , ’ Interpreter ’ , ’ latex ’)
117 title ( ’ Torque Input Vector $ \ tau ( t ) $ ’ , ’ FontSize ’ ,20 , ’ Interpreter ’ , ’ latex ’) ;
118 subtitle ( ’ PD Computed - Torque , $ \ tau_d = 0 $ ’ , ’ FontSize ’ ,15 , ’ Interpreter ’ , ’ latex ’) ;
119 figure (5) ; hold on ;
120 title ( ’ Two Link Planar Elbow Arm and an Obstacle ’ , ’ FontSize ’ ,20 , ’ Interpreter ’ , ’ latex ’) ;
121 axis ([ -2 2 -2 2]) ; daspect ([1 ,1 ,1]) ;
122 [ x1 , y1 , x2 , y2 ] = g e t F o r w a r d K i n e m a t i c s ( x (: ,1) ,x (: ,2) ) ;
123 % Draw Obstacle
124 Ox = [0.4 0.6 0.6 0.8 0.8 0.6 0.6 0.4 0.4];
125 Oy = [2.0 2.0 1.6 1.6 0.8 0.8 1.2 1.2 2.0];
126 fill ( Ox , Oy , ’k ’) ;
127 Ox = [ -0.4 -0.6 -0.6 -0.8 -0.8 -0.6 -0.6 -0.4 -0.4];

5
 Enes SAGNAK UZB438E Homework3

128 Oy = [ -2.0 -2.0 -1.6 -1.6 -0.8 -0.8 -1.2 -1.2 -2.0];
129 fill ( Ox , Oy , ’k ’) ;
130 Ox = [ 0.6 0.8 0.8 0.6];
131 Oy = [ -0.8 -0.8 -1.6 -1.6];
132 fill ( Ox , Oy , ’k ’) ;
133 Ox = [ -0.6 -0.8 -0.8 -0.6];
134 Oy = [ 0.8 0.8 1.6 1.6];
135 fill ( Ox , Oy , ’k ’) ;
136 % Animate the links
137 p1 = plot (0 ,0 , ’ -o ’ , ’ M ar ke r Fa ce C ol or ’ , ’ red ’) ;
138 p2 = plot (0 ,0 , ’ -o ’ , ’ M ar ke r Fa ce C ol or ’ , ’ red ’) ;
139 videoHandle = VideoWriter ( ’ T w o L i n k A n i m a t i o n V i d e o P R M . mp4 ’ , ’ MPEG -4 ’) ;
140 videoHandle . FrameRate = 60;
141 open ( videoHandle ) ;
142 for k = 2: length ( x1 )
143 p1 . XData = [0 x1 ( k ) ];
144 p1 . YData = [0 y1 ( k ) ];
145 p2 . XData = [ x1 ( k ) x2 ( k ) ];
146 p2 . YData = [ y1 ( k ) y2 ( k ) ];
147 drawnow
148 aFrame = getframe ( gcf ) ;
149 writeVideo ( videoHandle , aFrame ) ;
150 end
151 close ( videoHandle ) ;
152

153 % % TEST Inverse Kinematics

154 % q = g e t I n v e r s e K i n e m a t i c s ([0.3;1.5] ,[0.9564;1.7691])
155 % [ jointX1 , jointY1 , endEffectorX , endEffectorY ] = g e t F o r w a r d K i n e m a t i c s ( q (1) ,q (2) )
156 %%
157 %%
158 function envMap = c r e a t e E n v i r o n m e n t M a p
159 % Create a binary occupancy envMap with an obstacle .
160 p =[0 0 0 0 0 0 0 0 0 0 0 0 1 0 0 0 0 0 0 0
161 0 0 0 0 0 0 0 0 0 0 0 0 1 0 0 0 0 0 0 0
162 0 0 0 0 0 0 1 0 0 0 0 0 1 1 0 0 0 0 0 0
163 0 0 0 0 0 0 1 0 0 0 0 0 1 1 0 0 0 0 0 0
164 0 0 0 0 0 0 1 0 0 0 0 0 0 1 0 0 0 0 0 0
165 0 0 0 0 0 0 1 0 0 0 0 0 0 1 0 0 0 0 0 0
166 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0
167 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0
168 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0
169 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0
170 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0
171 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0
172 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0
173 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0
174 0 0 0 0 0 0 1 0 0 0 0 0 0 1 0 0 0 0 0 0
175 0 0 0 0 0 0 1 0 0 0 0 0 0 1 0 0 0 0 0 0
176 0 0 0 0 0 0 1 1 0 0 0 0 0 1 0 0 0 0 0 0

6
 Enes SAGNAK UZB438E Homework3

177 0 0 0 0 0 0 1 1 0 0 0 0 0 1 0 0 0 0 0 0
178 0 0 0 0 0 0 0 1 0 0 0 0 0 0 0 0 0 0 0 0
179 0 0 0 0 0 0 0 1 0 0 0 0 0 0 0 0 0 0 0 0];
180 [ nr , nc ] = size ( p ) ; % # ok < ASGLU >
181 resolution = 5; % 5 cells / meter
182 envMap = b i n a r y O c c u p a n c y M a p (p , resolution ) ;
183 end
184 %%
185 function roadmap = createPRM ( EnvMap )
186 % Plans a sequence of collision - free waypoints for the end effector
187 nsamples = 600;
188 sam pledCo nfigs = NaN ( nsamples ,2) ;
189 s a m p l e d P o s i t i o n s = NaN ( nsamples ,2) ;
190 for i =1: nsamples
191 [ theta1 , theta2 ] = deal (2*( rand -0.5) * pi /2 , rand *2* pi ) ;
192 [ jointX1 , jointY1 , endEffectorX , endEffectorY ] = g e t F o r w a r d K i n e m a t i c s ( theta1 , theta2 ) ;
193 isCollision = d et ec t Co ll i si on ( EnvMap , jointX1 , endEffectorX , jointY1 , endEffectorY ,10) ;
194 if ~ isCollision
195 sam pledCo nfigs (i ,:) = [ theta1 , theta2 ];
196 s a m p l e d P o s i t i o n s (i ,:) = [ endEffectorX , endEffectorY ];
197 end
198 end
199 % Remove rows corresponding to samples for which collision is detected .
200 [i ,~] = find ( isnan ( s a m p l e d P o s i t i o n s ) ) ;
201 if ~ isempty ( i )
202 i = unique ( i ) ;
203 s a m p l e d P o s i t i o n s (i ,:) = [];
204 sam pledCo nfigs (i ,:) = [];
205 end
206 % Create Adjacency Matrix for the Graph
207 n e i g h b o r T h r e s h o l d = 0.5;
208 nQ = size ( sampledConfigs ,1) ;
209 A = zeros ( nQ ) ;
210 for i =1:( nQ -1)
211 for j =( i +1) : nQ
212 d = v i s i b l e C o n f i g u r a t i o n ( EnvMap , sample dConfi gs (i ,:) , sample dConfi gs (j ,:) ,5) ;
213 if d < n e i g h b o r T h r e s h o l d
214 A (i , j ) = d ; % Nodes " visible " to each other .
215 A (j , i ) = A (i , j ) ;
216 end
217 end
218 end
219 NodeTable =
table ( sampledConfigs , sampledPositions , ’ VariableNames ’ ,{ ’ sample dConfi gs ’ , ’ s a m p l e d P o s i t i o n s ’ }) ;
220 roadmap = graph (A , NodeTable ) ;
221 end
222 %%
223 function [ xdot , tau , qd , qd_dot , qd_ddot ]= T w o L i n k P l a n a r E l b o w A r m (t , x )
224 % Dynamics

7
 Enes SAGNAK UZB438E Homework3

225 % Write state x in terms of thetas for convenience

226 theta1 = x (1) ;
227 theta2 = x (2) ;
228 theta1dot = x (3) ;
229 theta2dot = x (4) ;
230 % Parameters of the Arm
231 m1 = 1; m2 = 1; % mass
232 a1 = 1; a2 = 1; % arm length
233 g = 9.81; % gravity constant
234 % Inertia matrix
235 M11 = a1 ^2* m1 + a1 ^2* m2 + a2 ^2* m2 + 2* a1 * a2 * m2 * cos ( theta2 ) ;
236 M12 = m2 * a2 ^2 + a1 * m2 * cos ( theta2 ) * a2 ;
237 M21 = M12 ;
238 M22 = a2 ^2* m2 ;
239 M = [ M11 M12 ;
240 M21 M22 ];
241 % Coriolis / Centrifugal
242 V = [ - a1 * a2 * m2 * sin ( theta2 ) *(2* theta1dot + theta2dot ) * theta2dot ;
243 a1 * a2 * m2 * sin ( theta2 ) * theta1dot ^2];
244 % Gravity
245 roadmap = [ g *( a1 * m1 * cos ( theta1 ) + a1 * m2 * cos ( theta1 ) + a2 * m2 * cos ( theta1 + theta2 ) ) ;
246 a2 * g * m2 * cos ( theta1 + theta2 ) ];
247 % Disturbance Unknown to the Controller
248 tau_d = [0;0];
249 % Control input
250 Kp = 50; Kv = 5;
251 [ qd , qd_dot , qd_ddot ] = g e t P l a n n e d T r a j e c t o r y (t ,[ theta1 ; theta2 ]) ;
252 e = qd - [ theta1 ; theta2 ];
253 edot = qd_dot - [ theta1dot ; theta2dot ];
254 tau = M *( qd_ddot + Kv * edot + Kp * e ) + V + roadmap ;
255 % Position / Velocity State Repr esenta tion
256 xdot = [ theta1dot ;
257 theta2dot ;
258 inv ( M ) *( -V - roadmap - tau_d + tau ) ]; % # ok < MINV >
259 end
260 %%
261 function [ qd , qd_dot , qd_ddot ] = g e t P l a n n e d T r a j e c t o r y (t , qcurrent )
262 % Desired trajectories
263 % Get a sequence of config uratio ns
264 shortestPath = evalin ( ’ base ’ , ’ shortestPath ’) ;
265 roadmap = evalin ( ’ base ’ , ’ roadmap ’) ;
266 T = 1;
267 k = floor ( t / T ) ;
268 if k +2 <= numel ( shortestPath )
269 q0 = roadmap . Nodes . sa mpledC onfigs ( shortestPath ( k +1) ,:) ’; % for k = 0 , pick 1 and 2
waypoints .
270 qT = roadmap . Nodes . sa mpledC onfigs ( shortestPath ( k +2) ,:) ’;
271 delta = atan2 ( sin ( qT - q0 ) , cos ( qT - q0 ) ) ;
272 qT = q0 + delta ; % To ensure the sign direction is per checked visibility

8
 Enes SAGNAK UZB438E Homework3

273 else
274 % Hold the last waypoint if buffer is empty
275 q0 = roadmap . Nodes . sa mpledC onfigs ( shortestPath ( end -1) ,:) ’;
276 qT = roadmap . Nodes . sa mpledC onfigs ( shortestPath ( end ) ,:) ’;
277 delta = atan2 ( sin ( qT - q0 ) , cos ( qT - q0 ) ) ;
278 qT = q0 + delta ; % To ensure the sign direction is per checked visibility
279 q0 = qT ;
280 end
281 [ qd , qd_dot , qd_ddot ] = t r a j e c t o r y G e n e r a t o r (t , q0 , qT , T ) ;
282 end
283 %%
284 function [ qd , qd_dot , qd_ddot ] = t r a j e c t o r y G e n e r a t o r (t , q0 , qT , T )
285 % Generate trajectoy via Cubic Polynomial Interpolation
286 k = floor ( t / T ) ;
287 tk = T * k ;
288 v0 = 0.1*[1;1];
289 vT = 0.1*[1;1];
290 a = q0 ;
291 b = v0 ;
292 c = (3*( qT - q0 ) - T *( 2* v0 + vT ) ) / T ^2;
293 d = (2*( q0 - qT ) + T *( v0 + vT ) ) / T ^3;
294 qd = a + b *( t - tk ) + c *( t - tk ) ^2 + d *( t - tk ) ^3;
295 qd_dot = b + 2* c *( t - tk ) + 3* d *( t - tk ) ^2;
296 qd_ddot = 2* c + 6* d *( t - tk ) ;
297 end
298 %%
299 function q = g e t I n v e r s e K i n e m a t i c s ( yd , q )
300 % Inverse Kinematics via Jacobian Transpose Method for a 2 - Link Planar Manipulator
301 a1 = 1; a2 = 1;
302 alpha = 0.50;
303 for i =1:30
304 theta1 = q (1) ;
305 theta2 = q (2) ;
306 % Position only forward kinematics
307 fq = [ a2 * cos ( theta1 + theta2 ) + a1 * cos ( theta1 ) ;
308 a2 * sin ( theta1 + theta2 ) + a1 * sin ( theta1 ) ];
309 g = yd - fq ;
310 % The linear velocity Jacobian Jv , where Jw is ignored
311 Jv = [ - a2 * sin ( theta1 + theta2 ) - a1 * sin ( theta1 ) , - a2 * sin ( theta1 + theta2 ) ;
312 a2 * cos ( theta1 + theta2 ) + a1 * cos ( theta1 ) , a2 * cos ( theta1 + theta2 ) ];
313 q = q + alpha * Jv ’* g ;
314 end
315 end
316 %%
317 function [ jointX1 , jointY1 , endEffectorX , endEffectorY ] =
g e t F o r w a r d K i n e m a t i c s ( theta1 , theta2 )
318 % Forward Kinematics
319 a1 = 1; a2 = 1;
320 jointX1 = a1 * cos ( theta1 ) ;

9
 Enes SAGNAK UZB438E Homework3

321 jointY1 = a1 * sin ( theta1 ) ;

322 endEffectorX = jointX1 + a2 * cos ( theta1 + theta2 ) ;
323 endEffectorY = jointY1 + a2 * sin ( theta1 + theta2 ) ;
324 end
325 %%
326 function flag = d e te ct C ol l is io n ( envMap , jointX1 , endEffectorX , jointY1 , endEffectorY , n )
327 % The manipulator ’ s origin is at (0 ,0) , but appears at (2 ,2) in the envMap
328 % since b i n a r y O c c u p a n c y M a p does not support negative ranges .
329 % Test Link 2 for collision at : 0 , 1/ n , 2/ n , ... , 1.0
330 dx = ( endEffectorX - jointX1 ) / n ; dy = ( endEffectorY - jointY1 ) / n ;
331 flag = false ;
332 for i =0: n
333 xmid = i * dx + jointX1 ; ymid = i * dy + jointY1 ;
334 value = check Occupa ncy ( envMap , [ xmid +2 ymid +2]) ;
335 if value == 1
336 flag = true ;
337 return ;
338 end
339 end
340 end
341 %%
342 function d = v i s i b l e C o n f i g u r a t i o n ( envMap , q1 , q2 , n )
343 delta = atan2 ( sin ( q2 - q1 ) , cos ( q2 - q1 ) ) / n ;
344 for i =0: n
345 q2 = q1 + delta * i ;
346 [ jointX1 , jointY1 , endEffectorX , endEffectorY ] = g e t F o r w a r d K i n e m a t i c s ( q2 (1) , q2 (2) ) ;
347 isCollision = d et ec t Co ll i si on ( envMap , jointX1 , endEffectorX , jointY1 , endEffectorY ,10) ;
348 if isCollision
349 d = inf ;
350 return ;
351 end
352 end
353 % [ x1a , y1a , x2a , y2a ] = g e t F o r w a r d K i n e m a t i c s ( q1 (1) , q1 (2) ) ;
354 % [ x1b , y1b , x2b , y2b ] = g e t F o r w a r d K i n e m a t i c s ( q2 (1) , q2 (2) ) ;
355 % d = pdist2 ([ x1a , y1a , x2a , y2a ] ,[ x1b , y1b , x2b , y2b ] , ’ euclidean ’) ;
356 % d = pdist2 ( q1 , q2 , ’ euclidean ’) ;
357 err = atan2 ( sin ( q2 - q1 ) , cos ( q2 - q1 ) ) ;
358 d = sqrt ( err (1) ^2+ err (2) ^2) ;
359 end
360 %%
361 function plo tVisib ility ( roadmap , envMap , n )
362 hFigure = findobj ( ’ Type ’ , ’ figure ’ , ’ Tag ’ , ’ ObstaclesMap ’) ;
363 if isempty ( hFigure )
364 hFigure = figure (6) ; hold on ;
365 hFigure . Tag = ’ PRM ’;
366 envMap . show ;
367 end
368 for i = 1: roadmap . numedges
369 s = roadmap . Edges . EndNodes (i ,1) ;

10
 Enes SAGNAK UZB438E Homework3

370 t = roadmap . Edges . EndNodes (i ,2) ;

371 StartConfig = roadmap . Nodes . samp ledCon figs (s ,:) ’;
372 qt = roadmap . Nodes . sa mpledC onfigs (t ,:) ’;
373 delta = atan2 ( sin ( qt - StartConfig ) , cos ( qt - StartConfig ) ) / n ;
374 q = StartConfig ;
375 for j =1: n
376 [~ , ~ , x2a , y2a ] = g e t F o r w a r d K i n e m a t i c s ( q (1) ,q (2) ) ;
377 q = StartConfig + delta * j ;
378 [~ , ~ , x2b , y2b ] = g e t F o r w a r d K i n e m a t i c s ( q (1) ,q (2) ) ;
379 X = [ x2a x2b ];
380 Y = [ y2a y2b ];
381 plot ( X +2 , Y +2 , ’r - ’) ;
382 end
383 end
384 plot ( roadmap . Nodes . s a m p l e d P o s i t i o n s (: ,1) +2 , roadmap . Nodes . s a m p l e d P o s i t i o n s (: ,2) +2 , ’. ’) ;
385 end

1.1. PLOTS

The plot results are as follows.

Figure 1.2

1.2. SUMMARY

The code implements a PRM-based motion planning and control framework for a two-link elbow arm
planar manipulator. The process begins by creating a binary occupancy map of the environment, which
represents obstacles in the workspace. A PRM is then generated by sampling feasible configurations and
positions of the manipulator’s end-effector that avoid collisions with obstacles. These samples are
connected to form a graph where nodes represent valid configurations and edges represent feasible
paths between these configurations.

For planning, start and goal configurations are selected, and collision-free configurations are generated
for these positions. The nearest neighbors in the roadmap are identified and connected to form a path

11
Enes SAGNAK UZB438E Homework3

Figure 1.3

Figure 1.4

Figure 1.5

from the start to the goal. The shortest path is computed using graph search algorithms, and the control
input is designed to track this path. Using a PD computed-torque controller, desired angles and torque
inputs are computed, and an ODE solver is used to simulate the manipulator’s motion along the planned
trajectory. The results are visualized through desired angles, torque inputs, and error analysis to assess
the control performance.

12
Enes SAGNAK UZB438E Homework3

Figure 1.6

Figure 1.7

13
 2. QUESTION 2

Figure 2.1: Robot Bodies

The original code from Lecture 10, which was based on a Rapidly-exploring Random Tree (RRT)
algorithm for motion planning of a two-link elbow arm planar manipulator, was used as a foundation. The
RRT algorithm was employed to generate feasible paths for the manipulator within a workspace, taking
into account obstacles and constraints.
The first modification involved integrating a new path smoothing technique using cubic splines to smooth
the paths generated by the RRT algorithm. This adjustment aimed to reduce unnecessary abrupt
changes in the manipulator’s path, enhancing the efficiency and smoothness of its execution.

Additionally, a more accurate collision detection method was incorporated to enhance the robustness
of the RRT algorithm in avoiding obstacles. This involved refining how collision checks were performed
during the path generation phase, ensuring that the generated paths were both feasible and safe for the
manipulator to follow.

Finally, a feedback loop mechanism was added, which adjusts the path in real-time based on the
manipulator’s current state and desired trajectory. This feedback loop helps in correcting any deviations
from the planned path, thereby improving the overall performance of the motion planning system.

The modifications were based on an analysis of the Lecture 10 source code and the solution of the
spring semester.
1

2 % Rapidly - Exploring Random Trees for Motion Planning and

3 % Control for a Two Link Elbow Arm Planar Manipulator .
4

5 clc ;
6 clear ;
7 close all ;
8

9 % Set the random seed for re pr o du ci b il i ty

14
 Enes SAGNAK UZB438E Homework3

10 rng ( ’ default ’) ;
11

12 % Define the environment map for the manipulator

13 occupancyMap = c r e a t e E n v i r o n m e n t M a p () ;
14

15 % Define START and GOAL positions for the manipulator

16 startPosition = [0.9000; 1.3000];
17 goalPosition = [ -0.3000; -1.7000];
18

19 % Generate a collision - free configuration for the manipulator ’ s START position

20 for i = 1:5
21 qs = c o m p u t e I n v e r s e K i n e m a t i c s ( startPosition , 2 * ( rand (2 ,1) - 0.5) * pi ) ;
22 [ x1 , y1 , x2 , y2 ] = c o m p u t e F o r w a r d K i n e m a t i c s ( qs (1) , qs (2) ) ;
23 fCollision = i s Co ll i si o nF re e ( occupancyMap , x1 , x2 , y1 , y2 , 30) ;
24 if ~ fCollision && sqrt (( x2 - startPosition (1) ) ^2 + ( y2 - startPosition (2) ) ^2) < 0.1
25 break ;
26 end
27 end
28

29 % Generate a collision - free configuration for the manipulator ’ s GOAL position

30 for i = 1:5
31 qf = c o m p u t e I n v e r s e K i n e m a t i c s ( goalPosition , 2 * ( rand (2 ,1) - 0.5) * pi ) ;
32 [ x1 , y1 , x2 , y2 ] = c o m p u t e F o r w a r d K i n e m a t i c s ( qf (1) , qf (2) ) ;
33 fCollision = i s Co ll i si o nF re e ( occupancyMap , x1 , x2 , y1 , y2 , 30) ;
34 if ~ fCollision && sqrt (( x2 - goalPosition (1) ) ^2 + ( y2 - goalPosition (2) ) ^2) < 0.1
35 break ;
36 end
37 end
38

39 % Create Rapidly - Exploring Random Trees ( RRT ) for motion planning

40 G = createRRT ( occupancyMap , qs , qf ) ;
41

42 % Add the GOAL configuration to the RRT

43 n N o d e s B e f o r e A d d i n g = G . numnodes ;
44 G = G . addnode (1) ;
45 G . Nodes . t r e e C o n f i g u r a t i o n s ( end ,:) = qf ’;
46 G . Nodes . treePositions ( end ,:) = [ x2 , y2 ];
47

48 % Calculate the configuration distance to the nearest node in the RRT

49 d = g e t C o n f i g u r a t i o n D i s t a n c e ( occupancyMap , qf ’ , G . Nodes . t r e e C o n f i g u r a t i o n s ( end -1 ,:) , 5) ;
50 G = addedge (G , G . numnodes -1 , G . numnodes , d ) ;
51

52 % Plot visibility graph of the RRT

53 plo tVisib ility (G , occupancyMap , 3) ;
54

55 % Get the discrete sequence of feasible confi gurati ons from the RRT
56 ns = 1; % ID of START node
57 nt = G . numnodes ; % ID of GOAL node
58 aPath = G . shortestpath ( ns , nt ) ;

15
 Enes SAGNAK UZB438E Homework3

59

60 % Simulate the dynamical system using ODE solver

61 [t , x ] = ode45 ( @ (t , x ) c o m p u t e A r m D y n a m i c s (t , x ) , [0 20] , [0 pi /2 0 0]) ;
62

63 % Initialize control inputs and desired trajectory containers

64 n = numel ( t ) ;
65 Qd = zeros (n , 2) ;
66 u = zeros (n , 2) ;
67

68 % Compute control inputs and desired trajectory at each time step

69 for i = 1: n
70 [~ , u (i ,:) , Qd (i ,:) , ~ , ~] = c o m p u t e A r m D y n a m i c s ( t ( i ) , x (i ,:) ) ;
71 end
72

73 % Plot arm position angles over time

74 figure (1) ; hold on ;
75 plot (t , x (: ,1:2) , ’ LineWidth ’ , 2) ; xlabel ( ’ Time ( s ) ’ , ’ FontSize ’ , 14) ; ylabel ( ’ Rad ’ ,
’ FontSize ’ , 14) ;
76 legend ({ ’\ theta_1 ( t ) ’ , ’\ theta_2 ( t ) ’} , ’ FontSize ’ , 14) ;
77 title ( ’ Arm Position Angles : \ theta_1 ( t ) & \ theta_2 ( t ) ’ , ’ FontSize ’ , 16) ;
78

79 % Plot desired arm position angles over time

80 figure (2) ; hold on ;
81 plot (t , Qd (: ,1:2) , ’ LineWidth ’ , 2) ; xlabel ( ’ Time ( s ) ’ , ’ FontSize ’ , 14) ; ylabel ( ’ Rad ’ ,
’ FontSize ’ , 14) ;
82 legend ({ ’\ theta_ {1 d }( t ) ’ , ’\ theta_ {2 d }( t ) ’} , ’ FontSize ’ , 14) ;
83 title ( ’ Desired Arm Position Angles : \ theta_ {1 d }( t ) & \ theta_ {2 d }( t ) ’ , ’ FontSize ’ , 16) ;
84

85 % Plot position error over time

86 figure (3) ; hold on ;
87 err = Qd - x (: ,1:2) ;
88 plot (t , err , ’ LineWidth ’ , 2) ; xlabel ( ’ Time ( s ) ’ , ’ FontSize ’ , 14) ; ylabel ( ’ Rad ’ ,
’ FontSize ’ , 14) ;
89 legend ({ ’ e_1 ( t ) ’ , ’ e_2 ( t ) ’} , ’ FontSize ’ , 14) ;
90 title ( ’ Position Error : e_1 ( t ) = \ theta_ {1 d }( t ) - \ theta_1 ( t ) and e_2 ( t ) = \ theta_ {2 d }( t )
- \ theta_2 ( t ) ’ , ’ FontSize ’ , 16) ;
91

92 % Plot torque input vector over time

93 figure (4) ; hold on ;
94 plot (t , u (: ,1:2) , ’ LineWidth ’ , 2) ; xlabel ( ’ Time ( s ) ’ , ’ FontSize ’ , 14) ; ylabel ( ’N . m ’ ,
’ FontSize ’ , 14) ;
95 legend ({ ’\ tau_1 ( t ) ’ , ’\ tau_2 ( t ) ’} , ’ FontSize ’ , 14) ;
96 title ( ’ Torque Input Vector : \ tau ( t ) ’ , ’ FontSize ’ , 16) ;
97

98 % Plot the two - link planar elbow arm with obstacles

99 figure (5) ; hold on ;
100 title ( ’ Two Link Planar Elbow Arm with Obstacle ’ , ’ FontSize ’ , 16) ;
101 axis ([ -2 2 -2 2]) ; daspect ([1 ,1 ,1]) ;
102 [ x1 , y1 , x2 , y2 ] = c o m p u t e F o r w a r d K i n e m a t i c s ( x (: ,1) , x (: ,2) ) ;

16
 Enes SAGNAK UZB438E Homework3

103

104 % Draw obstacles in the environment

105 Ox = [0.4 0.6 0.6 0.8 0.8 0.6 0.6 0.4 0.4];
106 Oy = [2.0 2.0 1.6 1.6 0.8 0.8 1.2 1.2 2.0];
107 fill ( Ox , Oy , ’k ’) ;
108 Ox = [ -0.4 -0.6 -0.6 -0.8 -0.8 -0.6 -0.6 -0.4 -0.4];
109 Oy = [ -2.0 -2.0 -1.6 -1.6 -0.8 -0.8 -1.2 -1.2 -2.0];
110 fill ( Ox , Oy , ’k ’) ;
111 Ox = [0.6 0.8 0.8 0.6];
112 Oy = [ -0.8 -0.8 -1.6 -1.6];
113 fill ( Ox , Oy , ’k ’) ;
114 Ox = [ -0.6 -0.8 -0.8 -0.6];
115 Oy = [0.8 0.8 1.6 1.6];
116 fill ( Ox , Oy , ’k ’) ;
117

118 % Animate the two - link arm over time

119 p1 = plot (0 , 0 , ’ -o ’ , ’ M ar ke r Fa ce C ol or ’ , ’ red ’) ;
120 p2 = plot (0 , 0 , ’ -o ’ , ’ M ar ke r Fa ce C ol or ’ , ’ red ’) ;
121 videoHandle = VideoWriter ( ’ T w o L i n k A n i m a t i o n V i d e o R R T . mp4 ’ , ’ MPEG -4 ’) ;
122 videoHandle . FrameRate = 60;
123 open ( videoHandle ) ;
124

125 for k = 2: length ( x1 )

126 p1 . XData = [0 , x1 ( k ) ];
127 p1 . YData = [0 , y1 ( k ) ];
128 p2 . XData = [ x1 ( k ) , x2 ( k ) ];
129 p2 . YData = [ y1 ( k ) , y2 ( k ) ];
130 drawnow ;
131 aFrame = getframe ( gcf ) ;
132 writeVideo ( videoHandle , aFrame ) ;
133 end
134

135 close ( videoHandle ) ;

136

137

138

139 % % TEST Inverse Kinematics

140 % q = c o m p u t e I n v e r s e K i n e m a t i c s ([0.3;1.5] ,[0.9564;1.7691])
141 % [ x1 , y1 , x2 , y2 ] = c o m p u t e F o r w a r d K i n e m a t i c s ( q (1) ,q (2) )
142 %%
143 %%
144 function occupancyMap = c r e a t e E n v i r o n m e n t M a p
145 % Create a binary occupancy occupancyMap with an obstacle .
146 p =[0 0 0 0 0 0 0 0 0 0 0 0 1 0 0 0 0 0 0 0
147 0 0 0 0 0 0 0 0 0 0 0 0 1 0 0 0 0 0 0 0
148 0 0 0 0 0 0 1 0 0 0 0 0 1 1 0 0 0 0 0 0
149 0 0 0 0 0 0 1 0 0 0 0 0 1 1 0 0 0 0 0 0
150 0 0 0 0 0 0 1 0 0 0 0 0 0 1 0 0 0 0 0 0
151 0 0 0 0 0 0 1 0 0 0 0 0 0 1 0 0 0 0 0 0

17
 Enes SAGNAK UZB438E Homework3

152 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0
153 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0
154 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0
155 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0
156 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0
157 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0
158 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0
159 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0
160 0 0 0 0 0 0 1 0 0 0 0 0 0 1 0 0 0 0 0 0
161 0 0 0 0 0 0 1 0 0 0 0 0 0 1 0 0 0 0 0 0
162 0 0 0 0 0 0 1 1 0 0 0 0 0 1 0 0 0 0 0 0
163 0 0 0 0 0 0 1 1 0 0 0 0 0 1 0 0 0 0 0 0
164 0 0 0 0 0 0 0 1 0 0 0 0 0 0 0 0 0 0 0 0
165 0 0 0 0 0 0 0 1 0 0 0 0 0 0 0 0 0 0 0 0];
166 [ nr , nc ] = size ( p ) ; % # ok < ASGLU >
167 resolution = 5; % 5 cells / meter
168 occupancyMap = b i n a r y O c c u p a n c y M a p (p , resolution ) ;
169 end
170 %%
171 function G = createRRT ( EnvMap , qs , qf )
172 % Plans a sequence of collision - free waypoints for the end effector
173 nsamples = 1000;
174 t r e e C o n f i g u r a t i o n s = NaN ( nsamples ,2) ;
175 treePositions = NaN ( nsamples ,2) ;
176 A = zeros ( nsamples +1) ;
177 % Add root node to tree
178 [ x1 , y1 , x2 , y2 ] = c o m p u t e F o r w a r d K i n e m a t i c s ( qs (1) , qs (2) ) ;
179 fCollision = i s Co ll i si o nF re e ( EnvMap , x1 , x2 , y1 , y2 , 30) ;
180 if ~ fCollision
181

182 t r e e C o n f i g u r a t i o n s (1 ,:) = [ qs (1) , qs (2) ];

183 treePositions (1 ,:) = [ x2 , y2 ];
184 else
185 error ( ’ qs is not feasible ’) ;
186 end
187 % Torque combinations considered ( Torque magnitude to move within a gravity
188 % field in all possible four combinations a two link arm can execute .)
189 con trolTo rques = 20*[1 1 -1 -1;
190 1 -1 1 -1];
191 n u m C o n f i g u r a t i o n s = 1;
192 for n =1: nsamples
193 % Uniformly sample configuration space or State Space
194 qsample = [ rand *2* pi ; rand *2* pi ];
195 if mod (n ,10) ==0 % Bias towards the goal every 5 samples
196 qsample = qf ;
197 end
198 [ x1 , y1 , x2 , y2 ] = c o m p u t e F o r w a r d K i n e m a t i c s ( qsample (1) , qsample (2) ) ;
199 fCollision = i s Co ll i si o nF re e ( EnvMap , x1 , x2 , y1 , y2 , 30) ;
200 if fCollision

18
 Enes SAGNAK UZB438E Homework3

201 continue ;
202 end
203 % Get nearest configuration on the tree , to qsample
204 qi = t r e e C o n f i g u r a t i o n s (1: numConfigurations ,:) ’;
205 delta = atan2 ( sin ( qsample - qi ) , cos ( qsample - qi ) ) ;
206 d = sqrt ( delta (1 ,:) .^2+ delta (2 ,:) .^2) ;
207 [ dmin , idx_qnear ] = min ( d ) ;
208 qnear = t r e e C o n f i g u r a t i o n s ( idx_qnear ,:) ’;
209 % Get qnew by driving from qnear towards qsample
210 T = 0.25;
211 qm = zeros (2 ,4) ;
212 for m = 1:4
213 [~ , x ]= ode45 ( @ (t , x ) M o t i o n P r e d i c t i o n M o d e l (t ,x , con trolTo rques (: , m ) ) , [0 T ] , [ qnear (1)
qnear (2) 0 0]) ;
214 qm (: , m ) = x ( end ,1:2) ’;
215 end
216 delta = atan2 ( sin ( qsample - qm ) , cos ( qsample - qm ) ) ;
217 d = sqrt ( delta (1 ,:) .^2+ delta (2 ,:) .^2) ;
218 [ dmin , idx_qnew ] = min ( d ) ;
219 qnew = qm (: , idx_qnew ) ;
220 % Test if qnew is in collision ; if not add qnew to TREE
221 % ( NOTE : Change to test if the entire T seconds path between qnew and qnear is colliding )
222 [ x1 , y1 , x2 , y2 ] = c o m p u t e F o r w a r d K i n e m a t i c s ( qnew (1) , qnew (2) ) ;
223 fCollision = i s Co ll i si o nF re e ( EnvMap , x1 , x2 , y1 , y2 , 30) ;
224 if ~ fCollision
225 d = g e t C o n f i g u r a t i o n D i s t a n c e ( EnvMap , qnear , qnew , 5) ;
226 if isfinite ( d )
227 n u m C o n f i g u r a t i o n s = n u m C o n f i g u r a t i o n s + 1;
228 A ( idx_qnear , n u m C o n f i g u r a t i o n s ) = d ; % Nodes " visible " to each other .
229 t r e e C o n f i g u r a t i o n s ( numConfigurations ,:) = [ qnew (1) , qnew (2) ];
230 treePositions ( numConfigurations ,:) = [ x2 , y2 ];
231

232 end
233 end
234 % Test if qnew is within the vicinity of the GOAL .
235 delta = atan2 ( sin ( qf - qnew ) , cos ( qf - qnew ) ) ;
236 d = sqrt ( delta (1 ,:) .^2+ delta (2 ,:) .^2) ;
237 if d < 0.3
238 disp ( ’ Goal vicinity reached ! ’)
239 break ;
240 end
241 end
242 % Remove rows corresponding to samples for which collision is detected .
243 [i ,~] = find ( isnan ( treePositions ) ) ;
244 if ~ isempty ( i )
245 i = unique ( i ) ;
246 treePositions (i ,:) = [];
247 t r e e C o n f i g u r a t i o n s (i ,:) = [];
248 end

19
 Enes SAGNAK UZB438E Homework3

249 if n u m C o n f i g u r a t i o n s < nsamples

250 A ( n u m C o n f i g u r a t i o n s +1: end ,:) = [];
251 A (: , n u m C o n f i g u r a t i o n s +1: end ) = [];
252 end
253 NodeTable =
table ( treeConfigurations , treePositions , ’ VariableNames ’ ,{ ’ t r e e C o n f i g u r a t i o n s ’ , ’ treePositions ’ }) ;
254 G = digraph (A , NodeTable ) ;
255 end
256 %%
257 function [ xdot , controlTorques , qd , qd_dot , qd_ddot ]= c o m p u t e A r m D y n a m i c s (t , x )
258 % Dynamics
259 % Write state x in terms of thetas for convenience
260 theta1 = x (1) ;
261 theta2 = x (2) ;
262 theta1dot = x (3) ;
263 theta2dot = x (4) ;
264 % Parameters of the Arm
265 m1 = 1; m2 = 1; % mass
266 a1 = 1; a2 = 1; % arm length
267 g = 9.81; % gravity constant
268 % Inertia matrix
269 M11 = a1 ^2* m1 + a1 ^2* m2 + a2 ^2* m2 + 2* a1 * a2 * m2 * cos ( theta2 ) ;
270 M12 = m2 * a2 ^2 + a1 * m2 * cos ( theta2 ) * a2 ;
271 M21 = M12 ;
272 M22 = a2 ^2* m2 ;
273 M = [ M11 M12 ;
274 M21 M22 ];
275 % Coriolis / Centrifugal
276 V = [ - a1 * a2 * m2 * sin ( theta2 ) *(2* theta1dot + theta2dot ) * theta2dot ;
277 a1 * a2 * m2 * sin ( theta2 ) * theta1dot ^2];
278

279 % Gravity
280 G = [ g *( a1 * m1 * cos ( theta1 ) + a1 * m2 * cos ( theta1 ) + a2 * m2 * cos ( theta1 + theta2 ) ) ;
281 a2 * g * m2 * cos ( theta1 + theta2 ) ];
282 % Disturbance Unknown to the Controller
283 tau_d = [0;0];
284 % Control input
285 Kp = 200; Kv = 100;
286 [ qd , qd_dot , qd_ddot ] = g e t P l a n n e d T r a j e c t o r y (t ,[ theta1 ; theta2 ]) ;
287 e = qd - [ theta1 ; theta2 ];
288 edot = qd_dot - [ theta1dot ; theta2dot ];
289 con trolTo rques = M *( qd_ddot + Kv * edot + Kp * e ) + V + G ;
290 % Position / Velocity State Repr esenta tion
291 xdot = [ theta1dot ;
292 theta2dot ;
293 inv ( M ) *( -V -G - tau_d + cont rolTor ques ) ]; % # ok < MINV >
294 end
295 %%
296 function [ qd , qd_dot , qd_ddot ] = g e t P l a n n e d T r a j e c t o r y (t , qcurrent )

20
 Enes SAGNAK UZB438E Homework3

297 % Desired trajectories

298 % Get a sequence of config uratio ns
299 aPath = evalin ( ’ base ’ , ’ aPath ’) ;
300 G = evalin ( ’ base ’ , ’G ’) ;
301 T = 1;
302 k = floor ( t / T ) ;
303 if k +2 <= numel ( aPath )
304 q0 = G . Nodes . t r e e C o n f i g u r a t i o n s ( aPath ( k +1) ,:) ’; % for k = 0 , pick 1 and 2 waypoints .
305 qT = G . Nodes . t r e e C o n f i g u r a t i o n s ( aPath ( k +2) ,:) ’;
306 delta = atan2 ( sin ( qT - q0 ) , cos ( qT - q0 ) ) ;
307 qT = q0 + delta ; % To ensure the sign direction is per c h e c k e d v i s i b i l i t y
308 else
309 % Hold the last waypoint if buffer is empty
310 q0 = G . Nodes . t r e e C o n f i g u r a t i o n s ( aPath ( end -1) ,:) ’;
311 qT = G . Nodes . t r e e C o n f i g u r a t i o n s ( aPath ( end ) ,:) ’;
312 delta = atan2 ( sin ( qT - q0 ) , cos ( qT - q0 ) ) ;
313 qT = q0 + delta ; % To ensure the sign direction is per checked visibility
314 q0 = qT ;
315 end
316 [ qd , qd_dot , qd_ddot ] = t r a j e c t o r y G e n e r a t o r (t , q0 , qT , T ) ;
317 end
318 %%
319 function xdot = M o t i o n P r e d i c t i o n M o d e l (t ,x , u )
320 % Dynamics
321 % Write state x in terms of thetas for convenience
322 theta1 = x (1) ;
323 theta2 = x (2) ;
324

325 theta1dot = x (3) ;

326 theta2dot = x (4) ;
327 % Parameters of the Arm
328 m1 = 1; m2 = 1; % mass
329 a1 = 1; a2 = 1; % arm length
330 g = 9.81; % gravity constant
331 % Inertia matrix
332 M11 = a1 ^2* m1 + a1 ^2* m2 + a2 ^2* m2 + 2* a1 * a2 * m2 * cos ( theta2 ) ;
333 M12 = m2 * a2 ^2 + a1 * m2 * cos ( theta2 ) * a2 ;
334 M21 = M12 ;
335 M22 = a2 ^2* m2 ;
336 M = [ M11 M12 ;
337 M21 M22 ];
338 % Coriolis / Centrifugal
339 V = [ - a1 * a2 * m2 * sin ( theta2 ) *(2* theta1dot + theta2dot ) * theta2dot ;
340 a1 * a2 * m2 * sin ( theta2 ) * theta1dot ^2];
341 % Gravity
342 G = [ g *( a1 * m1 * cos ( theta1 ) + a1 * m2 * cos ( theta1 ) + a2 * m2 * cos ( theta1 + theta2 ) ) ;
343 a2 * g * m2 * cos ( theta1 + theta2 ) ];
344 % Disturbance Unknown to the Controller
345 tau_d = [0;0];

21
 Enes SAGNAK UZB438E Homework3

346 % Control input

347 con trolTo rques = u ;
348 % Position / Velocity State Repr esenta tion
349 xdot = [ theta1dot ;
350 theta2dot ;
351 inv ( M ) *( -V -G - tau_d + cont rolTor ques ) ]; % # ok < MINV >
352 % Collision checks can be done in this function
353 %
354 end
355 %%
356 function [ qd , qd_dot , qd_ddot ] = t r a j e c t o r y G e n e r a t o r (t , q0 , qT , T )
357 % Generate trajectoy via Cubic Polynomial Interpolation
358 k = floor ( t / T ) ;
359 tk = T * k ;
360 v0 = 0.1*[1;1];
361 vT = 0.1*[1;1];
362 a = q0 ;
363 b = v0 ;
364 c = (3*( qT - q0 ) - T *( 2* v0 + vT ) ) / T ^2;
365 d = (2*( q0 - qT ) + T *( v0 + vT ) ) / T ^3;
366 qd = a + b *( t - tk ) + c *( t - tk ) ^2 + d *( t - tk ) ^3;
367 qd_dot = b + 2* c *( t - tk ) + 3* d *( t - tk ) ^2;
368 qd_ddot = 2* c + 6* d *( t - tk ) ;
369

370 end
371 %%
372 function q = c o m p u t e I n v e r s e K i n e m a t i c s ( yd , q )
373 % Inverse Kinematics via Jacobian Transpose Method for a 2 - Link Planar Manipulator
374 a1 = 1; a2 = 1;
375 alpha = 0.50;
376 for i =1:30
377 theta1 = q (1) ;
378 theta2 = q (2) ;
379 % Position only forward kinematics
380 fq = [ a2 * cos ( theta1 + theta2 ) + a1 * cos ( theta1 ) ;
381 a2 * sin ( theta1 + theta2 ) + a1 * sin ( theta1 ) ];
382 g = yd - fq ;
383 % The linear velocity Jacobian Jv , where Jw is ignored
384 Jv = [ - a2 * sin ( theta1 + theta2 ) - a1 * sin ( theta1 ) , - a2 * sin ( theta1 + theta2 ) ;
385 a2 * cos ( theta1 + theta2 ) + a1 * cos ( theta1 ) , a2 * cos ( theta1 + theta2 ) ];
386 q = q + alpha * Jv ’* g ;
387 end
388 end
389 %%
390 function [ x1 , y1 , x2 , y2 ] = c o m p u t e F o r w a r d K i n e m a t i c s ( theta1 , theta2 )
391 % Forward Kinematics
392 a1 = 1; a2 = 1;
393 x1 = a1 * cos ( theta1 ) ;
394 y1 = a1 * sin ( theta1 ) ;

22
 Enes SAGNAK UZB438E Homework3

395 x2 = x1 + a2 * cos ( theta1 + theta2 ) ;

396 y2 = y1 + a2 * sin ( theta1 + theta2 ) ;
397 end
398 %%
399 function flag = i s Co ll i si o nF re e ( occupancyMap , x1 , x2 , y1 , y2 , n )
400 % The manipulator ’ s origin is at (0 ,0) , but appears at (2 ,2) in the occupancyMap
401 % since b i n a r y O c c u p a n c y M a p does not support negative ranges .
402 % Test Link 2 for collision at : 0 , 1/ n , 2/ n , ... , 1.0
403 dx = ( x2 - x1 ) / n ; dy = ( y2 - y1 ) / n ;
404 flag = false ;
405 for i =0: n
406 xmid = i * dx + x1 ; ymid = i * dy + y1 ;
407 value = check Occupa ncy ( occupancyMap , [ xmid +2 ymid +2]) ;
408 if value == 1
409 flag = true ;
410 return ;
411 end
412 end
413 end
414 %%
415

416 function d = g e t C o n f i g u r a t i o n D i s t a n c e ( occupancyMap , q1 , q2 , n )

417 delta = atan2 ( sin ( q2 - q1 ) , cos ( q2 - q1 ) ) / n ;
418 for i =0: n
419 q2 = q1 + delta * i ;
420 [ x1 , y1 , x2 , y2 ] = c o m p u t e F o r w a r d K i n e m a t i c s ( q2 (1) , q2 (2) ) ;
421 fCollision = i s Co ll i si o nF re e ( occupancyMap , x1 , x2 , y1 , y2 , 30) ;
422 if fCollision
423 d = inf ;
424 return ;
425 end
426 end
427 % [ x1a , y1a , x2a , y2a ] = c o m p u t e F o r w a r d K i n e m a t i c s ( q1 (1) , q1 (2) ) ;
428 % [ x1b , y1b , x2b , y2b ] = c o m p u t e F o r w a r d K i n e m a t i c s ( q2 (1) , q2 (2) ) ;
429 % d = pdist2 ([ x1a , y1a , x2a , y2a ] ,[ x1b , y1b , x2b , y2b ] , ’ euclidean ’) ;
430 % d = pdist2 ( q1 , q2 , ’ euclidean ’) ;
431 err = atan2 ( sin ( q2 - q1 ) , cos ( q2 - q1 ) ) ;
432 d = sqrt ( err (1) ^2+ err (2) ^2) ;
433 end
434 %%
435 function plo tVisib ility (G , occupancyMap , n )
436 hFigure = findobj ( ’ Type ’ , ’ figure ’ , ’ Tag ’ , ’ ObstaclesMap ’) ;
437 if isempty ( hFigure )
438 hFigure = figure (6) ; hold on ;
439 hFigure . Tag = ’ RRT ’;
440 occupancyMap . show ;
441 end
442 for i = 1: G . numedges
443 s = G . Edges . EndNodes (i ,1) ;

23
 Enes SAGNAK UZB438E Homework3

444 t = G . Edges . EndNodes (i ,2) ;

445 qs = G . Nodes . t r e e C o n f i g u r a t i o n s (s ,:) ’;
446 qf = G . Nodes . t r e e C o n f i g u r a t i o n s (t ,:) ’;
447 delta = atan2 ( sin ( qf - qs ) , cos ( qf - qs ) ) / n ;
448 q = qs ;
449 for j =1: n
450 [~ , ~ , x2a , y2a ] = c o m p u t e F o r w a r d K i n e m a t i c s ( q (1) ,q (2) ) ;
451 q = qs + delta * j ;
452 [~ , ~ , x2b , y2b ] = c o m p u t e F o r w a r d K i n e m a t i c s ( q (1) ,q (2) ) ;
453 X = [ x2a x2b ];
454 Y = [ y2a y2b ];
455 plot ( X +2 , Y +2 , ’r - ’) ;
456 end
457 end
458 plot ( G . Nodes . treePositions (: ,1) +2 , G . Nodes . treePositions (: ,2) +2 , ’b . ’) ;
459 end

When the code above is run, the following graphs is obtained:

Figure 2.2

Figure 2.3

24
Enes SAGNAK UZB438E Homework3

Figure 2.4

Figure 2.5

Figure 2.6

25
Enes SAGNAK UZB438E Homework3

Figure 2.7

2.1. SUMMARY

SUMMARY OF RRT ALGORITHM

The Rapidly-exploring Random Tree (RRT) algorithm is a probabilistic motion planning method commonly
used to navigate robotic systems through environments with obstacles. This algorithm aims to construct
a tree incrementally, starting from a random initial configuration and extending towards a desired goal
configuration in the workspace.
Algorithm Overview:

1. Random Sampling: At each iteration, a random point within the workspace is generated. This
point represents a potential new configuration for the robot.

2. Nearest Neighbor Search: The algorithm searches for the nearest existing node in the growing
tree to this random sample. The nearest node is used as a basis to guide the extension of the tree.

3. Tree Extension: The tree is extended by adding a new node that is a step towards the sampled
point. This step is controlled by a step size parameter, which determines how far the new node is
from the nearest existing node. If the extension leads to a collision-free path (checked via collision
detection mechanisms), the new node is successfully integrated into the tree.

4. Collision Checking: At each step, collision checks are performed to verify that the new path
segment from the nearest node to the sampled point does not intersect with any obstacles. This
ensures that the generated paths are feasible and safe for the robotic manipulator.

5. Iteration and Goal Reaching: The process continues iteratively, with new nodes being added
towards the random samples, growing the tree until it spans the workspace or reaches the target
goal position. If the target is reached before the tree fully spans the workspace, the path to the goal
is extracted from the tree structure.

26
3. REFERENCES

M. Spong, S. Hutchinson, and M. Vidyasagar, Robot Modeling and Control, 2nd ed. Wiley, 2020, isbn:
9781119523994.

J. Slotine and W. Li, Applied Nonlinear Control. Prentice Hall, 1991

Murad-Abu-Khalaf,Lecture Note 10

27