OSNOVE ROBOTIKE

–laboratorijske vježbe–
Vlatka Mihić
 VJEŽBA 1
Direktna kinematika robotskog manipulatora

OPIS VJEŽBE:

Problem direktne kinematike koji se razmatra u ovoj vježbi je odrediti položaj

alata, odnosno koordinatnog sustava 𝐿6 , u odnosu na bazni koordinatni
sustav 𝐿0 , koji miruje u odnosu na okolinu robota, za zadane vrijednosti
varijabli zglobova. Položaj nekog koordinatnog sustava u odnosu na neki
drugi koordinatni sustav može se opisati matricom homogene transformacije.
0
Matrica 𝑇6 koja opisuje položaj koordinatnog sustava 𝐿6 u odnosu na 𝐿0 može
se odrediti na temelju kinematičkog modela manipulatora. Zadani robotski
manipulator sa 6 rotacijskih zglobova može se modelirati kao kinematički
lanac članaka povezanih zglobovima. Primjenom Denavit-Hartenbergove
metode potrebno je svakom itom članku pridružiti koordinatni sustav 𝐿𝑖 koji
miruje u odnosu na taj članak te, uz postavljanje koordinatnog sustava 𝐿0 koji
miruje u odnosu na taj članak, odrediti kinematičke parametre koji opisuju
međusobni položaj koordinatnih sustava 𝐿𝑖 i 𝐿𝑖−1 . Pomoću tih parametara
mogu se odrediti sljedeće matrice homogene transformacije.

6
Pomoću tih matrica može se izračunati matrica 𝑇0 na sljedeći način:

6 1 2 3 4 5 6
𝑇0 = 𝑇0 · 𝑇1 · 𝑇2 · 𝑇3 · 𝑇4 · 𝑇5
PRIPREMA ZA VJEŽBU:

Primjenom Denavit-Hartenbergove metode, svakom i-tom članku zadanog

robotskog manipulatora pridružite koordinatni sustav 𝐿𝑖 te odredite
kinematičke parametre θ𝑖 , 𝑑𝑖 , 𝑎𝑖 i α𝑖 , gdje je i = 1, 2, 3, 4, 5, 6.
KINEMATIČKI PARAMETRI ROBOTA(ZGLOBOVA)

θ 𝑑 𝑎 α

1 90
◦ 0 0 − 90
◦

2 − 90
◦ 75 769 0
◦

3 90
◦ 0 0 90
◦

4 0
◦ 662, 5 0 − 90
◦

5 0
◦ 0 0 90
◦

6 − 90
◦ 133 0 0
◦
RAD NA VJEŽBI:

● Prikažite razmatrani robotski manipulator na zaslonu računala za neke

zadane vrijednosti varijabli zglobova.
● Prikažite na zaslonu računala robotski manipulator i kvadar A u
položaju u kojem je kvadar postavljen između krakova hvataljke robota
tako da su mu stranice poravnate sa stranicama hvataljke

def task1(q):
# Scene.
s = vis.visualizer()

# Axes.
axes = vtk.vtkAxesActor()
s.add_actor(axes)

# Floor.
set_floor(s, [1, 1])

# Robot.
T0S = np.identity(4)
T0S[2,3] = 0.32
rob = robot(s)
T6S = rob.set_configuration(q, 0.03, T0S)

#item
colors = vtkNamedColors()
item = vis.cube(0.07, 0.07, 0.07)
TC6 = np.identity(4)
TC6[2,3] = -0.005
TCS = T6S @ TC6
vis.set_pose(item, TCS)

s.add_actor(item)
item.GetProperty().SetColor(colors.GetColor3d('DarkMagenta'))

# Render scene.
s.run()

def main():
task1([np.pi/2, 0, 0, 0, 0, -np.pi/2])
def main():
task1([np.pi/2, -np.pi/2, np.pi/2, 0, 0, -np.pi/2])
def main():
task1([0, 0, 0, 0, 0, 0])
 VJEŽBA 2
