ROBOTIC CONTROL SYSTEMS

UZB 438E

PROBLEM SET 10
By
Enes Sagnak 110200063

Course given by:

Murad Abu-Khalaf

Department of Aeronautics and Astronautics

Aeronautics Engineering
CONTENTS

1 Question 1 2
1.0.1 Introduction ..................................................................................................... 2
1.0.2 Modifications ................................................................................................... 2
1.0.3 Outputs........................................................................................................... 4
1.0.4 Discussion ...................................................................................................... 11

2 REFERENCES 15

1
1. QUESTION 1

Figure 1.1: Question 1

1.0.1. Introduction

This report presents the development and enhancement of a trajectory optimization framework for a two-
link planar elbow arm, incorporating obstacle-awareness capabilities. The main task involves modifying
the existing trajectory planning constraints to include a specific condition: the arm must not leverage an
equilibrium point where the second linkâĂŹs mass is at the coordinates (0, 2), between the 4th and 13th
seconds of the trajectory. This additional constraint, representing a circular obstacle with a radius of 0.1
centered at (0, 2), has been incorporated into the function planning nonlcon within the provided code. The
updated code and simulation results demonstrate the arms ability to adapt to the new constraint while
achieving the desired trajectory.

1.0.2. Modifications

The following changes have been made to the code based on the Lecture 10 implementation for the
obstacle-aware Two-Link Planar Elbow Arm:

Added New Constraint (c3): In the planningnonlcon function, I introduced an additional constraint c3
to prevent the arm from leveraging the equilibrium point at the coordinates (0, 2). This is achieved by
defining a circular constraint of radius 0.1 around the point (0, 2), ensuring that the arm avoids this region.

Enhanced Animation (draw function): To improve the visual representation of the arm’s movement, a
draw function was added. This function enables the visualization of the robot’s trajectory, obstacle

2
 Enes SAGNAK UZB438E Homework3

avoidance, and the interaction between the links of the arm during the optimization process.

Increased the Grid Resolution (N): The number of grid points, N, was increased to improve the
accuracy of the trajectory optimization. This higher resolution allows for more precise control over the
motion planning, especially around critical areas such as the equilibrium point.

Constraining the Maximum Speed Values: The bounds on the joint velocities and torques were
adjusted to limit the maximum values that the arm’s joints can reach. Specifically, the joint velocities were
constrained to mx spd 1 and mx spd 2 for the first and second joints, respectively. The upper and lower
bounds on the state were also set to ensure that the arm operates within a reasonable range of motion.
This prevents excessive movement and improves stability.

The added code snippets are as follows:

2 % Constraint at (0 , 2) with radius 0.1

3 xo = 0; yo = 2;
4 c3 = (0.1) ^2 - ( x2 - xo ) .^2 - ( y2 - yo ) .^2;
5

6 % Append the new constraint to the list of obstacle constraints

7 c = [ c1 , c2 , c3 ];
8

9 % Enhanced Animation
10 function draw (t , X )
11 % This function visualizes the arms motion in a more intuitive way .
12 [ x1 , y1 , x2 , y2 ] = g e t F o r w a r d K i n e ma t i c s ( X (1 ,:) , X (2 ,:) ) ;
13 % Plot the arm and obstacles
14 plot ([0 x1 ] , [0 y1 ] , ’r ’ , ’ LineWidth ’ , 2) ; % First link
15 plot ([ x1 x2 ] , [ y1 y2 ] , ’b ’ , ’ LineWidth ’ , 2) ; % Second link
16 % Add obstacle visualizations
17 fill ( Ox , Oy , ’k ’) ; % Obstacle
18 plot ( x1 , y1 , ’ ro ’) ; % Joint 1
19 plot ( x2 , y2 , ’ bo ’) ; % Joint 2
20 axis equal ;
21 drawnow ;
22 end
23

24 % Increase N for higher accuracy

25 N = 100; % Increased grid size for more accurate trajectory
optimization

3
 Enes SAGNAK UZB438E Homework3

26

27 % Limiting maximum joint speeds

28 mx_spd_1 = 5; % Maximum joint speed for joint 1
29 mx_spd_2 = 5; % Maximum joint speed for joint 2
30 bounds . x . lower = [ -2* pi ; -2* pi ; - mx_spd_1 ; - mx_spd_1 ];
31 bounds . x . upper = [2* pi ; 2* pi ; mx_spd_2 ; mx_spd_2 ];

1.0.3. Outputs

