roboticexplorationlab / trajectoryoptimization.jl Goto Github PK

A fast trajectory optimization library written in Julia

Home Page: https://roboticexplorationlab.org

License: MIT License

Julia 100.00%

trajectoryoptimization.jl's Introduction

TrajectoryOptimization

Build Status	Documentation

This package is built for the express purpose of defining and evaluating trajectory optimization problems. Although early versions (pre v0.3) also included methods to solve these problems, this is now left to separate packages that implement the interface defined in TrajectoryOptimization.jl. For example, Altro.jl implements the ALTRO solver that used to be included in TrajectoryOptimization.jl. This change was done to make this package lighter and allow more abstraction in how solvers set up and solve the problems defined by this package.

TrajectoryOptimization.jl aims to provide both a convenient API for setting up and defining trajectory optimization problem and extremely efficient methods for evaluating them. Nearly all of the methods implemented have zero memory allocations and have been highly optimized for speed. Since trajectory optimization problem have a unique structure that set them apart from generic NLPs (nonlinear programs), use of the specialized methods in TrajectoryOptimization.jl can provide dramatic improvements in the computational efficiency of the solvers that implement the API.

All methods utilize Julia's extensive autodifferentiation capabilities via ForwardDiff.jl so that the user does not need to specify derivatives of dynamics, cost, or constraint functions.

Installation

To install TrajectoryOptimization.jl, run the following from the Julia REPL:

Pkg.add("TrajectoryOptimization")

What's New

TrajectoryOptimization.jl underwent significant changes between versions v0.1 and v0.2. The new code is significantly faster (up to 100x faster). The core part of the ALTRO solver (everything except the projected newton phase) is completely allocation-free once the solver has been initialized. Most of the API has changed significantly. See the documentation for more information on the new API.

In v0.3 the package was split into several different packages for increased modularity. These include RobotDynamics.jl, Altro.jl, RobotZoo.jl, and TrajOptPlots.jl.

Quick Start

To run a simple example of a constrained 1D block move see script in /examples/quickstart.jl.

Examples

Notebooks with more detailed examples can be found here

trajectoryoptimization.jl's People

Contributors

Stargazers

Watchers

Forkers

rejuvyesh gatherera junli-hit loirblitzt avikde artkuo jdebecdelievre boyali capri2014 jilinzhou sz73 vladimirivan eric-heiden jdr-neu chenjianxing1 arlk dongvision mayataka zeta1999 danineamati rafeemusabbir serenity4 schusterlab ky-lin22 xinsongyan hiroishida freemin7 shuoyangrobotics hansrobo nmboyd shreyshet xiaojake standardgalactic kevin3062 haoyw sheng-cheng hanchwang playfloor nhcho91 zachjiang marc-philip-ecker beamiter mamariomiamo blakerbuchanan vietanhle0101 tuna-on-a-bagel zhxiaoyu218 christopherhaotianwu xkhainguyen zhengdaoli0602 kalebbennaveed fanfufei xuchengyyt baggepinnen vortexmath manumerous qyshen815 skylerhawkins

trajectoryoptimization.jl's Issues

Problems running the example code

Hi,
I'm struggling to get the example code to run.
I'm getting the following error messages when running the example code from the readme file:

model = Model(dynamics!,n,m) # create model
ERROR: UndefVarError: Model not defined

I also tried to run the example scripts. In examples/pendulum.jl I get:

LoadError: MethodError: Cannot `convert` an object of type iLQRSolverOptions{Float64} to an object of type TrajectoryOptimization.UnconstrainedSolverOptions{Float64}

The cartpole and couple others fail with the same massage.
I'm using Julia 1.4.0 and I'm on the the latest master.
Any pointers?

Version 0.2.0

@JuliaRegistrator register
Register new version v0.2.0

Problems Running the Examples

I had a very hard time trying to get some of the examples running. I tried

examples/car_3obs.jl
examples/benchmark_problems.jl
benchmark/run_benchmarks.jl

After commenting out some code, they somehow all got stuck when importing TrajOptPlots. Example error:

ERROR: LoadError: ArgumentError: Package TrajOptPlots not found in current path:
- Run `import Pkg; Pkg.add("TrajOptPlots")` to install the TrajOptPlots package.

Stacktrace:
 [1] require(::Module, ::Symbol) at ./loading.jl:892
 [2] include(::Module, ::String) at ./Base.jl:377
 [3] exec_options(::Base.JLOptions) at ./client.jl:288
 [4] _start() at ./client.jl:484
in expression starting at /home/jingchi/TrajectoryOptimization.jl/examples/benchmark_problems.jl:3

I tried install the latest version of TrajOptPlots with the following cmd:

(@v1.4) pkg> add https://github.com/RoboticExplorationLab/TrajOptPlots.jl.git

which failed with the following error:

ERROR: Unsatisfiable requirements detected for package TrajOptCore [6e71adf8]:
 TrajOptCore [6e71adf8] log:
 ├─TrajOptCore [6e71adf8] has no known versions!
 └─restricted to versions * by TrajOptPlots [7770976a] — no versions left
   └─TrajOptPlots [7770976a] log:
     ├─possible versions are: 0.1.0 or uninstalled
     └─TrajOptPlots [7770976a] is fixed to version 0.1.0

Then I tried to install TrajOptCore, which also failed.

My questions are

How do I get a slightly more complex problem up and running without messing with dependencies?
What is the proper way to install TrajOptPlots.jl and TrajOptCore?

Examples folder do not contain the examples from the paper

Hi,
unlike it says at the end of the readme

Examples

Notebooks with more detailed examples can be found here, including all the examples from our IROS 2019 paper.

only the quickstart.jl file is provided.

Could you please make the files available?

Best
Simon

Unable to compile

I have installed Julia Pro: (v1.4.2-1) and try to install the package but it gives me the following error. It asks for a username. I thought it was asking for my GitHub username but it did not work. I installed Pkg.add("Plots") and it just smoothly installed without asking for such username. I don't know if it is an issue of Julia or this package but I am able to install other packages. Am I doing something wrong, btw I am new to Julia env.

julia> Pkg.add("TrajectoryOptimization")
Resolving package versions...
Updating git-repo https://github.com/JuliaComputing/BinaryProvider.jl.git
Username for 'https://github.com':

simple trajectory optimization seems to fail with inequality constraint

Problem

With the help of the quickstart.jl, I defined a simple path-planning problem in configuration space (not in the phase space). The snippets in pasted in the last of this issue. From the result of this optimization, the solver seems to be aware of the inequality constraint, but it just jumps over the circle, which usually does not happens with other trajectory optimization algorithms (like the one usign SQP). The resulting path is shown in below for your information.

After that, I noticed that maybe I have to put some input constraints because the formulation of the DDP is different from the ordinal trajectory optimization as input is involved, so I added the following constraint (please de-comment-out the corresponding part in the snippet).

u_bnd = [0.1, 0.1]
bnd = BoundConstraint(n,m, u_min=-u_bnd, u_max=u_bnd)
add_constraint!(conSet, bnd, 1:N-1)

However the result is like

It would be really helpful if you point out the mistakes in my snippets to obtain a proper result.

Finally, I'm really impressed the result reported in the ICRA paper. Releasing a such a well-structured open-source software must take a lot of effort, and I appreciate a lot to your effort.

Snippet to reproduce this issue

using Altro
using TrajectoryOptimization
using StaticArrays, LinearAlgebra
using RobotDynamics
using PyPlot

struct PointModel <: AbstractModel end
function RobotDynamics.dynamics(model::PointModel, x, u)
    return u
end
size(model::PointModel) = (2, 2)
RobotDynamics.state_dim(model::PointModel) = 2
RobotDynamics.control_dim(model::PointModel) = 2
model = PointModel()
n,m = size(model)