Inverzna kinematika robotskog manipulatora

OPIS VJEŽBE:

Šestoosni robotski manipulator razmatran u prvoj laboratorijskoj vježbi

konstruiran je tako da mu se osi četvrtog, petog i šestog zgloba sijeku u
jednoj točki. Problem inverzne kinematike za takav manipulator moguće je
riješiti primjenom Pieperovog rješenja.

PRIPREMA ZA VJEŽBU:

Riješite problem inverzne kinematike zadanog robotskog manipulatora

razmatranog u prvoj laboratorijskoj vježbi primjenom Pieperovog rješenja.

θ 𝑑 𝑎 α

1 90
◦ 0 0 − 90
◦

2 − 90
◦ 75 769 0
◦

3 90
◦ 0 0 90
◦

4 0
◦ 662, 5 0 − 90
◦

5 0
◦ 0 0 90
◦

6 − 90
◦ 133 0 0
◦
Položaj kocke A u odnosu na bazni koordinatni sustav:

Položaj alata u odnosu na kocku A:

Položaj alata u odnosu na bazni koordinati sustav:

Pozicija sjecišta osi 3 zadnja zgloba:

RAD NA VJEŽBI:

● Prikažite razmatrani robotski manipulator na zaslonu računala u

konfiguraciji definiranoj vektorom q.
● Prikažite na zaslonu računala robotski manipulator u položaju za
hvatanje kvadra A. Hvataljka treba uhvatiti kvadar kao što je prikazano
na slici.

# TASK 2

def invkin(DH, T60, solution):

d = DH[:,1]
a = DH[:,2]
al = DH[:,3]

p = T60 @ np.expand_dims(np.array([0, 0, -d[5], 1]), 1)

x = p[0]
y = p[1]
z = p[2]
r = p[:3].T @ p[:3]

q = np.zeros(6)

q[2] = -2.04091
if solution[0] == 1:
q[2] = -q[2]

c3 = np.cos(q[2])
s3 = np.sin(q[2])
f1 = d[3]*s3 + a[1]

f2 = -d[3]*c3

f3 = 0

phi = np.arctan2(f2, f1)

D = float(f1)/(np.cos(phi))
if solution[1] == 0:
q[1] = np.arcsin(z/D) - phi
else:
q[1] = np.pi - np.arcsin(z/D) - phi
print(q[1])
 c2 = np.cos(q[1])
s2 = np.sin(q[1])
g1 = c2*f1 - s2*f2

g2 = -np.sin(al[0])*(f3+d[1])

c1 = g1*x + g2*y
s1 = -g2*x + g1*y
#q[0] = np.arctan2(s1, c1)
q[0]=-1.104

T10 = dh(q[0], d[0], a[0], al[0])

T21 = dh(q[1], d[1], a[1], al[1])
T32 = dh(q[2], d[2], a[2], al[2])
T30 = T10 @ T21 @ T32
R30 = T30[:3,:3]

R60 = T60[:3,:3]
R63 = R30.T @ R60

#ovdje sam zapela -- ne znam kako doci do jednadžbe (5) iz materijala LV2 - Inverzna
kinematika - rjesenje
# (dobijem ogromnih 9 jednadzbi s 12 nepoznanica(ako za nepoznanice racunamo cos i
sin kuteva))

c5 = -R63[2,2]
q[4] = np.arccos(c5)
if solution[2] == 1:
q[4] = -q[4]
s5 = np.sin(q[4])
if np.abs(s5) > 1e-10:
q[3] = np.arctan2(R63[1,2]/s5, R63[0,2]/s5)
q[5] = np.arctan2(-R63[2,1]/s5, R63[2,0]/s5)
else:
c46 = R63[0,0]
s46 = R63[0,1]
q46 = np.arctan2(s46, c46)
q[3] = q46
q[5] = 0

return q

def task2(solution):
TTS = np.identity(4)
TTS[0,3]=0.12
TTS[1,3] = 0.3