The following code was run with new obstacle sizes of different radii and the result was monitored.

1 % Demonstrating Trajectory Optimization for a two - link planar elbow

arm
2 % with obstacle awareness .
3 clc ; clear ; close all ;
4 tau_max = [20;20]; % Maximum actuator forces
5 tf = 15; % final time
6 N = 50; % Mesh grid size
7 initialXY = [0.9000; 1.3000];
8 goalXY = [0.3000; 1.7000];
9 % qs = [0.2903; 1.3461];
10 % qf = [1.9384; -1.0830];
11 qs = [0; 0];
12 qf = [1.9384; -1.0830];
13 %% Function handles for costs and constraints
14 func . dynamics = @ (t ,x , u ) ( robotDynamics (t ,x , u ) ) ;
15 func . Lcost = @ (t ,x , u ) ( sum ( u .* u ,1) ) ;
16 func . terminalCost = [];
17 func . t r a j e ct o r y C o n s t r a i n t = [];
18 func . t er min al Co nst ra in t = [];
19 %% Bounds on Decision Variables
20 % Fixed ( t0 , x0 ) , and ( tf , xf )
21 bounds . x0 . lower = [ qs (1) ; qs (2) ;0;0];
22 bounds . xf . lower = [ qf (1) ; qf (2) ;0;0];
23 bounds . x0 . upper = bounds . x0 . lower ;
24 bounds . xf . upper = bounds . xf . lower ;
25 % Constraints on x ( t ) and u ( t )
26

27 %% max joint speeds

28 mx_spd_1 = 5; % rad / s

4
 Enes SAGNAK UZB438E Homework3

29 mx_spd_2 = 5; %1.5 rad / s

30

31 %%
32 bounds . x . lower = [ -2* pi ; -2* pi ; - mx_spd_1 ; - mx_spd_1 ];
33 bounds . x . upper = [2* pi ;2* pi ; mx_spd_2 ; mx_spd_2 ];
34 bounds . u . lower = - tau_max ;
35 bounds . u . upper = tau_max ;
36 %% Initial guess at trajectory %
37 t_ = [0 , tf ];
38 x_ = [ bounds . x0 . lower , bounds . xf . lower ];
39 u_ = [0 0;0 0];
40 guess . T = t_ ([1 , end ]) ;
41 guess . t = linspace ( guess . T (1) , guess . T (2) , N ) ;
42 guess . x = interp1 ( t_ ’ , x_ ’ , guess . t ’) ’;
43 guess . u = interp1 ( t_ ’ , u_ ’ , guess . t ’) ’;
44 zGuess = [ reshape ([ guess . x ; guess . u ] , numel ( guess . x ) + numel ( guess . u ) ,
1) ];
45 %% Create Decision Variables vector
46 x_Lower = [ bounds . x0 . lower , bounds . x . lower * ones (1 ,N -2) ,
bounds . xf . lower ];
47 u_Lower = bounds . u . lower * ones (1 , N ) ;
48 z_Lower = [ reshape ([ x_Lower ; u_Lower ] , numel ( x_Lower ) + numel ( u_Lower ) ,
1) ];
49 % Note that for time , the upper and lower limits are the same ,
essentially fixed grid points .
50 x_Upper = [ bounds . x0 . upper , bounds . x . upper * ones (1 ,N -2) ,
bounds . xf . upper ];
51 u_Upper = bounds . u . upper * ones (1 , N ) ;
52 z_Upper = [ reshape ([ x_Upper ; u_Upper ] , numel ( x_Upper ) + numel ( u_Upper ) ,
1) ];
53 %% fmincon problem structure formulation
54 problem . objective = @ ( z ) ( planning_Jcost (z , func . Lcost ,
func . terminalCost , tf , N ) ) ;
55 problem . x0 = zGuess ;
56 problem . Aineq = [];
57 problem . bineq = [];
58 problem . Aeq = [];
59 problem . beq = [];
60 problem . lb = z_Lower ;

5
 Enes SAGNAK UZB438E Homework3

