OptimizationProblems.jl/src/ADNLPProblems/robotarm.jl at f60efc25b69ca8de7bdb877bbb15221665379559 · JuliaSmoothOptimizers/OptimizationProblems.jl · GitHub

1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66
67
68
69
70
71
72
73
74
75
76
77
78
79
80
81
82
83
84
85
86
87
88
89
90
91
92
93
94
95
96
97
98
99
100
101
102
103
104
105
106
107
108
109
110
111
112
113
export robotarm

# Minimize the time taken for a robot arm to travel between two points.

#  This is problem 8 in the COPS (Version 3) collection of
#   E. Dolan and J. More
#   see "Benchmarking Optimization Software with COPS"
#   Argonne National Labs Technical Report ANL/MCS-246 (2004)

#  classification OOR2-AN-V-V

function robotarm(; n::Int = default_nvar, L = 4.5, type::Type{T} = Float64, kwargs...) where {T}
  n_orig = n
  N = max(2, div(n_orig, 9))
  n = N + 1
  nvars = 9 * n + 1
  @adjust_nvar_warn("robotarm", n_orig, nvars)
  L = T(L)

  # x : vector of variables, of the form : [ρ(t=t1); ρ(t=t2); ... ρ(t=tf), θ(t=t1), ..., then ρ_dot, ..., then ρ_acc, .. ϕ_acc, tf]
  # There are N+1 values of each 9 variables
  # x = [ρ, θ, ϕ, ρ_dot, θ_dot, ϕ_dot, ρ_acc, θ_acc, ϕ_acc, tf]

  # objective function
  function f(x)
    x[end]
  end

  A = zeros(T, n, 9n + 1)
  for i = 1:n
    A[i, 6n + i] = L
  end

  # constraints function
  function c!(cx, x; L = L, n = n)
    # dynamic bounds on ρ_acc, θ_acc, ϕ_acc
    for i = 1:n
      cx[6 * n - 6 + i] = x[7n + i] * ((L - x[i])^3 + x[i]^3) / 3 * sin(x[2n + i])^2
      cx[7 * n - 6 + i] = x[8n + i] * ((L - x[i])^3 + x[i]^3) / 3
    end
    for i = 1:(n - 1)
      cx[i] = x[1 + i] - x[i] - x[3n + i] * x[end] / n
      cx[n - 1 + i] = x[n + 1 + i] - x[n + i] - x[4n + i] * x[end] / n
      cx[2 * n - 2 + i] = x[2n + 1 + i] - x[2n + i] - x[5n + i] * x[end] / n
      cx[3 * n - 3 + i] = x[3n + 1 + i] - x[3n + i] - x[6n + i] * x[end] / n
      cx[4 * n - 4 + i] = x[4n + 1 + i] - x[4n + i] - x[7n + i] * x[end] / n
      cx[5 * n - 5 + i] = x[5n + 1 + i] - x[5n + i] - x[8n + i] * x[end] / n
    end
    return cx
  end

  lcon = T[
    -ones(n)
    zeros(6N)
    -ones(n)
    -ones(n)
  ]

  ucon = T[
    ones(n)
    zeros(6N)
    ones(n)
    ones(n)
  ]

  # Building a feasible x0
  tf0 = T(1)
  θ0 = T[2π / 3 * (t / n)^2 for t = 1:n]
  θ0[1] = T(0)
  x0 = [L * ones(T, n); θ0; π * ones(T, n) / 4; zeros(T, 6n); tf0]

  # defining the bounds on the variables
  lvar = T[zeros(n); -π * ones(n); zeros(n); -Inf * ones(6n); 0]
  uvar = T[L * ones(n); π * ones(n); 2π * ones(n); Inf * ones(6n); Inf]
  lvar[1] = uvar[1] = lvar[n] = uvar[n] = T(L)
  lvar[n + 1] = uvar[n + 1] = T(0)
  lvar[2n] = uvar[2n] = T(2 * π / 3)
  lvar[2n + 1] = uvar[2n + 1] = lvar[3n] = uvar[3n] = T(π / 4)
  lvar[3n + 1] =
    uvar[3n + 1] =
      lvar[4n] =
        uvar[4n] =
          lvar[4n + 1] =
            uvar[4n + 1] =
              lvar[5n] =
                uvar[5n] =
                  lvar[5n + 1] =
                    uvar[5n + 1] =
                      lvar[6n] =
                        uvar[6n] =
                          lvar[6n + 1] =
                            uvar[6n + 1] =
                              lvar[7n] =
                                uvar[7n] =
                                  lvar[7n + 1] =
                                    uvar[7n + 1] =
                                      lvar[8n] =
                                        uvar[8n] =
                                          lvar[8n + 1] = uvar[8n + 1] = lvar[9n] = uvar[9n] = T(0)

  return ADNLPModels.ADNLPModel!(
    f,
    x0,
    lvar,
    uvar,
    findnz(sparse(A))...,
    c!,
    lcon,
    ucon,
    name = "robotarm";
    kwargs...,
  )
end