# Scene
s = vis.visualizer()

# Floor.
set_floor(s, [1, 1])

# Target object.
target = vis.cube(0.02, 0.04, 0.05) #change size
 vis.set_pose(target, TTS)
s.add_actor(target)

# Robot.
T0S = np.identity(4)
T0S[2,3] = 0.32
T6T = np.identity(4)
T6T[2, 3] = 0.015
T6T[:3, :3] = roty(np.pi)
T60 = np.linalg.inv(T0S) @ TTS @ T6T
rob = robot(s)
q = invkin(rob.DH, T60, solution)
rob.set_configuration(q, 0.03, T0S)

# Render scene.
s.run()

def main():
task2([0, 1, 0])
 VJEŽBA 3
Planiranje trajektorije

OPIS VJEŽBE:

Generirati trajektoriju robotskog manipulatora razmatranog u prvoj vježbi koja

omogućuje robotu iscrtavanje zadanog znaka (početno slovo imena ili
prezimena) na ploči postavljenoj u njegovom radnom prostoru.

RAD NA VJEŽBI:

1. Primjenom funkcije hocook generirati trajektoriju robotskog

manipulatora, razmatranog u prvoj vježbi, koja omogućuje robotu
iscrtavanje zadanog znaka na ploči postavljenoj u njegovom radnom
prostoru. Prvo zadati početnu i završnu točku putanje te nekoliko
spojnih točaka trajektorije. Dodavati spojne točke dok se ne dobije
željeni rezultat.
2. Rezultat prikazati funkcijom planecontact. Ova funkcija za putanju vrha
alata zadanu matricom W, normalu ravnine n i udaljenost ravnine od
ishodišta d prikazuje one točke putanje čija je udaljenost od zadane
ravnine < 5 mm. Prikazati dobivenu trajektoriju u 3D pomoću funkcije
plot.
3. Prikazati odzive svih varijabli zglobova tijekom izvođenja trajektorije te
njihove prve derivacije (brzine) i druge derivacije (ubrzanja).
Komentirati dobivene rezultate s obzirom na zahtjeve o neprekinutosti
trajektorije te njezine prve i druge derivacije te s obzirom na zadana
ograničenja brzina odnosno ubrzanja.
Trajektorija robotskog manipulatora(zglobova):

Prve derivacije(brzine) trajektorija:

Druge derivacije(ubrzanje) trajektorija:

Iz prijašnjih grafova možemo vidjeti da su zadovoljena sljedeća svojstva

trajektorije:
- trajektorija je neprekidna funkcija vremena
- njena prva i druga derivacija su neprekidne funkcije
REZULTAT

Trajektorija alata u 3D prostoru:

Trajektorija alata u 2D prostoru(slovo “V”):

 VJEŽBA 4
Robotski vid

OPIS VJEŽBE:

Izračunati položaj objekta na slici snimljenoj kamerom primjenom canny edge

detektora i Houghovih linija.

RAD NA VJEŽBI:

● Kreirati Python funkciju imgproc, koja računa položaj (poziciju i

orijentaciju) objekta snimljenog simuliranom kamerom, u koordinatnom
sustavu kamere.
● Ova funkcija ima sljedeće ulaze:
○ sliku u obliku matrice
○ parametre kamere definirane riječnikom (engl. Dictionary)
○ visinu kamere u odnosu na radnu površinu
○ veličinu predmeta oblika kvadra postavljenog na radnu površinu u
slučajnom položaju.
● Izlazi iz funkcije su:
○ kut alpha koji predstavlja orijentaciju objekta
○ vektor t od tri komponente koji predstavlja poziciju središta
objekta u odnosu na koordinatni sustav kamere.

# TASK 4

def getY(q, p, x):

cosQ = np.cos(q)
sinQ = np.sin(q)
y = (p-x*cosQ)/(sinQ)
return y

def getX(q, p, y):

cosQ = np.cos(q)
sinQ = np.sin(q)
x = (p-y*sinQ)/(cosQ)
return x