61 problem . ub = z_Upper ;
62 problem . nonlcon = @ ( z ) ( planning_nonlcon (z , func . dynamics , tf , N ) ) ;
63 problem . solver = ’ fmincon ’;
64 problem . options = optimoptions ( " fmincon " ,...
65 " Algorithm " ," interior - point " ,...
66 " E n a b l e F e a s i b i l i t y M o d e " , true ,...
67 " S u bp r ob l e mA l go r i th m " ," cg " ,...
68 ’ Display ’ , ’ iter ’ ,...
69 ’ TolFun ’ ,1e -6 ,...
70 ’ MaxIter ’ ,400 ,...
71 ’ M a x F u n c t i o n E v a l u a t i o n s ’ ,5 e5 ) ;
72 [ zStar , objVal , exitFlag , output ] = fmincon ( problem ) ;
73 %% Process the output of fmincon
74 tStar = linspace (0 , tf , N ) ;
75 xu = reshape ( zStar (1:(6* N ) ) , [6 , N ]) ;
76 xStar = xu (1:4 ,:) ;
77 uStar = xu (5:6 ,:) ;
78 % Use piecewise linear interpolation for the control
79 u = @ ( t ) ( interp1 ( tSta r ’ , uStar ’ ,t ’) ’ ) ;
80 % Use piecewise quadratic interpolation for the state :
81 f = func . dynamics ( tStar , xStar , uStar ) ;
82 x = @ ( t ) ( q u a d r a t i c I n t e r p o l a t i o n (t , xStar ,f , tf , N ) ) ;
83 figure (1) ; hold on ;
84 X = x (0:0.1: tf ) ;
85 plot (0:0.1: tf , X (1:2 ,:) , ’ LineWidth ’ ,2) ; xlabel ( ’ Time
( s ) ’ , ’ FontSize ’ ,20) ; ylabel ( ’ rad ’ , ’ FontSize ’ ,20) ;
86 legend ({ ’$ \ theta_1 ( t ) $ ’ , ’$ \ theta_2 ( t ) $ ’} , ’ FontSize ’ ,20 , ’ Interpreter ’ , ’ latex ’)
87 title ( ’ Planned Trajectory for Arm Position Angles $ \ theta_1 ( t ) $ \&
$ \ theta_2 ( t ) $ ’ , ’ FontSize ’ ,20 , ’ Interpreter ’ , ’ latex ’) ;
88 subtitle ( ’ Trajectory Optimization , assumes $ \ tau_d =
0 $ ’ , ’ FontSize ’ ,15 , ’ Interpreter ’ , ’ latex ’) ;
89 figure (2) ; hold on ;
90 plot (0:0.1: tf , u (0:0.1: tf ) , ’ LineWidth ’ ,2) ; xlabel ( ’ Time
( s ) ’ , ’ FontSize ’ ,20) ; ylabel ( ’N . m ’ , ’ FontSize ’ ,20)
91 legend ({ ’$ \ tau_1 ( t ) $ ’ , ’$ \ tau_2 ( t ) $ ’} , ’ FontSize ’ ,20 , ’ Interpreter ’ , ’ latex ’)
92 title ( ’ Planned Torque $ \ tau ( t ) $ ’ , ’ FontSize ’ ,20 , ’ Interpreter ’ , ’ latex ’) ;
93 subtitle ( ’ Trajectory Optimization , assumes $ \ tau_d =
0 $ ’ , ’ FontSize ’ ,15 , ’ Interpreter ’ , ’ latex ’) ;
94 figure (3) ; hold on ;

6
 Enes SAGNAK UZB438E Homework3

95 title ( ’ Two Link Planar Elbow Arm and an

Obstacle ’ , ’ FontSize ’ ,20 , ’ Interpreter ’ , ’ latex ’) ;
96 axis ([ -2 2 -2 2]) ; daspect ([1 ,1 ,1]) ;
97 [ x1 , y1 , x2 , y2 ] = g et F o r w a r d K i n e m a t i c s ( X (1 ,:) ,X (2 ,:) ) ;
98 % Draw Obstacle
99 Ox = [0.4 0.6 0.6 0.8 0.8 0.6 0.6 0.4 0.4];
100 Oy = [2.0 2.0 1.6 1.6 0.8 0.8 1.2 1.2 2.0];
101 fill ( Ox , Oy , ’k ’) ;
102 % Circle , C (0 ,2) , r = 0.1
103 theta = linspace (0 , 2* pi , 100) ;
104 circleX = 0 + 0.1 * cos ( theta ) ;
105 circleY = 2 + 0.1 * sin ( theta ) ;
106 fill ( circleX , circleY , ’k ’) ;
107

108 % Animate the links

