2 Robot dynamics, free space, and obstacle space
First of all, we have to represent the robot configuration space, the configuration being q = (θ1, θ2), and
coinciding with the system state x = (x1, x2). We assume to be able to impose the speed of each link as a
component of the input vector u = (u1, u2), thus obtaining a system dynamics given by
x˙ 1 = u1
x˙ 2 = u2
Regarding collisions, from a visual inspection, we notice that link 1 can collide with the wall, but not
with any of the circular obstacles: as a consequence, there is no need to define spheres around link 1. As for
link 2, this will instead be necessary: you can choose an arbitrary number of spheres, but the suggestion is
to use at least 3 of them to cover the whole link length.
The motion has to take place in T = 5 s, with the imposed initial and final configurations chosen as
x(0) =
0
0
, x(T) =
π
0
.
During the robot motion, we need to avoid the obstacles and guarantee that the joint angles are within the
given limits. No additional limits