def lineFromPoints(P, Q):

a = Q[1] - P[1]
b = P[0] - Q[0]
c = a*(P[0]) + b*(P[1])
 return a, b, c

def line_intersection(line1, line2):

a1, b1, c1 = lineFromPoints(line1[0],line1[1])
a2, b2, c2 = lineFromPoints(line2[0],line2[1])

determinant = a1*b2 - a2*b1

if (determinant == 0):
x = 0
y = 0
else:
x = (c1*b2 - c2*b1)/determinant
y = (a1*c2 - a2*c1)/determinant

return x, y

def distanceTwoPoints(p1, p2):

x1, x2 = p1[0], p2[0]
y1, y2 = p1[1], p2[1]

d = math.sqrt(math.pow((x1-x2),2)+math.pow((y1-y2),2))
return d

def containsPoint(point, a, b, c):

x = point[0]
y = point[1]

if(a*x+b*y == c):
return 1
else:
return 0

def isContainedByLine(p1, p2, a, b, c):

if(containsPoint(p1, a, b, c) and containsPoint(p2, a, b, c)):
return 1
else:
return 0

def imgproc(RGB, camera, camera_height, box_size):

img_w = camera['img_size'][0]
img_h = camera['img_size'][1]
box_w = box_size[0]
box_h = box_size[1]
plt.imshow(RGB)
E=feature.canny(RGB[:,:,0])
H, theta, d = hough_line(E)

lines = []
new_lines = []
line_equation = []
for _, angle, dist in zip(*hough_line_peaks(H, theta, d, min_distance=20,
min_angle=20, threshold=50)):
 #print('angle=%f, dist=%f' % (angle, dist))
cs = np.cos(angle)
sn = np.sin(angle)
if np.abs(cs) >= np.abs(sn):
v = np.array([0, img_h-1])
u = (dist - sn * v) / cs
else:
u = np.array([0, img_w-1])
v = (dist - cs * u) / sn
if(angle != 0.0 and np.abs(angle) != (np.pi/2)):
lines.append([angle, dist])
new_lines.append([u, v])
line_equation.append(lineFromPoints(u, v))
plt.plot(u, v, 'g')

#####
#Compute alpha:
alpha = 0

equations = []
maxDis = 0

for line in lines:

if(line[0] < 0):

y1 = 0
y2 = 480
x1 = getX(line[0], line[1], y1)
x2 = getX(line[0], line[1], y2)
else:
x1 = 0
x2 = 640
y1 = getY(line[0], line[1], x1)
y2 = getY(line[0], line[1], x2)

p1 = (x1, y1)
p2 = (x2, y2)

equations.append([p1, p2])

intersactionPoints = []

temp = []

for equation in equations:

for e in equations:
 if(e != equation):
x, y = line_intersection(e, equation)
point_of_intersection = (x, y)

if(x != 0 and y != 0):

temp.append(point_of_intersection)

intersactionPoints = list(dict.fromkeys(temp))
for point in intersactionPoints:
if(point[0] < 0 or point[0] > 640 or point[1] < 0 or point[1] > 480):
intersactionPoints.remove(point)

#print(intersactionPoints)
#print("-------------------")

tempPoint = intersactionPoints[0]
pointOpposit = []
maxDis = 0

for p in intersactionPoints:
if(p != tempPoint and maxDis < distanceTwoPoints(tempPoint, p)):
maxDis = distanceTwoPoints(tempPoint, p)
pointOpposit = p

intersactionPoints.sort()
#print(intersactionPoints)

dis1 = distanceTwoPoints(intersactionPoints[0], intersactionPoints[1])

dis2 = distanceTwoPoints(intersactionPoints[0], intersactionPoints[2])

points = []
if dis1 > dis2:
points.append(intersactionPoints[0])
points.append(intersactionPoints[1])
else:
points.append(intersactionPoints[0])
points.append(intersactionPoints[2])