109 p1 = plot (0 ,0 , ’ -o ’ , ’ MarkerFaceColor ’ , ’ red ’) ;
110 p2 = plot (0 ,0 , ’ -o ’ , ’ MarkerFaceColor ’ , ’ red ’) ;
111 videoHandle = VideoWriter ( ’ P l a n a r E l b o w T r a j O p t O b s A w a r e . mp4 ’ , ’ MPEG -4 ’) ;
112 videoHandle . FrameRate = numel ( x1 ) / tf ;
113 open ( videoHandle ) ;
114 for k = 2: length ( x1 )
115 p1 . XData = [0 x1 ( k ) ];
116 p1 . YData = [0 y1 ( k ) ];
117 p2 . XData = [ x1 ( k ) x2 ( k ) ];
118 p2 . YData = [ y1 ( k ) y2 ( k ) ];
119 drawnow
120 aFrame = getframe ( gcf ) ;
121 writeVideo ( videoHandle , aFrame ) ;
122 end
123 close ( videoHandle ) ;
124 function Jcost = planning_Jcost (z , Lcost , terminalCost , tf , N )
125 t = [0; tf ];
126 xu = reshape ( z (1:(6* N ) ) , [6 , N ]) ;
127 x = xu (1:4 ,:) ;
128 u = xu (5:6 ,:) ;
129 % Trapezoid integrations quadrature weights
130 Quadr atureWei ghts = ones (N ,1) ;
131 Quadr atureWei ghts ([1 , end ]) = 0.5;
132 % Cost at the end point

7
 Enes SAGNAK UZB438E Homework3

133 if isempty ( terminalCost )

134 phiCost = 0;
135 else
136 t0 = t (1) ;
137 tF = t ( end ) ;
138 x0 = x (: ,1) ;
139 xF = x (: , end ) ;
140 phiCost = terminalCost ( t0 , x0 , tF , xF ) ;
141 end
142 % Cost integrated along trajectory from start point to end point
143 if isempty ( Lcost )
144 accumularedCost = 0;
145 else
146 h = ( t ( end ) -t (1) ) /( N -1) ;
147 accumularedCost = h * Lcost (t ,x , u ) * Quadr atureWei ghts ; % Trapazoidal
integration
148 end
149 Jcost = phiCost + accumularedCost ;
150 end
151 function [c , ceq ] = planning_nonlcon (z , fDynamics , tf , N )
152 t = linspace (0 , tf , N ) ;
153 xu = reshape ( z (1:(6* N ) ) , [6 , N ]) ;
154 x = xu (1:4 ,:) ;
155 u = xu (5:6 ,:) ;
156 %%%% Compute defects along the trajectory :
157 h = ( t ( end ) -t (1) ) /( N -1) ;
158 f = fDynamics (t ,x , u ) ;
159 % Defect constraints
160 xk = x (: ,1:( N -1) ) ;
161 xk1 = x (: ,2: N ) ;
162 fk = f (: ,1:( N -1) ) ;
163 fk1 = f (: ,2: N ) ;
164 defects = xk1 - xk - 0.5* h *( fk + fk1 ) ;
165 % Nonlinear equality constraints
166 ceq = reshape ( defects , numel ( defects ) ,1) ;
167 % Constrains due to obstacles
168 [ x1 , y1 , x2 , y2 ] = g et F o r w a r d K i n e m a t i c s ( xk (1 ,:) , xk (2 ,:) ) ; % # ok < ASGLU >
169 xo = 0.75; yo =0.75;
170 c1 = (0.3) ^2 - ( x2 - xo ) .^2 -( y2 - yo ) .^2;

8
 Enes SAGNAK UZB438E Homework3

171 xo = 0.75; yo =1.75;

172 c2 = (0.3) ^2 - ( x2 - xo ) .^2 -( y2 - yo ) .^2;
173 xo = 0; yo = 2;
174 c3 = (0.1) ^2 - ( x2 - xo ) .^2 - ( y2 - yo ) .^2;
175

176 % Nonlinear inequality constraints

177 c = [ c1 , c2 , c3 ];
178 end
179

