Group Abstract Group Abstract

Message Boards Message Boards

Optimization with recursive variables

Posted 2 years ago
POSTED BY: Christian Vitale

I tookthe liberty to change somewhat the style of the coding, but the only meaningful change is in the variable constraintsm1, which was very deeply nested. However, the following code gives an answer very quiclky:

delta = 1/10; dt = 1/10; horizon = 20; x = 0;
y = 1/10; psi = 1/2; cx = 8; cy = 8; r = 3/2;
Ak[dt_, uv_, upsi_] := {{1, 0, dt*uv, 0},
   {0, 1, 0, dt*uv},
   {0, 0, Cos[dt*upsi], -Sin[dt*upsi]},
   {0, 0, Sin[dt*upsi], Cos[dt*upsi]}};
Amom1[i_] := Ak[dt, uv[i], upsi[i]];
m1[0] = {{x}, {y}, {Cos[psi]}, {Sin[psi]}};
m1[i_] := Table[Subscript[a[i], k, l],
    {k, 4}, {l, 1}];
uvArray = Array[uv, horizon];
upsiArray = Array[upsi, horizon];
List1 = Array[m1, horizon];
List2 = Table[Amom1[i] . m1[i - 1], {i, horizon}];
constraintsm1 = Thread[Flatten[List1] == Flatten[List2]];
vars = Join[uvArray, upsiArray, Flatten[List1]];
f[x_] := objectiveFunction;  (* Objective function *)
PrintPartialResult[iteration_, vars_] :=
      Print["Iteration ", iteration, ": f(x) = ",
    f[vars], ", x = ", vars];
iteration = 1;  (* Initialize iteration count *)
averageObstacleExpression[m1Aux_, obstacleExpression_] := 
  obstacleExpression /. {x1 -> m1Aux[[1, 1]], x2 -> m1Aux[[2, 1]]};
squaredObstacleExpression = averageObstacleExpression;
averageObstacleAux2 = Expand[(x1 - cx)^2 + (x2 - cy)^2 - r^2];
constraintsaverageObstacle1 = 
  Table[-averageObstacleExpression[m1[i], averageObstacleAux2] <= 0,
   {i, horizon}];
objectiveFunctionAux2 = Expand[(x1 - cx)^2 + (x2 - cy)^2];
squaredObstacleAux2 = Expand[((x1 - cx)^2 + (x2 - cy)^2 - r^2)^2];
constraintsaverageObstacle2 = 
  Table[(4/9 - delta) squaredObstacleExpression[m1[i], 
       squaredObstacleAux2] -
     4/9 averageObstacleExpression[m1[i], averageObstacleAux2]^2 <= 
    0,
   {i, horizon}];
objectiveFunction = 
  averageObstacleExpression[m1[horizon], objectiveFunctionAux2];
constraintsaverageObstacle3 = 
  Table[5/8 squaredObstacleExpression[m1[i], squaredObstacleAux2] - 
     averageObstacleExpression[m1[i], averageObstacleAux2]^2 <= 0,
   {i, horizon}];
NMinimize[{objectiveFunction,
  Thread[0 <= uvArray <= 20],
  Thread[-Pi <= upsiArray <= Pi],
  constraintsaverageObstacle1,
  constraintsaverageObstacle2,
  constraintsaverageObstacle3,
  constraintsm1},
 vars,
 StepMonitor :> PrintPartialResult[iteration++, vars]]

Actually, the constraints constraintsaverageObstacle2 are superfluous, because they are all of the form k^2>=0.

POSTED BY: Gianluca Gorni
Reply to this discussion
Community posts can be styled and formatted using the Markdown syntax.
Reply Preview
Attachments
Remove
or Discard