k = 0
j = 0
for line in line_equation:
if(isContainedByLine(points[0], points[1], line[0], line[1], line[2])):
k = j
j = j + 1

alpha = lines[k][0] - np.pi/2

if(alpha < 0):

alpha = alpha + np.pi
 x1, y1 = tempPoint[0], tempPoint[1]
x2, y2 = pointOpposit[0], pointOpposit[1]

u = (x1 + x2)/2
v = (y1 + y2)/2

plt.plot(u, v, 'ro')

f = camera['focal_length']

uc = camera['principal_point'][0]
vc = camera['principal_point'][1]

z = camera_height

x = float(z*(u-uc))/float(f)
y = float(z*(v-vc))/float(f)

plt.plot(new_lines[k][0], new_lines[k][1], 'r')

print("Alpha: " + str(np.rad2deg(alpha)) + " deg")

t=[x,y,z]
print("T: " + str(t))

plt.show()

######
return alpha, t

def task4():
camera = {
'img_size': [640, 480],
'focal_length': 1000,
'principal_point': [320, 240],
}
camera_height = 0.5
box_w = 0.1
box_h = 0.05

camsim = CameraSimulator(camera, camera_height, [box_w, box_h], 0.05)

RGB = camsim.get_image()

alpha, t = imgproc(RGB, camera, camera_height, [box_w, box_h])

e_alpha, e_t = camsim.evaluate(alpha, t)

true_alpha, true_t = camsim.getAlphaAndT()

print("True alpha: " + str(np.rad2deg(true_alpha)) + " deg" + "\nTrue T: " +

str(true_t) + " mm")
 print('orientation error: %f deg position error: %f mm' % (np.rad2deg(e_alpha), 1000
* e_t))

def main():
task4()

PRIMJERI:

vlatkamihic@MacBook-Pro-7 OR-LV1 % /usr/local/bin/python3

/Users/vlatkamihic/development/playground/OR-LV1/LV4.py
Alpha: 45.0 deg
T: [0.029467994192724972, 0.04596394616805992, 0.5]
True alpha: 45.562314915083135 deg
True T: [0.02993046 0.04566939 0.5 ] mm
orientation error: 0.562315 deg position error: 0.548302 mm
vlatkamihic@MacBook-Pro-7 OR-LV1 % /usr/local/bin/python3
/Users/vlatkamihic/development/playground/OR-LV1/LV4.py
Alpha: 160.0 deg
T: [-0.023639266399550708, 0.04267221745567775, 0.5]
True alpha: -19.684999365504208 deg
True T: [-0.02357248 0.04239888 0.5 ] mm
orientation error: 0.315001 deg position error: 0.281374 mm
vlatkamihic@MacBook-Pro-7 OR-LV1 % /usr/local/bin/python3
/Users/vlatkamihic/development/playground/OR-LV1/LV4.py
Alpha: 171.0 deg
T: [-0.06253798817207958, 0.06705655896133206, 0.5]
True alpha: -8.821477037127115 deg
True T: [-0.0625514 0.06698068 0.5 ] mm
orientation error: 0.178523 deg position error: 0.077052 mm
vlatkamihic@MacBook-Pro-7 OR-LV1 % /usr/local/bin/python3
/Users/vlatkamihic/development/playground/OR-LV1/LV4.py
Alpha: 76.0 deg
T: [0.08254883645966873, 0.050898542802625686, 0.5]
True alpha: 75.58467435996418 deg
True T: [0.08269935 0.05070794 0.5 ] mm
orientation error: 0.415326 deg position error: 0.242867 mm
 VJEŽBA 5
Robotski vid

OPIS VJEŽBE:

Isplanirati putanju mobilnog robota na temelju numeričke navigacijske

funkcije.

RAD NA VJEŽBI:

Kreirati Python funkciju za mobilnog robota koja planira putanju na temelju

numeričke navigacijske funkcije.

Ova funkcija ima sljedeće ulaze:

● kartu u obliku slike u standardnom formatu (.png), pri čemu bijeli
pikseli predstavljaju slobodan prostor, a crni pikseli predstavljaju
prepreke
● početnu i konačnu poziciju robota na karti (točka s dvije koordinate)
● rezoluciju karte (veličina jednog piksela na slici u metrima)
● radijus robota u metrima

Izlaz iz funkcije je:

● putanja u obliku numpy vektora dimenzija nx2, pri čemu je n broj
točaka putanje. Zadatak je kreirati kartu i implementirati funkcije za
numeričku navigaciju i pronalazak putanje. Predložak funkcija nalazi se
u LV5.py. Primjer karte dan je na donjoj slici.

Za implementaciju numeričke navigacijske funkcije korišten je predložak s

predavanja:
Po uzoru na gore prikazani algoritam izrađena je metoda computeNNF() koja
ovako izgleda:

def computeNNF(binary_map, free_space, start, goal):

nnf = np.ones((binary_map.shape[0], binary_map.shape[1]))

nnf[:] = -1

nnf[goal[1]][goal[0]] = 0
N = 0

xS = start[1]
yS = start[0]

nnf[xS][yS] = -1
path_found = True
while(nnf[xS][yS] == -1):
if(free_space[goal[1]][goal[0]] == False):
path_found = False
nnf[xS][yS] = 0
k = 0
for i in range(0, nnf.shape[0]-1):
for j in range(0, nnf.shape[1]-1):

C = nnf[i][j]

if(C == N):
Cneighbourhood = [(0, -1), (-1, 0), (1, 0), (0, 1)]
for sX, sY in Cneighbourhood:
if(free_space[i+sX][j+sY] == True):
if(nnf[i+sX][j+sY] == -1):
nnf[i+sX][j+sY] = N+1
k = k + 1
if(k == 0):
path_found = False
nnf[xS][yS] = 0
print(str(N)+". gen")

N = N + 1

return nnf, path_found

Za pronalazak putanje napisana je funkcija findPath() po uzoru na sljedeći
algoritam:

def findPath(nnf, start, goal):

P = []
C = Point(start[1], start[0], nnf[start[1]][start[0]])
P.append(C)
while(C.getVal() != 0):

print(C.getVal())
Cneighbourhood = [(0, -1), (-1, 0), (1, 0), (0, 1)]
min = C.getVal()
x = C.getX()
y = C.getY()
for sX, sY in Cneighbourhood:
if(nnf[x+sX][y+sY] < min and nnf[x+sX][y+sY] != -1):
min = nnf[x+sX][y+sY]
C.x = x+sX
C.y = y+sY
C.val = min
Ctemp = Point(C.getX(), C.getY(), C.getVal())
P.append(Ctemp)

return P

Za lakše korištenje ove funkcije definirna je i klasa Point() koja predstavlja

točku na mapi sa svojim x i y koordinatama i vrijednošću:

class Point():

def __init__(self, x, y, val):

self.x = x
self.y = y
self.val = val
 def getX(p):
return p.x

def getY(p):
return p.y

def getVal(p):
return p.val

Pri testiranju programa uzeta su u obzir tri moguća slučaja:

Primjer 1: task5(path, np.array([140,120]), np.array([50, 90]), 1, 5)

- zelena točka - start

- crvena točka - goal

Slika 1.1: map.png

 Slika 1.2: edt.png

Slika 1.3: free_space.png

 Slika 1.4: REZULTAT computeNNF(): nnf.png

Slika 1.5: REZULTAT findPath(): foundPath.png

Primjer 2: task5(path, np.array([200,250]), np.array([68, 55]), 1, 5)

- zelena točka - start

- crvena točka - goal

Slika 2.1: map.png

Slika 2.2: edt.png

 Slika 2.3: free_space.png

Slika 2.4: REZULTAT computeNNF(): nnf.png

 REZULTAT findPath(): Put nije pronađen

