Telos4/RoboRally
1
0
Fork 1
Code Issues Pull Requests Releases Wiki Activity

added option to set target angle

Browse Source
This commit is contained in:
Simon Pirkelmann 2019-06-14 10:38:21 +02:00
parent 465309ee45
commit 843b30f5d3
1 changed files with 6 additions and 4 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

10
remote_control/casadi_opt.py
View File

@ -4,7 +4,7 @@ import time
# look at: https://github.com/casadi/casadi/blob/master/docs/examples/python/vdp_indirect_multiple_shooting.py # look at: https://github.com/casadi/casadi/blob/master/docs/examples/python/vdp_indirect_multiple_shooting.py
class OpenLoopSolver: class OpenLoopSolver:
def __init__(self, N=20, T=2.0): def __init__(self, N=10, T=2.0):
self.T = T self.T = T
self.N = N self.N = N
@ -19,7 +19,8 @@ class OpenLoopSolver:
r = 0.03 r = 0.03
R = 0.05 R = 0.05
d = 0.02 d = 0.02
omega_max = 13.32 #omega_max = 13.32
omega_max = 10.0
omegar = SX.sym('omegar') omegar = SX.sym('omegar')
omegal = SX.sym('omegal') omegal = SX.sym('omegal')
@ -182,7 +183,7 @@ class OpenLoopSolver:
theta)) * omegal * omega_max theta)) * omegal * omega_max
f3 = -(r / (2 * R) * omegar - r / (2 * R) * omegal) * omega_max f3 = -(r / (2 * R) * omegar - r / (2 * R) * omegal) * omega_max
xdot = vertcat(f1, f2, f3) xdot = vertcat(f1, f2, f3)
L = (x - target[0]) ** 2 + (y - target[1]) ** 2 + 1e-2 * theta ** 2 + 1e-2 * (omegar ** 2 + omegal ** 2) L = (x - target[0]) ** 2 + (y - target[1]) ** 2 + 1e-2 * (theta - target[2]) ** 2 + 1e-2 * (omegar ** 2 + omegal ** 2)
self.f = Function('f', [state, control], [xdot, L]) self.f = Function('f', [state, control], [xdot, L])
# ---- solve NLP ------ # ---- solve NLP ------
@ -209,7 +210,8 @@ class OpenLoopSolver:
# ---- path constraints ----------- # ---- path constraints -----------
# limit = lambda pos: 1-sin(2*pi*pos)/2 # limit = lambda pos: 1-sin(2*pi*pos)/2
# self.opti.subject_to(speed<=limit(pos)) # track speed limit # self.opti.subject_to(speed<=limit(pos)) # track speed limit
self.opti.subject_to(self.opti.bounded(-0.5, self.U, 0.5)) # control is limited maxcontrol = 0.5
self.opti.subject_to(self.opti.bounded(-maxcontrol, self.U, maxcontrol)) # control is limited
# ---- boundary conditions -------- # ---- boundary conditions --------