New RobotDynamics API (#55)

* Use new RobotDynamics and update costs

* Update objective

* Update problem

* Update dynamics constraints

* Update constraint list

* Fix cost tests

* Tests passing, up version

* Update convals

* Update constraint vals

* Fixes for benchmark problems

* Use `statevectortype` trait

* Fix tests

* Tests passing

* Replace length with  output_dim

* Delete problem constructor w/ integration

* Loosen test tolerances

* Remove octavian

* Update deps

* Update news

NEWS.md

@@ -1,3 +1,19 @@
+# New `v0.6`
+## Updated to new RobotDynamics `v0.4` API
+Allows for both inplace and out-of-place dynamics, cost, and constraint evaluations.
+Jacobians can be calculated using finite differences, forward AD, or user-specified.
+Avoids prohibitively long compilation times for larger state and control dimensions.
+
+- Replaced `length` and `size` methods with `state_dim`, `control_dim`, `output_dim` and 
+`dims`. 
+- `Problem` constructor is now of the form `Problem(model, obj, xf, tf, [x0, constraints])
+Note that `x0` is now a required positional argument and `xf` is an optional keyword 
+argument.
+- Integration for dynamics is now specified by the model instead of a type parameter. 
+- Support for direct collocation via MathOptInterface has been removed (will be implemented
+in a separate repo in the future)
+
+
 # New in `v0.5`
 ## Support for Finite Differencing
 Added support for finite differencing with `FiniteDiff` for dynamics, constraints, and cost functions.

Project.toml

@@ -1,6 +1,6 @@
 name = "TrajectoryOptimization"
 uuid = "c79d492b-0548-5874-b488-5a62c1d9d0ca"
-version = "0.5.1"
+version = "0.6.0"
 
 [deps]
 DocStringExtensions = "ffbed154-4ef7-542d-bbb7-c09d3a79fcae"
@@ -9,6 +9,7 @@ ForwardDiff = "f6369f11-7733-5829-9624-2563aa707210"
 LinearAlgebra = "37e2e46d-f89d-539d-b4ee-838fcccc9c8e"
 MathOptInterface = "b8f27783-ece8-5eb3-8dc8-9495eed66fee"
 RobotDynamics = "38ceca67-d8d3-44e8-9852-78a5596522e1"
+RobotZoo = "74be38bb-dcc2-4b9e-baf3-d6373cd95f10"
 Rotations = "6038ab10-8711-5258-84ad-4b1120ba62dc"
 SparseArrays = "2f01184e-e22b-5df5-ae63-d93ebab69eaf"
 StaticArrays = "90137ffa-7385-5640-81b9-e52037218182"
@@ -19,8 +20,9 @@ DocStringExtensions = "0.8"
 FiniteDiff = "2"
 ForwardDiff = "0.10"
 MathOptInterface = "0.9"
-RobotDynamics = "0.3.2"
-Rotations = "1"
+RobotZoo = "0.3"
+RobotDynamics = "0.4"
+Rotations = "~1.0"
 StaticArrays = "1"
 UnsafeArrays = "1"
 julia = "1"

examples/Internal API.ipynb

@@ -17,9 +17,7 @@
      "name": "stderr",
      "output_type": "stream",
      "text": [
-      "\u001b[32m\u001b[1m Activating\u001b[22m\u001b[39m environment at `~/.julia/dev/TrajectoryOptimization/examples/Project.toml`\n",
-      "┌ Info: Precompiling TrajectoryOptimization [c79d492b-0548-5874-b488-5a62c1d9d0ca]\n",
-      "└ @ Base loading.jl:1278\n"
+      "\u001b[32m\u001b[1m  Activating\u001b[22m\u001b[39m environment at `~/.julia/dev/TrajectoryOptimization/examples/Project.toml`\n"
      ]
     },
     {
@@ -55,13 +53,13 @@
   },
   {
    "cell_type": "code",
-   "execution_count": 2,
+   "execution_count": 10,
    "metadata": {},
    "outputs": [],
    "source": [
     "model = Quadrotor()\n",
     "n,m = size(model)           # number of states and controls\n",
-    "n̄ = state_diff_size(model)  # size of error state\n",
+    "n̄ = RD.errstate_dim(model)  # size of error state\n",
     "N = 51                      # number of knot points\n",
     "tf = 5.0                    # final time\n",
     "\n",
@@ -82,12 +80,12 @@
     "add_constraint!(cons, GoalConstraint(xf, SA[1,2,3]), N)\n",
     "\n",
     "# problem\n",
-    "prob = Problem(model, obj, xf, tf, x0=x0, constraints=cons);"
+    "prob = Problem(model, obj, x0, tf, xf=xf, constraints=cons);"
    ]
   },
   {
    "cell_type": "code",
-   "execution_count": 3,
+   "execution_count": 11,
    "metadata": {},
    "outputs": [],
    "source": [
@@ -108,48 +106,49 @@
   },
   {
    "cell_type": "code",
-   "execution_count": 4,
+   "execution_count": 12,
    "metadata": {},
    "outputs": [
     {
      "data": {
       "text/plain": [
-       "13-element SArray{Tuple{13},Float64,1,13} with indices SOneTo(13):\n",
+       "13-element SVector{13, Float64} with indices SOneTo(13):\n",
        " 1.0\n",
        " 2.0\n",
-       " 1.5001876006215789\n",
-       " 0.7209485318742003\n",
+       " 1.5000008323087501\n",
+       " 0.7209493821666642\n",
        " 0.0\n",
        " 0.0\n",
-       " 0.6929869546740692\n",
+       " 0.6929877258289635\n",
        " 0.0\n",
        " 0.0\n",
-       " 0.20003656032918066\n",
+       " 0.20000020716969158\n",
        " 0.0\n",
        " 0.0\n",
        " 0.6124999999999969"
       ]
      },
-     "execution_count": 4,
+     "execution_count": 12,
      "metadata": {},
      "output_type": "execute_result"
     }
    ],
    "source": [
+    "using RobotDynamics: state, control, states, controls\n",
     "# simulate the system forward\n",
-    "rollout!(prob)\n",
-    "@test state(prob.Z[1]) == prob.x0\n",
-    "@test states(prob)[end] ≈ prob.x0\n",
+    "# rollout!(prob)\n",
+    "# @test state(prob.Z[1]) == prob.x0\n",
+    "# @test states(prob)[end] ≈ prob.x0\n",
     "\n",
     "# alternative method\n",
-    "rollout!(TO.integration(prob), prob.model, prob.Z, prob.x0)\n",
+    "rollout!(RD.StaticReturn(), prob.model, prob.Z, prob.x0)\n",
     "@test state(prob.Z[1]) == prob.x0\n",
     "@test states(prob)[end] ≈ prob.x0\n",
     "\n",
     "# change control so that the state changes\n",
     "u0 += [1,0,1,0]*1e-2\n",
     "initial_controls!(prob, u0)\n",
-    "rollout!(TO.integration(prob), prob.model, prob.Z, prob.x0)\n",
+    "RD.rollout!(RD.StaticReturn(),prob.model, prob.Z, prob.x0)\n",
     "states(prob)[end]"
    ]
   },
@@ -163,12 +162,33 @@
   },
   {
    "cell_type": "code",
-   "execution_count": 5,
+   "execution_count": 13,
    "metadata": {},
-   "outputs": [],
+   "outputs": [
+    {
+     "ename": "LoadError",
+     "evalue": "NotImplementedError: User-defined Jacobian not implemented for RobotDynamics.StaticReturn",
+     "output_type": "error",
+     "traceback": [
+      "NotImplementedError: User-defined Jacobian not implemented for RobotDynamics.StaticReturn",
+      "",
+      "Stacktrace:",
+      " [1] jacobian!(fun::RobotDynamics.StaticReturn, J::Function, y::RobotDynamics.DiscretizedDynamics{Quadrotor{UnitQuaternion{Float64}}, RobotDynamics.RK4, 9}, x::Matrix{Float64}, u::KnotPoint{13, 4, SVector{17, Float64}, Float64})",
+      "   @ RobotDynamics ~/.julia/dev/RobotDynamics/src/functionbase.jl:44",
+      " [2] dynamics_expansion!(sig::RobotDynamics.StaticReturn, fun::RobotDynamics.ForwardAD, model::RobotDynamics.DiscretizedDynamics{Quadrotor{UnitQuaternion{Float64}}, RobotDynamics.RK4, 9}, D::Vector{TrajectoryOptimization.DynamicsExpansion{Float64, 13, 12, 4}}, Z::Traj{13, 4, Float64, KnotPoint{13, 4, SVector{17, Float64}, Float64}})",
+      "   @ TrajectoryOptimization ~/.julia/dev/TrajectoryOptimization/src/expansions.jl:104",
+      " [3] top-level scope",
+      "   @ In[13]:2",
+      " [4] eval",
+      "   @ ./boot.jl:360 [inlined]",
+      " [5] include_string(mapexpr::typeof(REPL.softscope), mod::Module, code::String, filename::String)",
+      "   @ Base ./loading.jl:1116"
+     ]
+    }
+   ],
    "source": [
     "D = [TO.DynamicsExpansion{Float64}(n,n̄,m) for k = 1:N-1]\n",
-    "TO.dynamics_expansion!(TO.integration(prob), D, model, prob.Z)"
+    "TO.dynamics_expansion!(RD.StaticReturn(), RD.ForwardAD(), prob.model, D, prob.Z)"
    ]
   },
   {
@@ -346,7 +366,7 @@
     }
    ],
    "source": [
-    "TO.stage_cost(obj[1], prob.Z[1])"
+    "RD.evaluate(obj[1], prob.Z[1])"
    ]
   },
   {
@@ -1018,15 +1038,15 @@
  ],
  "metadata": {
   "kernelspec": {
-   "display_name": "Julia 1.5.3",
+   "display_name": "Julia 1.6.2",
    "language": "julia",
-   "name": "julia-1.5"
+   "name": "julia-1.6"
   },
   "language_info": {
    "file_extension": ".jl",
    "mimetype": "application/julia",
    "name": "julia",
-   "version": "1.5.3"
+   "version": "1.6.2"
   }
  },
  "nbformat": 4,

src/TrajectoryOptimization.jl

@@ -19,12 +19,15 @@ const MOI = MathOptInterface
 import RobotDynamics
 const RD = RobotDynamics
 
-using RobotDynamics: AbstractModel, LieGroupModel,
+using RobotDynamics: AbstractModel, DiscreteDynamics, LieGroupModel, DiscreteLieDynamics,
 	KnotPoint, StaticKnotPoint, AbstractKnotPoint,
-	QuadratureRule, Implicit, Explicit, DEFAULT_Q, HermiteSimpson,
-	is_terminal, state_diff, state_diff_jacobian!, state_diff_jacobian,
-	state, control, states, controls, get_times, Traj, AbstractTrajectory,
-	num_vars
+	QuadratureRule, Implicit, Explicit, 
+	state_dim, control_dim, output_dim,
+	is_terminal, state_diff, state_diff_jacobian!,
+	state, control, states, controls, gettimes, Traj, AbstractTrajectory,
+	num_vars, dims,
+	FunctionSignature, DiffMethod,
+	FiniteDifference, ForwardAD, StaticReturn, InPlace, UserDefined
 
 import RobotDynamics: jacobian!, state_dim, control_dim, states, controls, 
 	state_diff_jacobian!, rollout!
@@ -89,7 +92,11 @@ include("convals.jl")
 include("problem.jl")
 include("conset.jl")
 
-include("nlp.jl")
+# include("nlp.jl")
 
 include("utils.jl")
+# include("deprecated.jl")
+
+import Base.length
+@deprecate length(con::AbstractConstraint) RobotDynamics.output_dim(con)
 end