N = 21
tf = 5.
dt = tf/(N-1)

x0 = @SVector zeros(n)
xf = @SVector ones(n)
u0 = @SVector fill(0.0,m)
U0 = [u0 for k = 1:N-1]

Q = 0.0*Diagonal(@SVector ones(n))
Qf = 0.0*Diagonal(@SVector ones(n))
R = 10.0 * Diagonal(@SVector ones(m))
obj = LQRObjective(Q,R,Qf,xf,N)

circle = TrajectoryOptimization.CircleConstraint(2, [0.5], [0.5], [0.2])

conSet = ConstraintList(n,m,N)
goal = GoalConstraint(xf)
add_constraint!(conSet, goal, N)
add_constraint!(conSet, circle, 1:N)

# with input constraint
#u_bnd = [0.1, 0.1]
#bnd = BoundConstraint(n,m, u_min=-u_bnd, u_max=u_bnd)
#add_constraint!(conSet, bnd, 1:N-1)

prob = Problem(model, obj, xf, tf, x0=x0, constraints=conSet)
initial_controls!(prob, U0)

opts = SolverOptions(
    cost_tolerance_intermediate=1e-2,
    penalty_scaling=10.,
    penalty_initial=1.0
)
altro = ALTROSolver(prob, opts)
@time solve!(altro);
X = states(altro)
U = controls(altro)

xs = [x[1] for x in X]
ys = [x[2] for x in X]
scatter(xs, ys, c=:blue, s=10)
plt.gcf().gca().add_artist(plt.Circle((0.5, 0.5), 0.2, fill=false))

Update docs on defining a new integration scheme

Working examples and "stable" dev branch

Hello! I would like to run and test some of the things implemented here. I am aware of the development and dev_notebooks folders, but I'm having some minor issues trying to run them.

The most up-to-date branch is restructure, right? Are there any working examples, something along the lines of development/paper/kuka_tests.jl?

Get rid of `t0` and `tf` in `Problem` since these are stored in the trajectory

Unable to define discrete system

In order to minimize the number of continuous dynamics calls for my linear system, I have tried to implement my own custom integration type and discrete_dynamics functions. However, I still have to define a continuous dynamics function because the initialization rollout defaults to the default integration rule. The following lines of code seem to be the culprit:

https://github.com/RoboticExplorationLab/Altro.jl/blob/a33a013c28c6fdcc35dbc48ba02e0e372a56e995/src/ilqr/rollout.jl#L33

TrajectoryOptimization.jl/src/trajectories.jl

Line 178 in 20f726e

RobotDynamics.propagate_dynamics(DEFAULT_Q, model, Z[k], Z[k-1])

This currently prevents you from defining a purely discrete system.

Adding terminal state constraint

EDIT: Sorry, I think I was looking at the wrong docs! Will try v0.1.2

Hi! I'm new to the TrajectoryOptimization package but think it's very cool!

I've been trying to use the package to solve an optimal control problem that has a functional constraint on the terminal state. Specifically, I want something like g(xf) == 0 for an arbitrary (in my case, scalar) function g and terminal state xf.

When I'm constructing a new constraint type (I'd like it to be a mutable struct TerminalStateConstraint{T <: AbstractFloat, T2 <: Integer} <: AbstractConstraint{Equality,State,1}, I'm running into a few issues:

julia> using TrajectoryOptimization
julia> mutable struct TerminalStateConstraint{T <: AbstractFloat, T2 <: Integer} <: AbstractConstraint{Equality,State,1}; end
ERROR: UndefVarError: AbstractConstraint not defined
Stacktrace:
 [1] top-level scope at none:0

So then I explicitly call it from the package:

julia> mutable struct TerminalStateConstraint{T <: AbstractFloat, T2 <: Integer} <: TrajectoryOptimization.AbstractConstraint{Equality,State,1}; end
ERROR: UndefVarError: State not defined
Stacktrace:
 [1] top-level scope at none:0

and still:

julia> mutable struct TerminalStateConstraint{T <: AbstractFloat, T2 <: Integer} <: TrajectoryOptimization.AbstractConstraint{Equality,TrajectoryOptimization.State,1}; end
ERROR: UndefVarError: State not defined
Stacktrace:
 [1] getproperty(::Module, ::Symbol) at ./sysimg.jl:13
 [2] top-level scope at none:0

Then I tried the example from the docs (after a few tries)

julia> struct ControlNorm{T} <: TrajectoryOptimization.AbstractConstraint{Inequality,TrajectoryOptimization.Control,1}
         m::Int
         a::T
       end
ERROR: UndefVarError: Control not defined
Stacktrace:
 [1] getproperty(::Module, ::Symbol) at ./sysimg.jl:13
 [2] top-level scope at none:0

So, I'm probably missing something about using the interface and was how I might best implement a terminal state constraint, and whether I need to specify the function TrajectoryOptimization.evaluate or the in-place version TrajectoryOptimization.evaluate! (as well as jacobians) for the new struct.

Thanks!

Btw, all tests have passed for me:

(TrajOpt) pkg> test TrajectoryOptimization
   Testing TrajectoryOptimization
 Resolving package versions...
    Status `/var/folders/10/y7z26t512kbbfrsj12fx3wkr0000gn/T/tmpnXJAft/Manifest.toml`
  [bf4720bc] AssetRegistry v0.1.0
  [13072b0f] AxisAlgorithms v1.0.0
  [6e4b80f9] BenchmarkTools v0.5.0
  [9e28174c] BinDeps v1.0.0
  [b99e7846] BinaryProvider v0.5.8
  [8e7c35d0] BlockArrays v0.9.1
  [70588ee8] CSSUtil v0.1.0
  [7057c7e9] Cassette v0.2.6
  [523fee87] CodecBzip2 v0.6.0
  [944b1d66] CodecZlib v0.6.0
  [3da002f7] ColorTypes v0.9.1
  [5ae59095] Colors v0.11.2
  [861a8166] Combinatorics v1.0.0
  [bbf7d656] CommonSubexpressions v0.2.0
  [34da2185] Compat v2.2.0
  [d38c429a] Contour v0.5.2
  [150eb455] CoordinateTransformations v0.5.1
  [9a962f9c] DataAPI v1.1.0
  [864edb3b] DataStructures v0.17.11
  [163ba53b] DiffResults v0.0.4
  [b552c78f] DiffRules v0.1.0
  [ffbed154] DocStringExtensions v0.8.1
  [e30172f5] Documenter v0.24.7
  [c87230d0] FFMPEG v0.2.4
  [53c48c17] FixedPointNumbers v0.7.1
  [59287772] Formatting v0.4.1
  [f6369f11] ForwardDiff v0.10.3
  [de31a74c] FunctionalCollections v0.5.0
  [28b8d3ca] GR v0.48.0
  [4d00f742] GeometryTypes v0.7.10
  [cd3eb016] HTTP v0.8.13
  [9fb69e20] Hiccup v0.2.2
  [83e8ac13] IniFile v0.5.0
  [d3863d7c] InteractBase v0.10.4
  [a98d9a8b] Interpolations v0.12.2
  [b6b21f68] Ipopt v0.6.0
  [c8e1da08] IterTools v1.3.0
  [97c1335a] JSExpr v0.5.2
  [682c06a0] JSON v0.21.0
  [7d188eb4] JSONSchema v0.2.0
  [bcebb21b] Knockout v0.2.4
  [50d2b5c4] Lazy v0.14.0
  [9c8b4983] LightXML v0.8.1
  [39f5be34] LoopThrottle v0.1.0
  [1914dd2f] MacroTools v0.5.5
  [b8f27783] MathOptInterface v0.9.13
  [fdba3010] MathProgBase v0.7.8
  [739be429] MbedTLS v0.6.8
  [442fdcdd] Measures v0.3.1
  [931e9471] MechanismGeometries v0.4.1
  [283c5d60] MeshCat v0.10.0
  [6ad125db] MeshCatMechanisms v0.5.0
  [e1d29d7a] Missings v0.4.3
  [99f44e22] MsgPack v1.1.0
  [d8a4904e] MutableArithmetics v0.2.7
  [a975b10e] Mux v0.7.1
  [77ba4419] NaNMath v0.3.3
  [510215fc] Observables v0.3.1
  [6fe1bfb0] OffsetArrays v1.0.4
  [bac558e1] OrderedCollections v1.1.0
  [d96e819e] Parameters v0.12.0
  [69de0a69] Parsers v1.0.1
  [c45eb73e] PartedArrays v0.1.0
  [fa939f87] Pidfile v1.1.0
  [ccf2f8ad] PlotThemes v1.0.2
  [995b91a9] PlotUtils v0.6.4
  [91a5bcdd] Plots v0.29.9
  [f27b6e38] Polynomials v0.6.1
  [c84ed2f1] Ratios v0.4.0
  [3cdcf5f2] RecipesBase v0.8.0
  [189a3867] Reexport v0.2.0
  [ae029012] Requires v1.0.1
  [366cf18f] RigidBodyDynamics v2.2.0
  [6038ab10] Rotations v0.13.0
  [992d4aef] Showoff v0.3.1
  [a2af1166] SortingAlgorithms v0.3.1
  [276daf66] SpecialFunctions v0.8.0
  [90137ffa] StaticArrays v0.12.1
  [2913bbd2] StatsBase v0.33.0
  [a759f4b9] TimerOutputs v0.5.3
  [c79d492b] TrajectoryOptimization v0.1.2
  [3bb67fe8] TranscodingStreams v0.9.5
  [94a5cd58] TypeSortedCollections v1.1.0
  [30578b45] URIParser v0.4.0
  [c4a57d5a] UnsafeArrays v1.0.0
  [0f1e0344] WebIO v0.8.11
  [104b5d7c] WebSockets v1.5.2
  [cc8bc4a8] Widgets v0.6.2
  [efce3f68] WoodburyMatrices v0.5.2
  [2a0f44e3] Base64  [`@stdlib/Base64`]
  [ade2ca70] Dates  [`@stdlib/Dates`]
  [8bb1440f] DelimitedFiles  [`@stdlib/DelimitedFiles`]
  [8ba89e20] Distributed  [`@stdlib/Distributed`]
  [7b1f6079] FileWatching  [`@stdlib/FileWatching`]
  [b77e0a4c] InteractiveUtils  [`@stdlib/InteractiveUtils`]
  [76f85450] LibGit2  [`@stdlib/LibGit2`]
  [8f399da3] Libdl  [`@stdlib/Libdl`]
  [37e2e46d] LinearAlgebra  [`@stdlib/LinearAlgebra`]
  [56ddb016] Logging  [`@stdlib/Logging`]
  [d6f4376e] Markdown  [`@stdlib/Markdown`]
  [a63ad114] Mmap  [`@stdlib/Mmap`]
  [44cfe95a] Pkg  [`@stdlib/Pkg`]
  [de0858da] Printf  [`@stdlib/Printf`]
  [3fa0cd96] REPL  [`@stdlib/REPL`]
  [9a3f8284] Random  [`@stdlib/Random`]
  [ea8e919c] SHA  [`@stdlib/SHA`]
  [9e88b42a] Serialization  [`@stdlib/Serialization`]
  [1a1011a3] SharedArrays  [`@stdlib/SharedArrays`]
  [6462fe0b] Sockets  [`@stdlib/Sockets`]
  [2f01184e] SparseArrays  [`@stdlib/SparseArrays`]
  [10745b16] Statistics  [`@stdlib/Statistics`]
  [4607b0f0] SuiteSparse  [`@stdlib/SuiteSparse`]
  [8dfed614] Test  [`@stdlib/Test`]
  [cf7118a7] UUIDs  [`@stdlib/UUIDs`]
  [4ec0a83e] Unicode  [`@stdlib/Unicode`]
     5        10.5
Test Summary: | Pass  Total
Logging       |   43     43
Test Summary: | Pass  Total
Constraints   |   68     68
Test Summary: | Pass  Total
Model         |   44     44
Test Summary:  | Pass  Total
Modified Model |   20     20
Test Summary: | Pass  Total
Problems      |   35     35
Test Summary:             | Pass  Total
Square Root Backward Pass |  130    130
Test Summary:                              | Pass  Total
Infeasible State Trajectory Initialization |    6      6

******************************************************************************
This program contains Ipopt, a library for large-scale nonlinear optimization.
 Ipopt is released as open source code under the Eclipse Public License (EPL).
         For more information visit http://projects.coin-or.org/Ipopt
******************************************************************************

Test Summary: | Pass  Total
Minimum Time  |   15     15
Test Summary: | Pass  Total
Costs         |   45     45
Test Summary: | Pass  Total
Utils         |   28     28
Test Summary: | Pass  Total
Pendulum      |   14     14
Test Summary: | Pass  Total
Car           |    2      2
Test Summary: | Pass  Total
Quadrotor     |    7      7
Test Summary: | Pass  Total
DIRCOL        |   30     30
   Testing TrajectoryOptimization tests passed

Copy undefined for RobotDynamics models

I’m getting an error running code that was previously working.

ERROR: LoadError: MethodError: no method matching copy(::SingleIntegrator{4})

This happens when I call Problem() with a single model

si_prob = Problem(si_model, si_obj, x0, tf, xf=xT, constraints=si_cons)

From looking at the git blame, it looks like the Problem interface changed so that a single model gets copied into a vector of models.

TrajectoryOptimization.jl/src/problem.jl

Line 115 in 0e93ad3

models = [copy(model) for k = 1:N-1]

My original issue was with a custom model. A quick test on a model from RobotZoo shows a similar problem.

julia> using RobotZoo

julia> m = RobotZoo.Quadrotor();

julia> copy(m)
ERROR: MethodError: no method matching copy(::RobotZoo.Quadrotor{Rotations.UnitQuaternion{Float64}, 9})
Closest candidates are:
  copy(::SubArray{var"#s814", var"#s813", var"#s812", I, L} where {var"#s814", var"#s813", var"#s812"<:Union{SparseArrays.AbstractSparseMatrixCSC, SparseArrays.SparseVector}, I, L}) at /Users/julia/buildbot/worker/package_macos64/build/usr/share/julia/stdlib/v1.6/SparseArrays/src/sparsevector.jl:720
  copy(::SubArray) at subarray.jl:70
  copy(::LinearAlgebra.Cholesky) at /Users/julia/buildbot/worker/package_macos64/build/usr/share/julia/stdlib/v1.6/LinearAlgebra/src/cholesky.jl:439
  ...
Stacktrace:
 [1] top-level scope
   @ REPL[7]:1

Register v0.1.1

@JuliaRegistrator register()

Update version to v0.1.1 to fix an installation bug in previous version.

Citation

Thanks for this neat library! I have been using it in my work and wanted to know if you had a preferred way to cite this work. I haven't used ALTRO specifically, but just the iLQR solver - so I wasn't sure if I should cite the IROS paper or not.

Analytical dynamics jacobian

Warn user when discretizing a dynamics model that has an analytical Jacobian that the analytical Jacobian is going to be ignored.

FiniteDiff Error when calling DiscretizedDynamics