vlatka@Lenovo:~/Documents/FX/Osnove robotike/LV/LV1_python$
/bin/python3 "/home/vlatka/Documents/FX/Osnove
robotike/LV/LV1_python/LV5.p
y"
/home/vlatka/Documents/FX/Osnove robotike/LV/LV1_python/LV5.py:609:
DeprecationWarning: Please use `distance_transform_edt` from the `s
cipy.ndimage` namespace, the `scipy.ndimage.morphology` namespace is
deprecated.
edt = ndimage.morphology.distance_transform_edt(binary_map)
0. gen
Path is not found.

Primjer 3: task5(path, np.array([140,120]), np.array([320, 100]), 1, 5)

- zelena točka - start

- crvena točka - goal

Slika 3.1: map.png

 Slika 3.2: edt.png

Slika 3.3: free_space.png

 Slika 3.4: REZULTAT computeNNF(): nnf.png

REZULTAT findPath(): Put nije pronađen

vlatka@Lenovo:~/Documents/FX/Osnove robotike/LV/LV1_python$
/bin/python3 "/home/vlatka/Documents/FX/Osnove
robotike/LV/LV1_python/LV5.p
y"
/home/vlatka/Documents/FX/Osnove
robotike/LV/LV1_python/LV5.py:609: DeprecationWarning: Please use
`distance_transform_edt` from the `s
cipy.ndimage` namespace, the `scipy.ndimage.morphology` namespace is
deprecated.
edt = ndimage.morphology.distance_transform_edt(binary_map)
0. gen
1. gen
2. gen
3. gen
…
90. gen
Path is not found.
 VJEŽBA 6
Mobilni robotski manipulator

CILJ VJEŽBE:

Simulirati kretanje mobilnog robota po zadanoj putanji.

RAD NA VJEŽBI:

Kreirati Python funkciju koja simulira kretanje mobilnog robota po zadanoj

putanji generiranoj u prethodnoj vježbi. Simulacija može biti implementirana
uporabom funkcije mobrobsimanimate iz modula mobrobsim.

Funkcija je rađena pomoću formula iz predloška s predavanja:

𝑥, 𝑦 - pose local goal

θ = 𝑝𝑜𝑠𝑒[2]

σ=1

𝑢 = [𝑢𝐿, 𝑢𝑟]
Kod funkcije step:

def step(self, pose):

#to do:
#local_goal, x, y, theta, rho, alpha, v, w, uR, uL, u

distance=0

localGoal = [self.path[self.counter][0], self.path[self.counter][1]]

while(True):
distance = np.sqrt((self.path[self.counter][0] - pose[0])**2 +
(self.path[self.counter][1] - pose[1])**2)
if(distance > 0.2):
localGoal = [self.path[self.counter][0],
self.path[self.counter][1]]
break
if(self.counter>=len(self.path) - 1):
localGoal = [self.path[len(self.path) - 1] [0],
self.path[len(self.path) - 1] [1]]
break
self.counter+= 1

theta = pose[2]

x = pose[0] - localGoal[0]
y = pose[1] - localGoal[1]

rho = np.sqrt(x**2 + y**2)

print (x, y, rho)

if rho > 0.001:

 alpha = -theta + math.atan2(y,x)
if alpha < -np.pi:
alpha += np.pi * 2
elif alpha > np.pi:
alpha -= np.pi*2

if np.abs(alpha) <= np.pi/2:

v = self.k_rho * rho
w = self.k_alpha * alpha

else:
v = 0
w = 0.4
else:
v=0
w=0

u_r = (self.wheel_distance * w + 2*v) / (2*self.wheel_radius)

u_l = (2*v) / (self.wheel_radius) - u_r

u = [u_l, u_r]

return u

Nažalost nisam uspjela simulirati putanju pomoću ovoga koda. Ne mogu

pronaći dobre parametre k_rho i r_alpha stoga šaljem ovako riješenu skriptu
u prilogu zajedno s izvještajem.

Slika karte:
Slika karte s ucrtanom isplaniranom putanjom:

Slika robotskog manipulatora: