├── .gitignore ├── MC_data.jld2 ├── Manifest.toml ├── Project.toml ├── WAFR_submission.pdf ├── barrell_roll.jld2 ├── cost_comparison.jld2 ├── examples ├── Flexible_Spacecraft │ ├── Manifest.toml │ ├── Project.toml │ ├── solve_con.jl │ └── solve_uncon.jl ├── Manifest.toml ├── Project.toml ├── Wahbas_problem │ ├── Manifest.toml │ ├── NLLS_Wahba.jl │ ├── Project.toml │ ├── convergence_plot.tex │ └── wahba_results.jld2 ├── quadflip_mc.jl ├── timing_results.jld2 └── trajopt_examples.jl ├── figs ├── quadflip.gif ├── quadflip.mp4 ├── quadflip_modified.mp4 ├── quadflip_naive.gif └── quadflip_naive.mp4 ├── flip_montecarlo.jdl2 ├── mlqr_basin_2000.jld2 ├── mlqr_monte_carlo_2000.jld2 ├── paper ├── .gitignore ├── IEEEtran.cls ├── figures │ ├── barrellroll.png │ ├── c_max_convergence.tikz │ ├── kevins_plots │ │ └── attitude_slew_plot.tikz │ ├── quadflip.png │ ├── tangent_plane.tikz │ ├── timing_results.tex │ └── wahba_convergence.tikz ├── iros_abstract │ ├── ieeeconf.cls │ ├── iros_abstract.pdf │ ├── iros_abstract.tex │ └── quadflip.png ├── main.pdf ├── main.tex ├── old_figs │ ├── barrellroll.png │ ├── barrellroll_timing.tikz │ ├── c_max_convergence.tikz │ ├── cost_comparison.tex │ ├── kevins_plots │ │ ├── attitude_slew_plot.tikz │ │ └── convergence_plot.tikz │ ├── quadflip.png │ ├── sat_iterations.tikz │ ├── sat_success_rate.tikz │ ├── sat_success_rate_byangle.tikz │ ├── sat_time_per_iter.tikz │ ├── tangent_plane.tikz │ ├── test.tex │ ├── test.tikz │ ├── timing_results.tex │ └── wahba_convergence.tikz ├── references.bib └── submission.zip ├── src ├── PlanningWithAttitude.jl ├── airplane_problem.jl ├── flexible_spacecraft_dynamics.jl ├── quadflip_problem.jl ├── quat_cons.jl ├── quat_costs.jl ├── quat_norm.jl └── vecmodel.jl └── test └── quatslack.jl /.gitignore: -------------------------------------------------------------------------------- 1 | 2 | .DS_Store 3 | 4 | paper/.texpadtmp/ 5 | -------------------------------------------------------------------------------- /MC_data.jld2: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/RoboticExplorationLab/PlanningWithAttitude/14ec74ef9408946a758c87f5f15e48451b5e5d84/MC_data.jld2 -------------------------------------------------------------------------------- /Manifest.toml: -------------------------------------------------------------------------------- 1 | # This file is machine-generated - editing it directly is not advised 2 | 3 | [[Base64]] 4 | uuid = "2a0f44e3-6c83-55bd-87e4-b1978d98bd5f" 5 | 6 | [[CommonSubexpressions]] 7 | deps = ["Test"] 8 | git-tree-sha1 = "34aa50efad19a788db0cb2eb44d149942f64816a" 9 | uuid = "bbf7d656-a473-5ed7-a52c-81e309532950" 10 | version = "0.2.1" 11 | 12 | [[CompilerSupportLibraries_jll]] 13 | deps = ["Libdl", "Pkg"] 14 | git-tree-sha1 = "7c4f882c41faa72118841185afc58a2eb00ef612" 15 | uuid = "e66e0078-7015-5450-92f7-15fbd957f2ae" 16 | version = "0.3.3+0" 17 | 18 | [[Dates]] 19 | deps = ["Printf"] 20 | uuid = "ade2ca70-3891-5945-98fb-dc099432e06a" 21 | 22 | [[DiffResults]] 23 | deps = ["StaticArrays"] 24 | git-tree-sha1 = "da24935df8e0c6cf28de340b958f6aac88eaa0cc" 25 | uuid = "163ba53b-c6d8-5494-b064-1a9d43ac40c5" 26 | version = "1.0.2" 27 | 28 | [[DiffRules]] 29 | deps = ["NaNMath", "Random", "SpecialFunctions"] 30 | git-tree-sha1 = "eb0c34204c8410888844ada5359ac8b96292cfd1" 31 | uuid = "b552c78f-8df3-52c6-915a-8e097449b14b" 32 | version = "1.0.1" 33 | 34 | [[Distributed]] 35 | deps = ["Random", "Serialization", "Sockets"] 36 | uuid = "8ba89e20-285c-5b6f-9357-94700520ee1b" 37 | 38 | [[Formatting]] 39 | deps = ["Printf"] 40 | git-tree-sha1 = "a0c901c29c0e7c763342751c0a94211d56c0de5c" 41 | uuid = "59287772-0a20-5a39-b81b-1366585eb4c0" 42 | version = "0.4.1" 43 | 44 | [[ForwardDiff]] 45 | deps = ["CommonSubexpressions", "DiffResults", "DiffRules", "NaNMath", "Random", "SpecialFunctions", "StaticArrays"] 46 | git-tree-sha1 = "88b082d492be6b63f967b6c96b352e25ced1a34c" 47 | uuid = "f6369f11-7733-5829-9624-2563aa707210" 48 | version = "0.10.9" 49 | 50 | [[InteractiveUtils]] 51 | deps = ["Markdown"] 52 | uuid = "b77e0a4c-d291-57a0-90e8-8db25a27a240" 53 | 54 | [[LibGit2]] 55 | deps = ["Printf"] 56 | uuid = "76f85450-5226-5b5a-8eaa-529ad045b433" 57 | 58 | [[Libdl]] 59 | uuid = "8f399da3-3557-5675-b5ff-fb832c97cbdb" 60 | 61 | [[LinearAlgebra]] 62 | deps = ["Libdl"] 63 | uuid = "37e2e46d-f89d-539d-b4ee-838fcccc9c8e" 64 | 65 | [[Logging]] 66 | uuid = "56ddb016-857b-54e1-b83d-db4d58db5568" 67 | 68 | [[Markdown]] 69 | deps = ["Base64"] 70 | uuid = "d6f4376e-aef5-505a-96c1-9c027394607a" 71 | 72 | [[NaNMath]] 73 | git-tree-sha1 = "c84c576296d0e2fbb3fc134d3e09086b3ea617cd" 74 | uuid = "77ba4419-2d1f-58cd-9bb1-8ffee604a2e3" 75 | version = "0.3.4" 76 | 77 | [[OpenSpecFun_jll]] 78 | deps = ["CompilerSupportLibraries_jll", "Libdl", "Pkg"] 79 | git-tree-sha1 = "d51c416559217d974a1113522d5919235ae67a87" 80 | uuid = "efe28fd5-8261-553b-a9e1-b2916fc3738e" 81 | version = "0.5.3+3" 82 | 83 | [[OrderedCollections]] 84 | git-tree-sha1 = "16c08bf5dba06609fe45e30860092d6fa41fde7b" 85 | uuid = "bac558e1-5e72-5ebc-8fee-abe8a469f55d" 86 | version = "1.3.1" 87 | 88 | [[Parameters]] 89 | deps = ["OrderedCollections", "UnPack"] 90 | git-tree-sha1 = "38b2e970043613c187bd56a995fe2e551821eb4a" 91 | uuid = "d96e819e-fc66-5662-9728-84c9c7592b0a" 92 | version = "0.12.1" 93 | 94 | [[Pkg]] 95 | deps = ["Dates", "LibGit2", "Libdl", "Logging", "Markdown", "Printf", "REPL", "Random", "SHA", "UUIDs"] 96 | uuid = "44cfe95a-1eb2-52ea-b672-e2afdf69b78f" 97 | 98 | [[Printf]] 99 | deps = ["Unicode"] 100 | uuid = "de0858da-6303-5e67-8744-51eddeeeb8d7" 101 | 102 | [[REPL]] 103 | deps = ["InteractiveUtils", "Markdown", "Sockets"] 104 | uuid = "3fa0cd96-eef1-5676-8a61-b3b8758bbffb" 105 | 106 | [[Random]] 107 | deps = ["Serialization"] 108 | uuid = "9a3f8284-a2c9-5f02-9a11-845980a1fd5c" 109 | 110 | [[RecipesBase]] 111 | git-tree-sha1 = "6ee6c35fe69e79e17c455a386c1ccdc66d9f7da4" 112 | uuid = "3cdcf5f2-1ef4-517c-9805-6587b60abb01" 113 | version = "1.1.0" 114 | 115 | [[RobotDynamics]] 116 | deps = ["ForwardDiff", "LinearAlgebra", "RecipesBase", "Rotations", "StaticArrays", "UnsafeArrays"] 117 | git-tree-sha1 = "5cfad58e7bd304c9ee93f0b7ca92921fad49b258" 118 | uuid = "38ceca67-d8d3-44e8-9852-78a5596522e1" 119 | version = "0.2.2" 120 | 121 | [[RobotZoo]] 122 | deps = ["LinearAlgebra", "Parameters", "RobotDynamics", "Rotations", "StaticArrays"] 123 | git-tree-sha1 = "7c918fa028d9e3056e76dbe214db2ed877e013e0" 124 | uuid = "74be38bb-dcc2-4b9e-baf3-d6373cd95f10" 125 | version = "0.1.1" 126 | 127 | [[Rotations]] 128 | deps = ["LinearAlgebra", "StaticArrays", "Statistics"] 129 | git-tree-sha1 = "445b72242dbdecba9bfc42034daafdd901bbf6a9" 130 | uuid = "6038ab10-8711-5258-84ad-4b1120ba62dc" 131 | version = "1.0.1" 132 | 133 | [[SHA]] 134 | uuid = "ea8e919c-243c-51af-8825-aaa63cd721ce" 135 | 136 | [[Serialization]] 137 | uuid = "9e88b42a-f829-5b0c-bbe9-9e923198166b" 138 | 139 | [[Sockets]] 140 | uuid = "6462fe0b-24de-5631-8697-dd941f90decc" 141 | 142 | [[SparseArrays]] 143 | deps = ["LinearAlgebra", "Random"] 144 | uuid = "2f01184e-e22b-5df5-ae63-d93ebab69eaf" 145 | 146 | [[SpecialFunctions]] 147 | deps = ["OpenSpecFun_jll"] 148 | git-tree-sha1 = "d8d8b8a9f4119829410ecd706da4cc8594a1e020" 149 | uuid = "276daf66-3868-5448-9aa4-cd146d93841b" 150 | version = "0.10.3" 151 | 152 | [[StaticArrays]] 153 | deps = ["LinearAlgebra", "Random", "Statistics"] 154 | git-tree-sha1 = "5a3bcb6233adabde68ebc97be66e95dcb787424c" 155 | uuid = "90137ffa-7385-5640-81b9-e52037218182" 156 | version = "0.12.1" 157 | 158 | [[Statistics]] 159 | deps = ["LinearAlgebra", "SparseArrays"] 160 | uuid = "10745b16-79ce-11e8-11f9-7d13ad32a3b2" 161 | 162 | [[Test]] 163 | deps = ["Distributed", "InteractiveUtils", "Logging", "Random"] 164 | uuid = "8dfed614-e22c-5e08-85e1-65c5234f0b40" 165 | 166 | [[UUIDs]] 167 | deps = ["Random", "SHA"] 168 | uuid = "cf7118a7-6976-5b1a-9a39-7adc72f591a4" 169 | 170 | [[UnPack]] 171 | git-tree-sha1 = "387c1f73762231e86e0c9c5443ce3b4a0a9a0c2b" 172 | uuid = "3a884ed6-31ef-47d7-9d2a-63182c4928ed" 173 | version = "1.0.2" 174 | 175 | [[Unicode]] 176 | uuid = "4ec0a83e-493e-50e2-b9ac-8f72acf5a8f5" 177 | 178 | [[UnsafeArrays]] 179 | git-tree-sha1 = "9740b414f85ec2fa9135066f81b1fb14212befd6" 180 | uuid = "c4a57d5a-5b31-53a6-b365-19f8c011fbd6" 181 | version = "1.0.1" 182 | -------------------------------------------------------------------------------- /Project.toml: -------------------------------------------------------------------------------- 1 | name = "PlanningWithAttitude" 2 | uuid = "35fb9e8a-3ec1-11ea-3514-d75c41db21f5" 3 | version = "0.0.1" 4 | 5 | [deps] 6 | Formatting = "59287772-0a20-5a39-b81b-1366585eb4c0" 7 | ForwardDiff = "f6369f11-7733-5829-9624-2563aa707210" 8 | LinearAlgebra = "37e2e46d-f89d-539d-b4ee-838fcccc9c8e" 9 | Logging = "56ddb016-857b-54e1-b83d-db4d58db5568" 10 | RobotZoo = "74be38bb-dcc2-4b9e-baf3-d6373cd95f10" 11 | Rotations = "6038ab10-8711-5258-84ad-4b1120ba62dc" 12 | StaticArrays = "90137ffa-7385-5640-81b9-e52037218182" 13 | Statistics = "10745b16-79ce-11e8-11f9-7d13ad32a3b2" 14 | 15 | [compat] 16 | Rotations = "1" 17 | -------------------------------------------------------------------------------- /WAFR_submission.pdf: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/RoboticExplorationLab/PlanningWithAttitude/14ec74ef9408946a758c87f5f15e48451b5e5d84/WAFR_submission.pdf -------------------------------------------------------------------------------- /barrell_roll.jld2: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/RoboticExplorationLab/PlanningWithAttitude/14ec74ef9408946a758c87f5f15e48451b5e5d84/barrell_roll.jld2 -------------------------------------------------------------------------------- /cost_comparison.jld2: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/RoboticExplorationLab/PlanningWithAttitude/14ec74ef9408946a758c87f5f15e48451b5e5d84/cost_comparison.jld2 -------------------------------------------------------------------------------- /examples/Flexible_Spacecraft/Manifest.toml: -------------------------------------------------------------------------------- 1 | # This file is machine-generated - editing it directly is not advised 2 | 3 | [[Adapt]] 4 | deps = ["LinearAlgebra"] 5 | git-tree-sha1 = "42c42f2221906892ceb765dbcb1a51deeffd86d7" 6 | uuid = "79e6a3ab-5dfb-504d-930d-738a2a938a0e" 7 | version = "2.3.0" 8 | 9 | [[Altro]] 10 | deps = ["BenchmarkTools", "Crayons", "DocStringExtensions", "Interpolations", "LinearAlgebra", "Logging", "Parameters", "RobotDynamics", "RobotZoo", "Rotations", "SolverLogging", "SparseArrays", "StaticArrays", "Statistics", "TrajectoryOptimization", "UnsafeArrays"] 11 | git-tree-sha1 = "f3405dad88b12c46b763206208636e64e82f6cc2" 12 | repo-rev = "socp" 13 | repo-url = "https://github.com/RoboticExplorationLab/Altro.jl.git" 14 | uuid = "5dcf52e5-e2fb-48e0-b826-96f46d2e3e73" 15 | version = "0.2.0" 16 | 17 | [[AxisAlgorithms]] 18 | deps = ["LinearAlgebra", "Random", "SparseArrays", "WoodburyMatrices"] 19 | git-tree-sha1 = "a4d07a1c313392a77042855df46c5f534076fab9" 20 | uuid = "13072b0f-2c55-5437-9ae7-d433b7a33950" 21 | version = "1.0.0" 22 | 23 | [[Base64]] 24 | uuid = "2a0f44e3-6c83-55bd-87e4-b1978d98bd5f" 25 | 26 | [[BenchmarkTools]] 27 | deps = ["JSON", "Logging", "Printf", "Statistics", "UUIDs"] 28 | git-tree-sha1 = "9e62e66db34540a0c919d72172cc2f642ac71260" 29 | uuid = "6e4b80f9-dd63-53aa-95a3-0cdb28fa8baf" 30 | version = "0.5.0" 31 | 32 | [[Blosc]] 33 | deps = ["Blosc_jll"] 34 | git-tree-sha1 = "84cf7d0f8fd46ca6f1b3e0305b4b4a37afe50fd6" 35 | uuid = "a74b3585-a348-5f62-a45c-50e91977d574" 36 | version = "0.7.0" 37 | 38 | [[Blosc_jll]] 39 | deps = ["Libdl", "Lz4_jll", "Pkg", "Zlib_jll", "Zstd_jll"] 40 | git-tree-sha1 = "aa9ef39b54a168c3df1b2911e7797e4feee50fbe" 41 | uuid = "0b7ba130-8d10-5ba8-a3d6-c5182647fed9" 42 | version = "1.14.3+1" 43 | 44 | [[BufferedStreams]] 45 | deps = ["Compat", "Test"] 46 | git-tree-sha1 = "5d55b9486590fdda5905c275bb21ce1f0754020f" 47 | uuid = "e1450e63-4bb3-523b-b2a4-4ffa8c0fd77d" 48 | version = "1.0.0" 49 | 50 | [[Bzip2_jll]] 51 | deps = ["Libdl", "Pkg"] 52 | git-tree-sha1 = "03a44490020826950c68005cafb336e5ba08b7e8" 53 | uuid = "6e34b625-4abd-537c-b88f-471c36dfa7a0" 54 | version = "1.0.6+4" 55 | 56 | [[CodecBzip2]] 57 | deps = ["Bzip2_jll", "Libdl", "TranscodingStreams"] 58 | git-tree-sha1 = "2e62a725210ce3c3c2e1a3080190e7ca491f18d7" 59 | uuid = "523fee87-0ab8-5b00-afb7-3ecf72e48cfd" 60 | version = "0.7.2" 61 | 62 | [[CodecZlib]] 63 | deps = ["TranscodingStreams", "Zlib_jll"] 64 | git-tree-sha1 = "ded953804d019afa9a3f98981d99b33e3db7b6da" 65 | uuid = "944b1d66-785c-5afd-91f1-9de20f533193" 66 | version = "0.7.0" 67 | 68 | [[ColorSchemes]] 69 | deps = ["ColorTypes", "Colors", "FixedPointNumbers", "Random", "StaticArrays"] 70 | git-tree-sha1 = "5d472aa8908568bc198564db06983913a6c2c8e7" 71 | uuid = "35d6a980-a343-548e-a6ea-1d62b119f2f4" 72 | version = "3.10.1" 73 | 74 | [[ColorTypes]] 75 | deps = ["FixedPointNumbers", "Random"] 76 | git-tree-sha1 = "4bffea7ed1a9f0f3d1a131bbcd4b925548d75288" 77 | uuid = "3da002f7-5984-5a60-b8a6-cbb66c0b333f" 78 | version = "0.10.9" 79 | 80 | [[Colors]] 81 | deps = ["ColorTypes", "FixedPointNumbers", "InteractiveUtils", "Reexport"] 82 | git-tree-sha1 = "008d6bc68dea6beb6303fdc37188cb557391ebf2" 83 | uuid = "5ae59095-9a9b-59fe-a467-6f913c188581" 84 | version = "0.12.4" 85 | 86 | [[CommonSubexpressions]] 87 | deps = ["MacroTools", "Test"] 88 | git-tree-sha1 = "7b8a93dba8af7e3b42fecabf646260105ac373f7" 89 | uuid = "bbf7d656-a473-5ed7-a52c-81e309532950" 90 | version = "0.3.0" 91 | 92 | [[Compat]] 93 | deps = ["Base64", "Dates", "DelimitedFiles", "Distributed", "InteractiveUtils", "LibGit2", "Libdl", "LinearAlgebra", "Markdown", "Mmap", "Pkg", "Printf", "REPL", "Random", "SHA", "Serialization", "SharedArrays", "Sockets", "SparseArrays", "Statistics", "Test", "UUIDs", "Unicode"] 94 | git-tree-sha1 = "cf03b37436c6bc162e7c8943001568b4cad4bee3" 95 | uuid = "34da2185-b29b-5c13-b0c7-acf172513d20" 96 | version = "3.19.0" 97 | 98 | [[CompilerSupportLibraries_jll]] 99 | deps = ["Libdl", "Pkg"] 100 | git-tree-sha1 = "7c4f882c41faa72118841185afc58a2eb00ef612" 101 | uuid = "e66e0078-7015-5450-92f7-15fbd957f2ae" 102 | version = "0.3.3+0" 103 | 104 | [[Contour]] 105 | deps = ["StaticArrays"] 106 | git-tree-sha1 = "d05a3a25b762720d40246d5bedf518c9c2614ef5" 107 | uuid = "d38c429a-6771-53c6-b99e-75d170b6e991" 108 | version = "0.5.5" 109 | 110 | [[Crayons]] 111 | git-tree-sha1 = "3f71217b538d7aaee0b69ab47d9b7724ca8afa0d" 112 | uuid = "a8cc5b0e-0ffa-5ad4-8c14-923d3ee1735f" 113 | version = "4.0.4" 114 | 115 | [[DataAPI]] 116 | git-tree-sha1 = "176e23402d80e7743fc26c19c681bfb11246af32" 117 | uuid = "9a962f9c-6df0-11e9-0e5d-c546b8b5ee8a" 118 | version = "1.3.0" 119 | 120 | [[DataStructures]] 121 | deps = ["Compat", "InteractiveUtils", "OrderedCollections"] 122 | git-tree-sha1 = "db07bb22795762895b60e44d62b34b16c982a687" 123 | uuid = "864edb3b-99cc-5e75-8d2d-829cb0a9cfe8" 124 | version = "0.18.7" 125 | 126 | [[DataValueInterfaces]] 127 | git-tree-sha1 = "bfc1187b79289637fa0ef6d4436ebdfe6905cbd6" 128 | uuid = "e2d170a0-9d28-54be-80f0-106bbe20a464" 129 | version = "1.0.0" 130 | 131 | [[Dates]] 132 | deps = ["Printf"] 133 | uuid = "ade2ca70-3891-5945-98fb-dc099432e06a" 134 | 135 | [[DelimitedFiles]] 136 | deps = ["Mmap"] 137 | uuid = "8bb1440f-4735-579b-a4ab-409b98df4dab" 138 | 139 | [[DiffResults]] 140 | deps = ["StaticArrays"] 141 | git-tree-sha1 = "da24935df8e0c6cf28de340b958f6aac88eaa0cc" 142 | uuid = "163ba53b-c6d8-5494-b064-1a9d43ac40c5" 143 | version = "1.0.2" 144 | 145 | [[DiffRules]] 146 | deps = ["NaNMath", "Random", "SpecialFunctions"] 147 | git-tree-sha1 = "eb0c34204c8410888844ada5359ac8b96292cfd1" 148 | uuid = "b552c78f-8df3-52c6-915a-8e097449b14b" 149 | version = "1.0.1" 150 | 151 | [[Distributed]] 152 | deps = ["Random", "Serialization", "Sockets"] 153 | uuid = "8ba89e20-285c-5b6f-9357-94700520ee1b" 154 | 155 | [[DocStringExtensions]] 156 | deps = ["LibGit2", "Markdown", "Pkg", "Test"] 157 | git-tree-sha1 = "50ddf44c53698f5e784bbebb3f4b21c5807401b1" 158 | uuid = "ffbed154-4ef7-542d-bbb7-c09d3a79fcae" 159 | version = "0.8.3" 160 | 161 | [[EarCut_jll]] 162 | deps = ["Libdl", "Pkg"] 163 | git-tree-sha1 = "eabac56550a7d7e0be499125673fbff560eb8b20" 164 | uuid = "5ae413db-bbd1-5e63-b57d-d24a61df00f5" 165 | version = "2.1.5+0" 166 | 167 | [[FFMPEG]] 168 | deps = ["FFMPEG_jll", "x264_jll"] 169 | git-tree-sha1 = "9a73ffdc375be61b0e4516d83d880b265366fe1f" 170 | uuid = "c87230d0-a227-11e9-1b43-d7ebe4e7570a" 171 | version = "0.4.0" 172 | 173 | [[FFMPEG_jll]] 174 | deps = ["Bzip2_jll", "FreeType2_jll", "FriBidi_jll", "LAME_jll", "LibVPX_jll", "Libdl", "Ogg_jll", "OpenSSL_jll", "Opus_jll", "Pkg", "Zlib_jll", "libass_jll", "libfdk_aac_jll", "libvorbis_jll", "x264_jll", "x265_jll"] 175 | git-tree-sha1 = "13a934b9e74a8722bf1786c989de346a9602e695" 176 | uuid = "b22a6f82-2f65-5046-a5b2-351ab43fb4e5" 177 | version = "4.3.1+2" 178 | 179 | [[FixedPointNumbers]] 180 | deps = ["Statistics"] 181 | git-tree-sha1 = "335bfdceacc84c5cdf16aadc768aa5ddfc5383cc" 182 | uuid = "53c48c17-4a7d-5ca2-90c5-79b7896eea93" 183 | version = "0.8.4" 184 | 185 | [[Formatting]] 186 | deps = ["Printf"] 187 | git-tree-sha1 = "a0c901c29c0e7c763342751c0a94211d56c0de5c" 188 | uuid = "59287772-0a20-5a39-b81b-1366585eb4c0" 189 | version = "0.4.1" 190 | 191 | [[ForwardDiff]] 192 | deps = ["CommonSubexpressions", "DiffResults", "DiffRules", "NaNMath", "Random", "SpecialFunctions", "StaticArrays"] 193 | git-tree-sha1 = "1d090099fb82223abc48f7ce176d3f7696ede36d" 194 | uuid = "f6369f11-7733-5829-9624-2563aa707210" 195 | version = "0.10.12" 196 | 197 | [[FreeType2_jll]] 198 | deps = ["Bzip2_jll", "Libdl", "Pkg", "Zlib_jll"] 199 | git-tree-sha1 = "720eee04e3b496c15e5e2269669c2532fb5005c0" 200 | uuid = "d7e528f0-a631-5988-bf34-fe36492bcfd7" 201 | version = "2.10.1+4" 202 | 203 | [[FriBidi_jll]] 204 | deps = ["Libdl", "Pkg"] 205 | git-tree-sha1 = "cfc3485a0a968263c789e314fca5d66daf75ed6c" 206 | uuid = "559328eb-81f9-559d-9380-de523a88c83c" 207 | version = "1.0.5+5" 208 | 209 | [[GR]] 210 | deps = ["Base64", "DelimitedFiles", "HTTP", "JSON", "LinearAlgebra", "Printf", "Random", "Serialization", "Sockets", "Test", "UUIDs"] 211 | git-tree-sha1 = "cd0f34bd097d4d5eb6bbe01778cf8a7ed35f29d9" 212 | uuid = "28b8d3ca-fb5f-59d9-8090-bfdbd6d07a71" 213 | version = "0.52.0" 214 | 215 | [[GeometryBasics]] 216 | deps = ["EarCut_jll", "IterTools", "LinearAlgebra", "StaticArrays", "StructArrays", "Tables"] 217 | git-tree-sha1 = "876a906eab3be990fdcbfe1e43bb3a76f4776f72" 218 | uuid = "5c1252a2-5f33-56bf-86c9-59e7332b4326" 219 | version = "0.3.3" 220 | 221 | [[GeometryTypes]] 222 | deps = ["ColorTypes", "FixedPointNumbers", "LinearAlgebra", "StaticArrays"] 223 | git-tree-sha1 = "34bfa994967e893ab2f17b864eec221b3521ba4d" 224 | uuid = "4d00f742-c7ba-57c2-abde-4428a4b178cb" 225 | version = "0.8.3" 226 | 227 | [[Grisu]] 228 | git-tree-sha1 = "03d381f65183cb2d0af8b3425fde97263ce9a995" 229 | uuid = "42e2da0e-8278-4e71-bc24-59509adca0fe" 230 | version = "1.0.0" 231 | 232 | [[HDF5]] 233 | deps = ["Blosc", "HDF5_jll", "Libdl", "Mmap", "Random"] 234 | git-tree-sha1 = "0713cbabdf855852dfab3ce6447c87145f3d9ea8" 235 | uuid = "f67ccb44-e63f-5c2f-98bd-6dc0ccc4ba2f" 236 | version = "0.13.6" 237 | 238 | [[HDF5_jll]] 239 | deps = ["Libdl", "Pkg", "Zlib_jll"] 240 | git-tree-sha1 = "3dbc683172cb53428907485a4bb98a29d3874083" 241 | uuid = "0234f1f7-429e-5d53-9886-15a909be8d59" 242 | version = "1.10.5+6" 243 | 244 | [[HTTP]] 245 | deps = ["Base64", "Dates", "IniFile", "MbedTLS", "Sockets"] 246 | git-tree-sha1 = "c7ec02c4c6a039a98a15f955462cd7aea5df4508" 247 | uuid = "cd3eb016-35fb-5094-929b-558a96fad6f3" 248 | version = "0.8.19" 249 | 250 | [[Infiltrator]] 251 | deps = ["REPL"] 252 | git-tree-sha1 = "2be5c3e8adddf062c3903a6d7618f233fa4d2874" 253 | uuid = "5903a43b-9cc3-4c30-8d17-598619ec4e9b" 254 | version = "0.3.0" 255 | 256 | [[IniFile]] 257 | deps = ["Test"] 258 | git-tree-sha1 = "098e4d2c533924c921f9f9847274f2ad89e018b8" 259 | uuid = "83e8ac13-25f8-5344-8a64-a9f2b223428f" 260 | version = "0.5.0" 261 | 262 | [[InteractiveUtils]] 263 | deps = ["Markdown"] 264 | uuid = "b77e0a4c-d291-57a0-90e8-8db25a27a240" 265 | 266 | [[Interpolations]] 267 | deps = ["AxisAlgorithms", "LinearAlgebra", "OffsetArrays", "Random", "Ratios", "SharedArrays", "SparseArrays", "StaticArrays", "WoodburyMatrices"] 268 | git-tree-sha1 = "2b7d4e9be8b74f03115e64cf36ed2f48ae83d946" 269 | uuid = "a98d9a8b-a2ab-59e6-89dd-64a1c18fca59" 270 | version = "0.12.10" 271 | 272 | [[IterTools]] 273 | git-tree-sha1 = "05110a2ab1fc5f932622ffea2a003221f4782c18" 274 | uuid = "c8e1da08-722c-5040-9ed9-7db0dc04731e" 275 | version = "1.3.0" 276 | 277 | [[IteratorInterfaceExtensions]] 278 | git-tree-sha1 = "a3f24677c21f5bbe9d2a714f95dcd58337fb2856" 279 | uuid = "82899510-4779-5014-852e-03e436cf321d" 280 | version = "1.0.0" 281 | 282 | [[JSON]] 283 | deps = ["Dates", "Mmap", "Parsers", "Unicode"] 284 | git-tree-sha1 = "81690084b6198a2e1da36fcfda16eeca9f9f24e4" 285 | uuid = "682c06a0-de6a-54ab-a142-c8b1cf79cde6" 286 | version = "0.21.1" 287 | 288 | [[JSONSchema]] 289 | deps = ["HTTP", "JSON", "ZipFile"] 290 | git-tree-sha1 = "a9ecdbc90be216912a2e3e8a8e38dc4c93f0d065" 291 | uuid = "7d188eb4-7ad8-530c-ae41-71a32a6d4692" 292 | version = "0.3.2" 293 | 294 | [[LAME_jll]] 295 | deps = ["Libdl", "Pkg"] 296 | git-tree-sha1 = "a7999edc634307964d5651265ebf7c2e14b4ef91" 297 | uuid = "c1c5ebd0-6772-5130-a774-d5fcae4a789d" 298 | version = "3.100.0+2" 299 | 300 | [[LaTeXStrings]] 301 | git-tree-sha1 = "c7aebfecb1a60d59c0fe023a68ec947a208b1e6b" 302 | uuid = "b964fa9f-0449-5b57-a5c2-d3ea65f4040f" 303 | version = "1.2.0" 304 | 305 | [[Latexify]] 306 | deps = ["Formatting", "InteractiveUtils", "LaTeXStrings", "MacroTools", "Markdown", "Printf", "Requires"] 307 | git-tree-sha1 = "829b033e31573b8ffdd14e0d47154fd3ddc7abbf" 308 | uuid = "23fbe1c1-3f47-55db-b15f-69d7ec21a316" 309 | version = "0.14.0" 310 | 311 | [[LibGit2]] 312 | deps = ["Printf"] 313 | uuid = "76f85450-5226-5b5a-8eaa-529ad045b433" 314 | 315 | [[LibVPX_jll]] 316 | deps = ["Libdl", "Pkg"] 317 | git-tree-sha1 = "e02378f5707d0f94af22b99e4aba798e20368f6e" 318 | uuid = "dd192d2f-8180-539f-9fb4-cc70b1dcf69a" 319 | version = "1.9.0+0" 320 | 321 | [[Libdl]] 322 | uuid = "8f399da3-3557-5675-b5ff-fb832c97cbdb" 323 | 324 | [[LinearAlgebra]] 325 | deps = ["Libdl"] 326 | uuid = "37e2e46d-f89d-539d-b4ee-838fcccc9c8e" 327 | 328 | [[Logging]] 329 | uuid = "56ddb016-857b-54e1-b83d-db4d58db5568" 330 | 331 | [[Lz4_jll]] 332 | deps = ["Libdl", "Pkg"] 333 | git-tree-sha1 = "51b1db0732bbdcfabb60e36095cc3ed9c0016932" 334 | uuid = "5ced341a-0733-55b8-9ab6-a4889d929147" 335 | version = "1.9.2+2" 336 | 337 | [[MAT]] 338 | deps = ["BufferedStreams", "CodecZlib", "HDF5", "SparseArrays"] 339 | git-tree-sha1 = "7e36f6a52274ddb8515ec1f559306be3f412d6a6" 340 | uuid = "23992714-dd62-5051-b70f-ba57cb901cac" 341 | version = "0.8.1" 342 | 343 | [[MATLAB]] 344 | deps = ["Libdl", "SparseArrays", "Test"] 345 | git-tree-sha1 = "89ab46e322f9216728961119f7131038f5e4d22b" 346 | uuid = "10e44e05-a98a-55b3-a45b-ba969058deb6" 347 | version = "0.7.3" 348 | 349 | [[MacroTools]] 350 | deps = ["Markdown", "Random"] 351 | git-tree-sha1 = "f7d2e3f654af75f01ec49be82c231c382214223a" 352 | uuid = "1914dd2f-81c6-5fcd-8719-6d5c9610ff09" 353 | version = "0.5.5" 354 | 355 | [[Markdown]] 356 | deps = ["Base64"] 357 | uuid = "d6f4376e-aef5-505a-96c1-9c027394607a" 358 | 359 | [[MathOptInterface]] 360 | deps = ["BenchmarkTools", "CodecBzip2", "CodecZlib", "JSON", "JSONSchema", "LinearAlgebra", "MutableArithmetics", "OrderedCollections", "SparseArrays", "Test", "Unicode"] 361 | git-tree-sha1 = "5a1d631e0a9087d425e024d66b9c71e92e78fda8" 362 | uuid = "b8f27783-ece8-5eb3-8dc8-9495eed66fee" 363 | version = "0.9.17" 364 | 365 | [[MbedTLS]] 366 | deps = ["Dates", "MbedTLS_jll", "Random", "Sockets"] 367 | git-tree-sha1 = "426a6978b03a97ceb7ead77775a1da066343ec6e" 368 | uuid = "739be429-bea8-5141-9913-cc70e7f3736d" 369 | version = "1.0.2" 370 | 371 | [[MbedTLS_jll]] 372 | deps = ["Libdl", "Pkg"] 373 | git-tree-sha1 = "c0b1286883cac4e2b617539de41111e0776d02e8" 374 | uuid = "c8ffd9c3-330d-5841-b78e-0817d7145fa1" 375 | version = "2.16.8+0" 376 | 377 | [[Measures]] 378 | git-tree-sha1 = "e498ddeee6f9fdb4551ce855a46f54dbd900245f" 379 | uuid = "442fdcdd-2543-5da2-b0f3-8c86c306513e" 380 | version = "0.3.1" 381 | 382 | [[Missings]] 383 | deps = ["DataAPI"] 384 | git-tree-sha1 = "ed61674a0864832495ffe0a7e889c0da76b0f4c8" 385 | uuid = "e1d29d7a-bbdc-5cf2-9ac0-f12de2c33e28" 386 | version = "0.4.4" 387 | 388 | [[Mmap]] 389 | uuid = "a63ad114-7e13-5084-954f-fe012c677804" 390 | 391 | [[MutableArithmetics]] 392 | deps = ["LinearAlgebra", "SparseArrays", "Test"] 393 | git-tree-sha1 = "6cf09794783b9de2e662c4e8b60d743021e338d0" 394 | uuid = "d8a4904e-b15c-11e9-3269-09a3773c0cb0" 395 | version = "0.2.10" 396 | 397 | [[NaNMath]] 398 | git-tree-sha1 = "c84c576296d0e2fbb3fc134d3e09086b3ea617cd" 399 | uuid = "77ba4419-2d1f-58cd-9bb1-8ffee604a2e3" 400 | version = "0.3.4" 401 | 402 | [[OffsetArrays]] 403 | git-tree-sha1 = "3fdfca8a532507d65f39ff0ad34fe81097a55337" 404 | uuid = "6fe1bfb0-de20-5000-8ca7-80f57d26f881" 405 | version = "1.3.0" 406 | 407 | [[Ogg_jll]] 408 | deps = ["Libdl", "Pkg"] 409 | git-tree-sha1 = "4c3275cda1ba99d1244d0b82a9d0ca871c3cf66b" 410 | uuid = "e7412a2a-1a6e-54c0-be00-318e2571c051" 411 | version = "1.3.4+1" 412 | 413 | [[OpenSSL_jll]] 414 | deps = ["Libdl", "Pkg"] 415 | git-tree-sha1 = "997359379418d233767f926ea0c43f0e731735c0" 416 | uuid = "458c3c95-2e84-50aa-8efc-19380b2a3a95" 417 | version = "1.1.1+5" 418 | 419 | [[OpenSpecFun_jll]] 420 | deps = ["CompilerSupportLibraries_jll", "Libdl", "Pkg"] 421 | git-tree-sha1 = "d51c416559217d974a1113522d5919235ae67a87" 422 | uuid = "efe28fd5-8261-553b-a9e1-b2916fc3738e" 423 | version = "0.5.3+3" 424 | 425 | [[Opus_jll]] 426 | deps = ["Libdl", "Pkg"] 427 | git-tree-sha1 = "cc90a125aa70dbb069adbda2b913b02cf2c5f6fe" 428 | uuid = "91d4177d-7536-5919-b921-800302f37372" 429 | version = "1.3.1+2" 430 | 431 | [[OrderedCollections]] 432 | git-tree-sha1 = "16c08bf5dba06609fe45e30860092d6fa41fde7b" 433 | uuid = "bac558e1-5e72-5ebc-8fee-abe8a469f55d" 434 | version = "1.3.1" 435 | 436 | [[Parameters]] 437 | deps = ["OrderedCollections", "UnPack"] 438 | git-tree-sha1 = "38b2e970043613c187bd56a995fe2e551821eb4a" 439 | uuid = "d96e819e-fc66-5662-9728-84c9c7592b0a" 440 | version = "0.12.1" 441 | 442 | [[Parsers]] 443 | deps = ["Dates"] 444 | git-tree-sha1 = "6fa4202675c05ba0f8268a6ddf07606350eda3ce" 445 | uuid = "69de0a69-1ddd-5017-9359-2bf0b02dc9f0" 446 | version = "1.0.11" 447 | 448 | [[Pkg]] 449 | deps = ["Dates", "LibGit2", "Libdl", "Logging", "Markdown", "Printf", "REPL", "Random", "SHA", "UUIDs"] 450 | uuid = "44cfe95a-1eb2-52ea-b672-e2afdf69b78f" 451 | 452 | [[PlotThemes]] 453 | deps = ["PlotUtils", "Requires", "Statistics"] 454 | git-tree-sha1 = "c6f5ea535551b3b16835134697f0c65d06c94b91" 455 | uuid = "ccf2f8ad-2431-5c83-bf29-c5338b663b6a" 456 | version = "2.0.0" 457 | 458 | [[PlotUtils]] 459 | deps = ["ColorSchemes", "Colors", "Dates", "Printf", "Random", "Reexport", "Statistics"] 460 | git-tree-sha1 = "4e098f88dad9a2b518b83124a116be1c49e2b2bf" 461 | uuid = "995b91a9-d308-5afd-9ec6-746e21dbc043" 462 | version = "1.0.7" 463 | 464 | [[Plots]] 465 | deps = ["Base64", "Contour", "Dates", "FFMPEG", "FixedPointNumbers", "GR", "GeometryBasics", "GeometryTypes", "JSON", "Latexify", "LinearAlgebra", "Measures", "NaNMath", "Pkg", "PlotThemes", "PlotUtils", "Printf", "REPL", "Random", "RecipesBase", "RecipesPipeline", "Reexport", "Requires", "Showoff", "SparseArrays", "Statistics", "StatsBase", "UUIDs"] 466 | git-tree-sha1 = "dea71b6f8214a97dc16c3a9e4e7bd0b71ddcd132" 467 | uuid = "91a5bcdd-55d7-5caf-9e0b-520d859cae80" 468 | version = "1.6.9" 469 | 470 | [[Printf]] 471 | deps = ["Unicode"] 472 | uuid = "de0858da-6303-5e67-8744-51eddeeeb8d7" 473 | 474 | [[REPL]] 475 | deps = ["InteractiveUtils", "Markdown", "Sockets"] 476 | uuid = "3fa0cd96-eef1-5676-8a61-b3b8758bbffb" 477 | 478 | [[Random]] 479 | deps = ["Serialization"] 480 | uuid = "9a3f8284-a2c9-5f02-9a11-845980a1fd5c" 481 | 482 | [[Ratios]] 483 | git-tree-sha1 = "37d210f612d70f3f7d57d488cb3b6eff56ad4e41" 484 | uuid = "c84ed2f1-dad5-54f0-aa8e-dbefe2724439" 485 | version = "0.4.0" 486 | 487 | [[RecipesBase]] 488 | git-tree-sha1 = "6ee6c35fe69e79e17c455a386c1ccdc66d9f7da4" 489 | uuid = "3cdcf5f2-1ef4-517c-9805-6587b60abb01" 490 | version = "1.1.0" 491 | 492 | [[RecipesPipeline]] 493 | deps = ["Dates", "NaNMath", "PlotUtils", "RecipesBase"] 494 | git-tree-sha1 = "4a325c9bcc2d8e62a8f975b9666d0251d53b63b9" 495 | uuid = "01d81517-befc-4cb6-b9ec-a95719d0359c" 496 | version = "0.1.13" 497 | 498 | [[Reexport]] 499 | deps = ["Pkg"] 500 | git-tree-sha1 = "7b1d07f411bc8ddb7977ec7f377b97b158514fe0" 501 | uuid = "189a3867-3050-52da-a836-e630ba90ab69" 502 | version = "0.2.0" 503 | 504 | [[Requires]] 505 | deps = ["UUIDs"] 506 | git-tree-sha1 = "28faf1c963ca1dc3ec87f166d92982e3c4a1f66d" 507 | uuid = "ae029012-a4dd-5104-9daa-d747884805df" 508 | version = "1.1.0" 509 | 510 | [[RobotDynamics]] 511 | deps = ["ForwardDiff", "LinearAlgebra", "RecipesBase", "Rotations", "StaticArrays", "UnsafeArrays"] 512 | git-tree-sha1 = "ded14998ef6baa6773c8f3c8356656ffedae7095" 513 | repo-rev = "master" 514 | repo-url = "https://github.com/RoboticExplorationLab/RobotDynamics.jl.git" 515 | uuid = "38ceca67-d8d3-44e8-9852-78a5596522e1" 516 | version = "0.2.2" 517 | 518 | [[RobotZoo]] 519 | deps = ["LinearAlgebra", "Parameters", "RobotDynamics", "Rotations", "StaticArrays"] 520 | git-tree-sha1 = "7c918fa028d9e3056e76dbe214db2ed877e013e0" 521 | uuid = "74be38bb-dcc2-4b9e-baf3-d6373cd95f10" 522 | version = "0.1.1" 523 | 524 | [[Rotations]] 525 | deps = ["LinearAlgebra", "StaticArrays", "Statistics"] 526 | git-tree-sha1 = "445b72242dbdecba9bfc42034daafdd901bbf6a9" 527 | uuid = "6038ab10-8711-5258-84ad-4b1120ba62dc" 528 | version = "1.0.1" 529 | 530 | [[SHA]] 531 | uuid = "ea8e919c-243c-51af-8825-aaa63cd721ce" 532 | 533 | [[Serialization]] 534 | uuid = "9e88b42a-f829-5b0c-bbe9-9e923198166b" 535 | 536 | [[SharedArrays]] 537 | deps = ["Distributed", "Mmap", "Random", "Serialization"] 538 | uuid = "1a1011a3-84de-559e-8e89-a11a2f7dc383" 539 | 540 | [[Showoff]] 541 | deps = ["Dates", "Grisu"] 542 | git-tree-sha1 = "ee010d8f103468309b8afac4abb9be2e18ff1182" 543 | uuid = "992d4aef-0814-514b-bc4d-f2e9a6c4116f" 544 | version = "0.3.2" 545 | 546 | [[Sockets]] 547 | uuid = "6462fe0b-24de-5631-8697-dd941f90decc" 548 | 549 | [[SolverLogging]] 550 | deps = ["Formatting", "Logging"] 551 | git-tree-sha1 = "38651153b05d3784860dd50049f237c34c4fa4f2" 552 | uuid = "c2e08473-88be-4f39-9d3c-afcdb6e3aeb8" 553 | version = "0.1.0" 554 | 555 | [[SortingAlgorithms]] 556 | deps = ["DataStructures", "Random", "Test"] 557 | git-tree-sha1 = "03f5898c9959f8115e30bc7226ada7d0df554ddd" 558 | uuid = "a2af1166-a08f-5f64-846c-94a0d3cef48c" 559 | version = "0.3.1" 560 | 561 | [[SparseArrays]] 562 | deps = ["LinearAlgebra", "Random"] 563 | uuid = "2f01184e-e22b-5df5-ae63-d93ebab69eaf" 564 | 565 | [[SpecialFunctions]] 566 | deps = ["OpenSpecFun_jll"] 567 | git-tree-sha1 = "d8d8b8a9f4119829410ecd706da4cc8594a1e020" 568 | uuid = "276daf66-3868-5448-9aa4-cd146d93841b" 569 | version = "0.10.3" 570 | 571 | [[StaticArrays]] 572 | deps = ["LinearAlgebra", "Random", "Statistics"] 573 | git-tree-sha1 = "016d1e1a00fabc556473b07161da3d39726ded35" 574 | uuid = "90137ffa-7385-5640-81b9-e52037218182" 575 | version = "0.12.4" 576 | 577 | [[Statistics]] 578 | deps = ["LinearAlgebra", "SparseArrays"] 579 | uuid = "10745b16-79ce-11e8-11f9-7d13ad32a3b2" 580 | 581 | [[StatsBase]] 582 | deps = ["DataAPI", "DataStructures", "LinearAlgebra", "Missings", "Printf", "Random", "SortingAlgorithms", "SparseArrays", "Statistics"] 583 | git-tree-sha1 = "7bab7d4eb46b225b35179632852b595a3162cb61" 584 | uuid = "2913bbd2-ae8a-5f71-8c99-4fb6c76f3a91" 585 | version = "0.33.2" 586 | 587 | [[StructArrays]] 588 | deps = ["Adapt", "DataAPI", "Tables"] 589 | git-tree-sha1 = "8099ed9fb90b6e754d6ba8c6ed8670f010eadca0" 590 | uuid = "09ab397b-f2b6-538f-b94a-2f83cf4a842a" 591 | version = "0.4.4" 592 | 593 | [[TableTraits]] 594 | deps = ["IteratorInterfaceExtensions"] 595 | git-tree-sha1 = "b1ad568ba658d8cbb3b892ed5380a6f3e781a81e" 596 | uuid = "3783bdb8-4a98-5b6b-af9a-565f29a5fe9c" 597 | version = "1.0.0" 598 | 599 | [[Tables]] 600 | deps = ["DataAPI", "DataValueInterfaces", "IteratorInterfaceExtensions", "LinearAlgebra", "TableTraits", "Test"] 601 | git-tree-sha1 = "24a584cf65e2cfabdadc21694fb69d2e74c82b44" 602 | uuid = "bd369af6-aec1-5ad0-b16a-f7cc5008161c" 603 | version = "1.1.0" 604 | 605 | [[Test]] 606 | deps = ["Distributed", "InteractiveUtils", "Logging", "Random"] 607 | uuid = "8dfed614-e22c-5e08-85e1-65c5234f0b40" 608 | 609 | [[TrajectoryOptimization]] 610 | deps = ["DocStringExtensions", "ForwardDiff", "LinearAlgebra", "MathOptInterface", "RobotDynamics", "Rotations", "SparseArrays", "StaticArrays", "UnsafeArrays"] 611 | git-tree-sha1 = "c4fa9e69a7db9e4c9ff8ad3505babb09afc6df5c" 612 | repo-rev = "PlanningWithAttitude" 613 | repo-url = "https://github.com/RoboticExplorationLab/TrajectoryOptimization.jl.git" 614 | uuid = "c79d492b-0548-5874-b488-5a62c1d9d0ca" 615 | version = "0.3.2" 616 | 617 | [[TranscodingStreams]] 618 | deps = ["Random", "Test"] 619 | git-tree-sha1 = "7c53c35547de1c5b9d46a4797cf6d8253807108c" 620 | uuid = "3bb67fe8-82b1-5028-8e26-92a6c54297fa" 621 | version = "0.9.5" 622 | 623 | [[UUIDs]] 624 | deps = ["Random", "SHA"] 625 | uuid = "cf7118a7-6976-5b1a-9a39-7adc72f591a4" 626 | 627 | [[UnPack]] 628 | git-tree-sha1 = "387c1f73762231e86e0c9c5443ce3b4a0a9a0c2b" 629 | uuid = "3a884ed6-31ef-47d7-9d2a-63182c4928ed" 630 | version = "1.0.2" 631 | 632 | [[Unicode]] 633 | uuid = "4ec0a83e-493e-50e2-b9ac-8f72acf5a8f5" 634 | 635 | [[UnsafeArrays]] 636 | git-tree-sha1 = "9740b414f85ec2fa9135066f81b1fb14212befd6" 637 | uuid = "c4a57d5a-5b31-53a6-b365-19f8c011fbd6" 638 | version = "1.0.1" 639 | 640 | [[WoodburyMatrices]] 641 | deps = ["LinearAlgebra", "SparseArrays"] 642 | git-tree-sha1 = "28ffe06d28b1ba8fdb2f36ec7bb079fac81bac0d" 643 | uuid = "efce3f68-66dc-5838-9240-27a6d6f5f9b6" 644 | version = "0.5.2" 645 | 646 | [[ZipFile]] 647 | deps = ["Libdl", "Printf", "Zlib_jll"] 648 | git-tree-sha1 = "254975fef2fc526583bb9b7c9420fe66ffe09f2f" 649 | uuid = "a5390f91-8eb1-5f08-bee0-b1d1ffed6cea" 650 | version = "0.9.2" 651 | 652 | [[Zlib_jll]] 653 | deps = ["Libdl", "Pkg"] 654 | git-tree-sha1 = "fdd89e5ab270ea0f2a0174bd9093e557d06d4bfa" 655 | uuid = "83775a58-1f1d-513f-b197-d71354ab007a" 656 | version = "1.2.11+16" 657 | 658 | [[Zstd_jll]] 659 | deps = ["Libdl", "Pkg"] 660 | git-tree-sha1 = "4de91f4313d9e88162d461e282fe3066ab3a3c09" 661 | uuid = "3161d3a3-bdf6-5164-811a-617609db77b4" 662 | version = "1.4.5+1" 663 | 664 | [[libass_jll]] 665 | deps = ["Bzip2_jll", "FreeType2_jll", "FriBidi_jll", "Libdl", "Pkg", "Zlib_jll"] 666 | git-tree-sha1 = "f02d0db58888592e98c5f4953cef620ce9274eee" 667 | uuid = "0ac62f75-1d6f-5e53-bd7c-93b484bb37c0" 668 | version = "0.14.0+3" 669 | 670 | [[libfdk_aac_jll]] 671 | deps = ["Libdl", "Pkg"] 672 | git-tree-sha1 = "e17b4513993b4413d31cffd1b36a63625ebbc3d3" 673 | uuid = "f638f0a6-7fb0-5443-88ba-1cc74229b280" 674 | version = "0.1.6+3" 675 | 676 | [[libvorbis_jll]] 677 | deps = ["Libdl", "Ogg_jll", "Pkg"] 678 | git-tree-sha1 = "8014e1c1033009edcfe820ec25877a9f1862ba4c" 679 | uuid = "f27f6e37-5d2b-51aa-960f-b287f2bc3b7a" 680 | version = "1.3.6+5" 681 | 682 | [[x264_jll]] 683 | deps = ["Libdl", "Pkg"] 684 | git-tree-sha1 = "e496625b900df1b02ab0e02fad316b77446616ef" 685 | uuid = "1270edf5-f2f9-52d2-97e9-ab00b5d0237a" 686 | version = "2020.7.14+1" 687 | 688 | [[x265_jll]] 689 | deps = ["Libdl", "Pkg"] 690 | git-tree-sha1 = "ac7d44fa1639a780d0ae79ca1a5a7f4181131825" 691 | uuid = "dfaa095f-4041-5dcd-9319-2fabd8486b76" 692 | version = "3.0.0+2" 693 | -------------------------------------------------------------------------------- /examples/Flexible_Spacecraft/Project.toml: -------------------------------------------------------------------------------- 1 | [deps] 2 | Altro = "5dcf52e5-e2fb-48e0-b826-96f46d2e3e73" 3 | Infiltrator = "5903a43b-9cc3-4c30-8d17-598619ec4e9b" 4 | MAT = "23992714-dd62-5051-b70f-ba57cb901cac" 5 | MATLAB = "10e44e05-a98a-55b3-a45b-ba969058deb6" 6 | Parameters = "d96e819e-fc66-5662-9728-84c9c7592b0a" 7 | Plots = "91a5bcdd-55d7-5caf-9e0b-520d859cae80" 8 | RobotDynamics = "38ceca67-d8d3-44e8-9852-78a5596522e1" 9 | Rotations = "6038ab10-8711-5258-84ad-4b1120ba62dc" 10 | StaticArrays = "90137ffa-7385-5640-81b9-e52037218182" 11 | TrajectoryOptimization = "c79d492b-0548-5874-b488-5a62c1d9d0ca" 12 | -------------------------------------------------------------------------------- /examples/Flexible_Spacecraft/solve_con.jl: -------------------------------------------------------------------------------- 1 | import Pkg; Pkg.activate(@__DIR__) 2 | # using MATLAB 3 | using StaticArrays 4 | using Parameters 5 | using RobotDynamics 6 | using Rotations 7 | using LinearAlgebra 8 | # using Plots 9 | using TrajectoryOptimization 10 | using PlanningWithAttitude 11 | using Altro 12 | 13 | 14 | const TO = TrajectoryOptimization 15 | 16 | using PlanningWithAttitude: post_process, dcm_from_q 17 | 18 | ## 19 | prob,opts = SatelliteKeepOutProblem(vecstate=true, costfun=LQRCost) 20 | prob,opts = SatelliteKeepOutProblem(vecstate=false, costfun=QuatLQRCost) 21 | solver = ALTROSolver(prob, opts) 22 | solve!(solver) 23 | 24 | 25 | ## 26 | bodyvec = prob.constraints[3].bodyvec 27 | keepoutdir = prob.constraints[3].keepoutdir 28 | ω_hist, q_hist, u_hist,η_hist, ηd_hist, r_hist,θ_hist = post_process(solver) 29 | 30 | keepout_truth = zeros(size(q_hist,2)) 31 | g_hist = zeros(3,size(q_hist,2)) 32 | for i = 1:size(q_hist,2) 33 | # keepout_truth[i] = dot(dcm_from_q(q_hist[:,i])*bodyvec,keepoutdir) - cosd(20.0) 34 | keepout_truth[i] = acosd(dot(dcm_from_q(q_hist[:,i])*bodyvec,keepoutdir)) 35 | q = q_hist[:,i] 36 | g_hist[:,i] = q[2:4]/q[1] 37 | end 38 | 39 | using Plots 40 | plot(keepout_truth) 41 | plot(controls(solver)) 42 | 43 | mat" 44 | figure 45 | hold on 46 | title('Pointing Error') 47 | ylabel('Pointing Error (Degrees)') 48 | xlabel('Time (s)') 49 | plot(0.0:$dt:(($N-2)*$dt),rad2deg($θ_hist)) 50 | hold off 51 | " 52 | 53 | mat" 54 | figure 55 | hold on 56 | ylabel('Torque (Nm)') 57 | xlabel('Time (s)') 58 | title('Controls') 59 | plot(0.0:$dt:(($N-2)*$dt),$u_hist') 60 | hold off 61 | " 62 | 63 | mat" 64 | figure 65 | hold on 66 | title('Angle from Sun (Constrained)') 67 | a = area([0 100],[40 40]); 68 | a(1).FaceColor = [1 0.8 0.8]; 69 | plot((0:($N-2))*$dt,$keepout_truth,'b','linewidth',2) 70 | legend('Keep Out Zone','Trajectory') 71 | xlabel('Time (s)') 72 | ylabel('Angle from Sun (Degrees)') 73 | hold off 74 | " 75 | 76 | mat" 77 | f = @(x,y,z) -(3657152068667140*x^2 + 16745716480053206*x*y - 1426901590817633*x*z + 10142677805652334*y^2 - 1426901590817633*y + 10142677805652334*z^2 - 16745716480053206*z + 3657152068667140)/(9007199254740992*(x^2 + y^2 + z^2 + 1)) 78 | 79 | endind = 200 80 | figure 81 | hold on 82 | title('Rodrigues Parameter Trajectory History') 83 | plot3($g_hist(1,:),$g_hist(2,:),$g_hist(3,:),'b','linewidth',2) 84 | %plot3(g_hist_uncon(1,1:endind),g_hist_uncon(2,1:endind),g_hist_uncon(3,1:endind),'r','linewidth',2) 85 | %plot3(g_hist_con(1,1:endind),g_hist_con(2,1:endind),g_hist_con(3,1:endind),'b','linewidth',2) 86 | fimplicit3(f,'FaceColor',[.5 .5 .5]) 87 | xlim([-.5 1.1]) 88 | ylim([-.2 2.1]) 89 | zlim([-.5 3]) 90 | legend('Constrained Trajectory','Constraint Surface') 91 | 92 | xlabel('g_1') 93 | ylabel('g_2') 94 | zlabel('g_3') 95 | % axis equal 96 | hold off 97 | " 98 | 99 | # g_hist_con = copy(g_hist) 100 | # file = matopen("g_hist_con.mat", "w") 101 | # write(file, "g_hist_con", g_hist_con) 102 | # close(file) 103 | -------------------------------------------------------------------------------- /examples/Flexible_Spacecraft/solve_uncon.jl: -------------------------------------------------------------------------------- 1 | cd(dirname(@__FILE__)) 2 | Pkg.activate(".") 3 | using MATLAB 4 | using StaticArrays 5 | using Parameters 6 | using RobotDynamics 7 | using Rotations 8 | using LinearAlgebra 9 | # using Plots 10 | using TrajectoryOptimization 11 | using Altro 12 | 13 | 14 | include(joinpath(dirname(@__FILE__),"flexible_spacecraft_dynamics.jl")) 15 | 16 | const TO = TrajectoryOptimization 17 | 18 | 19 | # Discretization 20 | tf = 100.0 21 | N = 401 22 | dt = tf/(N-1) 23 | 24 | # Model 25 | model = FlexSatellite() 26 | n,m = size(model) 27 | 28 | # quaternion to dcm functions 29 | function skew(v) 30 | """Skew-symmetric matrix from 3 element array""" 31 | return @SMatrix [0 -v[3] v[2]; v[3] 0 -v[1]; -v[2] v[1] 0] 32 | end 33 | function dcm_from_q(q) 34 | """DCM from quaternion, scalar first""" 35 | s = @views q[1] 36 | v = @views q[2:4] 37 | return I + 2*skew(v)*(s*I + skew(v)) 38 | end 39 | 40 | 41 | struct AttitudeKeepOut{T} <: TO.StateConstraint 42 | n::Int 43 | keepoutdir::SVector{3,T} 44 | bodyvec::SVector{3,T} 45 | keepoutangle::Float64 46 | function AttitudeKeepOut(n::Int, keepoutdir::SVector, bodyvec::SVector, keepoutangle::T) where T 47 | new{T}(n,keepoutdir,bodyvec,keepoutangle) 48 | end 49 | end 50 | 51 | TO.state_dim(con::AttitudeKeepOut) = con.n 52 | TO.sense(::AttitudeKeepOut) = Inequality() 53 | Base.length(::AttitudeKeepOut) = 1 54 | TO.evaluate(con::AttitudeKeepOut, x::SVector) = ( 55 | SA[ dot(dcm_from_q(x[4:7])*con.bodyvec,con.keepoutdir)-cosd(con.keepoutangle)]) 56 | 57 | # Initial and final states 58 | ω = @SVector zeros(3) 59 | q0 = Rotations.params(expm(deg2rad(150) * normalize(@SVector [1,2,3]))) 60 | qf = Rotations.params(UnitQuaternion(I)) 61 | r0 = zeros(4) 62 | x0 = [ω; q0;zeros(3);zeros(3);r0] 63 | xf = [ω; qf;zeros(3);zeros(3);r0] 64 | 65 | # Objective 66 | Q = Diagonal(@SVector [10,10,10,100,100,100,100,10,10.0,10,10,10,10,0,0,0,0]) 67 | R = Diagonal(@SVector fill(1.0, m)) 68 | Qf = Q 69 | obj = LQRObjective(Q,R,Qf,xf,N) 70 | 71 | # constaint 72 | cons = ConstraintList(n,m,N) 73 | bnd = BoundConstraint(n,m, u_min=-.5, u_max=.5) 74 | add_constraint!(cons,bnd,1:N-1) 75 | 76 | ## keep out 77 | keepoutdir = @SVector [1.0,0,0] 78 | bodyvec = @SVector [0.360019,-0.92957400,0.07920895] 79 | 80 | 81 | prob = Problem(model, obj, xf, tf, x0=x0,constraints = cons,N=N;integration=RK4) 82 | 83 | solver = ALTROSolver(prob) 84 | solver.opts.projected_newton = true 85 | solver.solver_al.opts.verbose = true 86 | set_options!(solver, iterations = 4000) 87 | set_options!(solver,constraint_tolerance = 1e-4) 88 | solve!(solver) 89 | 90 | println(iterations(solver)) 91 | 92 | 93 | ω_hist, q_hist, u_hist,η_hist, ηd_hist, r_hist,θ_hist = post_process(solver) 94 | 95 | keepout_truth = zeros(size(q_hist,2)) 96 | g_hist = zeros(3,size(q_hist,2)) 97 | for i = 1:size(q_hist,2) 98 | # keepout_truth[i] = dot(dcm_from_q(q_hist[:,i])*bodyvec,keepoutdir) - cosd(20.0) 99 | keepout_truth[i] = acosd(dot(dcm_from_q(q_hist[:,i])*bodyvec,keepoutdir)) 100 | q = q_hist[:,i] 101 | g_hist[:,i] = q[2:4]/q[1] 102 | end 103 | 104 | 105 | mat" 106 | figure 107 | hold on 108 | title('Pointing Error') 109 | ylabel('Pointing Error (Degrees)') 110 | xlabel('Time (s)') 111 | plot(0.0:$dt:(($N-2)*$dt),rad2deg($θ_hist)) 112 | hold off 113 | " 114 | 115 | mat" 116 | figure 117 | hold on 118 | ylabel('Torque (Nm)') 119 | xlabel('Time (s)') 120 | title('Controls') 121 | plot(0.0:$dt:(($N-2)*$dt),$u_hist') 122 | hold off 123 | " 124 | 125 | mat" 126 | figure 127 | hold on 128 | title('Angle from Sun (Unconstrained)') 129 | a = area([0 100],[40 40]); 130 | a(1).FaceColor = [1 0.8 0.8]; 131 | plot((0:($N-2))*$dt,$keepout_truth,'b','linewidth',2) 132 | legend('Keep Out Zone','Trajectory') 133 | xlabel('Time (s)') 134 | ylabel('Angle from Sun (Degrees)') 135 | hold off 136 | " 137 | 138 | mat" 139 | f = @(x,y,z) -(3657152068667140*x^2 + 16745716480053206*x*y - 1426901590817633*x*z + 10142677805652334*y^2 - 1426901590817633*y + 10142677805652334*z^2 - 16745716480053206*z + 3657152068667140)/(9007199254740992*(x^2 + y^2 + z^2 + 1)) 140 | 141 | endind = 200 142 | figure 143 | hold on 144 | title('Rodrigues Parameter Trajectory History') 145 | plot3($g_hist(1,:),$g_hist(2,:),$g_hist(3,:),'b','linewidth',2) 146 | %plot3(g_hist_uncon(1,1:endind),g_hist_uncon(2,1:endind),g_hist_uncon(3,1:endind),'r','linewidth',2) 147 | %plot3(g_hist_con(1,1:endind),g_hist_con(2,1:endind),g_hist_con(3,1:endind),'b','linewidth',2) 148 | fimplicit3(f,'FaceColor',[.5 .5 .5]) 149 | xlim([-.5 1.1]) 150 | ylim([-.2 2.1]) 151 | zlim([-.5 3]) 152 | legend('Unconstrained Trajectory','Constraint Surface') 153 | 154 | xlabel('g_1') 155 | ylabel('g_2') 156 | zlabel('g_3') 157 | % axis equal 158 | hold off 159 | " 160 | 161 | # g_hist_uncon = copy(g_hist) 162 | # file = matopen("g_hist_uncon.mat", "w") 163 | # write(file, "g_hist_uncon", g_hist_uncon) 164 | # close(file) 165 | -------------------------------------------------------------------------------- /examples/Project.toml: -------------------------------------------------------------------------------- 1 | [deps] 2 | Altro = "5dcf52e5-e2fb-48e0-b826-96f46d2e3e73" 3 | BenchmarkTools = "6e4b80f9-dd63-53aa-95a3-0cdb28fa8baf" 4 | Blink = "ad839575-38b3-5650-b840-f874b8c74a25" 5 | Colors = "5ae59095-9a9b-59fe-a467-6f913c188581" 6 | ControlSystems = "a6e380b2-a6ca-5380-bf3e-84a91bcd477e" 7 | DataFrames = "a93c6f00-e57d-5684-b7b6-d8193f3e46c0" 8 | Distributions = "31c24e10-a181-5473-b8eb-7969acd0382f" 9 | JLD2 = "033835bb-8acc-5ee8-8aae-3f567f8a3819" 10 | LaTeXStrings = "b964fa9f-0449-5b57-a5c2-d3ea65f4040f" 11 | LaTeXTabulars = "266f59ce-6e72-579c-98bb-27b39b5c037e" 12 | MeshIO = "7269a6da-0436-5bbc-96c2-40638cbb6118" 13 | PGFPlotsX = "8314cec4-20b6-5062-9cdb-752b83310925" 14 | PlanningWithAttitude = "35fb9e8a-3ec1-11ea-3514-d75c41db21f5" 15 | Plots = "91a5bcdd-55d7-5caf-9e0b-520d859cae80" 16 | RobotDynamics = "38ceca67-d8d3-44e8-9852-78a5596522e1" 17 | RobotZoo = "74be38bb-dcc2-4b9e-baf3-d6373cd95f10" 18 | Rotations = "6038ab10-8711-5258-84ad-4b1120ba62dc" 19 | StaticArrays = "90137ffa-7385-5640-81b9-e52037218182" 20 | TrajectoryOptimization = "c79d492b-0548-5874-b488-5a62c1d9d0ca" 21 | -------------------------------------------------------------------------------- /examples/Wahbas_problem/Manifest.toml: -------------------------------------------------------------------------------- 1 | # This file is machine-generated - editing it directly is not advised 2 | 3 | [[ArgCheck]] 4 | git-tree-sha1 = "dedbbb2ddb876f899585c4ec4433265e3017215a" 5 | uuid = "dce04be8-c92d-5529-be00-80e4d2c0e197" 6 | version = "2.1.0" 7 | 8 | [[Base64]] 9 | uuid = "2a0f44e3-6c83-55bd-87e4-b1978d98bd5f" 10 | 11 | [[CodecZlib]] 12 | deps = ["TranscodingStreams", "Zlib_jll"] 13 | git-tree-sha1 = "ded953804d019afa9a3f98981d99b33e3db7b6da" 14 | uuid = "944b1d66-785c-5afd-91f1-9de20f533193" 15 | version = "0.7.0" 16 | 17 | [[CommonSubexpressions]] 18 | deps = ["MacroTools", "Test"] 19 | git-tree-sha1 = "7b8a93dba8af7e3b42fecabf646260105ac373f7" 20 | uuid = "bbf7d656-a473-5ed7-a52c-81e309532950" 21 | version = "0.3.0" 22 | 23 | [[Compat]] 24 | deps = ["Base64", "Dates", "DelimitedFiles", "Distributed", "InteractiveUtils", "LibGit2", "Libdl", "LinearAlgebra", "Markdown", "Mmap", "Pkg", "Printf", "REPL", "Random", "SHA", "Serialization", "SharedArrays", "Sockets", "SparseArrays", "Statistics", "Test", "UUIDs", "Unicode"] 25 | git-tree-sha1 = "cf03b37436c6bc162e7c8943001568b4cad4bee3" 26 | uuid = "34da2185-b29b-5c13-b0c7-acf172513d20" 27 | version = "3.19.0" 28 | 29 | [[CompilerSupportLibraries_jll]] 30 | deps = ["Libdl", "Pkg"] 31 | git-tree-sha1 = "7c4f882c41faa72118841185afc58a2eb00ef612" 32 | uuid = "e66e0078-7015-5450-92f7-15fbd957f2ae" 33 | version = "0.3.3+0" 34 | 35 | [[DataAPI]] 36 | git-tree-sha1 = "176e23402d80e7743fc26c19c681bfb11246af32" 37 | uuid = "9a962f9c-6df0-11e9-0e5d-c546b8b5ee8a" 38 | version = "1.3.0" 39 | 40 | [[DataStructures]] 41 | deps = ["Compat", "InteractiveUtils", "OrderedCollections"] 42 | git-tree-sha1 = "db07bb22795762895b60e44d62b34b16c982a687" 43 | uuid = "864edb3b-99cc-5e75-8d2d-829cb0a9cfe8" 44 | version = "0.18.7" 45 | 46 | [[DataValueInterfaces]] 47 | git-tree-sha1 = "bfc1187b79289637fa0ef6d4436ebdfe6905cbd6" 48 | uuid = "e2d170a0-9d28-54be-80f0-106bbe20a464" 49 | version = "1.0.0" 50 | 51 | [[Dates]] 52 | deps = ["Printf"] 53 | uuid = "ade2ca70-3891-5945-98fb-dc099432e06a" 54 | 55 | [[DefaultApplication]] 56 | deps = ["InteractiveUtils"] 57 | git-tree-sha1 = "fc2b7122761b22c87fec8bf2ea4dc4563d9f8c24" 58 | uuid = "3f0dd361-4fe0-5fc6-8523-80b14ec94d85" 59 | version = "1.0.0" 60 | 61 | [[DelimitedFiles]] 62 | deps = ["Mmap"] 63 | uuid = "8bb1440f-4735-579b-a4ab-409b98df4dab" 64 | 65 | [[DiffResults]] 66 | deps = ["StaticArrays"] 67 | git-tree-sha1 = "da24935df8e0c6cf28de340b958f6aac88eaa0cc" 68 | uuid = "163ba53b-c6d8-5494-b064-1a9d43ac40c5" 69 | version = "1.0.2" 70 | 71 | [[DiffRules]] 72 | deps = ["NaNMath", "Random", "SpecialFunctions"] 73 | git-tree-sha1 = "eb0c34204c8410888844ada5359ac8b96292cfd1" 74 | uuid = "b552c78f-8df3-52c6-915a-8e097449b14b" 75 | version = "1.0.1" 76 | 77 | [[Distributed]] 78 | deps = ["Random", "Serialization", "Sockets"] 79 | uuid = "8ba89e20-285c-5b6f-9357-94700520ee1b" 80 | 81 | [[DocStringExtensions]] 82 | deps = ["LibGit2", "Markdown", "Pkg", "Test"] 83 | git-tree-sha1 = "50ddf44c53698f5e784bbebb3f4b21c5807401b1" 84 | uuid = "ffbed154-4ef7-542d-bbb7-c09d3a79fcae" 85 | version = "0.8.3" 86 | 87 | [[ForwardDiff]] 88 | deps = ["CommonSubexpressions", "DiffResults", "DiffRules", "NaNMath", "Random", "SpecialFunctions", "StaticArrays"] 89 | git-tree-sha1 = "1d090099fb82223abc48f7ce176d3f7696ede36d" 90 | uuid = "f6369f11-7733-5829-9624-2563aa707210" 91 | version = "0.10.12" 92 | 93 | [[Infiltrator]] 94 | deps = ["REPL"] 95 | git-tree-sha1 = "2be5c3e8adddf062c3903a6d7618f233fa4d2874" 96 | uuid = "5903a43b-9cc3-4c30-8d17-598619ec4e9b" 97 | version = "0.3.0" 98 | 99 | [[InteractiveUtils]] 100 | deps = ["Markdown"] 101 | uuid = "b77e0a4c-d291-57a0-90e8-8db25a27a240" 102 | 103 | [[IteratorInterfaceExtensions]] 104 | git-tree-sha1 = "a3f24677c21f5bbe9d2a714f95dcd58337fb2856" 105 | uuid = "82899510-4779-5014-852e-03e436cf321d" 106 | version = "1.0.0" 107 | 108 | [[JLD2]] 109 | deps = ["CodecZlib", "DataStructures", "MacroTools", "Mmap", "Pkg", "Printf", "Requires", "UUIDs"] 110 | git-tree-sha1 = "276e53d8d4ca4d3775478059595deedfe3610b44" 111 | uuid = "033835bb-8acc-5ee8-8aae-3f567f8a3819" 112 | version = "0.2.3" 113 | 114 | [[LibGit2]] 115 | deps = ["Printf"] 116 | uuid = "76f85450-5226-5b5a-8eaa-529ad045b433" 117 | 118 | [[Libdl]] 119 | uuid = "8f399da3-3557-5675-b5ff-fb832c97cbdb" 120 | 121 | [[LinearAlgebra]] 122 | deps = ["Libdl"] 123 | uuid = "37e2e46d-f89d-539d-b4ee-838fcccc9c8e" 124 | 125 | [[Logging]] 126 | uuid = "56ddb016-857b-54e1-b83d-db4d58db5568" 127 | 128 | [[MATLAB]] 129 | deps = ["Libdl", "SparseArrays", "Test"] 130 | git-tree-sha1 = "89ab46e322f9216728961119f7131038f5e4d22b" 131 | uuid = "10e44e05-a98a-55b3-a45b-ba969058deb6" 132 | version = "0.7.3" 133 | 134 | [[MacroTools]] 135 | deps = ["Markdown", "Random"] 136 | git-tree-sha1 = "f7d2e3f654af75f01ec49be82c231c382214223a" 137 | uuid = "1914dd2f-81c6-5fcd-8719-6d5c9610ff09" 138 | version = "0.5.5" 139 | 140 | [[Markdown]] 141 | deps = ["Base64"] 142 | uuid = "d6f4376e-aef5-505a-96c1-9c027394607a" 143 | 144 | [[Mmap]] 145 | uuid = "a63ad114-7e13-5084-954f-fe012c677804" 146 | 147 | [[NaNMath]] 148 | git-tree-sha1 = "c84c576296d0e2fbb3fc134d3e09086b3ea617cd" 149 | uuid = "77ba4419-2d1f-58cd-9bb1-8ffee604a2e3" 150 | version = "0.3.4" 151 | 152 | [[OpenSpecFun_jll]] 153 | deps = ["CompilerSupportLibraries_jll", "Libdl", "Pkg"] 154 | git-tree-sha1 = "d51c416559217d974a1113522d5919235ae67a87" 155 | uuid = "efe28fd5-8261-553b-a9e1-b2916fc3738e" 156 | version = "0.5.3+3" 157 | 158 | [[OrderedCollections]] 159 | git-tree-sha1 = "16c08bf5dba06609fe45e30860092d6fa41fde7b" 160 | uuid = "bac558e1-5e72-5ebc-8fee-abe8a469f55d" 161 | version = "1.3.1" 162 | 163 | [[PGFPlotsX]] 164 | deps = ["ArgCheck", "DataStructures", "Dates", "DefaultApplication", "DocStringExtensions", "MacroTools", "Parameters", "Requires", "Tables"] 165 | git-tree-sha1 = "1adde3d07cce96b6a3bb88572612db4bd9d6153b" 166 | uuid = "8314cec4-20b6-5062-9cdb-752b83310925" 167 | version = "1.2.10" 168 | 169 | [[Parameters]] 170 | deps = ["OrderedCollections", "UnPack"] 171 | git-tree-sha1 = "38b2e970043613c187bd56a995fe2e551821eb4a" 172 | uuid = "d96e819e-fc66-5662-9728-84c9c7592b0a" 173 | version = "0.12.1" 174 | 175 | [[Pkg]] 176 | deps = ["Dates", "LibGit2", "Libdl", "Logging", "Markdown", "Printf", "REPL", "Random", "SHA", "UUIDs"] 177 | uuid = "44cfe95a-1eb2-52ea-b672-e2afdf69b78f" 178 | 179 | [[Printf]] 180 | deps = ["Unicode"] 181 | uuid = "de0858da-6303-5e67-8744-51eddeeeb8d7" 182 | 183 | [[REPL]] 184 | deps = ["InteractiveUtils", "Markdown", "Sockets"] 185 | uuid = "3fa0cd96-eef1-5676-8a61-b3b8758bbffb" 186 | 187 | [[Random]] 188 | deps = ["Serialization"] 189 | uuid = "9a3f8284-a2c9-5f02-9a11-845980a1fd5c" 190 | 191 | [[Requires]] 192 | deps = ["UUIDs"] 193 | git-tree-sha1 = "28faf1c963ca1dc3ec87f166d92982e3c4a1f66d" 194 | uuid = "ae029012-a4dd-5104-9daa-d747884805df" 195 | version = "1.1.0" 196 | 197 | [[SHA]] 198 | uuid = "ea8e919c-243c-51af-8825-aaa63cd721ce" 199 | 200 | [[Serialization]] 201 | uuid = "9e88b42a-f829-5b0c-bbe9-9e923198166b" 202 | 203 | [[SharedArrays]] 204 | deps = ["Distributed", "Mmap", "Random", "Serialization"] 205 | uuid = "1a1011a3-84de-559e-8e89-a11a2f7dc383" 206 | 207 | [[Sockets]] 208 | uuid = "6462fe0b-24de-5631-8697-dd941f90decc" 209 | 210 | [[SparseArrays]] 211 | deps = ["LinearAlgebra", "Random"] 212 | uuid = "2f01184e-e22b-5df5-ae63-d93ebab69eaf" 213 | 214 | [[SpecialFunctions]] 215 | deps = ["OpenSpecFun_jll"] 216 | git-tree-sha1 = "d8d8b8a9f4119829410ecd706da4cc8594a1e020" 217 | uuid = "276daf66-3868-5448-9aa4-cd146d93841b" 218 | version = "0.10.3" 219 | 220 | [[StaticArrays]] 221 | deps = ["LinearAlgebra", "Random", "Statistics"] 222 | git-tree-sha1 = "016d1e1a00fabc556473b07161da3d39726ded35" 223 | uuid = "90137ffa-7385-5640-81b9-e52037218182" 224 | version = "0.12.4" 225 | 226 | [[Statistics]] 227 | deps = ["LinearAlgebra", "SparseArrays"] 228 | uuid = "10745b16-79ce-11e8-11f9-7d13ad32a3b2" 229 | 230 | [[TableTraits]] 231 | deps = ["IteratorInterfaceExtensions"] 232 | git-tree-sha1 = "b1ad568ba658d8cbb3b892ed5380a6f3e781a81e" 233 | uuid = "3783bdb8-4a98-5b6b-af9a-565f29a5fe9c" 234 | version = "1.0.0" 235 | 236 | [[Tables]] 237 | deps = ["DataAPI", "DataValueInterfaces", "IteratorInterfaceExtensions", "LinearAlgebra", "TableTraits", "Test"] 238 | git-tree-sha1 = "24a584cf65e2cfabdadc21694fb69d2e74c82b44" 239 | uuid = "bd369af6-aec1-5ad0-b16a-f7cc5008161c" 240 | version = "1.1.0" 241 | 242 | [[Test]] 243 | deps = ["Distributed", "InteractiveUtils", "Logging", "Random"] 244 | uuid = "8dfed614-e22c-5e08-85e1-65c5234f0b40" 245 | 246 | [[TranscodingStreams]] 247 | deps = ["Random", "Test"] 248 | git-tree-sha1 = "7c53c35547de1c5b9d46a4797cf6d8253807108c" 249 | uuid = "3bb67fe8-82b1-5028-8e26-92a6c54297fa" 250 | version = "0.9.5" 251 | 252 | [[UUIDs]] 253 | deps = ["Random", "SHA"] 254 | uuid = "cf7118a7-6976-5b1a-9a39-7adc72f591a4" 255 | 256 | [[UnPack]] 257 | git-tree-sha1 = "387c1f73762231e86e0c9c5443ce3b4a0a9a0c2b" 258 | uuid = "3a884ed6-31ef-47d7-9d2a-63182c4928ed" 259 | version = "1.0.2" 260 | 261 | [[Unicode]] 262 | uuid = "4ec0a83e-493e-50e2-b9ac-8f72acf5a8f5" 263 | 264 | [[Zlib_jll]] 265 | deps = ["Libdl", "Pkg"] 266 | git-tree-sha1 = "fdd89e5ab270ea0f2a0174bd9093e557d06d4bfa" 267 | uuid = "83775a58-1f1d-513f-b197-d71354ab007a" 268 | version = "1.2.11+16" 269 | -------------------------------------------------------------------------------- /examples/Wahbas_problem/NLLS_Wahba.jl: -------------------------------------------------------------------------------- 1 | # activate the virtual environment 2 | # cd(dirname(@__FILE__)) 3 | import Pkg; Pkg.activate(@__DIR__); Pkg.instantiate() 4 | 5 | using ForwardDiff, LinearAlgebra, Statistics, JLD2 6 | # using MATLAB 7 | 8 | function randq() 9 | """Random quaternion""" 10 | return normalize(randn(4)) 11 | end 12 | 13 | function so3noise(v,std) 14 | """Apply 3d gaussian noise on S03""" 15 | noise_phi = std*randn(3) 16 | return exp(skew(noise_phi))*v 17 | end 18 | 19 | function angle_error(q1,q2) 20 | """Get angle error between two attitudes""" 21 | q = qconj(q1) ⊙ q2 22 | v = q[2:4] 23 | s = q[1] 24 | normv = norm(v) 25 | θ = (2 * atan(normv, s)) 26 | return rad2deg(θ) 27 | end 28 | 29 | function p_from_q(q) 30 | """Rodgrigues parameter from quaternion""" 31 | s = q[1] 32 | v = q[2:4] 33 | return v/s 34 | end 35 | 36 | function q_from_p(p) 37 | """quaternion from rodrigues parameter""" 38 | return (1/(sqrt(1 + p'*p)))*[1;p] 39 | end 40 | 41 | function qconj(q) 42 | """quaternion conjugate""" 43 | s = q[1] 44 | v = q[2:4] 45 | return [s;-v] 46 | end 47 | 48 | function q_shorter(q) 49 | """Get the quaternion that represents θ < π """ 50 | if q[1]<0 51 | q = -q 52 | end 53 | return q 54 | end 55 | 56 | function dcm_from_q(q) 57 | """DCM from quaternion""" 58 | s = q[1] 59 | v = q[2:4] 60 | return I + 2*skew(v)*(s*I + skew(v)) 61 | end 62 | function q_from_phi(phi) 63 | """Quaternion from axis angle""" 64 | θ = norm(phi) 65 | r = phi/θ 66 | return [cos(θ/2);r*sin(θ/2)] 67 | end 68 | 69 | function ⊙(q1, q2) 70 | """Hamilton product, quaternion multiplication""" 71 | v1 = q1[2:4] 72 | s1 = q1[1] 73 | v2 = q2[2:4] 74 | s2 = q2[1] 75 | return [(s1 * s2 - dot(v1, v2)); (s1 * v2 + s2 * v1 + cross(v1, v2))] 76 | end 77 | 78 | function skew(v) 79 | """Skew-symmetric matrix from 3 element vector""" 80 | return [0 -v[3] v[2]; v[3] 0 -v[1]; -v[2] v[1] 0] 81 | end 82 | 83 | # function L(q) 84 | # qs = q[1] 85 | # qv = q[2:4] 86 | # return [qs -qv'; qv (qs*I + skew(qv))] 87 | # end 88 | # 89 | # 90 | # function R(q) 91 | # qs = q[1] 92 | # qv = q[2:4] 93 | # return [qs -qv'; qv (qs*I - skew(qv))] 94 | # end 95 | # 96 | # function H() 97 | # return [zeros(1,3);diagm(ones(3))] 98 | # end 99 | 100 | function G(q) 101 | """Quaternion to rodrigues parameter Jacobian""" 102 | s = q[1] 103 | v = q[2:4] 104 | return [-v'; (s*I + skew(v))] 105 | end 106 | 107 | function generate_data(std,N) 108 | """Generate the data for Wahba's problem""" 109 | # create random attitude 110 | ᴺqᴮ = randq() 111 | ᴺQᴮ = dcm_from_q(ᴺqᴮ) 112 | 113 | # equal weights 114 | w = ones(N)/N 115 | 116 | # create true and measured vectors 117 | Rn = fill(zeros(3),N) 118 | Rb = fill(zeros(3),N) 119 | for i = 1:N 120 | Rn[i] = normalize(randn(3)) 121 | Rb[i] = transpose(ᴺQᴮ)*so3noise(Rn[i],deg2rad(std)) 122 | end 123 | 124 | return w, Rn, Rb, ᴺqᴮ, ᴺQᴮ 125 | end 126 | function q_from_dcm(dcm) 127 | """Kane/Levinson convention, scalar first""" 128 | R = dcm 129 | T = R[1,1] + R[2,2] + R[3,3] 130 | if T> R[1,1] && T > R[2,2] && T>R[3,3] 131 | q4 = .5*sqrt(1+T) 132 | r = .25/q4 133 | q1 = (R[3,2] - R[2,3])*r 134 | q2 = (R[1,3] - R[3,1])*r 135 | q3 = (R[2,1] - R[1,2])*r 136 | elseif R[1,1]>R[2,2] && R[1,1]>R[3,3] 137 | q1 = .5*sqrt(1-T + 2*R[1,1]) 138 | r = .25/q1 139 | q4 = (R[3,2] - R[2,3])*r 140 | q2 = (R[1,2] + R[2,1])*r 141 | q3 = (R[1,3] + R[3,1])*r 142 | elseif R[2,2]>R[3,3] 143 | q2 = .5*sqrt(1-T + 2*R[2,2]) 144 | r = .25/q2 145 | q4 = (R[1,3] - R[3,1])*r 146 | q1 = (R[1,2] + R[2,1])*r 147 | q3 = (R[2,3] + R[3,2])*r 148 | else 149 | q3 = .5*sqrt(1-T + 2*R[3,3]) 150 | r = .25/q3 151 | q4 = (R[2,1] - R[1,2])*r 152 | q1 = (R[1,3] + R[3,1])*r 153 | q2 = (R[2,3] + R[3,2])*r 154 | end 155 | q = [q4;q1;q2;q3] 156 | return q 157 | end 158 | 159 | function svd_wahba(Rn,Rb,w) 160 | """Wahba's problem SVD solution""" 161 | 162 | # attitude profile matrix 163 | B = zeros(3,3) 164 | for i = 1:length(Rn) 165 | B = B + w[i]*Rb[i]*Rn[i]' 166 | end 167 | 168 | # solve WAHBAHs problem using SVD 169 | F = svd(B') 170 | 171 | # DCM for SVD solution 172 | Qsvd = F.U*diagm([1; 1; det(F.U)*det(F.V)])*F.V' 173 | 174 | # quaternion for SVD solution 175 | qsvd = q_from_dcm(Qsvd) 176 | 177 | return Qsvd, qsvd 178 | end 179 | 180 | function r_fx(q,Rn,Rb,w) 181 | """returns r where Wahba's cost function L is rᵀr""" 182 | 183 | # allocate in a fwd diff friendly way 184 | r = zeros(eltype(q),length(Rn)*3) 185 | 186 | # stack the residuals 187 | for i = 1:length(Rn) 188 | r[((i-1)*3 + 1):((i-1)*3 + 3)] = sqrt(w[i])*(Rn[i] - dcm_from_q(q)*Rb[i]) 189 | end 190 | 191 | return r 192 | end 193 | 194 | 195 | function S(q,Rn,Rb,w) 196 | """sum of squared residuals""" 197 | r = r_fx(q,Rn,Rb,w) 198 | return r'*r 199 | end 200 | 201 | function newton_wahba() 202 | """Solve Wahba's problem using a Gauss-Newton NLLS method""" 203 | 204 | # generate 4 vector measurements 205 | w, Rn, Rb, ᴺqᴮ, ᴺQᴮ = generate_data(5,20) 206 | 207 | # solve Wahba's problem SVD style 208 | Qsvd, qsvd = svd_wahba(Rn,Rb,w) 209 | 210 | # jacobian function for the residuals 211 | J_fx = q -> ForwardDiff.jacobian(q -> r_fx(q,Rn,Rb,w),q) 212 | 213 | # here we print the cost attained by the SVD solution 214 | SVD_cost = S(qsvd,Rn,Rb,w) 215 | 216 | # set initial guess to q 217 | q = qsvd ⊙ q_from_phi(deg2rad(10)*normalize(randn(3))) 218 | 219 | err_hist = NaN*ones(21) 220 | 221 | # Gauss-Newton 222 | for ii = 1:4 223 | 224 | # jacobian (we use G to get the rodrigues parameter version) 225 | J = J_fx(q)*G(q) 226 | 227 | # step direction 228 | v = -J\r_fx(q,Rn,Rb,w) 229 | 230 | # current cost value 231 | S_k = S(q,Rn,Rb,w) 232 | α = 1.0 233 | dS = 0.0 234 | S_new = 0.0 235 | 236 | # print first cost difference 237 | if ii==1 238 | err_hist[ii] = angle_error(q,qsvd) 239 | 240 | println("") 241 | println("Iteration: ",ii-1," α: ",α," ΔJ: ",S_k-SVD_cost) 242 | end 243 | 244 | # line search 245 | for jj = 1:20 246 | 247 | # take step (apply step multiplicatively) 248 | qnew = q ⊙ q_from_p(α*v) 249 | 250 | # get new cost value 251 | S_new = S(qnew,Rn,Rb,w) 252 | 253 | if S_new ForwardDiff.jacobian(q -> r_fx(q,Rn,Rb,w),q) 288 | 289 | # here we print the cost attained by the SVD solution 290 | SVD_cost = S(qsvd,Rn,Rb,w) 291 | 292 | # set initial guess to q from triad 293 | q = qsvd ⊙ q_from_phi(deg2rad(10)*normalize(randn(3))) 294 | 295 | err_hist = NaN*ones(21) 296 | 297 | # Gauss-Newton 298 | for ii = 1:4 299 | 300 | # jacobian (we use G to get the rodrigues parameter version) 301 | J = J_fx(q) 302 | 303 | # step direction 304 | v = -J\r_fx(q,Rn,Rb,w) 305 | 306 | # current cost value 307 | S_k = S(q,Rn,Rb,w) 308 | α = 1.0 309 | dS = 0.0 310 | S_new = 0.0 311 | 312 | # print first cost difference 313 | if ii==1 314 | err_hist[ii] = angle_error(q,qsvd) 315 | println("") 316 | println("Iteration: ",ii-1," α: ",α," ΔJ: ",S_k-SVD_cost) 317 | end 318 | 319 | # line search 320 | for jj = 1:200 321 | 322 | # take step (apply step multiplicatively) 323 | qnew = normalize(q + (α*v)) 324 | 325 | # get new cost value 326 | S_new = S(qnew,Rn,Rb,w) 327 | 328 | if S_new Symbol[], 21 | :errstate => Bool[], 22 | :time => Float64[], 23 | :iters => Int[], 24 | :cost => Vector{Float64}[], 25 | :c_max => Vector{Float64}[], 26 | ) 27 | function add_result!(name, errstate::Bool, solver) 28 | J0 = cost(solver) 29 | c0 = max_violation(solver) 30 | b = benchmark_solve!(solver) 31 | push!(results[:problem], name) 32 | push!(results[:errstate], errstate) 33 | push!(results[:time], median(b).time / 1e6) # ms 34 | push!(results[:iters], iterations(solver)) 35 | push!(results[:cost], [J0; solver.stats.cost]) 36 | push!(results[:c_max], [c0; solver.stats.c_max]) 37 | return b 38 | end 39 | results_path = joinpath(@__DIR__, "timing_results.jld2") 40 | 41 | """ 42 | Barrell Roll 43 | """ 44 | args = ( 45 | integration = RK4, 46 | termcon = :quatvec, 47 | # projected_newton = false, 48 | show_summary=true 49 | ) 50 | 51 | ## Original Method 52 | prob,opts = YakProblems(vecstate=true, costfun=:Quadratic; args...) 53 | solver = ALTROSolver(prob, opts) 54 | add_result!(:barrellroll, false, solver) 55 | 56 | ## Modified Method 57 | prob,opts = YakProblems(vecstate=false, costfun=:QuatLQR; args...) 58 | solver = ALTROSolver(prob, opts, show_summary=true) 59 | add_result!(:barrellroll, true, solver) 60 | 61 | @save results_path results 62 | 63 | """ 64 | Quadrotor Flip 65 | """ 66 | ## Original Method 67 | prob,opts = QuadFlipProblem(vecmodel=true, renorm=true; args...) 68 | solver = ALTROSolver(prob, opts, R_inf=1e-4, infeasible=true, static_bp=false) 69 | add_result!(:quadflip, false, solver) 70 | 71 | ## Modified Method 72 | prob,opts = QuadFlipProblem(vecmodel=false, renorm=true, costfun=QuatLQRCost; args...) 73 | solver = ALTROSolver(prob, opts, R_inf=1e-4, infeasible=true, static_bp=false) 74 | add_result!(:quadflip, true, solver) 75 | 76 | @save results_path results 77 | 78 | """ 79 | Flexible Satellite 80 | """ 81 | ## Original Method 82 | prob,opts = SatelliteKeepOutProblem(vecstate=true, costfun=LQRCost) 83 | solver = ALTROSolver(prob, opts) 84 | add_result!(:satellite, false, solver) 85 | 86 | ## Modified Method 87 | prob,opts = SatelliteKeepOutProblem(vecstate=false, costfun=QuatLQRCost; args...) 88 | solver = ALTROSolver(prob, opts) 89 | add_result!(:satellite, true, solver) 90 | 91 | @save results_path results 92 | 93 | 94 | ## Save the table 95 | import Pkg; Pkg.activate(@__DIR__); Pkg.instantiate() 96 | using LaTeXTabulars, JLD2, DataFrames, PGFPlotsX, Printf 97 | results_path = joinpath(@__DIR__, "timing_results.jld2") 98 | @load results_path results 99 | 100 | df = DataFrame(results) 101 | df.time .= round.(df.time, digits=2) 102 | df.prob = String.(df.problem) 103 | 104 | mod = df[df.errstate,:] 105 | org = df[.!df.errstate,:] 106 | tab = vcat(map(1:3) do i 107 | [mod.prob[i] @sprintf("%i / %i", org.iters[i], mod.iters[i]) @sprintf("%.2f / %.2f", org.time[i], mod.time[i])] 108 | end...) 109 | 110 | # tab[2,2] = "— / $(mod.iters[2])" # remove unsucessful quadrotor results 111 | # tab[2,3] = "— / $(mod.time[2])" 112 | latex_tabular(joinpath(@__DIR__,"..","paper/figures/timing_results.tex"), 113 | Tabular("lll"), 114 | [ 115 | Rule(:top), 116 | ["Problem", "Iterations", "time (ms)"], 117 | Rule(:mid), 118 | tab, 119 | Rule(:bottom) 120 | ] 121 | ) 122 | 123 | ## Save the Plot 124 | c_maxes = df[df.problem .== :barrellroll,:c_max] 125 | for c_max in c_maxes 126 | c_max[isinf.(c_max)] .= c_max[1] 127 | end 128 | p = @pgf Axis( 129 | { 130 | xlabel="iterations", 131 | ylabel="contraint satisfaction", 132 | "ymode=log", 133 | xmajorgrids, 134 | ymajorgrids, 135 | "legend style={at={(0.1,0.1)},anchor=south west}" 136 | }, 137 | Plot( 138 | { 139 | color="cyan", 140 | no_marks, 141 | "very thick" 142 | }, 143 | Coordinates(1:length(c_maxes[1]),c_maxes[1]) 144 | ), 145 | PlotInc( 146 | { 147 | color="orange", 148 | no_marks, 149 | "very thick" 150 | }, 151 | Coordinates(1:length(c_maxes[2]),c_maxes[2]) 152 | ), 153 | Legend("naive","modified") 154 | ) 155 | pgfsave(joinpath(@__DIR__, "..", "paper/figures/c_max_convergence.tikz"), p, include_preamble=false) 156 | 157 | 158 | ## Generate visualizations 159 | using TrajOptPlots, MeshCat, Colors 160 | vis = Visualizer() 161 | open(vis) 162 | 163 | # Barrell Roll 164 | prob,opts = YakProblems(vecstate=false, costfun=:QuatLQR; args...) 165 | solver = ALTROSolver(prob, opts, show_summary=true) 166 | solve!(solver) 167 | 168 | TrajOptPlots.set_mesh!(vis, prob.model) 169 | visualize!(vis, solver) 170 | TrajOptPlots.waypoints!(vis, solver, length=11) 171 | 172 | # Quadrotor Flip 173 | prob,opts = QuadFlipProblem(vecmodel=false, renorm=true, costfun=QuatLQRCost; args...) 174 | solver = ALTROSolver(prob, opts, R_inf=1e-4, infeasible=true, static_bp=false) 175 | solve!(solver) 176 | 177 | delete!(vis) 178 | TrajOptPlots.set_mesh!(vis, prob.model.model) 179 | visualize!(vis, prob.model.model, get_trajectory(solver)) 180 | TrajOptPlots.waypoints!(vis, prob.model.model, get_trajectory(solver), 181 | inds=[1,15,20,25,30,32,35,40,42,44,46,48,50,52,54,56,58,60,62,65,68,70,75,80,95,100], 182 | color=HSL(colorant"green"), color_end=HSL(colorant"red")) -------------------------------------------------------------------------------- /figs/quadflip.gif: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/RoboticExplorationLab/PlanningWithAttitude/14ec74ef9408946a758c87f5f15e48451b5e5d84/figs/quadflip.gif -------------------------------------------------------------------------------- /figs/quadflip.mp4: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/RoboticExplorationLab/PlanningWithAttitude/14ec74ef9408946a758c87f5f15e48451b5e5d84/figs/quadflip.mp4 -------------------------------------------------------------------------------- /figs/quadflip_modified.mp4: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/RoboticExplorationLab/PlanningWithAttitude/14ec74ef9408946a758c87f5f15e48451b5e5d84/figs/quadflip_modified.mp4 -------------------------------------------------------------------------------- /figs/quadflip_naive.gif: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/RoboticExplorationLab/PlanningWithAttitude/14ec74ef9408946a758c87f5f15e48451b5e5d84/figs/quadflip_naive.gif -------------------------------------------------------------------------------- /figs/quadflip_naive.mp4: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/RoboticExplorationLab/PlanningWithAttitude/14ec74ef9408946a758c87f5f15e48451b5e5d84/figs/quadflip_naive.mp4 -------------------------------------------------------------------------------- /flip_montecarlo.jdl2: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/RoboticExplorationLab/PlanningWithAttitude/14ec74ef9408946a758c87f5f15e48451b5e5d84/flip_montecarlo.jdl2 -------------------------------------------------------------------------------- /mlqr_basin_2000.jld2: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/RoboticExplorationLab/PlanningWithAttitude/14ec74ef9408946a758c87f5f15e48451b5e5d84/mlqr_basin_2000.jld2 -------------------------------------------------------------------------------- /mlqr_monte_carlo_2000.jld2: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/RoboticExplorationLab/PlanningWithAttitude/14ec74ef9408946a758c87f5f15e48451b5e5d84/mlqr_monte_carlo_2000.jld2 -------------------------------------------------------------------------------- /paper/.gitignore: -------------------------------------------------------------------------------- 1 | *.aux 2 | *.bbl 3 | *.bcf 4 | *.blg 5 | *.fdb_latexmk 6 | *.fls 7 | *.log 8 | *.run.xml 9 | *.synctex.gz 10 | *.out -------------------------------------------------------------------------------- /paper/figures/barrellroll.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/RoboticExplorationLab/PlanningWithAttitude/14ec74ef9408946a758c87f5f15e48451b5e5d84/paper/figures/barrellroll.png -------------------------------------------------------------------------------- /paper/figures/c_max_convergence.tikz: -------------------------------------------------------------------------------- 1 | \begin{tikzpicture} 2 | \begin{axis}[xlabel={iterations}, ylabel={contraint satisfaction}, ymode=log, xmajorgrids, ymajorgrids, legend style={at={(0.1,0.1)},anchor=south west}] 3 | \addplot[color={cyan}, no marks, very thick] 4 | coordinates { 5 | (1,0.9971560721275514) 6 | (2,0.0) 7 | (3,0.0) 8 | (4,0.0) 9 | (5,0.0) 10 | (6,0.0) 11 | (7,0.0) 12 | (8,0.0) 13 | (9,0.0) 14 | (10,0.0) 15 | (11,0.0) 16 | (12,0.0) 17 | (13,0.009795728288504169) 18 | (14,0.012965627076229946) 19 | (15,0.012965627076229946) 20 | (16,0.006297666614312414) 21 | (17,0.006297666614312414) 22 | (18,0.0012221985032168092) 23 | (19,0.001087828517986722) 24 | (20,0.0011820648730718197) 25 | (21,0.0011820648730718197) 26 | (22,0.0011820648730718197) 27 | (23,0.00014610389995117767) 28 | (24,1.1104879783785382e-7) 29 | } 30 | ; 31 | \addplot+[color={orange}, no marks, very thick] 32 | coordinates { 33 | (1,0.9971560721275514) 34 | (2,0.0) 35 | (3,0.0) 36 | (4,0.0) 37 | (5,0.0) 38 | (6,0.0) 39 | (7,0.0) 40 | (8,0.0) 41 | (9,0.0) 42 | (10,0.0) 43 | (11,0.0) 44 | (12,0.0) 45 | (13,0.0) 46 | (14,0.009130844853342257) 47 | (15,0.004453912954325734) 48 | (16,0.0010306461089480286) 49 | (17,0.000775574327870121) 50 | (18,9.430964198475777e-8) 51 | } 52 | ; 53 | \legend{{naive},{modified}} 54 | \end{axis} 55 | \end{tikzpicture} 56 | -------------------------------------------------------------------------------- /paper/figures/quadflip.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/RoboticExplorationLab/PlanningWithAttitude/14ec74ef9408946a758c87f5f15e48451b5e5d84/paper/figures/quadflip.png -------------------------------------------------------------------------------- /paper/figures/tangent_plane.tikz: -------------------------------------------------------------------------------- 1 | 2 | % Image credit: https://tex.stackexchange.com/questions/261408/sphere-tangent-to-plane 3 | \begin{tikzpicture}[ 4 | point/.style = {draw, circle, fill=black, inner sep=0.7pt}, 5 | ] 6 | \def\rad{2cm} 7 | \coordinate (O) at (0,0); 8 | \coordinate (N) at (0,\rad); 9 | \coordinate (W) at (1.25cm, \rad); 10 | 11 | \filldraw[ball color=white] (O) circle [radius=\rad]; 12 | \draw[dashed] 13 | (\rad,0) arc [start angle=0,end angle=180,x radius=\rad,y radius=5mm]; 14 | \draw 15 | (\rad,0) arc [start angle=0,end angle=-180,x radius=\rad,y radius=5mm]; 16 | \begin{scope}[xslant=0.5,yshift=\rad,xshift=-2] 17 | \filldraw[fill=gray!10,opacity=0.2] 18 | (-4,1) -- (3,1) -- (3,-1) -- (-4,-1) -- cycle; 19 | \node at (2,0.6) {$T$}; 20 | \end{scope} 21 | \draw[dashed] 22 | (N) node[above] {$q$} -- (O) node[below] {$O$}; 23 | \draw[->, thick] 24 | (N) -- (W) node[right] {$\delta q$}; 25 | \node[point] at (N) {}; 26 | \end{tikzpicture} -------------------------------------------------------------------------------- /paper/figures/timing_results.tex: -------------------------------------------------------------------------------- 1 | \begin{tabular}{lll} 2 | \toprule 3 | Problem & Iterations & time (ms) \\ 4 | \midrule 5 | barrellroll & 23 / 17 & 105.74 / 72.64 \\ 6 | quadflip & 31 / 25 & 505.59 / 433.59 \\ 7 | satellite & 16 / 17 & 170.98 / 263.10 \\ 8 | \bottomrule 9 | \end{tabular} 10 | -------------------------------------------------------------------------------- /paper/figures/wahba_convergence.tikz: -------------------------------------------------------------------------------- 1 | \begin{tikzpicture} 2 | \begin{axis}[xlabel={iterations}, ylabel={angle error (degrees)}, ymode=log, xmajorgrids, ymajorgrids, no markers, legend style={at={(0.1,0.1)},anchor=south west}] 3 | \addplot+[orange, ultra thick] 4 | coordinates { 5 | (1,10.0) 6 | (2,0.7678964995250577) 7 | (3,0.374300025506541) 8 | (4,0.3679627299910924) 9 | (5,0.3676273603961281) 10 | } 11 | ; 12 | \addplot+[orange, name path=A, line width=0.1pt, forget plot] 13 | coordinates { 14 | (1,10.000000000000005) 15 | (2,4.757112743094829) 16 | (3,3.2022627472300087) 17 | (4,3.07296845752766) 18 | (5,3.067476045883991) 19 | } 20 | ; 21 | \addplot+[orange, name path=B, line width=0.1pt, forget plot] 22 | coordinates { 23 | (1,9.999999999999986) 24 | (2,0.06380635255648336) 25 | (3,0.012967313656774965) 26 | (4,0.011255533610576535) 27 | (5,0.011122997621299442) 28 | } 29 | ; 30 | \addplot+[orange!10, forget plot] 31 | fill between [of=A and B]; 32 | ; 33 | \addplot+[cyan, ultra thick] 34 | coordinates { 35 | (1,10.0) 36 | (2,0.22639110461932632) 37 | (3,0.005209560034624235) 38 | (4,0.00015265929399267558) 39 | (5,4.93408997953017e-6) 40 | } 41 | ; 42 | \addplot+[cyan, solid, name path=C, line width=0.1pt, forget plot] 43 | coordinates { 44 | (1,10.000000000000012) 45 | (2,0.48612090632693833) 46 | (3,0.020255571615923933) 47 | (4,0.0009392553213255653) 48 | (5,4.283368523841055e-5) 49 | } 50 | ; 51 | \addplot+[cyan, solid, name path=D, line width=0.1pt, forget plot] 52 | coordinates { 53 | (1,9.999999999999988) 54 | (2,0.022877727290891384) 55 | (3,0.0005243109598923958) 56 | (4,2.4896868070253027e-6) 57 | (5,1.350442822177038e-8) 58 | } 59 | ; 60 | \addplot+[cyan!10, forget plot] 61 | fill between [of=C and D]; 62 | ; 63 | \legend{{naive},{modified}} 64 | \end{axis} 65 | \end{tikzpicture} 66 | -------------------------------------------------------------------------------- /paper/iros_abstract/iros_abstract.pdf: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/RoboticExplorationLab/PlanningWithAttitude/14ec74ef9408946a758c87f5f15e48451b5e5d84/paper/iros_abstract/iros_abstract.pdf -------------------------------------------------------------------------------- /paper/iros_abstract/iros_abstract.tex: -------------------------------------------------------------------------------- 1 | 2 | \documentclass[letterpaper, 10 pt, conference]{ieeeconf} % Comment this line out if you need a4paper 3 | \IEEEoverridecommandlockouts % This command is only needed if 4 | % you want to use the \thanks command 5 | \overrideIEEEmargins % Needed to meet printer requirements. 6 | 7 | 8 | % Bibliography 9 | \usepackage{biblatex} 10 | \addbibresource{references.bib} 11 | 12 | % Math 13 | \usepackage{physics} 14 | \usepackage{siunitx} 15 | \sisetup{output-exponent-marker=\ensuremath{\mathrm{e}}} 16 | \usepackage{amsmath} 17 | \usepackage{amsfonts} 18 | \usepackage{amssymb} 19 | 20 | % Optimization and Algorithms 21 | \usepackage{optidef} 22 | \usepackage{algorithmicx} 23 | \usepackage{algorithm,algpseudocode} 24 | 25 | % Formatting 26 | \usepackage{xcolor} 27 | \usepackage{bm} % for bold symbols 28 | \usepackage{booktabs} % better tables 29 | \usepackage{pifont} % for x mark 30 | \usepackage{graphicx} 31 | \usepackage{hyperref} 32 | 33 | % Plotting 34 | \usepackage{pgfplots} 35 | \pgfplotsset{compat=1.15, 36 | legend style={font=\footnotesize}, 37 | } 38 | \usepackage{tikzscale} 39 | 40 | % Custom commands 41 | \newcommand{\half}{\frac{1}{2}} 42 | \newcommand{\R}{\mathbb{R}} 43 | \newcommand{\Q}{\mathbb{S}^3} 44 | \newcommand{\skewmat}[1]{[#1]^\times} 45 | 46 | \newcommand{\rmap}{\varphi} 47 | \newcommand{\invrmap}{\varphi^{-1}} 48 | 49 | \newcommand{\dR}{\delta \mathcal{R}} 50 | \newcommand{\rot}{ \mathcal{R} } 51 | \newcommand{\dq}{\delta q} 52 | \newcommand{\q}{\textbf{q}} 53 | \newcommand{\eq}{_\text{eq}} 54 | \newcommand{\traj}[2][N]{#2_{0:{#1}}} 55 | \newcommand{\pass}{{\color{green} \checkmark}} 56 | \newcommand{\fail}{{\color{red} \ding{55}}} 57 | 58 | \newcommand{\todo}[1]{\textcolor{red}{TODO: #1}} 59 | 60 | 61 | \title{\LARGE \bf 62 | Planning with Attitude 63 | } 64 | 65 | \author{Brian Jackson$^1$ and Zachary Manchester$^1$% 66 | \thanks{ 67 | $^1$Robotics Institute, 68 | Carnegie Mellon University, 69 | 5000 Forbes Ave, Pittsburgh, PA, USA 70 | } 71 | } 72 | 73 | \begin{document} 74 | \maketitle 75 | 76 | % Planning and controlling trajectories for floating-base robotic systems that 77 | % experience large attitude changes is challenging due to the nontrivial group 78 | % structure of 3D rotations. This paper introduces a powerful, computationally 79 | % efficient, and accessible approach for adapting existing Newton and 80 | % gradient-based methods to correctly account for the group structure of 81 | % rotations. We demonstrate the effectiveness of the approach by modifying the 82 | % ALTRO solver to optimize a loop-de-loop trajectory for a quadrotor, enabling 83 | % the solver to find a dramatically better solution in less than a third the 84 | % time. 85 | 86 | \section{INTRODUCTION} 87 | 88 | Many robotic systems---including quadrotors, airplanes, satellites, 89 | underwater vehicles, and quadrupeds---can perform arbitrarily large three-dimensional 90 | translations and rotations as part of their normal operation. While simply 91 | representing the attitude of these sytems is nontrivial, generating and tracking 92 | motion plans for floating-base systems is an even more challenging problem. 93 | 94 | Many approaches have been taken to address the problem of motion planning and control 95 | on the space of rigid body motions \cite{Kobilarov2011, Saccon2013, 96 | watterson2020control}; most of these rely heavily on ideas and notation from 97 | differential geometry. Despite some impressive results, many of these ideas have not 98 | been widely adopted by the robotics community, and many practitioners continue to 99 | ignore the group structure of rotations. One issue preventing wider adoption is that 100 | accounting for this structure requires low-level changes to the underlying 101 | optimization algorithms, which is difficult or impossible when relying on existing 102 | off-the-shelf solvers. 103 | 104 | % \todo{I'm worried that SE(3) and SO(3) results are getting confused/conflated here. I 105 | % would explicitly talk about both, emphasize that we're handling SO(3) with the 106 | % quaternion stuff, and that $SE(3) = SO(3) \times \mathbb{R}^3$ is a straightforward 107 | % extension.} 108 | 109 | To address this lack of solver support, we formulate a straightforward, generic 110 | method for adapting existing Newton and gradient-based algorithms to correctly 111 | account for the group structure of rotations. Based entirely on basic mathematical 112 | tools from linear algebra and calculus, our method is computationally efficient and 113 | should be both accessible and familiar to most roboticists. We apply this method to 114 | the ALTRO solver \cite{howell2019altro} and demonstrate its performance on a 115 | constrained trajectory optimization problem. 116 | 117 | \section{Notation and Convensions} 118 | 119 | We begin by defining some useful conventions and notation. 120 | Attitude is defined as the rotation from the robot's body frame to a global inertial 121 | frame. 122 | We also define gradients to be row vectors, that is, for 123 | $f(x) : \R^n \to \R$, $\pdv{f}{x} \in \R^{1\times n}$. 124 | 125 | We represent---following the Hamilton convention---a quaternion $\q \in \mathbb{H}$ 126 | as a standard vector $q \in \R^4 := [q_s \;\; q_v^T]^T$ where $q_s \in \R$ and $q_v 127 | \in \R^3$ are the scalar and vector part of the quaternion, respectively. We denote 128 | the composition of two quaternions as $\q_3 = \q_2 \otimes \q_1$. We use 129 | $\skewmat{x}$ to denote the skew-symmetric matrix such that $\skewmat{x} y = x \times 130 | y$. 131 | 132 | \begin{figure} 133 | \centering 134 | \includegraphics[width=\columnwidth]{quadflip.png} 135 | \label{fig:quadflip} 136 | \caption{Snapshots of quadrotor flip trajectory solved using a modified version 137 | of ALTRO that correctly accounts for the group structure of rotations. These 138 | modifications make it easier to find trajectories that undergo large changes in 139 | attitude, such as the loop-de-loop shown here.} 140 | \end{figure} 141 | 142 | \section{Quaternion Differential Calculus} \label{sec:Quaternion_Calculus} 143 | Most modern methods for planning and control require derivatives with respect to the 144 | state vector. We present a simple but powerful method for taking derivatives of 145 | quaternion-valued state vectors. The extension of this method to more general state 146 | vectors that contain Cartesian products of $SO(3)$, $SE(3)$, and $\mathbb{R}^n$ is 147 | straightforward. 148 | 149 | The key idea of this section is that differential quaternions live in 150 | three-dimensional space instead of the four-dimensional space of quaternions; this 151 | idea should match our intuition given rotations are inherently three-dimensional and 152 | differential rotations should live in the same space as angular velocities, i.e. 153 | $\R^3$. 154 | 155 | % \begin{figure} 156 | % \centering 157 | % \includegraphics[height=5cm]{figures/tangent_plane.tikz} 158 | % \caption{ 159 | % When linearizing about a point $q$ on an sphere $\mathbb{S}^{n-1}$ in 160 | % n-dimensional space, the tangent space $T$ is a hyper-plane $\R^{n-1}$, 161 | % illustrated here with $n=3$. Therefore, when linearizing about a unit 162 | % quaternion $q \in \Q$, the space of differential rotations lives in $\R^3$. 163 | % } 164 | % \label{fig:tangent_plane} 165 | % \end{figure} 166 | In practice, we have found Rodrigues Parameters to be a very effective representation 167 | for three-dimensional differential rotations since they are computionally efficient 168 | and do not inherit the sign ambiguity of quaternions. 169 | 170 | The mapping between a vector of Rodrigues parameters $\phi \in \R^3$ and a unit 171 | quaternion $q$ is known as the Cayley map: 172 | \begin{equation} \label{eq:cayley} 173 | q = \varphi(\phi) = \frac{1}{\sqrt{1 + \norm{\phi}^2}} \begin{bmatrix} 1 \\ \phi \end{bmatrix}. 174 | \end{equation} 175 | We will also make use of the inverse Cayley map: 176 | \begin{equation} \label{eq:invcayley} 177 | \phi = \varphi^{-1}(q) = \frac{q_v}{q_s}. 178 | \end{equation} 179 | 180 | \subsection{Jacobian of Vector-Valued Functions} 181 | When taking derivatives with respect to quaternions, we must take into account 182 | both the composition rule and the nonlinear mapping between the space of unit 183 | quaternions and our chosen three-parameter error representation. 184 | 185 | Let $\phi \in \R^3$ be a differential rotation applied to a function with 186 | unit quaternion inputs $y = h(q): \Q \to \R^p$, such that 187 | \begin{equation} \label{eq:vector_function} 188 | y + \delta y = h(q \otimes \varphi(\phi)) \approx h(q) + \nabla h(q) \phi. 189 | \end{equation} 190 | We can calculate the Jacobian $\nabla h(q) \in \R^{p \times 3}$ by 191 | differentiating \eqref{eq:vector_function} with respect to $\phi$, evaluated at 192 | $\phi = 0$: 193 | \begin{equation} \label{eq:quat_gradient} 194 | \nabla h(q) = \pdv{h}{q} \begin{bmatrix} 195 | -q_v^T \\ 196 | q_s I_3 + \skewmat{q_v} 197 | \end{bmatrix} 198 | := \pdv{h}{q} G(q) 199 | \end{equation} 200 | where $G(q) \in \R^{4 \times 3}$ is referred to as the \textit{attitude Jacobian}, which 201 | essentially becomes a ``conversion factor'' allowing us to apply results from 202 | standard vector calculus to the space of unit quaternions. This form is 203 | particularly useful in practice since $\pdv*{h}{q} \in \R^{p \times 4}$ can be 204 | obtained using finite difference or automatic differentiation. 205 | As an aside, although we have used Rodrigues parameters, $G(q)$ is actually the 206 | same (up to a constant scaling factor) for any choice of three-parameter attitude 207 | representation. 208 | 209 | \subsection{Other derivatives} 210 | Using similar techniques, we find useful expressions for the Hessian of a 211 | scalar-valued function (i.e. $p = 1$): 212 | \begin{equation} \label{eq:quat_hessian} 213 | \nabla^2 h(q) = G(q)^T \pdv[2]{h}{q} G(q) + I_3 \pdv{h}{q}q, 214 | \end{equation} 215 | and the Jacobian of a quaternion-valued function $q' = f(q) : \Q \to \Q$: 216 | \begin{equation} \label{eq:quat_jacobian} 217 | \nabla f(q) = G(q')^T \pdv{f}{q} G(q). 218 | \end{equation} 219 | 220 | 221 | \section{Fast Constrained Trajectory Optimization with Attitude States} 222 | \subsection{Algorithmic Modifications} 223 | Here we briefly outline the changes to the ALTRO solver \cite{howell2019altro} to 224 | efficiently solve trajectory optimization problems for systems with 3D rotations in the 225 | state vector. 226 | 227 | The linearization of the nonlinear discrete dynamics function $x_{k+1} = f(x_k,u_k)$ 228 | is ``corrected'' using \ref{eq:quat_jacobian}: \begin{equation} A = E(f(x,u))^T 229 | \pdv{f}{x} E(x); \quad B = E(f(x,u))^T \pdv{f}{u}. \end{equation} Here we define the 230 | \textit{state attitude Jacobian} $E(x)$ to be a block-diagonal matrix where the block 231 | is an identity matrix for any vector-valued state and $G(q)$ for any quaternion in 232 | the state vector. Using \eqref{eq:quat_gradient}, \eqref{eq:quat_hessian}, 233 | \eqref{eq:quat_jacobian} we can derive similar modifications to the expansions of the 234 | cost and constraint functions. We refer the reader to the original ALTRO paper 235 | \cite{howell2019altro} for futher details on the algorithm. 236 | 237 | These adjustments effectively modify the optimization so that it is performed on 238 | differential rotations (or Rodrigues Parameters) instead of the space of unit 239 | quaternions. In practice, this is analagous to the Multiplicative Extended Kalman 240 | Filter \cite{markley2014fundamentals} in the state estimation community. 241 | 242 | The iLQR algorithm derives a locally-optimal feedback policy of the form $\delta u = 243 | K \delta x + d$ during the ``backward pass'' of each iteration, which is then used to 244 | simulate the system forward during the ``forward pass''. Instead of computing $\delta 245 | x$ using simple substraction as we would in the original algorithm, we now compute the 246 | error using the inverse Cayley map \eqref{eq:invcayley} for the quaternion states. The 247 | rest of the forward pass is unmodified. 248 | 249 | \subsection{Examples} 250 | To demonstrate the effectiveness of the proposed approach, we tested the modified 251 | version of ALTRO by solving for a quadrotor flip trajectory (see Figure \ref{fig:quadflip}). 252 | Four soft ``waypoint'' costs were placed along the trajectory, encouraging the 253 | quadrotor to follow a loop. The trajectory was initialized with hover controls and a 254 | state trajectory that smoothly rotated the quadrotor one full rotation about the 255 | x-axis while moving to the goal. 256 | The modified version of ALTRO solved the problem in 140 ms and 22 iterations, while 257 | the original version that na\"ively treated the quaternion as a vector in $\R^4$ 258 | solved in 420 ms and 58 iterations. The original version failed to provide a nice, 259 | smooth loop and instead reversed direction halfway through the loop. 260 | 261 | It's also worthwhile to note that this problem cannot be solved as-is with any 262 | three-parameter representation because of the singularities that occur at 90, 180, 263 | and 360 degrees for Euler angles, Rodrigues parameters, and MRP's, respectively. 264 | % A classic approach to overcome this issue is to do discrete switching of the 265 | % reference orientation, which requires previous knowledge of the topology of the 266 | % optimal solution. 267 | % \begin{figure} 268 | % \centering 269 | % \includegraphics[height=4cm, width=8cm]{figures/timing_chart.tikz} 270 | % \caption{Timing results for the airplane barrell roll, quadrotor flip, and 271 | % quadrotor zig-zag examples using iLQR with roll-pitch-yaw Euler angles (RPY), 272 | % quaternions, and the current method. The timing result for the Quadrotor flip 273 | % with Euler angles is omitted since it failed to converge. 274 | % } 275 | % \label{fig:timing_chart} 276 | % \end{figure} 277 | 278 | % \todo{It would be great to compare to both the minimal iLQR stuff and SNOPT or IPOPT with quaternion norm constraints + slack variables (referred to as the ``embedding approach'').} 279 | 280 | \section{Conclusion} 281 | 282 | We have proposed a general, accessible, and computationally efficient approach to 283 | doing planning and control for systems with one or more unit quaternions in their 284 | state vectors. The approach allows for straightforward adaptation of many 285 | gradient and Newton-based methods for optimal control and motion planning, which we demonstrated 286 | on the ALTRO solver. By exploiting the unique structure of both the trajectory 287 | optimization problem and the rotation group, the newly modified version of ALTRO will 288 | likely be able to solve more challenging problems with performance. 289 | Future work will extend these ideas to Newton-based methods such as direct 290 | collocation and other domains outside of trajectory optimization. 291 | 292 | 293 | \printbibliography 294 | 295 | \end{document} -------------------------------------------------------------------------------- /paper/iros_abstract/quadflip.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/RoboticExplorationLab/PlanningWithAttitude/14ec74ef9408946a758c87f5f15e48451b5e5d84/paper/iros_abstract/quadflip.png -------------------------------------------------------------------------------- /paper/main.pdf: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/RoboticExplorationLab/PlanningWithAttitude/14ec74ef9408946a758c87f5f15e48451b5e5d84/paper/main.pdf -------------------------------------------------------------------------------- /paper/main.tex: -------------------------------------------------------------------------------- 1 | % \documentclass[letterpaper, 10 pt, conference]{ieeeconf} % Comment this line out if you need a4paper 2 | \documentclass[letterpaper, 10 pt, journal, twoside]{ieeetran} 3 | % \IEEEoverridecommandlockouts % This command is only needed if 4 | % you want to use the \thanks command 5 | % \overrideIEEEmargins % Needed to meet printer requirements. 6 | % \usepackage[top=60pt, left=48pt, right=48pt, bottom=43pt]{geometry} 7 | 8 | 9 | % Bibliography 10 | \usepackage[style=ieee]{biblatex} 11 | \addbibresource{references.bib} 12 | 13 | % Math 14 | \usepackage{physics} 15 | \usepackage{siunitx} 16 | \sisetup{output-exponent-marker=\ensuremath{\mathrm{e}}} 17 | \usepackage{amsmath} 18 | \usepackage{amsfonts} 19 | \usepackage{amssymb} 20 | \usepackage{gensymb} 21 | \usepackage{tensor} 22 | 23 | % Optimization and Algorithms 24 | \usepackage{optidef} 25 | \usepackage{algorithmicx} 26 | \usepackage{algorithm,algpseudocode} 27 | 28 | % Formatting 29 | \usepackage{xcolor} 30 | \usepackage{bm} % for bold symbols 31 | \usepackage{booktabs} % better tables 32 | \usepackage{pifont} % for x mark 33 | \usepackage{graphicx} 34 | \usepackage{hyperref} 35 | 36 | % Plotting 37 | \usepackage{pgfplots} 38 | \pgfplotsset{compat=1.15, 39 | legend style={font=\footnotesize}, 40 | } 41 | \usepackage{tikzscale} 42 | \usepgfplotslibrary{fillbetween} 43 | % \pgfplotsset{every axis plot post/.style={ 44 | % very thick, 45 | % } 46 | % } 47 | 48 | % Custom commands 49 | \newcommand{\half}{\frac{1}{2}} 50 | \newcommand{\R}{\mathbb{R}} 51 | \newcommand{\Q}{\mathbb{S}^3} 52 | \newcommand{\skewmat}[1]{[#1]^\times} 53 | 54 | \newcommand{\rmap}{\varphi} 55 | \newcommand{\invrmap}{\varphi^{-1}} 56 | 57 | \DeclareMathOperator{\sign}{sign} 58 | 59 | \newcommand{\dR}{\delta \mathcal{R}} 60 | \newcommand{\rot}{ \mathcal{R} } 61 | \newcommand{\dq}{\delta q} 62 | \newcommand{\q}{\textbf{q}} 63 | \newcommand{\eq}{_\text{eq}} 64 | \newcommand{\traj}[2][N]{#2_{0:{#1}}} 65 | \newcommand{\pass}{{\color{green} \checkmark}} 66 | \newcommand{\fail}{{\color{red} \ding{55}}} 67 | \newcommand{\inframe}[2]{{}^{#1}\!#2} 68 | \newcommand{\toframe}[3]{\inframe{#1}{#3}^{#2}} 69 | 70 | \newcommand{\todo}[1]{\textcolor{red}{TODO: #1}} 71 | \newcommand{\added}[1]{\textcolor{black}{#1}} 72 | 73 | % Header, Title, etc. 74 | \markboth{IEEE Robotics and Automation Letters. Preprint Version. Accepted Dec, 2020} 75 | {Jackson \MakeLowercase{et al.}: Planning with Attitude} 76 | 77 | \title{\LARGE \bf 78 | Planning with Attitude 79 | } 80 | 81 | % Make room for more info lines in the \author command 82 | \author{Brian E Jackson$^{1}$, Kevin Tracy$^{1}$, and Zachary Manchester$^{1}$% 83 | \thanks{Manuscript received: October, 15, 2020; Revised January 2, 2021; Accepted January 4, 2021.}%Use only for final RAL version 84 | \thanks{This paper was recommended for publication by Editor Nancy Amato upon evaluation of the Associate Editor and Reviewers' comments. 85 | This work was supported by a NASA Early Career Faculty Award, NASA JPL, and NSF GFRP Grant No. DGE-1656518.} %Use only for final RAL version 86 | \thanks{$^{1}$All authors are with the Robotics Institute, Carnegie Mellon University, Pittsburgh, PA, USA 87 | {\tt\footnotesize brianjackson@cmu.edu}}% 88 | \thanks{Digital Object Identifier (DOI): see top of this page.} 89 | } 90 | 91 | 92 | \begin{document} 93 | \maketitle 94 | 95 | \begin{abstract} 96 | Planning trajectories for floating-base robotic systems that experience large 97 | attitude changes is challenging due to the nontrivial group structure of 3D 98 | rotations. This paper introduces a powerful and accessible approach for 99 | optimization-based planning on the space of rotations using only standard 100 | linear algebra and vector calculus. We demonstrate the effectiveness of the 101 | approach by adapting Newton's method to solve the canonical Wahba's problem, 102 | and modify the trajectory optimization solver ALTRO to plan directly on the 103 | space of unit quaternions, achieving superior convergence on problems 104 | involving significant changes in attitude. 105 | %\keywords{trajectory optimization, motion planning, control, quaternions} 106 | 107 | \end{abstract} 108 | 109 | \begin{IEEEkeywords} 110 | Motion and Path Planning, Optimization and Optimal Control, Computational Geometry, Underactuated Robots, Motion Control, 111 | \end{IEEEkeywords} 112 | 113 | \section{INTRODUCTION} 114 | 115 | \IEEEPARstart{M}{any} robotic systems---including quadrotors, airplanes, satellites, autonomous 116 | underwater vehicles, and quadrupeds---can perform arbitrarily large three-dimensional 117 | translations and rotations as part of their normal operation. While representing 118 | translations is straightforward and intuitive, effectively representing the 119 | nontrivial group structure of 3D rotations has been a topic of study for many 120 | decades. Although we can intuitively deduce that rotations are three-dimensional, a 121 | globally non-singular three-parameter representation of the space of rotations does 122 | not exist \cite{stuelpnagel1964parametrization}. As a result, when parameterizing 123 | rotations, we must either a) choose a three-parameter representation and deal with 124 | singularities and discontinuities, or b) choose a higher-dimensional representation and deal with 125 | constraints between the parameters. While simply representing attitude is nontrivial, 126 | generating and tracking motion plans for floating-base systems is an even more 127 | challenging problem. 128 | 129 | Early work on control problems involving the rotation group dates back to the 1970s, 130 | with extensions of linear control theory to spheres \cite{Brockett1973} and $SO(3)$ 131 | \cite{Baillieul1978}. Effective attitude tracking controllers have been developed for 132 | satellites \cite{wie1985quaternion}, quadrotors 133 | \cite{Fresk2013,Liu2015,lee2010geometric, 134 | Johnson2005,watterson2020control,mellinger2011minimum}, and a 3D inverted pendulum 135 | \cite{Chaturvedi2009} using various methods for calculating three-parameter attitude 136 | errors. 137 | 138 | More recently, these ideas have been extended to trajectory generation 139 | \cite{Zefran1998}, sample-based motion planning \cite{Zefran1999,Kuffner2004}, and 140 | optimal control. Approaches to optimal control with attitude states include 141 | analytical methods applied to satellites \cite{Spindler1998}, discrete mechanics 142 | \cite{Kobilarov2011,Kobilarov2014, Lee2008}, a combination of sampling-based planning 143 | and constrained trajectory optimization for satellite formations \cite{Garcia2005, 144 | Aoude2008}, projection operators \cite{Saccon2013}, or more general theory for 145 | optimization on manifolds \cites{watterson2018trajectory}. Nearly all of these 146 | methods rely heavily on principles from differential geometry and Lie group theory; 147 | however, despite these works, many recent papers in the robotics community continue 148 | to naively apply standard methods for motion planning and control with no regard for the 149 | group structure of rigid body motion. 150 | %\cite{Alothman2016,deCrousaz2015, Williams2017,Geisert2016} 151 | 152 | In this paper, we make a departure from previous approaches to geometric planning and 153 | control that rely heavily on ideas and notation from differential geometry, and 154 | instead use only basic mathematical tools from linear algebra and vector calculus 155 | that should be familiar to most roboticists. In Sec. \ref{sec:Quaternion_Calculus} we 156 | introduce an approach to quaternion differential calculus similar to 157 | \cite{Mandic2011,Xu2016}, but significantly simpler and more general, enabling 158 | straight-forward adaptation of existing algorithms to systems with quaternion states. 159 | For concreteness, we then apply our method to the canonical Wahba's 160 | problem~\cite{Wahba1965} in Sec. \ref{sec:Wahbas}, and demonstrate superior 161 | convergence to approaches that fail to properly account for the group structure. In 162 | Sec. \ref{sec:trajopt} we extend these ideas to the problem of trajectory 163 | optimization, and detail modifications to ALTRO, a state-of-the-art constrained 164 | trajectory optimization solver, and demonstrate performance gains on several 165 | benchmark problems. With the modifications presented in this paper, ALTRO explicitly 166 | leverages both the structure of the trajectory optimization problem as well as the 167 | group structure of 3D rotations, making it uniquely well-suited to solving challenging 168 | problems with near real-time performance. 169 | 170 | %To our knowledge, there does not currently exist a solver that is capable of leveraging the unique Markovian structure of the fixed-horizon trajectory optimization problem while correctly accounting for the group structure of 3D rotations. 171 | In summary, our contributions include: 172 | 173 | \begin{itemize} 174 | \item A unified approach to quaternion differential calculus entirely based on 175 | standard linear algebra and vector calculus. 176 | \item Derivation of a Newton-based algorithm for nonlinear optimization directly 177 | on the space of unit quaternions. 178 | \item Implementation of a fast and efficient solver for trajectory optimization 179 | problems with attitude dynamics and nonlinear constraints that correctly accounts 180 | for the group structure of 3D rotations. 181 | \end{itemize} 182 | 183 | 184 | \section{Background} 185 | 186 | We begin by defining some useful conventions and notation. 187 | Attitude is defined as the rotation from the robot's body frame to the world frame. 188 | We also define gradients to be row vectors, that is, for 189 | $f(x) : \R^n \to \R$, $\pdv{f}{x} \in \R^{1\times n}$. 190 | 191 | \subsection{Unit Quaternions} \label{sec:quaternions} 192 | We leverage the fact that quaternions are linear operators and that the space of 193 | quaternions $\mathbb{H}$ is isomorphic to $\R^4$ to explicitly 194 | represent---following the Hamilton convention---a quaternion $\q \in \mathbb{H}$ 195 | as a standard vector $q \in \R^4 := [q_s \;\; q_v^T]^T$ where $q_s \in \R$ and 196 | $q_v \in \R^3$ are referred to as the scalar and vector parts of the quaternion, 197 | respectively. The space of unit quaternions, $\Q = \{q : \norm{q}_2 = 1\}$, is a 198 | double-cover of the rotation group $SO(3)$, since $q$ and $-q$ represent the same 199 | rotation \cite{markley2014fundamentals}. 200 | 201 | Quaternion multiplication is defined as 202 | \begin{equation} \label{eq:quat_mult} 203 | \q_2 \otimes \q_1 = L(q_2) q_1 = R(q_1) q_2 204 | \end{equation} 205 | where $L(q)$ and $R(q)$ are orthonormal matrices defined as 206 | \begin{align} 207 | L(q) &:= \begin{bmatrix} 208 | q_s \;\; & -q_v^T \\ 209 | q_v \;\; & q_s I + \skewmat{q_v} 210 | \end{bmatrix} 211 | \label{eq:Lmult} \\ 212 | %= \begin{bmatrix} q & G(q) \end{bmatrix} \label{eq:Lmult} \\ 213 | R(q) &:=\begin{bmatrix} 214 | q_s \;\; & -q_v^T \\ 215 | q_v \;\; & q_s I - \skewmat{q_v} 216 | \end{bmatrix} \label{eq:Rmult}, 217 | \end{align} 218 | and $\skewmat{x}$ is the skew-symmetric matrix operator 219 | \begin{equation} 220 | \skewmat{x} := \begin{bmatrix} 221 | 0 & -x_3 & x_2 \\ 222 | x_3 & 0 & -x_1\\ 223 | -x_2 & x_1 & 0 224 | \end{bmatrix}. 225 | \end{equation} 226 | 227 | The inverse of a unit quaternion $\q^{-1}$, giving the opposite rotation, is equal 228 | to its conjugate $\q^*$, which is simply the same quaternion with a negated vector 229 | part: 230 | \begin{equation} \label{eq:T} 231 | \q^* = T q := \begin{bmatrix} 232 | 1 & \\ 233 | & -I_3 234 | \end{bmatrix} q. 235 | \end{equation} 236 | The following identities, which are easily derived from 237 | \eqref{eq:Lmult}--\eqref{eq:T}, are extremely useful: 238 | \begin{align} 239 | &L(Tq) = L(q)^T = L(q)^{-1} \\ 240 | &R(Tq) = R(q)^T = R(q)^{-1} . 241 | \end{align} 242 | 243 | We will sometimes find it helpful to create a quaternion with zero scalar part from 244 | a vector $r \in \R^3$. We denote this operation as, 245 | \begin{equation} 246 | \hat{r} = H r \equiv \begin{bmatrix} 0 \\ I_3 \end{bmatrix} r. 247 | \end{equation} 248 | Unit quaternions rotate a vector through the operation 249 | $\hat{r}' = \q \otimes \hat{r} \otimes \q^*$. 250 | This can be equivalently expressed using matrix multiplication as 251 | \begin{align} 252 | r' &= H^T L(q) R(q)^T H r = A(q)r , \label{eq:quaternion_rotation} 253 | \end{align} 254 | where $A(q)$ is the rotation matrix in terms of the elements of the quaternion 255 | \cite{kane1983spacecraftdynamics}. 256 | 257 | \subsection{Rigid Body Dynamics} \label{sec:rigidbody_dynamics} 258 | For clarity, we will restrict our attention to rigid bodies moving freely in 3D 259 | space. That is, we consider systems with dynamics of the following form: 260 | \begin{equation} \label{eq:rigid_body_dynamics} 261 | x = \begin{bmatrix} r \\ q \\ v \\ \omega \end{bmatrix}, \quad 262 | \dot{x} = \begin{bmatrix} 263 | v \\ 264 | \half \q \otimes \hat{\omega} = \half L(q) H \omega \\ 265 | \frac{1}{m} \inframe{W}{F(x,u)} \\ 266 | J^{-1}(\inframe{B}{\tau(x,u)} - \omega \times J \omega) 267 | \end{bmatrix} 268 | \end{equation} 269 | where $x$ and $u$ are the state and control vectors, $r \in \R^3$ is the 270 | position, $\q \in \Q$ is the attitude, $v \in \R^3$ is the linear velocity, and 271 | $\omega \in \R^3$ is the angular velocity. $m \in \R$ is the mass, $J \in 272 | \R^{3\times3}$ is the inertia matrix, $\inframe{W}{F(x,u)} \in \R^3$ are the 273 | forces in the world frame, and $\inframe{B}{\tau(x,u)}$ are the moments in the 274 | body frame. 275 | 276 | \section{Quaternion Differential Calculus} \label{sec:Quaternion_Calculus} 277 | We now present a simple but powerful method for taking derivatives of functions 278 | involving quaternions based on the notation and linear algebraic operations outlined 279 | in Sec. \ref{sec:quaternions}. 280 | 281 | \begin{figure} 282 | \centering 283 | \includegraphics[height=5cm]{figures/tangent_plane.tikz} 284 | \caption{ 285 | When linearizing about a point $q$ on an sphere $\mathbb{S}^{n-1}$ in 286 | n-dimensional space, the tangent space $T$ is a plane living in $\R^{n-1}$, 287 | illustrated here with $n=3$. Therefore, when linearizing about a unit 288 | quaternion $q \in \Q$, the space of differential rotations lives in $\R^3$. 289 | } 290 | \label{fig:tangent_plane} 291 | \end{figure} 292 | 293 | Derivatives consider the effect an infinitesimal perturbation to the input has on 294 | an infinitesimal perturbation to the output. For vector spaces, the composition 295 | of the perturbation with the nominal value is simple addition and the 296 | infinitesimal perturbation lives in the same space as the original vector. For 297 | unit quaternions, however, neither of these are true; instead, they compose 298 | according to \eqref{eq:quat_mult}, and infinitesimal unit quaternions are (to 299 | first order) confined to a 3-dimensional plane tangent to $\Q$ (see Fig. 300 | \ref{fig:tangent_plane}). 301 | 302 | The fact that differential unit quaternions are three-dimensional should make 303 | intuitive sense: Rotations are inherently three-dimensional and differential 304 | rotations should live in the same space as angular velocities, i.e. $\R^3$. 305 | 306 | There are many possible three-parameter representations for small rotations in 307 | the literature. Many authors use the exponential map \cite{Baillieul1978, 308 | Zefran1998, Lee2008, Saccon2013, Sola2017, Fan2016, watterson2018trajectory}, 309 | while others have used the Cayley map (also known as Rodrigues parameters) 310 | \cite{Kobilarov2011, Kobilarov2014}, Modified Rodrigues Parameters (MRPs) 311 | \cite{Terzakis2018}, or the vector part of the quaternion \cite{Fresk2013}. 312 | We choose Rodrigues parameters \cite{markley2014fundamentals} because they are 313 | computationally efficient and do not inherit the sign ambiguity associated with 314 | unit quaternions. The mapping between a vector of Rodrigues parameters $\phi \in 315 | \R^3$ and a unit quaternion $q$ is known as the Cayley map: \begin{equation} 316 | \label{eq:cayley} 317 | q = \varphi(\phi) = \frac{1}{\sqrt{1 + \norm{\phi}^2}} \begin{bmatrix} 1 \\ \phi \end{bmatrix}. 318 | \end{equation} 319 | We will also make use of the inverse Cayley map: 320 | \begin{equation} 321 | \phi = \varphi^{-1}(q) = \frac{q_v}{q_s}. 322 | \end{equation} 323 | 324 | \subsection{Jacobian of Vector-Valued Functions} 325 | When taking derivatives with respect to quaternions, we must take into account 326 | both the composition rule and the nonlinear mapping between the space of unit 327 | quaternions and our chosen three-parameter error representation. 328 | 329 | Let $\phi \in \R^3$ be a differential rotation applied to a function with 330 | quaternion inputs 331 | $y = h(q): \Q \to \R^p$, such that 332 | \begin{equation} \label{eq:vector_function} 333 | y + \delta y = h(L(q) \varphi(\phi)) \approx h(q) + \nabla h(q) \phi. 334 | \end{equation} 335 | \added{Note that we chose to represent $\phi$ in the body frame, consistent with the standard definition of angular velocity, and therefore it is applied to $q$ through right (rather than left) multiplication.} We can calculate the Jacobian $\nabla h(q) \in \R^{p \times 3}$ by 336 | differentiating \eqref{eq:vector_function} with respect to $\phi$, evaluated at 337 | $\phi = 0$: 338 | \begin{equation} \label{eq:quat_gradient} 339 | \nabla h(q) = \pdv{h}{q} L(q) H := \pdv{h}{q} G(q) 340 | = \pdv{h}{q} \begin{bmatrix} 341 | -q_v^T \\ 342 | q_s I_3 + \skewmat{q_v} 343 | \end{bmatrix} 344 | \end{equation} 345 | where $G(q) \in \R^{4 \times 3}$ is the \textit{attitude Jacobian}, which 346 | essentially becomes a ``conversion factor'' allowing us to apply results from 347 | standard vector calculus to the space of unit quaternions. This form is 348 | particularly useful in practice since $\pdv*{h}{q} \in \R^{p \times 4}$ can be 349 | obtained using finite differences or automatic differentiation. 350 | As an aside, although we have used Rodrigues parameters, $G(q)$ is actually the 351 | same (up to a constant scalar factor) for any choice of three-parameter attitude 352 | representation. 353 | 354 | \subsection{Hessian of Scalar-Valued Functions} 355 | If the output of $h$ is a scalar ($p = 1$), then we can find its Hessian by 356 | taking the Jacobian of \eqref{eq:quat_gradient} with respect to $\phi$ using the 357 | product rule, again evaluated at $\phi = 0$: 358 | 359 | \begin{equation} \label{eq:quat_hessian} 360 | \nabla^2 h(q) = G(q)^T \pdv[2]{h}{q} G(q) - I_3 \pdv{h}{q}q, 361 | \end{equation} 362 | where the second term comes from the second derivative of $\varphi(\phi)$. 363 | Similar to $G(q)$, this expression is the same (up to a constant scalar factor) for any 364 | choice of three-parameter attitude representation. 365 | 366 | \subsection{Jacobian of Quaternion-Valued Functions} 367 | We now consider the case of a function that maps unit quaternions to unit 368 | quaternions, $q' = f(q) : \Q \to \Q$. 369 | % \todo{Let's make sure we're bing consistent with our usage of $\Q$ vs. $\mathbb{H}$.} 370 | Here we must also consider the non-trivial 371 | effect of a differential rotation applied to the output, i.e.: 372 | \begin{equation} \label{eq:dqoutput} 373 | L(q') \varphi(\phi') = f(L(q)\varphi(\phi)) . 374 | \end{equation} 375 | Solving \eqref{eq:dqoutput} for $\phi'$ we find, 376 | \begin{equation} \label{eq:phiprime} 377 | \phi' = \varphi^{-1} \left( L(q')^T f(L(q)\varphi(\phi)) \right) \approx \nabla f(q) \, \phi. 378 | \end{equation} 379 | Finally, the desired Jacobian is obtained by taking the derivative of 380 | \eqref{eq:phiprime} with respect to $\phi$: 381 | \begin{equation} \label{eq:quat_jacobian} 382 | \nabla f(q) = H^T L(q')^T \pdv{f}{q} L(q) H = G(q')^T \pdv{f}{q} G(q). 383 | \end{equation} 384 | %The leading $G(q')^T$ comes from the fact that as $\phi' \to 0$, $L(q') f(q) \to 385 | %I_q$, where $I_q$ is the quaternion identity. 386 | Once again, \eqref{eq:quat_jacobian} holds (up to a constant) for any 387 | three-parameter attitude representation. 388 | 389 | %Differentiating through the inverse map, evaluated at the quaternion identity, we find that $\pdv*{\varphi^{-1}}{q}\to H^T$ for any three-parameter attitude representation. 390 | 391 | 392 | \section{Modifying Newton's Method} \label{sec:Wahbas} 393 | 394 | % \todo{I would re-work this section to only talk about the details of doing it the ``correct'' way (i.e. consistent with the previous section). Then just call the standard version a ``naive Newton method in which the quaternion is re-normalized at each step'' and don't bother writing any of the mathematical details for that version. I think it will get confusing if you mix derivative types/notation.} 395 | 396 | Newton's method uses derivative information about a function to iteratively 397 | approximate its roots. For unconstrained systems, this method is highly effective, 398 | and can exhibit quadratic convergence rates. For constrained systems, the updates 399 | can be projected back onto the feasible set at each iteration, but without the same 400 | convergence guarantees. 401 | %For the constraints on $SO(3)$, Newton's method struggles to 402 | %converge past a certain threshold due to this projection. 403 | 404 | In this section, we will leverage the quaternion calculus results introduced in the previous section to modify Newton's method so that it implicitly accounts for the quaternion unit-norm constraint. Unlike the projection approach, this modified form of Newton's method retains the fast convergence rates associated with the unconstrained method. We will demonstrate this behavior on Wahba's Problem, a least-squares attitude estimation problem~\cite{Wahba1965, markley2014fundamentals}. 405 | 406 | 407 | \subsection{Methodology} 408 | % \todo{Let's keep our notation and language consistent for body vs. inertial frames. I propose ``body'' (B) and ``inertial'' or ``world'' (W), which is the standard in robotics.} 409 | Given a set of known vectors in the world frame, $\inframe{W}{w_i}$, and measurements of these vectors in the body frame, $\inframe{B}{v_i}$, we seek the 410 | rotation from the body to the world frame $\toframe{W}{B}{A(q)}$ that solves the following optimization problem, 411 | \begin{mini*} 412 | {q}{ W(q) }{}{} 413 | \addConstraint { q\in \Q,} 414 | \end{mini*} 415 | where Wahba's loss function $W(q)$ is, 416 | \begin{equation} \label{eq:wahba_loss} 417 | W(q) = \sum_i \norm{\inframe{W}{w_i} - A(q) \,\, \inframe{B}{v_i} }_2^2 418 | = \norm{r(q)}_2^2 , 419 | \end{equation} 420 | and $r(q)$ is the residual vector. 421 | 422 | 423 | The Jacobian of $r(q)$ can be found using \eqref{eq:quat_gradient}: 424 | \begin{align} \label{eq:newton_foc} 425 | \nabla r(q) &= \pdv{r}{q} G(q) \\ 426 | &= -2 H^T R(q)^T \left( \sum_i R({}^B \hat{v}_i) \right) G(q) . 427 | \end{align} 428 | Given a guess solution, $q_k$, The standard Gauss-Newton method can then be used to compute a three-parameter update, $\phi_k$ via the Moore-Penrose psuedoinverse: 429 | 430 | \begin{equation} 431 | \phi_k = -({\nabla r}^T {\nabla r})^{-1} {\nabla r}^T r(q_k). 432 | \end{equation} 433 | The update is then applied using the composition for the group: 434 | \begin{equation} 435 | \q_{k+1} = \q_k \otimes \varphi(\phi_k). 436 | \end{equation} 437 | This ``multiplicative'' Gauss-Newton method is summarized in Algorithm \ref{alg:mgn}. 438 | 439 | \begin{algorithm} 440 | \begin{algorithmic}[1] 441 | \caption{Multiplicative Gauss-Newton Method}\label{alg:mgn} 442 | \State $k = 0$ 443 | \While{$\norm{\phi} > $ tolerance} 444 | \State ${\nabla r} = \pdv{r(q_k)}{q} G(q_k)$ 445 | \Comment{Compute Jacobian} 446 | \State $ \phi = -({\nabla r}^T {\nabla r})^{-1} {\nabla r}^T r(q_k)$ \Comment{Compute update step} 447 | \State $q_{k+1} = L(q_k) \varphi(\phi)$ \Comment{Apply update step} 448 | \State $k = k + 1$ 449 | \EndWhile 450 | \end{algorithmic} 451 | \end{algorithm} 452 | 453 | 454 | 455 | \subsection{Results} Figure \ref{fig:wahba_convergence} compares the 456 | multiplicative Gauss-Newton method with a na\"ive Newton's method in 457 | which the quaternion is simply projected back onto the unit sphere at 458 | every iteration. The na\"ive method makes progress initially, but quickly 459 | stalls. By correctly handling the group structure of unit quaternions, 460 | the multiplicative method is able to maintain the fast convergence rates 461 | typical of Newton's method. \added{By comparing our method with the 462 | global solution obtained from a singular-value decomposition 463 | \cite{markley1999estimate}, we see that our method recovers the globally 464 | optimal solution within a small number of iterations.} 465 | 466 | \begin{figure} 467 | \centering 468 | \includegraphics[width=\columnwidth, height=5cm]{figures/wahba_convergence.tikz} 469 | \caption{Convergence comparison for Wahba's problem. \added{The error 470 | is the angle between the current solution and the globally optimal 471 | solution computed using a singular-value decomposition. 472 | The thick line is the average result of 100 trials with randomized 473 | orientations and measurements. The thin lines are the maximum and 474 | minimum over all 100 trials.} By modfying Newton's method with the 475 | methods of section \ref{sec:Quaternion_Calculus}, quadratic 476 | convergence rates are achieved, while a na\"ive approach stalls after 477 | only a few iterations.} 478 | \label{fig:wahba_convergence} 479 | \end{figure} 480 | 481 | \section{Trajectory Optimization \added{for Rigid Bodies}} \label{sec:trajopt} 482 | Here we outline the modifications to the ALTRO solver \cite{howell2019altro} to 483 | solve trajectory optimization problems for rigid bodies, which extends easily to 484 | arbitrary systems whose state is in $\R^n \times \added{\Q}$. %ALTRO is an efficient solver for constrained nonlinear optimization problems that uses iterative LQR (iLQR) with an augmented Lagrangian framework. 485 | We consider trajectory optimization problems of the form, 486 | \begin{mini}[2] 487 | {x_{1:N},u_{1:N-1}}{\ell_f(x_N) + \sum_{k=1}^{N-1} \ell_k(x_k,u_k) }{}{} 488 | \addConstraint{x_{k+1} = f(x_k,u_k)} 489 | \addConstraint{g_k(x_k,u_k)}{\leq 0} 490 | \addConstraint{h_k(x_k,u_k)}{=0} 491 | \label{discrete_trajopt}, 492 | \end{mini} 493 | where $x$ and $u$ are the state and control vectors as described in Sec. \ref{sec:rigidbody_dynamics}, 494 | $f$ are the dynamics as defined in \eqref{eq:rigid_body_dynamics}, $\ell_k$ is a general 495 | nonlinear cost function at a single time step, 496 | $N$ is the number of time steps, and $g_k$, $h_k$ are general nonlinear inequality and 497 | equality constraints. 498 | 499 | ALTRO combines techniques from both differential dynamic programming (DDP) and direct transcription methods to achieve high performance on challenging constrained nonlinear trajectory optimization problems. Like most methods for nonlinear optimization, ALTRO iteratively approximates the 500 | nonlinear functions $f, \ell, g,$ and $h$ with their first or second-order Taylor 501 | series expansions. Leveraging the methods from Sec: \ref{sec:Quaternion_Calculus}, we 502 | adapt the algorithm to optimize directly on the error state $\delta x \in \R^{12}$: 503 | \begin{equation} \label{eq:state_error} 504 | \delta x_k = \begin{bmatrix} 505 | r_k - \bar{r}_k \\ \varphi^{-1}(\bar{\q}_k^{-1} \otimes \q_k) \\ v_k - \bar{v}_k \\ \omega_k - \bar{\omega}_k 506 | \end{bmatrix}. 507 | \end{equation} 508 | 509 | We begin by linearizing the dynamics about the reference state and input trajectories, $\bar{x}$ and $\bar{u}$, using 510 | \eqref{eq:quat_jacobian}. The linearized error dynamics become, 511 | \begin{equation} \label{eq:linearized_dynamics} 512 | \delta x_{k+1} = A_k \delta x_k + B_k \delta u_k , 513 | \end{equation} 514 | where \begin{equation} 515 | \begin{aligned} 516 | A_k = E(\bar{x}_{k+1})^T \pdv{f}{x}|_{\bar{x}_k,\bar{u}_k} E(\bar{x}_k), \\ 517 | B_k = E(\bar{x}_{k+1})^T \pdv{f}{u}|_{\bar{x}_k,\bar{u}_k}, 518 | \end{aligned} 519 | \end{equation} 520 | and $E(x_k) \in \R^{13 \times 12}$ is the error-state Jacobian: 521 | \begin{equation} \label{eq:state_error_jacobian} 522 | E(x) = \begin{bmatrix} 523 | I_3 & & & \\ 524 | & G(q) & & \\ 525 | & & I_3 & \\ 526 | & & & I_3 \\ 527 | \end{bmatrix}\!. 528 | \end{equation} 529 | By applying \eqref{eq:quat_gradient} and \eqref{eq:quat_hessian} to the nonlinear 530 | cost functions $\ell$ and \eqref{eq:quat_jacobian} to the nonlinear constraint 531 | functions $g_k$ and $h_k$, we can calculate the second-order expansion of the cost 532 | function: 533 | \begin{multline} 534 | \delta \ell(x,u) \approx \half \delta x^T \ell_{xx} \delta x 535 | + \half \delta u^T \ell_{uu} \delta u + \delta_u^T \ell_{ux} \delta x \\ 536 | + \ell_x^T \delta x^T + \ell_u^T \delta u. 537 | \end{multline} 538 | % \todo{I think you can safely leave out any references to the augmented Lagrangian, 539 | % etc. and just talk about the cost function.} 540 | % \begin{equation} 541 | % \mathcal{L}_A = \mathcal{L}_N(x_N,\lambda_N,\mu_N) + 542 | % \sum_{k=0}^{N-1} \mathcal{L}_k(x_k,u_k,\lambda_k,\mu_k) 543 | % \end{equation} 544 | % where 545 | % \begin{equation} 546 | % \mathcal{L}_k(x,u,\lambda,\mu) = \ell(x,u) + 547 | % (\lambda + \half I_\mu c(x,u))^T c(x,u), 548 | % \end{equation} 549 | % with $c(x,u)$ being the concatenation of the constraints $f,g$, and $h$ at a given time step, 550 | % and $I_\mu$ the penalty matrix. 551 | 552 | With these results, we can apply standard Newton and quasi-Newton techniques along the lines of Section \ref{sec:Wahbas}. We can also calculate a second-order expansion of the ``action-value function'' $Q(x,u)$ needed in DDP and LQR-based methods, 553 | \begin{align} 554 | Q_{xx} &= \ell_{xx} + A_{k}^T P_{k+1} A_{k} \label{Qxx_exp}\\ 555 | Q_{uu} &= \ell_{uu} + B_{k}^T P_{k+1} B_{k} \label{Quu_exp}\\ 556 | Q_{ux} &= \ell_{ux} + B_{k}^T P_{k+1} A_{k} \label{Qux_exp}\\ 557 | Q_x &= \ell_x + A_{k}^T p_{k+1} \label{Qx_exp}\\ 558 | Q_u &= \ell_u + B_{k}^T p_{k+1} \label{Qu_exp}, 559 | \end{align} 560 | from which we can calculate the quadratic expansion of the cost-to-go 561 | $P_k \in \R^{12 \times 12}$, $p_k \in \R^{12}$, and optimal linear feedback gains 562 | $K_k \in \R^{m \times 12}$ and feed-forward corrections $d_k \in \R^m$ by starting at the terminal state and 563 | performing a backward Riccati recursion as usual~\cite{li2004iterative,howell2019altro}. 564 | 565 | During the ``forward rollout'' of these methods, the dynamics are simulated forward in time using updated control inputs: 566 | \begin{equation} \label{eq:mlqr_control} 567 | u_k = \bar{u}_k - d_k - K_k \delta x_k. 568 | \end{equation} 569 | where $\bar{u}_k$ is the control value from the previous iteration, and $\delta x$ is 570 | computed using \eqref{eq:state_error}. %, with $x_k$ being the current state estimate and $\bar{x}_k$ the state from the previous iteration. 571 | For more details on the ALTRO algorithm, we refer the reader to~\cite{howell2019altro}. 572 | 573 | \subsection{Quaternion Cost Functions} \label{sec:cost_functions} 574 | In addition to the straight-forward modifications to the ALTRO algorithm itself, some care must be taken in designing cost functions that are well-suited to unit quaternions. We 575 | frequently minimize costs that penalize distance from a goal state, e.g. $\half 576 | (x-x_g)^T Q (x-x_g)$; however, na\"ive substraction of unit quaternions does not respect their group structure, and often results in undesired behavior. Instead, we have found the following cost function, which penalizes the geodesic 577 | distance between two unit quaternions \cite{Kuffner2004}, to work well in practice: 578 | \begin{equation} \label{eq:quat_geodesic} 579 | J_\text{geo} = (1-\abs{q_g^T q}) . 580 | \end{equation} 581 | Its gradient and Hessian are, 582 | \begin{align} 583 | \nabla J_\text{geo} &= -\sign(q_g^T q) q_g^T G(q) \\ 584 | \nabla^2 J_\text{geo} &= \sign(q_g^T q) I_3 q_g^T q , 585 | \end{align} 586 | where $\sign$ denotes the signum function. \added{This cost function is particularly 587 | useful for rotations since it eliminates the ambiguity arising from the quaternion 588 | double-cover of $SO(3)$.} 589 | 590 | % \subsubsection{Error Quadratic} \label{sec:error_quadratic} 591 | % Rather than simple subtraction, we can use a quadratic function on the 592 | % three-parameter error state \eqref{eq:state_error}: 593 | % \begin{equation} \label{eq:error_quadratic} 594 | % J_\text{err} = \half \phi^T Q \phi 595 | % = \half \left(\varphi^{-1}(\delta q)\right)^T Q 596 | % \left(\varphi^{-1}(\delta q)\right). 597 | % \end{equation} 598 | % where $\delta q = L(q_g)^T q$, and $\phi = \varphi{\delta q}$. The gradient 599 | % and Hessian of \eqref{eq:error_quadratic} are 600 | % \begin{equation} 601 | % \nabla J_\text{err }= \phi^T Q D(\delta q) G(\delta q) 602 | % \end{equation} 603 | % \begin{multline} 604 | % \nabla^2 J_\text{err} = 605 | % G(\delta q)^T \! \left( 606 | % D(\delta q)^T Q D(\delta q) + \nabla D \right) G(\delta q) \\ 607 | % + I_3 (\phi^T Q D(\delta q)) \delta q %\\ 608 | % \end{multline} 609 | % where, for the Cayley map, 610 | % \begin{equation} 611 | % D(q) = \pdv{\varphi^{-1}}{q} = -\frac{1}{q_s^2}\begin{bmatrix} 612 | % q_v \;\; & -\frac{1}{q_s} I_3 613 | % \end{bmatrix} 614 | % \end{equation} 615 | % \begin{equation} 616 | % \nabla D = \pdv{q}(D(q)^T Q \phi) 617 | % = -\frac{1}{q_s^2} \begin{bmatrix} 618 | % -2 \frac{q_v}{q_s}^T Q \phi & \phi^T Q \\ 619 | % Q \phi & 0 \\ 620 | % \end{bmatrix}. 621 | % \end{equation} 622 | 623 | % \subsubsection{Geodesic Distance} \label{sec:geodesic} 624 | 625 | % \todo{If the geodesic distance works better on all the examples, I vote for leaving out the error quadratic stuff.} 626 | 627 | \section{Experiments} \label{sec:experiments} 628 | In this section we present several trajectory optimization problems for systems that 629 | undergo large changes in attitude: an airplane barrel roll, a quadrotor flip, and a 630 | satellite with flexible solar panels that must slew to a new orientation while 631 | avoiding a keep-out zone. 632 | All problems are run using ALTRO, first without any of the modifications presented in 633 | the current paper, analagous to the na\"ive Newton's method in section \ref{sec:Wahbas} 634 | and labeled ``naive'', and then using the modifications listed in Sec. 635 | \ref{sec:trajopt} and the geodesic cost function described in Sec. 636 | \ref{sec:cost_functions}, labeled ``modified''. All cost functions are of the 637 | following form: 638 | \begin{align} 639 | \ell_\text{naive}(x, u, \bar{x}, Q, R) &= \half (x - \bar{x})^T Q (x - \bar{x}) + \half u^T R u \\ 640 | \ell_\text{modified}(x, u, \bar{x}, Q, R) 641 | &= \ell_\text{naive}(x, u, \bar{x}, \bar{Q}, R) + w (1 \pm \bar{q}^T q) 642 | \end{align} 643 | where $\bar{x}$ is the reference state and $\bar{Q} = \text{diag}(Q_r, \vec{0}_4, 644 | Q_v, Q_\omega)$, with $Q_r,Q_v,Q_\omega$ being the weights of $Q$ for position, and 645 | linear and angular velocity, respectively. 646 | 647 | Timing results are summarized in Table \ref{tab:timing_results}. All experiments are 648 | solved to a constraint satisfaction tolerance of $10^{-5}$ and discretized with a 4th 649 | order Runge-Kutta integrator. The results were run on a laptop computer with a 2.8 GHz 650 | i7-1165G7 processor with 16 GB of RAM. Code for all experiments is available on 651 | \href{https://github.com/RoboticExplorationLab/PlanningWithAttitude} 652 | {GitHub\footnote{\url{https://github.com/RoboticExplorationLab/PlanningWithAttitude}}}. 653 | 654 | \begin{table} 655 | \centering 656 | \caption{Trajectory Optimization Timing Results (naive/modified)} 657 | \input{figures/timing_results.tex} 658 | \label{tab:timing_results} 659 | \end{table} 660 | 661 | \subsection{Airplane Barrel Roll} 662 | % \todo{Reference \eqref{eq:rigid_body_dynamics} and then say that the forces and torques due to lift and drag are fit from wind tunnel data. Also, cite that paper.} 663 | 664 | A 180 degree barrel roll trajectory for a fixed-wing airplane was 665 | optimized. The airplane's dynamics model consists of the a simple 666 | rigid body as defined in Section \ref{sec:rigidbody_dynamics} with 667 | forces and torques due to lift and drag fit from wind tunnel data 668 | \cite{manchester2016udp}. The airplane was tasked to do a barrel roll 669 | by constraining the terminal state to upside-down (see Fig. 670 | \ref{fig:barrellroll}). To mitigate issues with integration error and 671 | drift in the magnitude of the quaternion, the following constraint 672 | function was used to enforce a terminal orientation of $\bar{q}$: 673 | \begin{equation} 674 | \frac{q_v}{\norm{q}} - \bar{q}_v 675 | \sign\left(\bar{q}^T \frac{q }{ \norm{q}}\right) = 0 676 | \end{equation} 677 | 678 | The solver was initialized with level flight trim 679 | conditions. The convergence of the different versions of ALTRO is compared in 680 | Fig. \ref{fig:c_max_convergence}. As expected, the modified version achieves 681 | better convergence and faster solve times compared to the na\"ive version since 682 | the expansions being provided to the algorithm more accurately capture the 683 | relationship between the attitude state and the goal and constraints. For this 684 | relatively simple problem, we gained a modest 31\% improvement in runtime, despite 685 | the additional matrix multiplications when calculating the cost and constraint 686 | expansions. 687 | 688 | \begin{figure}[t] 689 | \centering 690 | \includegraphics[width=\columnwidth]{figures/barrellroll.png} 691 | \caption{Barrel roll trajectory computed by ALTRO using a terminal cost to encourage an upside-down attitude.} 692 | \label{fig:barrellroll} 693 | \end{figure} 694 | 695 | \begin{figure}[t] 696 | \centering 697 | \includegraphics[height=4cm,width=\columnwidth]{figures/c_max_convergence.tikz} 698 | \caption{Constraint satisfaction as a function of iteration when solving the barrel roll problem using ALTRO both with and without the modifications for optimizing unit quaternions.} 699 | \label{fig:c_max_convergence} 700 | \end{figure} 701 | 702 | \subsection{Quadrotor Flip} 703 | A 360 degree flip trajectory for a quadrotor was optimized with dynamics adapted 704 | from \cite{mellinger2012trajectory}. To encourage the flip, we specified a 705 | ``waypoint'' cost function of the following form: 706 | \begin{multline} 707 | \sum_{k \in \mathcal{N}} \ell(x_k, u_k, \hat{x}, \hat{Q}, R) 708 | + \sum_{k \in \mathcal{W}} \ell(x_k, u_k, \bar{x}_k, Q_w, R) 709 | \end{multline} 710 | where $\hat{x}$, $\hat{Q}$ are the nominal state and state weight matrix, $Q_w$ is 711 | the weight matrix for the waypoints, and 712 | $\mathcal{W} = \{20,45,51,55,75,101\}$, $\mathcal{N} = \{1:101\} \setminus 713 | \mathcal{W}$. 714 | Four intermediary ``waypoints'' were used to encourage the quadrotor 715 | to reach angles of \ang{90}, \ang{180}, \ang{270}, and \ang{360} 716 | around an approximately circular arc. The last waypoint was used to 717 | encourage the quadrotor to move towards the final goal, and the first 718 | kept it above the floor before starting the loop. The solver was 719 | provided a dynamically infeasible initial trajectory that linearly 720 | interpolates between the initial and final states, rotating the 721 | quadrotor around the x-axis a full \ang{360}. 722 | 723 | Figure \ref{fig:quad_flip} shows snapshots of the trajectory as 724 | generated using ALTRO. \added{To compare the convergence properties of the 725 | two methods, the optimal state and control trajectories were 726 | perturbed with random Gaussian white noise with a mean of 1 for 727 | position, linear velocity, and angular velocity, 0.1 for the 728 | controls, and 145 degrees for the orientation (about a random axis). 729 | As shown in Fig. \ref{fig:flip_success}, the modified method converges 730 | more reliably than the na\"ive method.} 731 | It is also worth noting that this problem could not be solved using 732 | any three-parameter attitude representation, since it passes through 733 | the singularities at $90\degree, 180\degree$, and $360\degree$ 734 | associated with Euler angles, Rodrigues parameters, and Modified 735 | Rodrigues Parameters, respectively. 736 | 737 | \begin{figure}[t] 738 | \centering 739 | \includegraphics[width=\columnwidth]{figures/quadflip.png} 740 | \caption{Snapshots of the quadrotor flip trajectory. The 741 | gree-colored quadrotors represent the state near t=0 s and the 742 | red-colored quadrotors represent the state near t=5.0 s 743 | } 744 | \label{fig:quad_flip} 745 | \end{figure} 746 | 747 | \begin{figure} 748 | \centering 749 | \begin{tikzpicture} 750 | \begin{axis}[ 751 | ybar, 752 | width=\columnwidth, 753 | height=5cm, 754 | enlarge x limits=0.5, 755 | enlarge y limits=0.2, 756 | ylabel={Percent Success}, 757 | xtick={1,2}, 758 | xticklabels={Na\"ive, Modified}, 759 | bar width=0.6, 760 | nodes near coords, 761 | bar shift=0pt, 762 | axis on top 763 | ] 764 | \addplot [orange, fill] coordinates {(1,86)}; 765 | \addplot [cyan, fill] coordinates {(2,97)}; 766 | \end{axis} 767 | \end{tikzpicture} 768 | \caption{Convergence comparison for quadrotor flip. Percent of 100 trials 769 | that successfully converged, where each trial is initialized with locally-optimal 770 | trajectories perturbed with significant Gaussian white noise.} 771 | \label{fig:flip_success} 772 | \end{figure} 773 | 774 | 775 | \subsection{Satellite Attitude Keep-Out} 776 | % \todo{Let's add a little more detail here: How about saying the dynamics are \eqref{eq:rigid_body_dynamics} plus a few extra states to capture the bending modes. Also, let's just call the start tracker a camera or sensor.} 777 | A spacecraft with flexible appendages was tasked to perform a 150 degree slew maneuver while 778 | ensuring that a body-mounted camera did not point within 40 degrees of a ``keep-out zone'' around the sun vector. The spacecraft dynamics are presented in detail in~\cite{Tracy2020}, and are based 779 | on equation \eqref{eq:rigid_body_dynamics} with the addition of six states to account for three flexible modes. Control torques are generated by four reaction wheels. A quadratic cost function penalizes error from the desired final attitude as well as displacement of the flexible modes. 780 | % \todo{Kevin: add some info on cost function, initial guess (if any), etc.} 781 | We enforce the camera keep-out zone with the following constraint, 782 | \begin{equation} 783 | \left(\inframe{W}{r_{sun}}\right)^T 784 | \left(\toframe{W}{B}{A(q)} \, \inframe{B}{r_{cam}} \right) \leq \cos(40 ^\circ) , 785 | \end{equation} 786 | where $^Br_{cam}$ is the camera line-of-sight unit vector in the body frame and $^Wr_{sun}$ is the unit vector pointing to the sun in the world frame. The attitudes that satisfy this constraint comprise a non-convex set, with the constraint itself being nonlinear in $q$. 787 | \begin{figure}[ht] 788 | \centering 789 | \includegraphics{figures/kevins_plots/attitude_slew_plot.tikz} 790 | \caption{Visualization of the flexible spacecraft slew with a keep-out zone. Attitude is parameterized with a Rodrigues parameter to visualize the trajectory in three dimensions. The constraint surface represents attitudes where the camera line-of-sight is within 40$^\circ$ of the sun. The unconstrained solution violates this constraint, while the constrained solution is able to avoid the keep out zone.} 791 | \label{fig:keepout} 792 | \end{figure} 793 | 794 | ALTRO is able to converge to a locally optimal trajectory for this problem without an initial guess (all controls were initialized to zero). The resulting attitude trajectory is depicted in Fig. \ref{fig:keepout}. Without enforcing the camera constraint, the trajectory passes through the keep-out zone. As noted in Table \ref{tab:timing_results}, the quaternion modifications did not result in a significant improvement over the naive implementation of ALTRO for this problem, indicating that the computational benefits are problem dependent. We hypothesize that the more dynamic behaviors in the other examples benefit more from the quaternion modifications than the relatively slow-moving spacecraft. 795 | 796 | 797 | \section{Conclusions} \label{sec:conclusion} 798 | We have presented a general, unified method for optimization-based planning and control for rigid-body 799 | systems with arbitrary attitude using standard linear algebra and vector calculus. 800 | The application of this methodology is straightforward and 801 | yields substantial improvements in the convergence of Newton and DDP-based methods, while also offering improvements for nonlinear constrained 802 | trajectory optimization for floating-base systems. 803 | 804 | Many state-of-the-art trajectory optimization methods, including direct collocation and sequential convex programming, rely on general-purpose optimization solvers whose internal numerical methods are not exposed to the user. Therefore, these methods are unable to exploit the full structure of both the trajectory optimization problem and the rotation group at a low level. In contrast, we are able to implement deep, native support for quaternions into the ALTRO solver, making it possible to solve more challenging problems with higher performance than other algorithms. 805 | 806 | In future work, we plan to apply ALTRO to real-time model-predictive control problems for aerial vehicles like quadrotors and airships that experience large attitude changes. The methods we have presented can also be leveraged to adapt other classes of gradient or Newton-based algorithms to exploit the structure of 3D rotations. Future directions beyond trajectory optimization may include simulation and planning methods that leverage maximal-coordinate formulations of multi-body dynamics~\cite{brudigam2020linear}, system-identification for complex multi-body systems and fixed-wing aircraft in post-stall conditions, and state estimation for spacecraft with sparse measurements. 807 | 808 | \section*{Acknowledgements} 809 | This work was supported by a NASA Early Career Faculty Award (Grant Number 80NSSC18K1503). 810 | This research was carried out in part at the Jet Propulsion Laboratory, California Institute of Technology, under a contract with the National Aeronautics and Space Administration and funded through JPL’s Strategic University Research Partnerships (SURP) program. 811 | This material is based upon work supported by the National Science Foundation Graduate 812 | Research Fellowship Program under Grant No. DGE-1656518. Any opinions, findings, and 813 | conclusions or recommendations expressed in this material are those of the author(s) and 814 | do not necessarily reflect the views of the National Science Foundation. 815 | 816 | 817 | \printbibliography 818 | 819 | \end{document} -------------------------------------------------------------------------------- /paper/old_figs/barrellroll.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/RoboticExplorationLab/PlanningWithAttitude/14ec74ef9408946a758c87f5f15e48451b5e5d84/paper/old_figs/barrellroll.png -------------------------------------------------------------------------------- /paper/old_figs/barrellroll_timing.tikz: -------------------------------------------------------------------------------- 1 | \begin{tikzpicture} 2 | \begin{axis}[ybar, ylabel={solve time (ms)}, 3 | enlarge y limits = {0.2}, 4 | enlarge x limits = 0.2, 5 | legend style={at={(0.5,-0.12) 6 | }, anchor={north}, legend columns={-1}}, 7 | symbolic x coords={Airplane, Quad Flip}, xtick={data}, nodes near coords, nodes near coords align={vertical}, every node near coord/.append style={/pgf/number format/.cd, fixed,precision=0}] 8 | \addplot 9 | coordinates { 10 | (Airplane,22.3148315) 11 | (Quad Flip,102) 12 | }; 13 | \addplot 14 | coordinates { 15 | (Airplane,29.528505) 16 | (Quad Flip,138) 17 | }; 18 | \addplot 19 | coordinates { 20 | (Airplane,70.2168185) 21 | % (Quad Flip,200) 22 | }; 23 | \legend{iMLQR,Quat,RPY} 24 | \end{axis} 25 | \end{tikzpicture} 26 | -------------------------------------------------------------------------------- /paper/old_figs/c_max_convergence.tikz: -------------------------------------------------------------------------------- 1 | \begin{tikzpicture} 2 | \begin{axis}[xlabel={iterations}, ylabel={contraint satisfaction}, ymode=log, xmajorgrids, ymajorgrids, legend style={at={(0.1,0.1)},anchor=south west}] 3 | \addplot[color={cyan}, no marks, very thick] 4 | coordinates { 5 | (1,0.9971560721275514) 6 | (2,0.0) 7 | (3,0.0) 8 | (4,0.0) 9 | (5,0.0) 10 | (6,0.0) 11 | (7,0.0) 12 | (8,0.0) 13 | (9,0.0) 14 | (10,0.0) 15 | (11,0.0) 16 | (12,0.0) 17 | (13,0.009795728288504169) 18 | (14,0.012965627076229946) 19 | (15,0.012965627076229946) 20 | (16,0.006297666614312414) 21 | (17,0.006297666614312414) 22 | (18,0.0012221985032168092) 23 | (19,0.001087828517986722) 24 | (20,0.0011820648730718197) 25 | (21,0.0011820648730718197) 26 | (22,0.0011820648730718197) 27 | (23,0.00014610389995117767) 28 | (24,1.1104879783785382e-7) 29 | } 30 | ; 31 | \addplot+[color={orange}, no marks, very thick] 32 | coordinates { 33 | (1,0.9971560721275514) 34 | (2,0.0) 35 | (3,0.0) 36 | (4,0.0) 37 | (5,0.0) 38 | (6,0.0) 39 | (7,0.0) 40 | (8,0.0) 41 | (9,0.0) 42 | (10,0.0) 43 | (11,0.0) 44 | (12,0.0) 45 | (13,0.0) 46 | (14,0.009130844853342257) 47 | (15,0.004453912954325734) 48 | (16,0.0010306461089480286) 49 | (17,0.000775574327870121) 50 | (18,9.430964198475777e-8) 51 | } 52 | ; 53 | \legend{{naive},{modified}} 54 | \end{axis} 55 | \end{tikzpicture} 56 | -------------------------------------------------------------------------------- /paper/old_figs/cost_comparison.tex: -------------------------------------------------------------------------------- 1 | \begin{tabular}{lll} 2 | \toprule 3 | Problem & Iterations & time \\ 4 | \midrule 5 | barrellroll & 47 / 36 & 94.54 / 78.65 \\ 6 | quadflip & 58 / 28 & 457.58 / 217.69 \\ 7 | satellite & 35 / 35 & 446.52 / 517.47 \\ 8 | \bottomrule 9 | \end{tabular} 10 | -------------------------------------------------------------------------------- /paper/old_figs/quadflip.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/RoboticExplorationLab/PlanningWithAttitude/14ec74ef9408946a758c87f5f15e48451b5e5d84/paper/old_figs/quadflip.png -------------------------------------------------------------------------------- /paper/old_figs/sat_iterations.tikz: -------------------------------------------------------------------------------- 1 | \begin{tikzpicture} 2 | \begin{axis}[ybar, ylabel={\#iterations}, x={1.5cm}, legend style={at={(0.5,-0.15) 3 | }, anchor={north}, legend columns={-1}}, symbolic x coords={Exp,Cay,dMRP,Vec,Quat,RP,MRP,RPY}, xtick={data}, nodes near coords, nodes near coords align={vertical}, error bars/y dir=both, error bars/y explicit] 4 | \addplot 5 | coordinates { 6 | (Exp,3.0) +- (0,0.48304589153964794) 7 | (Cay,5.0) +- (0,0.7888106377466155) 8 | (dMRP,5.0) +- (0,0.5163977794943222) 9 | (Vec,4.0) +- (0,7.604092465391398) 10 | (Quat,3.0) +- (0,0.42163702135578385) 11 | (RP,3.0) +- (0,0.0) 12 | (MRP,3.0) +- (0,0.5163977794943223) 13 | (RPY,4.0) +- (0,0.6666666666666666) 14 | } 15 | ; 16 | \addplot 17 | coordinates { 18 | (Exp,4.0) +- (0,1.1785113019775793) 19 | (Cay,0.0) +- (0,0.0) 20 | (dMRP,6.0) +- (0,0.6992058987801011) 21 | (Vec,17.0) +- (0,22.642388959157504) 22 | (Quat,4.0) +- (0,0.0) 23 | (RP,0.0) +- (0,0.0) 24 | (MRP,5.5) +- (0,0.9660917830792959) 25 | (RPY,6.0) +- (0,1.509230856356236) 26 | } 27 | ; 28 | \addplot 29 | coordinates { 30 | (Exp,9.0) +- (0,2.3475755815545343) 31 | (Cay,6.0) +- (0,1.2292725943057181) 32 | (dMRP,11.0) +- (0,2.0789954839350235) 33 | (Vec,4.0) +- (0,0.7888106377466155) 34 | (Quat,6.0) +- (0,0.5676462121975466) 35 | (RP,3.0) +- (0,0.42163702135578385) 36 | (MRP,7.5) +- (0,10.022751895340697) 37 | (RPY,4.0) +- (0,0.9660917830792958) 38 | } 39 | ; 40 | \legend{{90 degrees},{180 degrees},{270 degrees}} 41 | \end{axis} 42 | \end{tikzpicture} 43 | -------------------------------------------------------------------------------- /paper/old_figs/sat_success_rate.tikz: -------------------------------------------------------------------------------- 1 | \begin{tikzpicture} 2 | \begin{axis}[ybar, ylabel={success rate}, x={1.5cm}, legend style={at={(0.5,-0.15) 3 | }, anchor={north}, legend columns={-1}}, symbolic x coords={Exp,Cay,dMRP,Vec,Quat,RP,MRP,RPY}, xtick={data}, nodes near coords, nodes near coords align={vertical}] 4 | \addplot 5 | coordinates { 6 | (Exp,1.0) 7 | (Cay,1.0) 8 | (dMRP,1.0) 9 | (Vec,1.0) 10 | (Quat,1.0) 11 | (RP,1.0) 12 | (MRP,1.0) 13 | (RPY,1.0) 14 | } 15 | ; 16 | \addplot 17 | coordinates { 18 | (Exp,1.0) 19 | (Cay,0.0) 20 | (dMRP,1.0) 21 | (Vec,0.7) 22 | (Quat,1.0) 23 | (RP,0.0) 24 | (MRP,1.0) 25 | (RPY,0.9) 26 | } 27 | ; 28 | \addplot 29 | coordinates { 30 | (Exp,0.0) 31 | (Cay,1.0) 32 | (dMRP,0.0) 33 | (Vec,1.0) 34 | (Quat,0.0) 35 | (RP,1.0) 36 | (MRP,0.0) 37 | (RPY,1.0) 38 | } 39 | ; 40 | \legend{{90 degrees},{180 degrees},{270 degrees}} 41 | \end{axis} 42 | \end{tikzpicture} 43 | -------------------------------------------------------------------------------- /paper/old_figs/sat_success_rate_byangle.tikz: -------------------------------------------------------------------------------- 1 | \begin{tikzpicture} 2 | \begin{axis}[ybar, ylabel={success rate}, enlarge x limits={0.25}, x={0.6mm}, legend style={at={(0.5,-0.15) 3 | }, anchor={north}, legend columns={-1}}, xtick={data}, nodes near coords, nodes near coords align={vertical}] 4 | \addplot 5 | coordinates { 6 | (90,1.0) 7 | (180,1.0) 8 | (270,0.0) 9 | } 10 | ; 11 | \addplot 12 | coordinates { 13 | (90,1.0) 14 | (180,0.0) 15 | (270,1.0) 16 | } 17 | ; 18 | \addplot 19 | coordinates { 20 | (90,1.0) 21 | (180,1.0) 22 | (270,0.0) 23 | } 24 | ; 25 | \addplot 26 | coordinates { 27 | (90,1.0) 28 | (180,0.7) 29 | (270,1.0) 30 | } 31 | ; 32 | \addplot 33 | coordinates { 34 | (90,1.0) 35 | (180,1.0) 36 | (270,0.0) 37 | } 38 | ; 39 | \addplot 40 | coordinates { 41 | (90,1.0) 42 | (180,0.0) 43 | (270,1.0) 44 | } 45 | ; 46 | \addplot 47 | coordinates { 48 | (90,1.0) 49 | (180,1.0) 50 | (270,0.0) 51 | } 52 | ; 53 | \addplot 54 | coordinates { 55 | (90,1.0) 56 | (180,0.9) 57 | (270,1.0) 58 | } 59 | ; 60 | \legend{{Exp},{Cay},{dMRP},{Vec},{Quat},{RP},{MRP},{RPY}} 61 | \end{axis} 62 | \end{tikzpicture} 63 | -------------------------------------------------------------------------------- /paper/old_figs/sat_time_per_iter.tikz: -------------------------------------------------------------------------------- 1 | \begin{tikzpicture} 2 | \begin{axis}[ybar, enlargelimits={0.15}, x={1.1cm}, legend style={at={(0.5,-0.15) 3 | }, anchor={north}, legend columns={-1}}, symbolic x coords={Exp,Cay,dMRP,Vec,Quat,RP,MRP,RPY}, xtick={data}, nodes near coords, nodes near coords align={vertical}, error bars/y dir=both, error bars/y explicit] 4 | \addplot 5 | coordinates { 6 | (Exp,0.374117594047619) +- (0,0.007885909367680218) 7 | (Cay,0.14896062857142855) +- (0,0.0034020892427994442) 8 | (dMRP,0.14855463) +- (0,0.0024290595062912944) 9 | (Vec,0.14345336625) +- (0,0.007823742999454476) 10 | (Quat,0.1822917083333333) +- (0,0.007830862423102419) 11 | (RP,0.1419583) +- (0,0.0019437496145180576) 12 | (MRP,0.1490564651785714) +- (0,0.0069432342939453225) 13 | (RPY,0.16607390625000001) +- (0,0.004782084383882967) 14 | } 15 | ; 16 | \legend{{time per iteration (ms)}} 17 | \end{axis} 18 | \end{tikzpicture} 19 | -------------------------------------------------------------------------------- /paper/old_figs/tangent_plane.tikz: -------------------------------------------------------------------------------- 1 | 2 | % Image credit: https://tex.stackexchange.com/questions/261408/sphere-tangent-to-plane 3 | \begin{tikzpicture}[ 4 | point/.style = {draw, circle, fill=black, inner sep=0.7pt}, 5 | ] 6 | \def\rad{2cm} 7 | \coordinate (O) at (0,0); 8 | \coordinate (N) at (0,\rad); 9 | \coordinate (W) at (1.25cm, \rad); 10 | 11 | \filldraw[ball color=white] (O) circle [radius=\rad]; 12 | \draw[dashed] 13 | (\rad,0) arc [start angle=0,end angle=180,x radius=\rad,y radius=5mm]; 14 | \draw 15 | (\rad,0) arc [start angle=0,end angle=-180,x radius=\rad,y radius=5mm]; 16 | \begin{scope}[xslant=0.5,yshift=\rad,xshift=-2] 17 | \filldraw[fill=gray!10,opacity=0.2] 18 | (-4,1) -- (3,1) -- (3,-1) -- (-4,-1) -- cycle; 19 | \node at (2,0.6) {$T$}; 20 | \end{scope} 21 | \draw[dashed] 22 | (N) node[above] {$q$} -- (O) node[below] {$O$}; 23 | \draw[->, thick] 24 | (N) -- (W) node[right] {$\delta q$}; 25 | \node[point] at (N) {}; 26 | \end{tikzpicture} -------------------------------------------------------------------------------- /paper/old_figs/test.tex: -------------------------------------------------------------------------------- 1 | \begin{tabular}{lll} 2 | \toprule 3 | Heading & Iterations & Time (ms) \\ 4 | \midrule 5 | -60.0 & $122$ / $\mathbf{90}$ & $459.96$ / $\mathbf{304.7}$ \\ 6 | -50.0 & $\mathbf{156}$ / $179$ & $\mathbf{502.71}$ / $618.73$ \\ 7 | -40.0 & $187$ / $\mathbf{107}$ & $699.64$ / $\mathbf{353.52}$ \\ 8 | -30.0 & -- / $\mathbf{69}$ & -- / $\mathbf{239.46}$ \\ 9 | -20.0 & $39$ / $\mathbf{30}$ & $148.83$ / $\mathbf{132.14}$ \\ 10 | -10.0 & $28$ / $\mathbf{22}$ & $123.22$ / $\mathbf{109.7}$ \\ 11 | 0.0 & $23$ / $\mathbf{17}$ & $98.43$ / $\mathbf{95.84}$ \\ 12 | 10.0 & $24$ / $\mathbf{23}$ & $116.77$ / $\mathbf{111.4}$ \\ 13 | 20.0 & $\mathbf{21}$ / $28$ & $\mathbf{119.24}$ / $121.67$ \\ 14 | 30.0 & $\mathbf{23}$ / $28$ & $\mathbf{107.96}$ / $113.61$ \\ 15 | 40.0 & $\mathbf{22}$ / $29$ & $\mathbf{102.12}$ / $107.0$ \\ 16 | 50.0 & $47$ / $\mathbf{32}$ & $171.72$ / $\mathbf{123.28}$ \\ 17 | 60.0 & $39$ / $\mathbf{38}$ & $\mathbf{142.48}$ / $152.89$ \\ 18 | \bottomrule 19 | \end{tabular} 20 | -------------------------------------------------------------------------------- /paper/old_figs/test.tikz: -------------------------------------------------------------------------------- 1 | \begin{tikzpicture} 2 | \begin{axis}[xlabel={iterations}, ylabel={angle error (degrees)}, ymode=log, xmajorgrids, ymajorgrids, no markers, legend style={at={(0.1,0.1)},anchor=south west}] 3 | \addplot+[orange, ultra thick] 4 | coordinates { 5 | (1,10.0) 6 | (2,0.7678964995250577) 7 | (3,0.374300025506541) 8 | (4,0.3679627299910924) 9 | (5,0.3676273603961281) 10 | } 11 | ; 12 | \addplot+[orange, name path=A, line width=0.1pt, forget plot] 13 | coordinates { 14 | (1,10.000000000000005) 15 | (2,4.757112743094829) 16 | (3,3.2022627472300087) 17 | (4,3.07296845752766) 18 | (5,3.067476045883991) 19 | } 20 | ; 21 | \addplot+[orange, name path=B, line width=0.1pt, forget plot] 22 | coordinates { 23 | (1,9.999999999999986) 24 | (2,0.06380635255648336) 25 | (3,0.012967313656774965) 26 | (4,0.011255533610576535) 27 | (5,0.011122997621299442) 28 | } 29 | ; 30 | \addplot+[orange!10, forget plot] 31 | fill between [of=A and B]; 32 | ; 33 | \addplot+[cyan, ultra thick] 34 | coordinates { 35 | (1,10.0) 36 | (2,0.22639110461932632) 37 | (3,0.005209560034624235) 38 | (4,0.00015265929399267558) 39 | (5,4.93408997953017e-6) 40 | } 41 | ; 42 | \addplot+[cyan, solid, name path=C, line width=0.1pt, forget plot] 43 | coordinates { 44 | (1,10.000000000000012) 45 | (2,0.48612090632693833) 46 | (3,0.020255571615923933) 47 | (4,0.0009392553213255653) 48 | (5,4.283368523841055e-5) 49 | } 50 | ; 51 | \addplot+[cyan, solid, name path=D, line width=0.1pt, forget plot] 52 | coordinates { 53 | (1,9.999999999999988) 54 | (2,0.022877727290891384) 55 | (3,0.0005243109598923958) 56 | (4,2.4896868070253027e-6) 57 | (5,1.350442822177038e-8) 58 | } 59 | ; 60 | \addplot+[cyan!10, forget plot] 61 | fill between [of=C and D]; 62 | ; 63 | \legend{{naive},{modified}} 64 | \end{axis} 65 | \end{tikzpicture} 66 | -------------------------------------------------------------------------------- /paper/old_figs/timing_results.tex: -------------------------------------------------------------------------------- 1 | \begin{tabular}{lll} 2 | \toprule 3 | Problem & Iterations & time (ms) \\ 4 | \midrule 5 | barrellroll & 23 / 17 & 105.74 / 72.64 \\ 6 | quadflip & 31 / 25 & 505.59 / 433.59 \\ 7 | satellite & 16 / 17 & 170.98 / 263.10 \\ 8 | \bottomrule 9 | \end{tabular} 10 | -------------------------------------------------------------------------------- /paper/old_figs/wahba_convergence.tikz: -------------------------------------------------------------------------------- 1 | \begin{tikzpicture} 2 | \begin{axis}[xlabel={iterations}, ylabel={angle error (degrees)}, ymode=log, xmajorgrids, ymajorgrids, no markers, legend style={at={(0.1,0.1)},anchor=south west}] 3 | \addplot+[orange, ultra thick] 4 | coordinates { 5 | (1,10.0) 6 | (2,0.7678964995250577) 7 | (3,0.374300025506541) 8 | (4,0.3679627299910924) 9 | (5,0.3676273603961281) 10 | } 11 | ; 12 | \addplot+[orange, name path=A, line width=0.1pt, forget plot] 13 | coordinates { 14 | (1,10.000000000000005) 15 | (2,4.757112743094829) 16 | (3,3.2022627472300087) 17 | (4,3.07296845752766) 18 | (5,3.067476045883991) 19 | } 20 | ; 21 | \addplot+[orange, name path=B, line width=0.1pt, forget plot] 22 | coordinates { 23 | (1,9.999999999999986) 24 | (2,0.06380635255648336) 25 | (3,0.012967313656774965) 26 | (4,0.011255533610576535) 27 | (5,0.011122997621299442) 28 | } 29 | ; 30 | \addplot+[orange!10, forget plot] 31 | fill between [of=A and B]; 32 | ; 33 | \addplot+[cyan, ultra thick] 34 | coordinates { 35 | (1,10.0) 36 | (2,0.22639110461932632) 37 | (3,0.005209560034624235) 38 | (4,0.00015265929399267558) 39 | (5,4.93408997953017e-6) 40 | } 41 | ; 42 | \addplot+[cyan, solid, name path=C, line width=0.1pt, forget plot] 43 | coordinates { 44 | (1,10.000000000000012) 45 | (2,0.48612090632693833) 46 | (3,0.020255571615923933) 47 | (4,0.0009392553213255653) 48 | (5,4.283368523841055e-5) 49 | } 50 | ; 51 | \addplot+[cyan, solid, name path=D, line width=0.1pt, forget plot] 52 | coordinates { 53 | (1,9.999999999999988) 54 | (2,0.022877727290891384) 55 | (3,0.0005243109598923958) 56 | (4,2.4896868070253027e-6) 57 | (5,1.350442822177038e-8) 58 | } 59 | ; 60 | \addplot+[cyan!10, forget plot] 61 | fill between [of=C and D]; 62 | ; 63 | \legend{{naive},{modified}} 64 | \end{axis} 65 | \end{tikzpicture} 66 | -------------------------------------------------------------------------------- /paper/submission.zip: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/RoboticExplorationLab/PlanningWithAttitude/14ec74ef9408946a758c87f5f15e48451b5e5d84/paper/submission.zip -------------------------------------------------------------------------------- /src/PlanningWithAttitude.jl: -------------------------------------------------------------------------------- 1 | module PlanningWithAttitude 2 | 3 | using StaticArrays 4 | using LinearAlgebra 5 | using Rotations 6 | using RobotZoo 7 | using Altro 8 | using TrajectoryOptimization 9 | import RobotDynamics 10 | const RD = RobotDynamics 11 | const TO = TrajectoryOptimization 12 | 13 | # include("rotatedmodel.jl") 14 | include("vecmodel.jl") 15 | include("quat_cons.jl") 16 | include("quat_costs.jl") 17 | include("quat_norm.jl") 18 | include("airplane_problem.jl") 19 | include("quadflip_problem.jl") 20 | include("flexible_spacecraft_dynamics.jl") 21 | 22 | export 23 | VecModel, 24 | YakProblems, 25 | QuadFlipProblem, 26 | SatelliteKeepOutProblem, 27 | QuatGeoCon, 28 | QuatErr, 29 | QuatVecEq, 30 | ErrorQuadratic, 31 | QuatLQRCost, 32 | LieLQR, 33 | QuatRenorm 34 | 35 | 36 | 37 | # include("logger.jl") 38 | # include("knotpoint.jl") 39 | # include("models.jl") 40 | # include("quaternions.jl") 41 | # include("objective.jl") 42 | # include("ilqr_solver.jl") 43 | # include("ilqr.jl") 44 | # include("attitude_costs.jl") 45 | 46 | # include("rbstate.jl") 47 | # include("plotting.jl") 48 | # include("tracking_control.jl") 49 | # include("lqr_monte_carlo.jl") 50 | end 51 | -------------------------------------------------------------------------------- /src/airplane_problem.jl: -------------------------------------------------------------------------------- 1 | # using TrajectoryOptimization: LieLQRCost 2 | using ForwardDiff 3 | using RobotDynamics 4 | 5 | function YakProblems(; 6 | integration=RD.RK4, 7 | N = 101, 8 | vecstate=false, 9 | scenario=:barrellroll, 10 | costfun=:Quadratic, 11 | termcon=:goal, 12 | quatnorm=:none, 13 | heading=0.0, # deg 14 | kwargs... 15 | ) 16 | model = RobotZoo.YakPlane(UnitQuaternion) 17 | 18 | opts = SolverOptions( 19 | cost_tolerance_intermediate = 1e-1, 20 | penalty_scaling = 10., 21 | penalty_initial = 10.; 22 | kwargs... 23 | ) 24 | 25 | s = RD.LieState(model) 26 | n,m = size(model) 27 | rsize = size(model)[1] - 9 28 | vinds = SA[1,2,3,8,9,10,11,12,13] 29 | 30 | # Discretization 31 | tf = 1.25 32 | dt = tf/(N-1) 33 | 34 | if scenario == :barrellroll 35 | ey = @SVector [0,1,0.] 36 | 37 | # Initial and final condition 38 | p0 = MRP(0.997156, 0., 0.075366) # initial orientation 39 | pf = MRP(0., -0.0366076, 0.) # final orientation (upside down) 40 | dq = expm(SA[0,0,1]*deg2rad(heading)) 41 | pf = pf * dq 42 | 43 | x0 = RD.build_state(model, [-3,0,1.5], p0, [5,0,0], [0,0,0]) 44 | utrim = @SVector [41.6666, 106, 74.6519, 106] 45 | xf = RD.build_state(model, [3,0,1.5], pf, dq * [5,0,0.], [0,0,0]) 46 | 47 | # Xref trajectory 48 | x̄0 = RBState(model, x0) 49 | x̄f = RBState(model, xf) 50 | Xref = map(1:N) do k 51 | x̄0 + (x̄f - x̄0)*((k-1)/(N-1)) 52 | end 53 | 54 | # Objective 55 | Qf_diag = RD.fill_state(model, 100, 500, 100, 100.) 56 | Q_diag = RD.fill_state(model, 0.1, 0.1, 0.1, 0.1) 57 | Qf = Diagonal(Qf_diag) 58 | Q = Diagonal(Q_diag) 59 | R = Diagonal(@SVector fill(1e-3,4)) 60 | if quatnorm == :slack 61 | m += 1 62 | R = Diagonal(push(R.diag, 1e-6)) 63 | utrim = push(utrim, 0) 64 | end 65 | if costfun == :Quadratic 66 | costfuns = map(Xref) do xref 67 | LQRCost(Q, R, xf, utrim) 68 | end 69 | costfun = LQRCost(Q, R, xf, utrim) 70 | costterm = LQRCost(Qf, R, xf, utrim) 71 | costfuns[end] = costterm 72 | elseif costfun == :QuatLQR 73 | costfuns = map(Xref) do xref 74 | QuatLQRCost(Q, R, xf, utrim, w=0.1) 75 | end 76 | costterm = QuatLQRCost(Qf, R, xf, utrim; w=200.0) 77 | costfuns[end] = costterm 78 | elseif costfun == :LieLQR 79 | costfun = LieLQR(s, Q, R, xf, utrim) 80 | costterm = LieLQR(s, Qf, R, xf, utrim) 81 | elseif costfun == :ErrorQuadratic 82 | costfuns = map(Xref) do xref 83 | ErrorQuadratic(model, Q, R, xref, utrim) 84 | end 85 | costfun = ErrorQuadratic(model, Q, R, xf, utrim) 86 | costterm = ErrorQuadratic(model, Qf, R, xf, utrim) 87 | costfuns[end] = costterm 88 | end 89 | obj = Objective(costfuns) 90 | 91 | # Constraints 92 | conSet = ConstraintList(n,m,N) 93 | vecgoal = GoalConstraint(xf, vinds) 94 | if termcon == :goal 95 | rotgoal = GoalConstraint(xf, SA[4,5,6,7]) 96 | elseif termcon == :quatvec 97 | rotgoal = QuatVecEq(n, UnitQuaternion(pf), SA[4,5,6,7]) 98 | elseif termcon == :quaterr 99 | rotgoal = QuatErr(n, UnitQuaternion(pf), SA[4,5,6,7]) 100 | else 101 | throw(ArgumentError("$termcon is not a known option for termcon. Options are :goal, :quatvec, :quaterr")) 102 | end 103 | add_constraint!(conSet, vecgoal, N) 104 | add_constraint!(conSet, rotgoal, N) 105 | 106 | else 107 | throw(ArgumentError("$scenario isn't a known scenario")) 108 | end 109 | 110 | # Initialization 111 | U0 = [copy(utrim) for k = 1:N-1] 112 | 113 | # Use a standard model (no special handling of rotation states) 114 | if quatnorm == :renorm 115 | model = QuatRenorm(model) 116 | elseif quatnorm == :slack 117 | model = QuatSlackModel(model) 118 | slackcon = UnitQuatConstraint(model) 119 | add_constraint!(conSet, slackcon, 1:N-1) 120 | end 121 | if vecstate 122 | model = VecModel(model) 123 | end 124 | 125 | # Build problem 126 | prob = Problem(model, obj, xf, tf, x0=x0, constraints=conSet, integration=integration) 127 | initial_controls!(prob, U0) 128 | prob, opts 129 | end 130 | -------------------------------------------------------------------------------- /src/flexible_spacecraft_dynamics.jl: -------------------------------------------------------------------------------- 1 | const RD = RobotDynamics 2 | # post processing 3 | function post_process(solver) 4 | X = states(solver) 5 | U = controls(solver) 6 | X = Vector.(X) 7 | U = Vector.(U) 8 | 9 | pts = length(X)-1 10 | q_hist = zeros(4,pts) 11 | ω_hist = zeros(3,pts) 12 | u_hist = zeros(4,pts) 13 | η_hist = zeros(pts) 14 | ηd_hist = zeros(pts) 15 | θ_hist = zeros(pts) 16 | r_hist = zeros(4,pts) 17 | for i = 1:pts 18 | ω_hist[:,i] = X[i][1:3] 19 | q_hist[:,i] = X[i][4:7] 20 | η_hist[i] = X[i][8] 21 | ηd_hist[i] = X[i][9] 22 | r_hist[:,i] = X[i][10:13] 23 | u_hist[:,i] = U[i][1:4] 24 | 25 | θ_hist[i] = 2*atan(norm(q_hist[2:4,i]),q_hist[1,i]) 26 | end 27 | 28 | return ω_hist, q_hist, u_hist,η_hist, ηd_hist, r_hist, θ_hist 29 | end 30 | 31 | import RobotDynamics: dynamics, forces, moments, wrenches, mass_matrix, inertia, inertia_inv, orientation 32 | import RobotDynamics: state_dim, control_dim 33 | 34 | 35 | 36 | 37 | import Rotations: lmult, vmat, hmat 38 | 39 | struct FlexSatellite <: RD.LieGroupModel 40 | J::SArray{Tuple{3,3},Float64,2,9} 41 | B::SArray{Tuple{3,4},Float64,2,12} 42 | C::SArray{Tuple{3,3},Float64,2,9} 43 | K::SArray{Tuple{3,3},Float64,2,9} 44 | δ::SArray{Tuple{3,3},Float64,2,9} 45 | first_inv::SArray{Tuple{3,3},Float64,2,9} 46 | ϕMΣ::SArray{Tuple{3,3},Float64,2,9} 47 | end 48 | 49 | 50 | # units of kg-m 51 | ϕMΣ = [0 1 0; 52 | 1 0 0; 53 | 0 .2 -.8]; 54 | 55 | ϕMΣ = SMatrix{3,3}(ϕMΣ) 56 | 57 | 58 | # units of kg-m^2 59 | δ = [0 0 1; 60 | 0 1 0; 61 | -.7 .1 .1] 62 | δ = copy(transpose(δ)) 63 | δ = SMatrix{3,3}(δ) 64 | 65 | J = diagm([1;2;3]) 66 | 67 | J = SMatrix{3,3}(J) 68 | 69 | mass = 28.54*14.5939 70 | 71 | B = @SMatrix [0.965926 0 -0.965926 0 ; 72 | 0.258819 -0.258819 0.258819 -0.258819; 73 | 0 0.965926 0 -0.965926] 74 | 75 | zeta = [.001;.001;.001] 76 | Delta = [.05; .2; .125] * (2*pi) 77 | 78 | # damping and stiffness matrices 79 | C = zeros(3,3) 80 | K = zeros(3,3) 81 | for i =1:3 82 | C[i,i] = 2*zeta[i]*Delta[i]; 83 | K[i,i] = Delta[i]^2; 84 | end 85 | 86 | C = SMatrix{3,3}(C) 87 | K = SMatrix{3,3}(K) 88 | 89 | first_inv = inv(J - δ*δ') 90 | FlexSatellite() = FlexSatellite(J,B,C,K,δ,first_inv,ϕMΣ) 91 | RobotDynamics.control_dim(::FlexSatellite) = 4 92 | # Base.size(::FlexSatellite) = 17,4 93 | Base.position(::FlexSatellite, x::SVector) = @SVector zeros(3) 94 | RD.orientation(::FlexSatellite, x::SVector) = UnitQuaternion(x[4], x[5], x[6], x[7]) 95 | RobotDynamics.LieState(::FlexSatellite) = RobotDynamics.LieState(UnitQuaternion{Float64}, (3,10)) 96 | function dynamics(model::FlexSatellite, x::SVector, u::SVector,t) 97 | ω = @SVector [x[1], x[2], x[3]] 98 | q = normalize(@SVector [x[4], x[5], x[6], x[7]]) 99 | η = @SVector [x[8],x[9],x[10]] 100 | η_dot = @SVector [x[11],x[12],x[13]] 101 | r = @SVector [x[14],x[15],x[16],x[17]] 102 | J = model.J 103 | B = model.B 104 | C = model.C 105 | K = model.K 106 | δ = model.δ 107 | first_inv = model.first_inv 108 | ϕMΣ = model.ϕMΣ 109 | 110 | 111 | 112 | τ = @SVector zeros(3) 113 | F = @SVector zeros(3) 114 | 115 | ωdot = first_inv*(τ -B*u/60 - 116 | cross(ω,J*ω + δ*η_dot + B*r) + 117 | δ*(C*η_dot + K*η + ϕMΣ*F)) 118 | qdot = 0.5*lmult(q)*hmat()*ω 119 | η_ddot = -δ'*ωdot -C*η_dot - K*η - ϕMΣ*F 120 | rdot = u/60 121 | return [ωdot; qdot;η_dot;η_ddot;rdot] 122 | end 123 | 124 | function RD.discrete_dynamics(::Type{Q}, model::FlexSatellite, x::SVector, u::SVector, t, dt) where Q <: RD.Explicit 125 | x2 = RD.integrate(Q, model, x, u, t, dt) 126 | return x2 127 | ω = x2[SA[1,2,3]] 128 | q = normalize(x[SA[4,5,6,7]]) 129 | x_ = x[SA[8,9,10,11,12,13,14,15,16,17]] 130 | return [ω; q; x_] 131 | end 132 | 133 | 134 | ## Problem definition 135 | struct AttitudeKeepOut{T} <: TO.StateConstraint 136 | n::Int 137 | keepoutdir::SVector{3,T} 138 | bodyvec::SVector{3,T} 139 | keepoutangle::Float64 140 | function AttitudeKeepOut(n::Int, keepoutdir::SVector, bodyvec::SVector, keepoutangle::T) where T 141 | new{T}(n,keepoutdir,bodyvec,keepoutangle) 142 | end 143 | end 144 | TO.state_dim(con::AttitudeKeepOut) = con.n 145 | TO.sense(::AttitudeKeepOut) = Inequality() 146 | Base.length(::AttitudeKeepOut) = 1 147 | TO.evaluate(con::AttitudeKeepOut, x::SVector) = ( 148 | SA[ dot(dcm_from_q(x[4:7])*con.bodyvec,con.keepoutdir)-cosd(con.keepoutangle)] 149 | ) 150 | 151 | 152 | # quaternion to dcm functions 153 | function skew(v) 154 | """Skew-symmetric matrix from 3 element array""" 155 | return @SMatrix [0 -v[3] v[2]; v[3] 0 -v[1]; -v[2] v[1] 0] 156 | end 157 | function dcm_from_q(q) 158 | """DCM from quaternion, scalar first""" 159 | s = @views q[1] 160 | v = @views q[2:4] 161 | return I + 2*skew(v)*(s*I + skew(v)) 162 | end 163 | 164 | function SatelliteKeepOutProblem(constrained::Bool=true; vecstate::Bool=false, 165 | costfun=LQRCost, N = 401, termcon = :quatvec, integration=RD.RK4, kwargs...) 166 | # Discretization 167 | tf = 100.0 168 | dt = tf/(N-1) 169 | 170 | # Model 171 | model = FlexSatellite() 172 | n,m = size(model) 173 | 174 | # Initial and final states 175 | ω = @SVector zeros(3) 176 | q0 = Rotations.params(expm(deg2rad(150) * normalize(@SVector [1,2,3]))) 177 | qf = Rotations.params(UnitQuaternion(I)) 178 | r0 = zeros(4) 179 | x0 = [ω; q0;zeros(3);zeros(3);r0] 180 | xf = SVector{17}([ω; qf;zeros(3);zeros(3);r0]) 181 | 182 | # Objective 183 | Q = Diagonal(@SVector [10,10,10, 1,1,1,1, 10,10.0,10, 10,10,10,1,1,1,1.]) 184 | R = Diagonal(@SVector fill(10.0, m)) 185 | Qf = Q * N 186 | cost0 = costfun(Q, R, xf, w=0.1) 187 | cost_term = costfun(Qf, R, xf, w=1.0) 188 | obj = Objective(cost0, cost_term, N) 189 | # obj = LQRObjective(Q,R,Qf,xf,N) 190 | 191 | # constaint 192 | cons = ConstraintList(n,m,N) 193 | bnd = BoundConstraint(n,m, u_min=-.5, u_max=.5) 194 | add_constraint!(cons,bnd,1:N-1) 195 | # add_constraint!(cons, QuatVecEq(n, UnitQuaternion(qf), SA[4,5,6,7]), N) 196 | add_constraint!(cons, QuatVecEq(n, UnitQuaternion(qf), SA[4,5,6,7]), N) 197 | 198 | # keep out 199 | keepoutdir = @SVector [1.0,0,0] 200 | bodyvec = @SVector [0.360019,-0.92957400,0.07920895] 201 | keepout = AttitudeKeepOut( n, keepoutdir, bodyvec, 40.0 ) 202 | 203 | if constrained 204 | add_constraint!(cons,keepout,1:N) 205 | end 206 | 207 | if vecstate 208 | model = VecModel(model) 209 | end 210 | 211 | ## Solve 212 | prob = Problem(model, obj, xf, tf, x0=x0,constraints = cons,N=N;integration=integration) 213 | # U0 = [@SVector randn(m) for k = 1:N-1] .* 1e-1 214 | # initial_controls!(prob, U0) 215 | 216 | opts = SolverOptions( 217 | verbose=1, 218 | constraint_tolerance = 1e-5, 219 | penalty_scaling = 200.0, 220 | penalty_initial = 1e3, 221 | cost_tolerance = 1e-4, 222 | cost_tolerance_intermediate = 1e-4, 223 | projected_newton_tolerance = 1e-4, 224 | show_summary = true, 225 | projected_newton = false, 226 | verbose_pn = 1; 227 | kwargs... 228 | ) 229 | return prob, opts 230 | end -------------------------------------------------------------------------------- /src/quadflip_problem.jl: -------------------------------------------------------------------------------- 1 | using RobotZoo: Quadrotor 2 | 3 | function QuadFlipProblem(Rot=UnitQuaternion; slack::Bool=false, vecmodel::Bool=false, 4 | renorm::Bool=false, costfun=LQRCost, integration=RD.RK3, termcon=:none, kwargs...) 5 | model = Quadrotor{Rot}() 6 | if renorm 7 | model = QuatRenorm(model) 8 | end 9 | n,m = size(model) 10 | m += slack 11 | rsize = n-9 12 | 13 | # discretization 14 | N = 101 # number of knot points 15 | tf = 5.0 16 | dt = tf/(N-1) # total time 17 | 18 | # Initial condition 19 | x0_pos = @SVector [0., -1., 1.] 20 | x0 = RobotDynamics.build_state(model, x0_pos, UnitQuaternion(I), zeros(3), zeros(3)) 21 | 22 | # cost 23 | s = costfun == LQRCost 24 | Q_diag = RobotDynamics.build_state(model, 25 | [1e-6,1e-6,1e-6], 26 | fill(1e-5,rsize)*s, 27 | fill(1e-3,3), 28 | fill(1e-2,3) 29 | ) 30 | R = Diagonal(@SVector fill(1e-3,m)) 31 | q_nom = UnitQuaternion(I) 32 | v_nom, ω_nom = zeros(3), zeros(3) 33 | x_nom = RobotDynamics.build_state(model, zeros(3), q_nom, v_nom, ω_nom) 34 | 35 | # waypoints 36 | ex = @SVector [1.0, 0, 0] 37 | wpts = [ 38 | ((@SVector [0, 1.0, 1.0,]), expm(ex*deg2rad(00))), 39 | ((@SVector [0, 1.5, 1.5,]), expm(ex*deg2rad(90))), 40 | # ((@SVector [0, 0.2, 1.5]), expm(ex*deg2rad(90))), 41 | # ((@SVector [0, 0.0, 2.0]), expm(ex*deg2rad(135))), 42 | ((@SVector [0, 1.0, 2.5]), expm(ex*deg2rad(180))), 43 | # ((@SVector [0, 0.0, 2.0]), expm(ex*deg2rad(225))), 44 | ((@SVector [0, 0.5, 1.5]), expm(ex*deg2rad(270))), 45 | ((@SVector [0, 0.65, 1.0]), expm(ex*deg2rad(360))), 46 | ((@SVector [0, 1.0, 1.0]), expm(ex*deg2rad(360))), 47 | ] 48 | # times = [35, 41, 47, 51, 55, 61, 70, 101] 49 | times = [20, 45, 51, 55, 75, 101] 50 | 51 | """ 52 | Costs 53 | """ 54 | # intermediate costs 55 | Qw_diag = RobotDynamics.build_state(model, 56 | [1e3,1e1,1e3], 57 | (@SVector fill(5e4,rsize))*s, 58 | fill(1,3), fill(10,3) 59 | ) 60 | Qf_diag = RobotDynamics.fill_state(model, 10., 100*s, 10, 10) 61 | xf = RobotDynamics.build_state(model, wpts[end][1], wpts[end][2], zeros(3), zeros(3)) 62 | cost_nom = costfun(Diagonal(Q_diag), R, xf, w=0.10) 63 | 64 | # waypoint costs 65 | costs = map(1:length(wpts)) do i 66 | r,q = wpts[i] 67 | xg = RobotDynamics.build_state(model, r, q, v_nom, [2pi/3, 0, 0]) 68 | if times[i] == N 69 | Q = Diagonal(Qf_diag) 70 | w = 100. 71 | else 72 | Q = Diagonal(1e-3*Qw_diag) 73 | w = 10. 74 | end 75 | costfun(Q, R, xg, w=w) 76 | end 77 | 78 | costs_all = map(1:N) do k 79 | i = findfirst(x->(x ≥ k), times) 80 | if k ∈ times 81 | costs[i] 82 | else 83 | cost_nom 84 | end 85 | end 86 | obj = Objective(costs_all) 87 | 88 | # Constraints 89 | conSet = ConstraintList(n,m,N) 90 | # add_constraint!(conSet, GoalConstraint(xf, SA[1,2,3,8,9,10]), N:N) 91 | xmin = fill(-Inf,n) 92 | xmin[3] = 0.0 93 | bnd = BoundConstraint(n,m, x_min=xmin) 94 | # add_constraint!(conSet, bnd, 1:N-1) 95 | 96 | if slack 97 | quad = QuatSlackModel(model) 98 | quatcon = UnitQuatConstraint(quad) 99 | add_constraint!(conSet, quatcon, 1:N-1) 100 | else 101 | quad = model 102 | end 103 | if vecmodel 104 | quad = VecModel(quad) 105 | end 106 | 107 | # Initialization 108 | u0 = zeros(quad)[2] 109 | U_hover = [copy(u0) for k = 1:N-1] # initial hovering control trajectory 110 | 111 | # Problem 112 | prob = Problem(quad, obj, xf, tf, x0=x0, constraints=conSet, integration=integration) 113 | initial_controls!(prob, U_hover) 114 | 115 | # Infeasible start trajectory 116 | RobotDynamics.build_state(model, zeros(3), UnitQuaternion(I), zeros(3), zeros(3)) 117 | X_guess = map(1:prob.N) do k 118 | t = (k-1)/prob.N 119 | x = (1-t)*x0 + t*xf 120 | RobotDynamics.build_state(model, position(model, x), 121 | expm(2pi*t*@SVector [1.,0,0]), 122 | RobotDynamics.linear_velocity(model, x), 123 | RobotDynamics.angular_velocity(model, x)) 124 | end 125 | initial_states!(prob, X_guess) 126 | 127 | opts = SolverOptions(; 128 | cost_tolerance=1e-5, 129 | cost_tolerance_intermediate=1e-5, 130 | constraint_tolerance=1e-5, 131 | projected_newton_tolerance=1e-2, 132 | iterations_outer=40, 133 | penalty_scaling=10., 134 | penalty_initial=0.1, 135 | show_summary=false, 136 | verbose_pn=false, 137 | verbose=0, 138 | projected_newton=true, 139 | kwargs... 140 | ) 141 | return prob, opts 142 | end -------------------------------------------------------------------------------- /src/quat_cons.jl: -------------------------------------------------------------------------------- 1 | ## Quaternion Goal Constraints 2 | struct QuatGeoCon{T} <: TO.StateConstraint 3 | n::Int 4 | qf::UnitQuaternion{T} 5 | qind::SVector{4,Int} 6 | end 7 | function TO.evaluate(con::QuatGeoCon, x::SVector) 8 | q = x[con.qind] 9 | qf = Rotations.params(con.qf) 10 | dq = qf'q 11 | return SA[min(1-dq, 1+dq)] 12 | end 13 | TO.sense(::QuatGeoCon) = TO.Equality() 14 | TO.state_dim(con::QuatGeoCon) = con.n 15 | Base.length(::QuatGeoCon) = 1 16 | 17 | 18 | struct QuatErr{T} <: TO.StateConstraint 19 | n::Int 20 | qf::UnitQuaternion{T} 21 | qind::SVector{4,Int} 22 | end 23 | function TO.evaluate(con::QuatErr, x::StaticVector) 24 | qf = con.qf 25 | q = UnitQuaternion(x[con.qind]) 26 | return Rotations.rotation_error(q, qf, Rotations.MRPMap()) 27 | end 28 | TO.sense(::QuatErr) = TO.Equality() 29 | TO.state_dim(con::QuatErr) = con.n 30 | Base.length(con::QuatErr) = 3 31 | 32 | struct QuatVecEq{T} <: TO.StateConstraint 33 | n::Int 34 | qf::UnitQuaternion{T} 35 | qind::SVector{4,Int} 36 | end 37 | function TO.evaluate(con::QuatVecEq, x::StaticVector) 38 | qf = Rotations.params(con.qf) 39 | q = normalize(x[con.qind]) 40 | dq = qf'q 41 | if dq < 0 42 | qf *= -1 43 | end 44 | return -SA[qf[2] - q[2], qf[3] - q[3], qf[4] - q[4]] 45 | end 46 | TO.sense(::QuatVecEq) = TO.Equality() 47 | TO.state_dim(con::QuatVecEq) = con.n 48 | Base.length(con::QuatVecEq) = 3 49 | 50 | 51 | -------------------------------------------------------------------------------- /src/quat_costs.jl: -------------------------------------------------------------------------------- 1 | using TrajectoryOptimization: QuadraticCostFunction, CostFunction 2 | import RobotDynamics: state_dim, control_dim 3 | import TrajectoryOptimization: is_blockdiag, is_diag, 4 | stage_cost, gradient!, hessian!, change_dimension 5 | import Base: + 6 | 7 | function LieLQR(s::RD.LieState{Rot,P}, Q::Diagonal{<:Any,<:StaticVector}, R::Diagonal, 8 | xf, uf=zeros(size(R,1)); kwargs...) where {Rot,P} 9 | if Rot <: UnitQuaternion && length(Q.diag) == 13 10 | Qd = deleteat(Q.diag, 4) 11 | Q = Diagonal(Qd) 12 | end 13 | n = length(s) 14 | n̄ = RD.state_diff_size(s) 15 | G = zeros(n,n̄) 16 | RD.state_diff_jacobian!(G, s, xf) 17 | Q_ = G*Q*G' 18 | # q = -Q_*xf 19 | # r = -R*uf 20 | # c = 0.5*xf'Q_*xf + 0.5*ur'R*uf 21 | LQRCost(Q_, R, xf, uf; kwargs...) 22 | end 23 | 24 | ############################################################################################ 25 | # QUADRATIC QUATERNION COST FUNCTION 26 | ############################################################################################ 27 | 28 | struct DiagonalQuatCost{N,M,T,N4} <: QuadraticCostFunction{N,M,T} 29 | Q::Diagonal{T,SVector{N,T}} 30 | R::Diagonal{T,SVector{M,T}} 31 | q::SVector{N,T} 32 | r::SVector{M,T} 33 | c::T 34 | w::T 35 | q_ref::SVector{4,T} 36 | q_ind::SVector{4,Int} 37 | Iq::SMatrix{N,4,T,N4} 38 | terminal::Bool 39 | function DiagonalQuatCost(Q::Diagonal{T,SVector{N,T}}, R::Diagonal{T,SVector{M,T}}, 40 | q::SVector{N,T}, r::SVector{M,T}, c::T, w::T, 41 | q_ref::SVector{4,T}, q_ind::SVector{4,Int}; terminal::Bool=false) where {T,N,M} 42 | Iq = @MMatrix zeros(N,4) 43 | for i = 1:4 44 | Iq[q_ind[i],i] = 1 45 | end 46 | Iq = SMatrix{N,4}(Iq) 47 | return new{N,M,T,N*4}(Q, R, q, r, c, w, q_ref, q_ind, Iq, terminal) 48 | end 49 | end 50 | 51 | state_dim(::DiagonalQuatCost{N,M,T}) where {T,N,M} = N 52 | control_dim(::DiagonalQuatCost{N,M,T}) where {T,N,M} = M 53 | is_blockdiag(::DiagonalQuatCost) = true 54 | is_diag(::DiagonalQuatCost) = true 55 | 56 | function DiagonalQuatCost(Q::Diagonal{T,SVector{N,T}}, R::Diagonal{T,SVector{M,T}}; 57 | q=(@SVector zeros(N)), r=(@SVector zeros(M)), c=zero(T), w=one(T), 58 | q_ref=(@SVector [1.0,0,0,0]), q_ind=(@SVector [4,5,6,7])) where {T,N,M} 59 | DiagonalQuatCost(Q, R, q, r, c, q_ref, q_ind) 60 | end 61 | 62 | function stage_cost(cost::DiagonalQuatCost, x::SVector, u::SVector) 63 | stage_cost(cost, x) + 0.5*u'cost.R*u + cost.r'u 64 | end 65 | 66 | function stage_cost(cost::DiagonalQuatCost, x::SVector) 67 | J = 0.5*x'cost.Q*x + cost.q'x + cost.c 68 | q = x[cost.q_ind] 69 | dq = cost.q_ref'q 70 | J += cost.w*min(1+dq, 1-dq) 71 | end 72 | 73 | function gradient!(E::QuadraticCostFunction, cost::DiagonalQuatCost{T,N,M}, 74 | x::SVector) where {T,N,M} 75 | Qx = cost.Q*x + cost.q 76 | q = x[cost.q_ind] 77 | dq = cost.q_ref'q 78 | if dq < 0 79 | Qx += cost.w*cost.Iq*cost.q_ref 80 | else 81 | Qx -= cost.w*cost.Iq*cost.q_ref 82 | end 83 | E.q .= Qx 84 | return false 85 | end 86 | 87 | function QuatLQRCost(Q::Diagonal{T,SVector{N,T}}, R::Diagonal{T,SVector{M,T}}, xf, 88 | uf=(@SVector zeros(M)); w=one(T), quat_ind=(@SVector [4,5,6,7])) where {T,N,M} 89 | r = -R*uf 90 | q = -Q*xf 91 | c = 0.5*xf'Q*xf + 0.5*uf'R*uf 92 | q_ref = xf[quat_ind] 93 | return DiagonalQuatCost(Q, R, q, r, c, w, q_ref, quat_ind) 94 | end 95 | 96 | function change_dimension(cost::DiagonalQuatCost, n, m, ix, iu) 97 | Qd = zeros(n) 98 | Rd = zeros(m) 99 | q = zeros(n) 100 | r = zeros(m) 101 | Qd[ix] = diag(cost.Q) 102 | Rd[iu] = diag(cost.R) 103 | q[ix] = cost.q 104 | r[iu] = cost.r 105 | qind = (1:n)[ix[cost.q_ind]] 106 | DiagonalQuatCost(Diagonal(SVector{n}(Qd)), Diagonal(SVector{m}(Rd)), 107 | SVector{n}(q), SVector{m}(r), cost.c, cost.w, cost.q_ref, qind) 108 | end 109 | 110 | function (+)(cost1::DiagonalQuatCost, cost2::TO.QuadraticCostFunction) 111 | @assert state_dim(cost1) == state_dim(cost2) 112 | @assert control_dim(cost1) == control_dim(cost2) 113 | is_diag(cost2) || @assert norm(cost2.H) ≈ 0 114 | DiagonalQuatCost(cost1.Q + cost2.Q, cost1.R + cost2.R, 115 | cost1.q + cost2.q, cost1.r + cost2.r, cost1.c + cost2.c, 116 | cost1.w, cost1.q_ref, cost1.q_ind) 117 | end 118 | 119 | (+)(cost1::TO.QuadraticCostFunction, cost2::DiagonalQuatCost) = cost2 + cost1 120 | 121 | function Base.copy(c::DiagonalQuatCost) 122 | DiagonalQuatCost(c.Q, c.R, c.q, c.r, c.c, c.w, c.q_ref, c.q_ind) 123 | end 124 | 125 | 126 | ############################################################################################ 127 | # Error Quadratic 128 | ############################################################################################ 129 | 130 | struct ErrorQuadratic{Rot,N,M} <: CostFunction 131 | model::RD.RigidBody{Rot} 132 | Q::Diagonal{Float64,SVector{12,Float64}} 133 | R::Diagonal{Float64,SVector{M,Float64}} 134 | r::SVector{M,Float64} 135 | c::Float64 136 | x_ref::SVector{N,Float64} 137 | q_ind::SVector{4,Int} 138 | end 139 | function Base.copy(c::ErrorQuadratic) 140 | ErrorQuadratic(c.model, c.Q, c.R, c.r, c.c, c.x_ref, c.q_ind) 141 | end 142 | 143 | state_dim(::ErrorQuadratic{Rot,N,M}) where {Rot,N,M} = N 144 | control_dim(::ErrorQuadratic{Rot,N,M}) where {Rot,N,M} = M 145 | 146 | function ErrorQuadratic(model::RD.RigidBody{Rot}, Q::Diagonal{T,<:SVector{N0}}, 147 | R::Diagonal{T,<:SVector{M}}, 148 | x_ref::SVector{N}, 149 | u_ref=(@SVector zeros(T,M)); 150 | r=(@SVector zeros(T,M)), 151 | c=zero(T), 152 | q_ind=(@SVector [4,5,6,7]) 153 | ) where {T,N,N0,M,Rot} 154 | if Rot <: UnitQuaternion && N0 == N 155 | Qd = deleteat(Q.diag, 4) 156 | Q = Diagonal(Qd) 157 | end 158 | r += -R*u_ref 159 | c += 0.5*u_ref'R*u_ref 160 | return ErrorQuadratic{Rot,N,M}(model, Q, R, r, c, x_ref, q_ind) 161 | end 162 | 163 | 164 | function stage_cost(cost::ErrorQuadratic, x::SVector) 165 | dx = RD.state_diff(cost.model, x, cost.x_ref, Rotations.ExponentialMap()) 166 | return 0.5*dx'cost.Q*dx + cost.c 167 | end 168 | 169 | function stage_cost(cost::ErrorQuadratic, x::SVector, u::SVector) 170 | stage_cost(cost, x) + 0.5*u'cost.R*u + cost.r'u 171 | end 172 | 173 | 174 | function gradient!(E::QuadraticCostFunction, cost::ErrorQuadratic, x) 175 | f(x) = stage_cost(cost, x) 176 | ForwardDiff.gradient!(E.q, f, x) 177 | return false 178 | 179 | model = cost.model 180 | Q = cost.Q 181 | q = RD.orientation(model, x) 182 | q_ref = RD.orientation(model, cost.x_ref) 183 | dq = Rotations.params(q_ref \ q) 184 | err = RD.state_diff(model, x, cost.x_ref) 185 | dx = @SVector [err[1], err[2], err[3], 186 | dq[1], dq[2], dq[3], dq[4], 187 | err[7], err[8], err[9], 188 | err[10], err[11], err[12]] 189 | # G = state_diff_jacobian(model, dx) # n × dn 190 | 191 | # Gradient 192 | dmap = inverse_map_jacobian(model, dx) # dn × n 193 | # Qx = G'dmap'Q*err 194 | Qx = dmap'Q*err 195 | E.q = Qx 196 | return false 197 | end 198 | function gradient!(E::QuadraticCostFunction, cost::ErrorQuadratic, x, u) 199 | gradient!(E, cost, x) 200 | Qu = cost.R*u 201 | E.r .= Qu 202 | return false 203 | end 204 | 205 | function hessian!(E::QuadraticCostFunction, cost::ErrorQuadratic, x) 206 | f(x) = stage_cost(cost, x) 207 | ForwardDiff.hessian!(E.Q, f, x) 208 | return false 209 | 210 | model = cost.model 211 | Q = cost.Q 212 | q = RD.orientation(model, x) 213 | q_ref = RD.orientation(model, cost.x_ref) 214 | dq = Rotations.params(q_ref\q) 215 | err = RD.state_diff(model, x, cost.x_ref) 216 | dx = @SVector [err[1], err[2], err[3], 217 | dq[1], dq[2], dq[3], dq[4], 218 | err[7], err[8], err[9], 219 | err[10], err[11], err[12]] 220 | # G = state_diff_jacobian(model, dx) # n × dn 221 | 222 | # Gradient 223 | dmap = inverse_map_jacobian(model, dx) # dn × n 224 | 225 | # Hessian 226 | ∇jac = inverse_map_∇jacobian(model, dx, Q*err) 227 | # Qxx = G'dmap'Q*dmap*G + G'∇jac*G + ∇²differential(model, x, dmap'Q*err) 228 | Qxx = dmap'Q*dmap + ∇jac #+ ∇²differential(model, x, dmap'Q*err) 229 | E.Q = Qxx 230 | E.H .*= 0 231 | return false 232 | end 233 | 234 | function hessian!(E::QuadraticCostFunction, cost::ErrorQuadratic, x, u) 235 | hessian!(E, cost, x) 236 | E.R .= cost.R 237 | return false 238 | end 239 | 240 | 241 | function change_dimension(cost::ErrorQuadratic, n, m) 242 | n0,m0 = state_dim(cost), control_dim(cost) 243 | Q_diag = diag(cost.Q) 244 | R_diag = diag(cost.R) 245 | r = cost.r 246 | if n0 != n 247 | dn = n - n0 # assumes n > n0 248 | pad = @SVector zeros(dn) # assume the new states don't have quaternions 249 | Q_diag = [Q_diag; pad] 250 | end 251 | if m0 != m 252 | dm = m - m0 # assumes m > m0 253 | pad = @SVector zeros(dm) 254 | R_diag = [R_diag; pad] 255 | r = [r; pad] 256 | end 257 | ErrorQuadratic(cost.model, Diagonal(Q_diag), Diagonal(R_diag), r, cost.c, 258 | cost.x_ref, cost.q_ind) 259 | end 260 | 261 | function (+)(cost1::ErrorQuadratic, cost2::QuadraticCost) 262 | @assert control_dim(cost1) == control_dim(cost2) 263 | @assert norm(cost2.H) ≈ 0 264 | @assert norm(cost2.q) ≈ 0 265 | if state_dim(cost2) == 13 266 | rm_quat = @SVector [1,2,3,4,5,6,8,9,10,11,12,13] 267 | Q2 = Diagonal(diag(cost2.Q)[rm_quat]) 268 | else 269 | Q2 = cost2.Q 270 | end 271 | ErrorQuadratic(cost1.model, cost1.Q + Q2, cost1.R + cost2.R, 272 | cost1.r + cost2.r, cost1.c + cost2.c, 273 | cost1.x_ref, cost1.q_ind) 274 | end 275 | 276 | (+)(cost1::QuadraticCost, cost2::ErrorQuadratic) = cost2 + cost1 277 | 278 | @generated function state_diff_jacobian(model::RD.RigidBody{<:UnitQuaternion}, 279 | x0::SVector{N,T}, errmap::D=Rotations.CayleyMap()) where {N,T,D} 280 | if D <: IdentityMap 281 | :(I) 282 | else 283 | quote 284 | q0 = RD.orientation(model, x0) 285 | # G = TrajectoryOptimization.∇differential(q0) 286 | G = Rotations.∇differential(q0) 287 | I1 = @SMatrix [1 0 0 0 0 0 0 0 0 0 0 0; 288 | 0 1 0 0 0 0 0 0 0 0 0 0; 289 | 0 0 1 0 0 0 0 0 0 0 0 0; 290 | 0 0 0 G[1] G[5] G[ 9] 0 0 0 0 0 0; 291 | 0 0 0 G[2] G[6] G[10] 0 0 0 0 0 0; 292 | 0 0 0 G[3] G[7] G[11] 0 0 0 0 0 0; 293 | 0 0 0 G[4] G[8] G[12] 0 0 0 0 0 0; 294 | 0 0 0 0 0 0 1 0 0 0 0 0; 295 | 0 0 0 0 0 0 0 1 0 0 0 0; 296 | 0 0 0 0 0 0 0 0 1 0 0 0; 297 | 0 0 0 0 0 0 0 0 0 1 0 0; 298 | 0 0 0 0 0 0 0 0 0 0 1 0; 299 | 0 0 0 0 0 0 0 0 0 0 0 1.] 300 | end 301 | end 302 | end 303 | function inverse_map_jacobian(model::RD.RigidBody{<:UnitQuaternion}, 304 | x::SVector, errmap=Rotations.CayleyMap()) 305 | q = RD.orientation(model, x) 306 | # G = TrajectoryOptimization.inverse_map_jacobian(q) 307 | G = Rotations.jacobian(inv(errmap), q) 308 | return @SMatrix [ 309 | 1 0 0 0 0 0 0 0 0 0 0 0 0; 310 | 0 1 0 0 0 0 0 0 0 0 0 0 0; 311 | 0 0 1 0 0 0 0 0 0 0 0 0 0; 312 | 0 0 0 G[1] G[4] G[7] G[10] 0 0 0 0 0 0; 313 | 0 0 0 G[2] G[5] G[8] G[11] 0 0 0 0 0 0; 314 | 0 0 0 G[3] G[6] G[9] G[12] 0 0 0 0 0 0; 315 | 0 0 0 0 0 0 0 1 0 0 0 0 0; 316 | 0 0 0 0 0 0 0 0 1 0 0 0 0; 317 | 0 0 0 0 0 0 0 0 0 1 0 0 0; 318 | 0 0 0 0 0 0 0 0 0 0 1 0 0; 319 | 0 0 0 0 0 0 0 0 0 0 0 1 0; 320 | 0 0 0 0 0 0 0 0 0 0 0 0 1; 321 | ] 322 | end 323 | 324 | function inverse_map_∇jacobian(model::RD.RigidBody{<:UnitQuaternion}, 325 | x::SVector, b::SVector, errmap=Rotations.CayleyMap()) 326 | q = RD.orientation(model, x) 327 | bq = @SVector [b[4], b[5], b[6]] 328 | # ∇G = TrajectoryOptimization.inverse_map_∇jacobian(q, bq) 329 | ∇G = Rotations.∇jacobian(inv(errmap), q, bq) 330 | return @SMatrix [ 331 | 0 0 0 0 0 0 0 0 0 0 0 0 0; 332 | 0 0 0 0 0 0 0 0 0 0 0 0 0; 333 | 0 0 0 0 0 0 0 0 0 0 0 0 0; 334 | 0 0 0 ∇G[1] ∇G[5] ∇G[ 9] ∇G[13] 0 0 0 0 0 0; 335 | 0 0 0 ∇G[2] ∇G[6] ∇G[10] ∇G[14] 0 0 0 0 0 0; 336 | 0 0 0 ∇G[3] ∇G[7] ∇G[11] ∇G[15] 0 0 0 0 0 0; 337 | 0 0 0 ∇G[4] ∇G[8] ∇G[12] ∇G[16] 0 0 0 0 0 0; 338 | 0 0 0 0 0 0 0 0 0 0 0 0 0; 339 | 0 0 0 0 0 0 0 0 0 0 0 0 0; 340 | 0 0 0 0 0 0 0 0 0 0 0 0 0; 341 | 0 0 0 0 0 0 0 0 0 0 0 0 0; 342 | 0 0 0 0 0 0 0 0 0 0 0 0 0; 343 | 0 0 0 0 0 0 0 0 0 0 0 0 0; 344 | ] 345 | end 346 | 347 | # function cost_expansion(cost::ErrorQuadratic{Rot}, model::AbstractModel, 348 | # z::KnotPoint{T,N,M}, G) where {T,N,M,Rot<:UnitQuaternion} 349 | # x,u = state(z), control(z) 350 | # model = cost.model 351 | # Q = cost.Q 352 | # q = orientation(model, x) 353 | # q_ref = orientation(model, cost.x_ref) 354 | # dq = SVector(q_ref\q) 355 | # err = state_diff(model, x, cost.x_ref) 356 | # dx = @SVector [err[1], err[2], err[3], 357 | # dq[1], dq[2], dq[3], dq[4], 358 | # err[7], err[8], err[9], 359 | # err[10], err[11], err[12]] 360 | # G = state_diff_jacobian(model, dx) # n × dn 361 | 362 | # # Gradient 363 | # dmap = inverse_map_jacobian(model, dx) # dn × n 364 | # Qx = G'dmap'Q*err 365 | # Qu = cost.R*u 366 | 367 | # # Hessian 368 | # ∇jac = inverse_map_∇jacobian(model, dx, Q*err) 369 | # Qxx = G'dmap'Q*dmap*G + G'∇jac*G + ∇²differential(model, x, dmap'Q*err) 370 | # Quu = cost.R 371 | # Qux = @SMatrix zeros(M,N-1) 372 | # return Qxx, Quu, Qux, Qx, Qu 373 | # end 374 | 375 | # function cost_expansion(cost::ErrorQuadratic, model::AbstractModel, 376 | # z::KnotPoint{T,N,M}, G) where {T,N,M} 377 | # x,u = state(z), control(z) 378 | # model = cost.model 379 | # q = orientation(model, x) 380 | # q_ref = orientation(model, cost.x_ref) 381 | # err = state_diff(model, x, cost.x_ref) 382 | # dx = err 383 | # G = state_diff_jacobian(model, dx) # n × n 384 | 385 | # # Gradient 386 | # dmap = inverse_map_jacobian(model, dx) # n × n 387 | # Qx = G'dmap'cost.Q*err 388 | # Qu = cost.R*u + cost.r 389 | 390 | # # Hessian 391 | # Qxx = G'dmap'cost.Q*dmap*G 392 | # Quu = cost.R 393 | # Qux = @SMatrix zeros(M,N) 394 | # return Qxx, Quu, Qux, Qx, Qu 395 | # end 396 | -------------------------------------------------------------------------------- /src/quat_norm.jl: -------------------------------------------------------------------------------- 1 | struct QuatRenorm{R,L} <: RD.RigidBody{R} 2 | model::L 3 | function QuatRenorm(model::RD.RigidBody{R}) where R 4 | new{R,typeof(model)}(model) 5 | end 6 | end 7 | RD.control_dim(model::QuatRenorm) = RD.control_dim(model.model) 8 | 9 | function RD.discrete_dynamics(::Type{Q}, 10 | model::QuatRenorm, x::StaticVector, u::StaticVector, t, dt) where Q 11 | x2 = RD.discrete_dynamics(Q, model.model, x, u, t, dt) 12 | # renormalize the quaternion 13 | r,q,v,ω = RobotDynamics.parse_state(model.model, x2, true) 14 | RobotDynamics.build_state(model.model, r,q,v,ω) 15 | end 16 | # RD.wrenches(model::QuatRenorm, z::RD.AbstractKnotPoint) = RD.wrenches(model.model, z) 17 | # RD.forces(model::QuatRenorm, x, u) = RD.forces(model.model, x, u) 18 | # RD.moments(model::QuatRenorm, x::StaticVector, u::StaticVector) = RD.moments(model.model, x, u) 19 | # RD.mass(model::QuatRenorm) = RD.mass(model.model) 20 | # RD.inertia(model::QuatRenorm) = RD.inertia(model.model) 21 | # RD.inertia_inv(model::QuatRenorm) = RD.inertia_inv(model.model) 22 | # RD.velocity_frame(model::QuatRenorm) = RD.velocity_frame(model.model) 23 | 24 | 25 | 26 | struct QuatSlackModel{R,M,L} <: RD.RigidBody{R} 27 | model::L 28 | function QuatSlackModel(model::L) where L <: RD.RigidBody 29 | M = RD.control_dim(model) 30 | R = RD.rotation_type(model) 31 | new{R,M,L}(model) 32 | end 33 | end 34 | RobotDynamics.state_dim(model::QuatSlackModel) = RobotDynamics.state_dim(model.model) 35 | RobotDynamics.control_dim(::QuatSlackModel{<:Any,M}) where M = M+1 36 | 37 | function RobotDynamics.discrete_dynamics(::Type{Q}, model::QuatSlackModel, x, u, t, dt) where {Q} 38 | s = u[end] # quaternion slack 39 | u0 = pop(u) # original controls 40 | x2 = RD.discrete_dynamics(Q, model.model, x, u, t, dt) 41 | r,q,v,ω = RobotDynamics.parse_state(model, x2) 42 | q̄ = (1-s)*q # scale the quaternion by the extra control 43 | x̄ = RobotDynamics.build_state(model, r, q̄, v, ω) 44 | end 45 | 46 | """ 47 | UnitQuatConstraint 48 | 49 | Enforce a that the unit quaternion in the state, at indices `qind`, has unit norm 50 | after being multiplied by a slack variable at index `sind` in the joint state-control vector 51 | `z = [x;u]`. 52 | """ 53 | struct UnitQuatConstraint <: TrajectoryOptimization.StateConstraint 54 | n::Int 55 | qind::SVector{4,Int} 56 | end 57 | 58 | function UnitQuatConstraint(n::Int, qind=4:7) 59 | UnitQuatConstraint(n, SVector{4}(qind)) 60 | end 61 | 62 | UnitQuatConstraint(model::QuatSlackModel) = UnitQuatConstraint(state_dim(model)) 63 | 64 | Base.copy(con::UnitQuatConstraint) = UnitQuatConstraint(con.n, con.qind) 65 | 66 | @inline TO.sense(::UnitQuatConstraint) = TO.Equality() 67 | @inline Base.length(::UnitQuatConstraint) = 1 68 | @inline RobotDynamics.state_dim(con::UnitQuatConstraint) = con.n 69 | 70 | function TO.evaluate(con::UnitQuatConstraint, z::RD.AbstractKnotPoint) 71 | q = z.z[con.qind] 72 | return SA[q'q - 1] 73 | end 74 | 75 | function TO.jacobian!(∇c, con::UnitQuatConstraint, z::RD.AbstractKnotPoint) 76 | q = z.z[con.qind] 77 | for (i,j) in enumerate(con.qind) 78 | ∇c[1,j] = 2*q[i] 79 | end 80 | # ∇c[con.sind] = 2s*dot(q,q) 81 | return false 82 | end 83 | 84 | function TO.change_dimension(con::UnitQuatConstraint, n::Int, m::Int, xi=1:n, ui=1:m) 85 | UnitQuatConstraint(n, xi[con.qind]) 86 | end -------------------------------------------------------------------------------- /src/vecmodel.jl: -------------------------------------------------------------------------------- 1 | struct VecModel{L} <: RD.AbstractModel 2 | model::L 3 | VecModel(model::L) where L <: RD.LieGroupModel = new{L}(model) 4 | end 5 | 6 | RobotDynamics.state_dim(model::VecModel) = RD.state_dim(model.model) 7 | RobotDynamics.control_dim(model::VecModel) = RD.control_dim(model.model) 8 | RobotDynamics.discrete_dynamics(::Type{Q}, model::VecModel, z::RobotDynamics.AbstractKnotPoint) where Q <: RD.Explicit = 9 | discrete_dynamics(Q, model.model, z) 10 | RobotDynamics.discrete_dynamics(::Type{Q}, model::VecModel, args...) where Q <: RD.Explicit = 11 | discrete_dynamics(Q, model.model, args...) 12 | # RobotDynamics.dynamics(model::VecModel, args...) = RD.dynamics(model.model, args...) -------------------------------------------------------------------------------- /test/quatslack.jl: -------------------------------------------------------------------------------- 1 | using Test 2 | using PlanningWithAttitude 3 | using Rotations 4 | using RobotDynamics 5 | using TrajectoryOptimization 6 | using Altro 7 | using StaticArrays, LinearAlgebra 8 | using ForwardDiff 9 | import RobotZoo.Quadrotor 10 | const TO = TrajectoryOptimization 11 | 12 | ## Test renorm 13 | quad = Quadrotor() 14 | x,u = rand(quad) 15 | x2 = discrete_dynamics(RK4,quad,x,u,0.0,0.1) 16 | q = orientation(quad, x) 17 | q2 = orientation(quad, x2) 18 | norm(q) 19 | norm(q2) 20 | 21 | model = QuatRenorm(quad,RK4) 22 | x3 = discrete_dynamics(RK4, model, x, u, 0.0, 0.1) 23 | q3 = orientation(quad, x3) 24 | 25 | ## Test model 26 | quad = Quadrotor() 27 | model = QuatSlackModel(quad) 28 | @test state_dim(quad) == 13 == state_dim(model) 29 | @test control_dim(model) == 5 30 | x,u = rand(quad) 31 | r,q,v,ω = RobotDynamics.parse_state(model, x) 32 | s = rand() 33 | x2 = RobotDynamics.build_state(model, r, q*(1/s), v, ω) 34 | x3 = RobotDynamics.build_state(model, r, q*s, v, ω) 35 | u2 = push(u, s) 36 | @test dynamics(model, x2, u2) ≈ dynamics(quad, x, u) 37 | @test dynamics(model, x, u2) ≈ dynamics(quad, x3, u) 38 | 39 | ## Test constraint 40 | con = UnitQuatConstraint(model) 41 | z = KnotPoint(x2,u2,0.1) 42 | z2 = KnotPoint(x,u2,0.1) 43 | @test TO.evaluate(con,z)[1] ≈ 1/s^2-1 atol=1e-9 44 | @test TO.evaluate(con,z2)[1] ≈ 0 atol=1e-12 45 | 46 | c(x) = TO.evaluate(con, StaticKnotPoint(z, x)) 47 | ∇c = ForwardDiff.jacobian(c, z.z) 48 | ∇c2 = zeros(1,18) 49 | TO.jacobian!(∇c2, con, z) 50 | q1 = z.z[4:7] 51 | @test ∇c2[4:7] ≈ 2*q1 52 | @test ∇c2 ≈ ∇c 53 | 54 | ∇c = ForwardDiff.jacobian(c, z2.z) 55 | TO.jacobian!(∇c2, con, z2) 56 | @test ∇c2[4:7] ≈ 2*Rotations.params(q) 57 | @test ∇c2 ≈ ∇c 58 | 59 | ## Test VecModel 60 | model2 = VecModel(model) 61 | @test dynamics(model2, x2, u2) ≈ dynamics(model, x2, u2) 62 | @test state_diff_size(model2) == 13 63 | @test RobotDynamics.state_diff(model2, x2, x) ≈ x2 - x 64 | @test !(model2 isa LieGroupModel) 65 | 66 | ## Test in a problem 67 | n,m = size(model) 68 | N,tf = 101, 5.0 69 | 70 | Q = Diagonal(RobotDynamics.fill_state(model, 1,10,10,10.)) 71 | R = Diagonal(SA[1,1,1,1,100.]*1e-2) 72 | x0 = zeros(model)[1] 73 | xf = RobotDynamics.build_state(model, 74 | RBState([2,1,0.5], expm(SA[0,0,1]*pi/4), zeros(3), zeros(3)) 75 | ) 76 | Qf = (N-1)*Q 77 | utrim = push(zeros(quad)[2],1) 78 | obj = LQRObjective(Q, R, Qf, xf, N, uf=utrim) 79 | 80 | cons = ConstraintList(n,m,N) 81 | slack = UnitQuatConstraint(model) 82 | noquat = deleteat(SVector{13}(1:13),4) 83 | goal = GoalConstraint(xf, noquat) 84 | add_constraint!(cons, slack, 1:N-1) 85 | add_constraint!(cons, goal, N) 86 | 87 | prob = Problem(model2, obj, xf, tf, x0=x0, constraints=cons) 88 | initial_controls!(prob, utrim) 89 | rollout!(prob) 90 | Z0 = copy(prob.Z) 91 | solver = ALTROSolver(prob) 92 | set_options!(solver, verbose=0, show_summary=true, projected_newton=true) 93 | solve!(solver) 94 | norm(RBState(model, states(solver)[end]) ⊖ RBState(model, xf)) 95 | X = states(solver) 96 | [norm(x[4:7]) for x in X] 97 | @test size(solver.solver_al.solver_uncon.K[1]) == (5,13) 98 | 99 | initial_trajectory!(solver, Z0) 100 | set_options!(solver, show_summary=false) 101 | b1 = benchmark_solve!(solver) 102 | iterations(solver) 103 | minimum(b1).time/iterations(solver) / 1e6 # ms/iter 104 | 105 | ## Test new method 106 | utrim0 = pop(utrim) 107 | R0 = Diagonal(pop(R.diag)) 108 | obj0 = LQRObjective(Q, R0, Qf, xf, N, uf=utrim0) 109 | cons0 = ConstraintList(size(quad)...,N) 110 | add_constraint!(cons0, goal, N) 111 | prob0 = Problem(quad, obj0, xf, tf, x0=x0, constraints=cons0) 112 | initial_controls!(prob0, utrim0) 113 | rollout!(prob0) 114 | Z0 = copy(prob0.Z) 115 | 116 | solver0 = ALTROSolver(prob0, show_summary=true) 117 | solve!(solver0) 118 | norm(RBState(quad, states(solver0)[end]) ⊖ RBState(quad, xf)) 119 | [norm(x[4:7]) for x in states(solver0)] 120 | 121 | initial_trajectory!(solver0, Z0) 122 | set_options!(solver0, show_summary=false) 123 | b0 = benchmark_solve!(solver0) 124 | iterations(solver0) 125 | minimum(b0).time/iterations(solver0) / 1e6 # ms/iter 126 | 127 | ## Normal quad problem 128 | R0 = Diagonal(pop(R.diag)) 129 | utrim0 = zeros(quad)[2] 130 | obj0 = LQRObjective(Q, R0, (N-1)*Q, xf, N, uf=utrim0) 131 | cons0 = ConstraintList(n,m-1,N) 132 | add_constraint!(cons0, GoalConstraint(xf), N) 133 | 134 | prob0 = Problem(quad, obj0, xf, tf, x0=x0, constraints=cons0) 135 | initial_controls!(prob0, utrim0) 136 | solver0 = ALTROSolver(prob0, show_summary=true) 137 | solve!(solver0) 138 | 139 | ## Ipopt 140 | using Ipopt 141 | using MathOptInterface 142 | const MOI = MathOptInterface 143 | 144 | prob = Problem(model, obj, xf, tf, x0=x0, constraints=copy(cons)) 145 | initial_controls!(prob, utrim) 146 | prob = Problem(quad, obj0, xf, tf, x0=x0, constraints=copy(cons0)) 147 | initial_controls!(prob, utrim0) 148 | rollout!(prob) 149 | TO.num_constraints(prob) 150 | TO.add_dynamics_constraints!(prob) 151 | nlp = TO.TrajOptNLP(prob, remove_bounds=true, jac_type=:vector) 152 | 153 | optimizer = Ipopt.Optimizer(max_iter=1000, 154 | tol=1e-4, constr_viol_tol=1e-4, dual_inf_tol=1e-4, compl_inf_tol=1e-4) 155 | TO.build_MOI!(nlp, optimizer) 156 | MOI.optimize!(optimizer) 157 | max_violation(nlp) 158 | 159 | ## Visualization 160 | using TrajOptPlots 161 | using MeshCat, Blink 162 | if !isdefined(Main, :vis) 163 | vis = Visualizer() 164 | open(vis, Blink.Window()) 165 | end 166 | TrajOptPlots.set_mesh!(vis, prob.model) 167 | visualize!(vis, nlp) 168 | visualize!(vis, solver0) 169 | 170 | 171 | ## Try another problem 172 | prob,opts = Altro.Problems.Quadrotor(:zigzag, MRP) 173 | prob, = Problems.YakProblems() 174 | TO.add_dynamics_constraints!(prob) 175 | rollout!(prob) 176 | nlp = TO.TrajOptNLP(prob, remove_bounds=true, jac_type=:vector) 177 | optimizer = Ipopt.Optimizer(max_iter=500, hessian_approximation="limited-memory") 178 | TO.build_MOI!(nlp, optimizer) 179 | MOI.optimize!(optimizer) 180 | 181 | x = nlp.Z.Z 182 | λ = nlp.data.λ 183 | D,d = nlp.data.D, nlp.data.d 184 | H,g = nlp.data.G, nlp.data.g --------------------------------------------------------------------------------