180 function xt = q u a d r a t i c I n t e r p o l a t i o n (t ,X ,F , tf , N )
181 % Generate trajectoy via Quadratic Polynomial Interpolation
182 % p ( t ) = a t ^2 + b t + c
183 % p ( t [ k ]) = c = x [ k ]
184 % pdot ( t [ k ]) = b = f ( xk )
185 % pdot ( t [ k +1]) = 2 a T + b = f ( x [ k +1])
186 Ts = ( tf -0) /( N -1) ;
187 if max ( t ) > tf
188 error ( ’t is larger than allowed . ’) ;
189 end
190 k = floor ( t / Ts ) ;
191 tk = Ts * k ;
192 xk = X (: , min (k ,N -1) +1) ; % for k = 0 , pick 1 and 2 waypoints .
193 fk = F (: , min (k ,N -1) +1) ;
194 fk1 = F (: , min (k ,N -2) +2) ;
195 c = xk ;
196 b = fk ;
197 a = ( fk1 - fk ) /(2* Ts ) ;
198 xt = a .*( t - tk ) .^2 + b .*( t - tk ) + c ;
199 end
200 %%
201 function xdot = robotDynamics (t ,x , u ) % # ok < INUSD >
202 % Robot dynamics
203 nU = size (u ,2) ;
204 xdot = zeros ( size ( x ) ) ;
205 for i =1: nU
206 % Write state x in terms of thetas for convenience
207 theta1 = x (1 , i ) ;
208 theta2 = x (2 , i ) ;
209 theta1dot = x (3 , i ) ;

9
 Enes SAGNAK UZB438E Homework3

210 theta2dot = x (4 , i ) ;
211 % Parameters of the Arm
212 m1 = 1; m2 = 1; % mass
213 a1 = 1; a2 = 1; % arm length
214 g = 9.81; % gravity constant
215 % Inertia matrix
216 M11 = a1 ^2* m1 + a1 ^2* m2 + a2 ^2* m2 + 2* a1 * a2 * m2 * cos ( theta2 ) ;
217 M12 = m2 * a2 ^2 + a1 * m2 * cos ( theta2 ) * a2 ;
218 M21 = M12 ;
219 M22 = a2 ^2* m2 ;
220 M = [ M11 M12 ;
221 M21 M22 ];
222 % Coriolis / Centrifugal
223 V = [ - a1 * a2 * m2 * sin ( theta2 ) *(2* theta1dot + theta2dot ) * theta2dot ;
224 a1 * a2 * m2 * sin ( theta2 ) * theta1dot ^2];
225 % Gravity
226 G = [ g *( a1 * m1 * cos ( theta1 ) + a1 * m2 * cos ( theta1 ) + a2 * m2 * cos ( theta1 +
theta2 ) ) ;
227 a2 * g * m2 * cos ( theta1 + theta2 ) ];
228 % Disturbance Unknown to the Controller
229 tau_d = [0;0];
230 % Control input
231 tau = u (: , i ) ;
232 % Position / Velocity State Representation
233 xdot (: , i ) = [ theta1dot ;
234 theta2dot ;
235 inv ( M ) *( -V -G - tau_d + tau ) ]; % # ok < MINV >
236 end
237 end
238 %%
239 function [ x1 , y1 , x2 , y2 ] = g e t F o r w a r d K i n e m a ti c s ( theta1 , theta2 )
240 % Forward Kinematics
241 a1 = 1; a2 = 1;
242 x1 = a1 * cos ( theta1 ) ;
243 y1 = a1 * sin ( theta1 ) ;
244 x2 = x1 + a2 * cos ( theta1 + theta2 ) ;
245 y2 = y1 + a2 * sin ( theta1 + theta2 ) ;
246 end

10
Enes SAGNAK UZB438E Homework3

Figure 1.2: radius 0.1

Figure 1.3: radius 0.1

1.0.4. Discussion

In the modified Two-Link Planar Elbow Arm simulation, the goal was to prevent the arm from utilizing the
equilibrium point at coordinates (0, 2) by adding a circular constraint around it. As the radius of the

11
Enes SAGNAK UZB438E Homework3

Figure 1.4: radius 0.2

Figure 1.5: radius 0.2

constraint increased, the arm’s feasible movement area was further restricted, reducing the number of
feasible trajectories.

12
Enes SAGNAK UZB438E Homework3

Figure 1.6: radius 0.3

Figure 1.7: radius 0.3

However, as the constraint radius grew, the arm’s ability to avoid the equilibrium point decreased. The
arm was increasingly confined to a smaller region, which led to higher oscillations as it struggled to
navigate around the constraint. This resulted in the arm passing through or staying near the equilibrium

13
Enes SAGNAK UZB438E Homework3

point, a situation that was initially intended to be avoided.

This highlights the trade-off between restricting the arm’s movement near critical points to prevent
staying at equilibrium and maintaining a stable trajectory. While the constraint minimized the risk of
staying at equilibrium, its increasing size ultimately limited the arm’s movement options, leading to higher
oscillations and instability.

14
2. 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

15