From 843b30f5d3d1651a2335066988307f470954cc29 Mon Sep 17 00:00:00 2001 From: spirkelmann Date: Fri, 14 Jun 2019 10:38:21 +0200 Subject: [PATCH] added option to set target angle --- remote_control/casadi_opt.py | 10 ++++++---- 1 file changed, 6 insertions(+), 4 deletions(-) diff --git a/remote_control/casadi_opt.py b/remote_control/casadi_opt.py index 9acb362..4ff5abf 100644 --- a/remote_control/casadi_opt.py +++ b/remote_control/casadi_opt.py @@ -4,7 +4,7 @@ import time # look at: https://github.com/casadi/casadi/blob/master/docs/examples/python/vdp_indirect_multiple_shooting.py class OpenLoopSolver: - def __init__(self, N=20, T=2.0): + def __init__(self, N=10, T=2.0): self.T = T self.N = N @@ -19,7 +19,8 @@ class OpenLoopSolver: r = 0.03 R = 0.05 d = 0.02 - omega_max = 13.32 + #omega_max = 13.32 + omega_max = 10.0 omegar = SX.sym('omegar') omegal = SX.sym('omegal') @@ -182,7 +183,7 @@ class OpenLoopSolver: theta)) * omegal * omega_max f3 = -(r / (2 * R) * omegar - r / (2 * R) * omegal) * omega_max 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]) # ---- solve NLP ------ @@ -209,7 +210,8 @@ class OpenLoopSolver: # ---- path constraints ----------- # limit = lambda pos: 1-sin(2*pi*pos)/2 # 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 --------