Hello,
I'm trying to discretize the continuous model of the quadrotor and I got this error:

ERROR: MethodError: Cannot `convert` an object of type 
  FiniteDiff.JacobianCache{Vector{Float64}, Vector{Float64}, Vector{Float64}, Vector{Float64}, UnitRange{Int64}, Nothing, Val{:forward}(), Float64} to an object of type 
  FiniteDiff.JacobianCache{Vector{Float64}, Vector{Float64}, Vector{Float64}, UnitRange{Int64}, Nothing, Val{:forward}(), Float64}
Closest candidates are:
  convert(::Type{T}, ::T) where T at ~/julia/share/julia/base/essentials.jl:218

Here's the code that can reproduce this error:


using TrajectoryOptimization
using RobotDynamics
using StaticArrays, LinearAlgebra
using ForwardDiff
using FiniteDiff

struct One_QuadRotor <: RobotDynamics.ContinuousDynamics
    mass::Float64
    J::Diagonal{Float64,SVector{3,Float64}}
    Jinv::Diagonal{Float64,SVector{3,Float64}}
    gravity::SVector{3,Float64}
    motor_dist::Float64
    kf::Float64
    km::Float64
end

mass=0.5
J=Diagonal(@SVector [0.0023, 0.0023, 0.004])
Jinv = inv(J)
gravity=@SVector[0,0,-9.81]
motor_dist=0.1750
kf=1.0
km=0.0245

One_QuadRotor() = One_QuadRotor(mass,J,Jinv,gravity,motor_dist,kf,km)


RobotDynamics.state_dim(::One_QuadRotor) = 13
RobotDynamics.control_dim(::One_QuadRotor) = 4

function RobotDynamics.dynamics(model::One_QuadRotor, x, u)
    m = model.mass
    J = model.J
    Jinv = model.Jinv
    g = model.gravity
    L = model.motor_dist
    kf = model.kf
    km = model.km

    # Extract motor speeds
    w1_1 = u[1]
    w2_1 = u[2]
    w3_1 = u[3]
    w4_1 = u[4]

    # Calculate motor forces
    F1_1 = max(0,kf*w1_1);
    F2_1 = max(0,kf*w2_1);
    F3_1 = max(0,kf*w3_1);
    F4_1 = max(0,kf*w4_1);
    
    F_1_b = @SVector [0., 0., F1_1+F2_1+F3_1+F4_1] #total rotor force in body frame

    # Calculate motor torques
    M1_1 = km*w1_1;
    M2_1 = km*w2_1;
    M3_1 = km*w3_1;
    M4_1 = km*w4_1;
    
    tau_1 = @SVector [L*(F2_1-F4_1), L*(F3_1-F1_1), (M1_1-M2_1+M3_1-M4_1)] #total rotor torque in body frame
    
    r1 = x[1:3]
    q1 = UnitQuaternion(x[4],x[5],x[6],x[7], false)
    v1 = x[8:10]
    w1 = x[11:13]
    
    F1 = m*g + q1*F_1_b
    
    q1dot = Rotations.kinematics(q1,w1)
    
    r1dot = v1
    v1dot = F1 ./m
    
    w1dot = Jinv*(tau_1 - w1 × (J*w1))
    
    return [r1dot; q1dot; v1dot; w1dot]
end

function RobotDynamics.dynamics!(model::One_QuadRotor, xdot, x, u)
    xstatic = SA[x[1], x[2], x[3], x[4], x[5], x[6], x[7], x[8], x[9], x[10], x[11], x[12], x[13]]
    ustatic = SA[u[1], u[2], u[3], u[4]]
    xdot .= RobotDynamics.dynamics(model, xstatic, ustatic)
    return nothing
end


rk4 = RobotDynamics.RK4(13, 4)
baseline_model = RobotDynamics.DiscretizedDynamics(One_QuadRotor(), rk4)

I also tried to follow the tutorial and add @autodiff before struct One_QuadRotor, but then this same error occurs when calling
One_QuadRotor() = One_QuadRotor(mass,J,Jinv,gravity,motor_dist,kf,km)

Add support for hybrid dynamics

Multiple Constraints

Problem

I'm playing around with multiple constraints for a Double Integrator model (taken from #49). In the toy example below, I've created two CircleConstraints and notice that the resulting output solution doesn't look good. I was wondering if you could give some pointers on how to fix this.

Thank you for your help!

[EDIT] I've now added BoundConstraints on all the knots (except the last knot):

add_constraint!(
    conSet,
    BoundConstraint(n, m, x_min = lower_bounds, x_max = upper_bounds),
    1:N-1,
)

Example

using Altro
using TrajectoryOptimization
using StaticArrays, LinearAlgebra
using RobotDynamics
using PyPlot


struct DoubleIntegrator{} <: TrajectoryOptimization.AbstractModel
    A::Any
    B::Any
    n::Integer  # state space size
    m::Integer  # ctrl space size

    function DoubleIntegrator()
        A = [
            zeros(2, 2) I(2)
            zeros(2, 2) zeros(2, 2)
        ]
        B = [
            zeros(2, 2)
            I(2)
        ]
        n = 4
        m = 2
        new(A, B, n, m)
    end
end


function RobotDynamics.dynamics(model::DoubleIntegrator, x, u)
    model.A * x + model.B * u
end

Base.size(::DoubleIntegrator) = 4, 2
RobotDynamics.state_dim(model::DoubleIntegrator) = 4
RobotDynamics.control_dim(model::DoubleIntegrator) = 2

lower_bounds = [0.0, 0.0, -0.1, -0.1]
upper_bounds = [1.0, 1.0, 0.1, 0.1]
start = @SVector [0.1, 0.1, 0.0, 0.0]
goal = @SVector [0.9, 0.9, 0.0, 0.0]

model = DoubleIntegrator()
n, m = size(model)

N = 50
tf = 20.0

# u0 = SVector{2,Float64}([0.0, 0.001] + rand(2) * 0.01)
# U0 = [u0 for k = 1:N-1]

Q = 1.0 * Diagonal(@SVector ones(n))
Qf = 10.0 * Diagonal(@SVector ones(n))
R = 1.0 * Diagonal(@SVector ones(m))
obj = LQRObjective(Q, R, Qf, goal, N)

r = [0.2, 0.1]
x = [0.5, 0.6]
y = [0.6, 0.4]
xi = 1
yi = 2
circle1 = TrajectoryOptimization.CircleConstraint(model.n, x, y, r, xi, yi)
# circle2 = TrajectoryOptimization.CircleConstraint(model.n, x[2:2], y[2:2], r[2:2], xi, yi)

conSet = ConstraintList(n, m, N)
# add_constraint!(conSet, GoalConstraint(goal), N:N)

# add_constraint!(conSet, circle2, 1:N)
add_constraint!(conSet, circle1, 1:N)

add_constraint!(
    conSet,
    BoundConstraint(n, m, x_min = lower_bounds, x_max = upper_bounds),
    1:N-1,
)

prob = Problem(model, obj, goal, tf, x0 = start, constraints = conSet)

altro = ALTROSolver(prob)
@time solve!(altro);
X = states(altro)
U = controls(altro)

plt.clf()
xs = [x[1] for x in X]
ys = [x[2] for x in X]
scatter(xs, ys, c = :blue, s = 10)
for i = 1:length(x)
    plt.gcf().gca().add_artist(plt.Circle((x[i], y[i]), r[i], fill = false))
end
plt.savefig("/home/alvin/research/dynamic_lqr_trees/examples/results.png")

Is there a c ++ version？

Gradient not defined for custom cost functions using RD.@autodiff

Based off of the tutorial code here:

http://roboticexplorationlab.org/TrajectoryOptimization.jl/stable/costfunction_interface.html

when I attempt to optimise cartpole controls with ALTRO, I find that the solver throws an error:

NotImplementedError: Gradient not implemented for scalar function CartpoleCost{5}

Is this intended behaviour? The documentation seems to suggest that the RD.@autodiff macro is supposed to automatically define these functions.

The code to produce this error is below, or otherwise I'm happy to share the Jupyter notebook and Project file I have been working with.

Thank you!

using TrajectoryOptimization
using RobotDynamics
import RobotZoo.Cartpole
using StaticArrays, LinearAlgebra
using ForwardDiff, FiniteDiff
using Altro
using Plots

RobotDynamics.@autodiff struct CartpoleCost <: TrajectoryOptimization.CostFunction
    Q
    R
    CartpoleCost() = CartpoleCost(1.0e-2*Diagonal(@SVector ones(n)), 1.0e-1*Diagonal(@SVector ones(m)))
end

RobotDynamics.state_dim(::CartpoleCost) = 4
RobotDynamics.control_dim(::CartpoleCost) = 1

function RobotDynamics.evaluate(cost::CartpoleCost, x, u)
    y = x[1]
    θ = x[2]
    ydot = x[3]
    θdot = x[4]
    J = cost.Q[2] * cos(θ/2)
    J += 0.5* (cost.Q[1] * y^2 + cost.Q[3] * ydot^2 + cost.Q[4] * θdot^2)
    if !isempty(u)
        J += 0.5 * cost.R[1] * u[1]^2
    end
    return J
end

model = Cartpole()
n,m = size(model);

N = 101
tf = 5.
dt = tf/(N-1)

x0 = @SVector [0, pi/2, 0, 0];#zeros(n)
xf = @SVector [0, pi, 0, 0];  # i.e. swing up

# Set up
# Q = 1.0e-2*Diagonal(@SVector ones(n))
# Qf = 100.0*Diagonal(@SVector ones(n))
# R = 1.0e-1*Diagonal(@SVector ones(m))
# obj = LQRObjective(Q,R,Qf,xf,N);

costfun = CartpoleCost()
obj = Objective(costfun, N)

# Create Empty ConstraintList
conSet = ConstraintList(n,m,N)

# Control Bounds
u_bnd = 3.0
bnd = BoundConstraint(n,m, u_min=-u_bnd, u_max=u_bnd)
add_constraint!(conSet, bnd, 1:N-1)

# Goal Constraint
goal = GoalConstraint(xf)
add_constraint!(conSet, goal, N)

prob = Problem(model, obj, x0, tf, constraints=conSet, xf=xf);
rollout!(prob)
solver = ALTROSolver(prob)
solve!(solver)

Git error checking out master due to filename "Underactuated Double Pendulum | Acrobot.ipynb"

This is the error from git (git version 2.17.1.windows.2) on Windows 10.

error: unable to create file examples/double pendulum/Underactuated Double Pendulum | Acrobot.ipynb: Invalid argument

Changing the "|" to "-" is a fine solution.

running on WINDOWS

I am trying to run the code on WINDOWS. I would like to know if there is package rely on Linux?

TagBot trigger issue

This issue is used to trigger TagBot; feel free to unsubscribe.

If you haven't already, you should update your TagBot.yml to include issue comment triggers.
Please see this post on Discourse for instructions and more details.

If you'd like for me to do this for you, comment TagBot fix on this issue.
I'll open a PR within a few hours, please be patient!

RA-L example on the distributed_team_lift branch

I have been trying to reproduce your simulations on the RA-L paper, Scalable Cooperative Transport of Cable-Suspended
Loads with UAVs using
Distributed Trajectory Optimization, which was published recently. I really liked the idea and wanted to use the methodology for my master thesis.

However, it seems that there is a Julia environment problem, and I could not run the code. It seems like there is a version problem with some of the packages.
I could not resolve the manifest, and there are two environments; one is inside the RA-L, and there are references from outside of that environment.

I would really appreciate it if you could help me with the compatibility issues related to that branch and that specific example.

@thowell @bjack205 @zacmanchester

Thank you for your amazing work!

Error related to @autodiff and DiscretizedDynamics()

There seems to have been something in a recent package update that is breaking the Quickstart (and my code). I’ve found that my older code works as long as I don’t run Pkg.update() but after I update I get an error. The error seems to be related to the @autodiff stuff when I try to create a discretized model. (also happens when calling Problem() on a continuous model, but this is an mwe).

The following mwe is basically two steps:

create a new Julia environment and add RobotDynamics StaticArrays TrajectoryOptimization LinearAlgebra Test
Copy + Paste Steps 1 and 2 from http://roboticexplorationlab.org/TrajectoryOptimization.jl/v0.7/quickstart.html#quickstart_page

❯ julia6
               _
   _       _ _(_)_     |  Documentation: https://docs.julialang.org
  (_)     | (_) (_)    |
   _ _   _| |_  __ _   |  Type "?" for help, "]?" for Pkg help.
  | | | | | | |/ _` |  |
  | | |_| | | | (_| |  |  Version 1.6.5 (2021-12-19)
 _/ |\__'_|_|_|\__'_|  |  Official https://julialang.org/ release
|__/                   |

(@v1.6) pkg> activate .
  Activating new environment at `~/scratch/trajopt_quickstart_issue/Project.toml`

(trajopt_quickstart_issue) pkg> add RobotDynamics StaticArrays TrajectoryOptimization LinearAlgebra Test
    Updating registry at `~/.julia/registries/General`
   Resolving package versions...
    Updating `~/scratch/trajopt_quickstart_issue/Project.toml`
  [38ceca67] + RobotDynamics v0.4.6
  [90137ffa] + StaticArrays v1.5.2
  [c79d492b] + TrajectoryOptimization v0.7.0
  [37e2e46d] + LinearAlgebra
  [8dfed614] + Test
    Updating `~/scratch/trajopt_quickstart_issue/Manifest.toml`
  [30b0a656] + ArrayInterfaceCore v0.1.17
  [d360d2e6] + ChainRulesCore v1.15.3
  [9e997f8a] + ChangesOfVariables v0.1.4
  [bbf7d656] + CommonSubexpressions v0.3.0
  [34da2185] + Compat v4.1.0
  [187b0558] + ConstructionBase v1.4.0
  [163ba53b] + DiffResults v1.0.3
  [b552c78f] + DiffRules v1.11.0
  [ffbed154] + DocStringExtensions v0.8.6
  [6a86dc24] + FiniteDiff v2.14.0
  [f6369f11] + ForwardDiff v0.10.30
  [3587e190] + InverseFunctions v0.1.7
  [92d709cd] + IrrationalConstants v0.1.1
  [692b3bcd] + JLLWrappers v1.4.1
  [2ab3a3ac] + LogExpFunctions v0.3.17
  [1914dd2f] + MacroTools v0.5.9
  [77ba4419] + NaNMath v1.0.1
  [21216c6a] + Preferences v1.3.0
  [3cdcf5f2] + RecipesBase v1.2.1
  [ae029012] + Requires v1.3.0
  [38ceca67] + RobotDynamics v0.4.6
  [74be38bb] + RobotZoo v0.3.1
  [6038ab10] + Rotations v1.0.4
  [efcf1570] + Setfield v1.0.0
  [276daf66] + SpecialFunctions v2.1.7
  [90137ffa] + StaticArrays v1.5.2
  [1e83bf80] + StaticArraysCore v1.0.1
  [c79d492b] + TrajectoryOptimization v0.7.0
  [efe28fd5] + OpenSpecFun_jll v0.5.5+0
  [0dad84c5] + ArgTools
  [56f22d72] + Artifacts
  [2a0f44e3] + Base64
  [ade2ca70] + Dates
  [f43a241f] + Downloads
  [9fa8497b] + Future
  [b77e0a4c] + InteractiveUtils
  [b27032c2] + LibCURL
  [76f85450] + LibGit2
  [8f399da3] + Libdl
  [37e2e46d] + LinearAlgebra
  [56ddb016] + Logging
  [d6f4376e] + Markdown
  [ca575930] + NetworkOptions
  [44cfe95a] + Pkg
  [de0858da] + Printf
  [3fa0cd96] + REPL
  [9a3f8284] + Random
  [ea8e919c] + SHA
  [9e88b42a] + Serialization
  [6462fe0b] + Sockets
  [2f01184e] + SparseArrays
  [10745b16] + Statistics
  [4607b0f0] + SuiteSparse
  [fa267f1f] + TOML
  [a4e569a6] + Tar
  [8dfed614] + Test
  [cf7118a7] + UUIDs
  [4ec0a83e] + Unicode
  [e66e0078] + CompilerSupportLibraries_jll
  [deac9b47] + LibCURL_jll
  [29816b5a] + LibSSH2_jll
  [c8ffd9c3] + MbedTLS_jll
  [14a3606d] + MozillaCACerts_jll
  [05823500] + OpenLibm_jll
  [83775a58] + Zlib_jll
  [8e850ede] + nghttp2_jll
  [3f19e933] + p7zip_jll

julia> using TrajectoryOptimization

julia> using RobotDynamics

julia> using StaticArrays

julia> using LinearAlgebra

julia> using RobotDynamics

julia> using Test

julia> const TO = TrajectoryOptimization
TrajectoryOptimization

julia> const RD = RobotDynamics
RobotDynamics

julia> ## Model Definition
       struct DoubleIntegrator{T} <: RD.ContinuousDynamics 
           mass::T
       end

julia> function RD.dynamics(model::DoubleIntegrator, x, u)
           mass = model.mass
           vx,vy = x[3], x[4]
           ax,ay = u[1] / mass, u[2] / mass
           SA[vx, vy, ax, ay]
       end

julia> RD.state_dim(::DoubleIntegrator) = 4

julia> RD.control_dim(::DoubleIntegrator) = 2

julia> model = DoubleIntegrator(1.0)
DoubleIntegrator{Float64}(1.0)

julia> dmodel = RD.DiscretizedDynamics{RD.RK4}(model)
ERROR: MethodError: Cannot `convert` an object of type 
  FiniteDiff.JacobianCache{Vector{Float64}, Vector{Float64}, Vector{Float64}, Vector{Float64}, UnitRange{Int64}, Nothing, Val{:forward}(), Float64} to an object of type 
  FiniteDiff.JacobianCache{Vector{Float64}, Vector{Float64}, Vector{Float64}, UnitRange{Int64}, Nothing, Val{:forward}(), Float64, returntype} where returntype
Closest candidates are:
  convert(::Type{T}, ::T) where T at essentials.jl:205
Stacktrace:
 [1] RobotDynamics.DiscretizedDynamics(continuous_dynamics::DoubleIntegrator{Float64}, integrator::RobotDynamics.RK4)
   @ RobotDynamics ~/.julia/packages/RobotDynamics/xW4Xp/src/discretized_dynamics.jl:763
 [2] (RobotDynamics.DiscretizedDynamics{RobotDynamics.RK4, Q, JCH} where {Q, JCH})(continuous_dynamics::DoubleIntegrator{Float64})
   @ RobotDynamics ~/.julia/packages/RobotDynamics/xW4Xp/src/discretized_dynamics.jl:183
 [3] top-level scope
   @ REPL[16]:1

Maintenance of this package?

Hello and thanks for this useful package!

I am asking what the current plans for maintenance are going forward? The package is currently somewhat hard to install in large environments due to the outdated compat requirement

Defining LQR problem with quaternion

Hi everyone!

I'm using the quadrotor example as the baseline to implement some modifications.

I wanted to consider the state error lying in the tangent space of the quaternion. I've seen there is a cost function called QuatLQRCost. I've tried to use it but when added to the problem I get the following error:
MethodError: no method matching copy(::TrajectoryOptimization.QuadraticQuatCost{Float64,13,4,52})

Probably I'm doing something wrong since I'm not familiar with Julia. Is there any already available cost function that considers the error on the tangent space?

Any help will be much appreciated! Thank you very much!

Stack overflow for LinearConstraint

For the following code:LinearConstraint(2, 2, [0 1], [0.], Equality(), 3:4)

The error printout is:
ERROR: StackOverflowError:
Stacktrace:
[1] LinearConstraint(::Int64, ::Int64, ::StaticArrays.SizedArray{Tuple{1,4},Int64,2,2}, ::StaticArrays.SArray{Tuple{1},Float64,1,1}, ::Equality, ::UnitRange{Int64}) at C:\Users\riann.julia\packages\TrajectoryOptimization\47JPg\src\constraints.jl:117
[2] LinearConstraint(::Int64, ::Int64, ::StaticArrays.SizedArray{Tuple{1,4},Int64,2,2}, ::StaticArrays.SArray{Tuple{1},Float64,1,1}, ::Equality, ::UnitRange{Int64}) at C:\Users\riann.julia\packages\TrajectoryOptimization\47JPg\src\constraints.jl:123 (repeats 32642 times)
[3] top-level scope at none:1

Problem in runnning the example

Hi,

Thanks for sharing the nice package. However, I met with the error when I run the examples "Cartpole.ipynb" and "Quadrotor.ipynb":

The following package names could not be resolved:

QuadraticObjective (not found in project, manifest or registry)

I can not find QuadraticObject package in "Project.toml". Is the QuadraticObject a package which has not been published? Can you share it and update "Project.toml"?

Looking forward to your kind reply.

Best wishes,
Yujie

visualize! is ambiguous in example notebook

Running the cartpole example notebook, I get the following error when calling

visualize!(vis, model, TrajectoryOptimization.get_trajectory(altro))

ERROR: LoadError: MethodError: visualize!(::Visualizer, ::Cartpole{Float64}, ::Traj{4,1,Float64,RobotDynamics.GeneralKnotPoint{Float64,4,1,SArray{Tuple{5},Float64,1,5}}}) is ambiguous. Candidates:
  visualize!(vis, model::AbstractModel, Z::RobotDynamics.AbstractTrajectory) in TrajOptPlots at C:\Users\janbr\.julia\packages\TrajOptPlots\VirKW\src\rigid_bodies.jl:32
  visualize!(vis, model::Cartpole, x::AbstractArray{T,1} where T) in TrajOptPlots at C:\Users\janbr\.julia\packages\TrajOptPlots\VirKW\src\models.jl:43
  visualize!(vis, model::AbstractModel, Zs::RobotDynamics.AbstractTrajectory...) in TrajOptPlots at C:\Users\janbr\.julia\packages\TrajOptPlots\VirKW\src\rigid_bodies.jl:42
Possible fix, define
  visualize!(::Any, ::Cartpole, ::RobotDynamics.AbstractTrajectory)

Can't use AmplNLWriter as Jump backend

I am following this tutorial: Cartpole.ipybn
but instead of using Ipopt i am trying to use a solver only accessible via AmplNLWriter. Instead of Block [17] i have

using AmplNLWriter
using MathOptInterface

const MOI = MathOptInterface

optimizer = AmplNLWriter.Optimizer("octeract-engine")

TrajectoryOptimization.build_MOI!(nlp, optimizer)
MOI.optimize!(optimizer)
MOI.get(optimizer, MOI.TerminationStatus())

I get the following stack trace:

MethodError: no method matching constraint_expr(::TrajOptNLP{4,1,Float64}, ::Int64)
Closest candidates are:
constraint_expr(!Matched::MathOptInterface.Test.FeasibilitySenseEvaluator, ::Int64) at /home/joto/.julia/packages/MathOptInterface/bygN7/src/Test/nlp.jl:204
constraint_expr(!Matched::MathOptInterface.Test.HS071, ::Int64) at /home/joto/.julia/packages/MathOptInterface/bygN7/src/Test/nlp.jl:37
constr_expr(::AmplNLWriter.NLPEvaluator{TrajOptNLP{4,1,Float64}}, ::Int64) at MOI_wrapper.jl:163
(::AmplNLWriter.var"#85#86"{AmplNLWriter.AmplNLMathProgModel})(::Int64) at AmplNLWriter.jl:211
iterate at generator.jl:47 [inlined]
_collect(::UnitRange{Int64}, ::Base.Generator{UnitRange{Int64},AmplNLWriter.var"#85#86"{AmplNLWriter.AmplNLMathProgModel}}, ::Base.EltypeUnknown, ::Base.HasShape{1}) at array.jl:678
collect_similar at array.jl:607 [inlined]
map at abstractarray.jl:2072 [inlined]
loadproblem!(::AmplNLWriter.AmplNLNonlinearModel, ::Int64, ::Int64, ::Array{Float64,1}, ::Array{Float64,1}, ::Array{Float64,1}, ::Array{Float64,1}, ::Symbol, ::AmplNLWriter.NLPEvaluator{TrajOptNLP{4,1,Float64}}) at AmplNLWriter.jl:210
optimize!(::AmplNLWriter.Model{Float64}) at MOI_wrapper.jl:371
top-level scope at Cartpole.jl:62

The same problem persist if i try another solver:
and replace octeract-engine with any other string. I open a twin issue at AmplNLWriter too. Until it is clear which package can fix this.
jump-dev/AmplNLWriter.jl#107

Incorrect Dense Costs on v0.7

I have found that using dense (diagonal) matrices results in an incorrectly computed cost. It seems that the cost is computed without subtracting xf. See the code and output. The assert that fails is on the last line of the code. It shows that costs computed using static arrays and dense matrices should be the same, but are not.

using TrajectoryOptimization
using RobotDynamics
using StaticArrays
using LinearAlgebra

let
Nx = 2 # state dimension
Nu = 2 # control dimension
Nk = 100 # timesteps

Qx_dense = diagm(ones(Nx))
QxT_dense = 10*diagm(ones(Nx))

R_dense = 0.5*diagm(ones(Nu))

xf_dense = zeros(Nx)
xf_dense[1] = -1

@show xf_dense

obj_dense = LQRObjective(Qx_dense, R_dense, QxT_dense, xf_dense, Nk)
@show RobotDynamics.evaluate(obj_dense[end], xf_dense, zeros(Nu))
@show RobotDynamics.evaluate(obj_dense[end], zeros(Nx), zeros(Nu));


Qx_static = (@SVector ones(Nx))
QxT_static = 10*(@SVector ones(Nx))

R_static = 0.5*(@SVector ones(Nu))

xf_static = [ -1; @SVector zeros(Nx-1)]

@show xf_static

obj_static = LQRObjective(Qx_static, R_static, QxT_static, xf_static, Nk)
@show RobotDynamics.evaluate(obj_static[end], xf_static, zeros(Nu))
@show RobotDynamics.evaluate(obj_static[end], zeros(Nx), zeros(Nu));
    
# cost function elements are the same
@assert xf_dense == xf_static
@assert diag(Qx_dense) == Qx_static
@assert diag(QxT_dense) == QxT_static
@assert diag(R_dense) == R_static
    
# but the costs are not
@assert RobotDynamics.evaluate(obj_static[end], xf_static, zeros(Nu)) == RobotDynamics.evaluate(obj_dense[end], xf_dense, zeros(Nu))
end

Output:

julia> include("trajopt_costs_issue.jl")
xf_dense = [-1.0, 0.0]
RobotDynamics.evaluate(obj_dense[end], xf_dense, zeros(Nu)) = 9.0
RobotDynamics.evaluate(obj_dense[end], zeros(Nx), zeros(Nu)) = 5.0
xf_static = [-1.0, 0.0]
RobotDynamics.evaluate(obj_static[end], xf_static, zeros(Nu)) = 0.0
RobotDynamics.evaluate(obj_static[end], zeros(Nx), zeros(Nu)) = 5.0
ERROR: LoadError: AssertionError: RobotDynamics.evaluate(obj_static[end], xf_static, zeros(Nu)) == RobotDynamics.evaluate(obj_dense[end], xf_dense, zeros(Nu))

Better warm starts

Provide a better interface for warm-starting the solver with initial primal and dual variables

Update to v0.1.2

@JuliaRegistrator register()

Update version to v0.1.2.

Changes:

Add package version requirements
Remove build step
Minor bug fixes

New package version # 1

@JuliaRegistrator register()

Update TrajectoryOptimization.jl package version

model from urdf

How can I create a model from urdf file?

I found urdf files in this repository, but it seems examples for using urdf files are not implemented.

Compilation time with large StaticArrays

The v1.3 branch is basically unusable for problems with larger state dimensions (I was working on a problem with a 26 dimensional state vector). I finally killed my script after letting it the compiler go for over an hour on the solve!() function. This appears to be due to overhead from the mandatory use of StaticArrays. The StaticArrays.jl documentation advises against using StaticArrays with >100 elements, so 26X26 Jacobians and Hessians are likely causing problems.

Register v0.3

@JuliaRegistrator register
Register version 0.3

Mixed up comment

TrajectoryOptimization.jl/src/constraints.jl

Line 7 in 2ea839b

"Inequality constraints"

Minor: comment of inequality and equality mixed up

`build.log` in deps causes build issues

Would it be possible to remove the deps directory since it's not being used anymore?

Update documentation for TrajOpt

Example Bug

On the readme, the example fails to run because it can't locate the rk3 function.

Quat is not defined?

When i run the distributed.jl,it will fail:

WARNING: could not import TrajectoryOptimization.Discrete into Main
WARNING: WARNING: Rotations.Quat is deprecated, use UnitQuaternion instead.
likely near C:\Users\jkklz.julia\packages\TrajectoryOptimization.jl-distributed_team_lift\dynamics\quaternions.jl:15
From worker 7: WARNING: Rotations.Quat is deprecated, use UnitQuaternion instead.
From worker 7: likely near C:\Users\jkklz.julia\packages\TrajectoryOptimization.jl-distributed_team_lift\dynamics\quaternions.jl:15
From worker 6: WARNING: Rotations.Quat is deprecated, use UnitQuaternion instead.
From worker 6: likely near C:\Users\jkklz.julia\packages\TrajectoryOptimization.jl-distributed_team_lift\dynamics\quaternions.jl:15
From worker 5: WARNING: Rotations.Quat is deprecated, use UnitQuaternion instead.
From worker 5: likely near C:\Users\jkklz.julia\packages\TrajectoryOptimization.jl-distributed_team_lift\dynamics\quaternions.jl:15
ERROR: LoadError: On worker 5:
LoadError: LoadError: LoadError: UndefVarError: Quat not defined
getproperty at .\Base.jl:26
top-level scope at C:\Users\jkklz.julia\packages\TrajectoryOptimization.jl-distributed_team_lift\dynamics\quaternions.jl:17
include at .\client.jl:439
top-level scope at C:\Users\jkklz.julia\packages\TrajectoryOptimization.jl-distributed_team_lift\examples\ADMM_V2\models.jl:1
include at .\client.jl:439
top-level scope at C:\Users\jkklz.julia\packages\TrajectoryOptimization.jl-distributed_team_lift\examples\ADMM_V2\problem.jl:2
include at .\client.jl:439
top-level scope at none:0
eval at .\boot.jl:331
#101 at D:\buildbot\worker\package_win64\build\usr\share\julia\stdlib\v1.4\Distributed\src\process_messages.jl:290
run_work_thunk at D:\buildbot\worker\package_win64\build\usr\share\julia\stdlib\v1.4\Distributed\src\process_messages.jl:79
run_work_thunk at D:\buildbot\worker\package_win64\build\usr\share\julia\stdlib\v1.4\Distributed\src\process_messages.jl:88
#94 at .\task.jl:358
in expression starting at C:\Users\jkklz.julia\packages\TrajectoryOptimization.jl-distributed_team_lift\dynamics\quaternions.jl:17
in expression starting at C:\Users\jkklz.julia\packages\TrajectoryOptimization.jl-distributed_team_lift\examples\ADMM_V2\models.jl:1
in expression starting at C:\Users\jkklz.julia\packages\TrajectoryOptimization.jl-distributed_team_lift\examples\ADMM_V2\problem.jl:2

...and 3 more exception(s).

I want to konw
1.why quat is not defined
2. "Rotations.Quat is deprecated."is a problem of version?
3.the problem "could not import TrajectoryOptimization.Discrete into Main"
How can i solve the problem?
Be eager for some soutions!

Error when running "using TrajectoryOptimization"

When running

using TrajectoryOptimization

I got the following error:

julia> using TrajectoryOptimization
[ Info: Precompiling TrajectoryOptimization [c79d492b-0548-5874-b488-5a62c1d9d0ca]
ERROR: LoadError: LoadError: LoadError: MethodError: no method matching TrajectoryOptimization.Model(::typeof(TrajectoryOptimization.Dynamics.pendulum_dynamics!), ::Int32, ::Int32)
Closest candidates are:
  TrajectoryOptimization.Model(::Function, ::Int64, ::Int64) at /home/jingchi/.julia/packages/TrajectoryOptimization/Mvt9D/src/model.jl:42
  TrajectoryOptimization.Model(::Function, ::Int64, ::Int64, ::Dict{Symbol,Any}) at /home/jingchi/.julia/packages/TrajectoryOptimization/Mvt9D/src/model.jl:42
Stacktrace:
 [1] top-level scope at none:0
 [2] include at ./boot.jl:317 [inlined]
 [3] include_relative(::Module, ::String) at ./loading.jl:1044
 [4] include at ./sysimg.jl:29 [inlined]
 [5] include(::String) at /home/jingchi/.julia/packages/TrajectoryOptimization/Mvt9D/src/dynamics.jl:1
 [6] top-level scope at none:0
 [7] include at ./boot.jl:317 [inlined]
 [8] include_relative(::Module, ::String) at ./loading.jl:1044
 [9] include at ./sysimg.jl:29 [inlined]
 [10] include(::String) at /home/jingchi/.julia/packages/TrajectoryOptimization/Mvt9D/src/TrajectoryOptimization.jl:9
 [11] top-level scope at none:0
 [12] include at ./boot.jl:317 [inlined]
 [13] include_relative(::Module, ::String) at ./loading.jl:1044
 [14] include(::Module, ::String) at ./sysimg.jl:29
 [15] top-level scope at none:2
 [16] eval at ./boot.jl:319 [inlined]
 [17] eval(::Expr) at ./client.jl:393
 [18] top-level scope at ./none:3
in expression starting at /home/jingchi/.julia/packages/TrajectoryOptimization/Mvt9D/dynamics/pendulum.jl:14
in expression starting at /home/jingchi/.julia/packages/TrajectoryOptimization/Mvt9D/src/dynamics.jl:14
in expression starting at /home/jingchi/.julia/packages/TrajectoryOptimization/Mvt9D/src/TrajectoryOptimization.jl:96
ERROR: Failed to precompile TrajectoryOptimization [c79d492b-0548-5874-b488-5a62c1d9d0ca] to /home/jingchi/.julia/compiled/v1.0/TrajectoryOptimization/UVgeA.ji.
Stacktrace:
 [1] error(::String) at ./error.jl:33
 [2] compilecache(::Base.PkgId, ::String) at ./loading.jl:1203
 [3] _require(::Base.PkgId) at ./loading.jl:960
 [4] require(::Base.PkgId) at ./loading.jl:858
 [5] require(::Module, ::Symbol) at ./loading.jl:853

Error when running the quickstart example

I tried to run the quickstart.jl example. However, I ran into the problem that no method matching QuadraticCost(::Int64, ::Int64, ::Int64)

ERROR: LoadError: MethodError: no method matching QuadraticCost(::Int64, ::Int64, ::Int64)
Closest candidates are:
QuadraticCost(::TQ, ::TR, ::TH, ::Tq, ::Tr, ::Real; checks, terminal, kwargs...) where {TQ, TR, TH, Tq, Tr, QC<:QuadraticCost} at /home/san/.julia/packages/TrajectoryOptimization/MxdKM/src/costfunctions.jl:330
Stacktrace:
[1] Altro.iLQRSolver(::Problem{RK3,Float64}, ::SolverOptions{Float64}, ::SolverStats{Float64}; kwarg_opts::Base.Iterators.Pairs{Union{},Union{},Tuple{},NamedTuple{(),Tuple{}}}) at /home/san/.julia/packages/Altro/ZbP1c/src/ilqr/ilqr.jl:71
[2] Altro.iLQRSolver(::Problem{RK3,Float64}, ::SolverOptions{Float64}, ::SolverStats{Float64}) at /home/san/.julia/packages/Altro/ZbP1c/src/ilqr/ilqr.jl:52
[3] Altro.AugmentedLagrangianSolver(::Problem{RK3,Float64}, ::SolverOptions{Float64}, ::SolverStats{Float64}; solver_uncon::Type{T} where T, kwarg_opts::Base.Iterators.Pairs{Union{},Union{},Tuple{},NamedTuple{(),Tuple{}}}) at /home/san/.julia/packages/Altro/ZbP1c/src/augmented_lagrangian/al_solver.jl:52
[4] ALTROSolver(::Problem{RK3,Float64}, ::SolverOptions{Float64}; infeasible::Bool, R_inf::Float64, solver_uncon::Type{T} where T, kwarg_opts::Base.Iterators.Pairs{Union{},Union{},Tuple{},NamedTuple{(),Tuple{}}}) at /home/san/.julia/packages/Altro/ZbP1c/src/altro/altro_solver.jl:35
[5] ALTROSolver(::Problem{RK3,Float64}, ::SolverOptions{Float64}) at /home/san/.julia/packages/Altro/ZbP1c/src/altro/altro_solver.jl:23 (repeats 2 times)
[6] top-level scope at /home/san/Documents/QC-Code/TrajectoryOptimization.jl/examples/quickstart.jl:41
[7] include(::String) at ./client.jl:457
[8] top-level scope at none:1
in expression starting at /home/san/Documents/QC-Code/TrajectoryOptimization.jl/examples/quickstart.jl:41

Add support for minimum time problems

Looks like current approach requires final time to be provided which is not always possible.

roboticexplorationlab / trajectoryoptimization.jl Goto Github PK

trajectoryoptimization.jl's Introduction

TrajectoryOptimization

Installation

What's New

Quick Start

Examples

Related Papers

trajectoryoptimization.jl's People

Contributors

Stargazers

Watchers

Forkers

trajectoryoptimization.jl's Issues

Examples

Problem

Snippet to reproduce this issue

Problem

Example

Recommend Projects

Recommend Topics

Recommend Org