diff --git a/LICENSE b/LICENSE
new file mode 100644
--- /dev/null
+++ b/LICENSE
@@ -0,0 +1,165 @@
+ GNU LESSER GENERAL PUBLIC LICENSE
+ Version 3, 29 June 2007
+
+ Copyright (C) 2007 Free Software Foundation, Inc.
+ Everyone is permitted to copy and distribute verbatim copies
+ of this license document, but changing it is not allowed.
+
+
+ This version of the GNU Lesser General Public License incorporates
+the terms and conditions of version 3 of the GNU General Public
+License, supplemented by the additional permissions listed below.
+
+ 0. Additional Definitions.
+
+ As used herein, "this License" refers to version 3 of the GNU Lesser
+General Public License, and the "GNU GPL" refers to version 3 of the GNU
+General Public License.
+
+ "The Library" refers to a covered work governed by this License,
+other than an Application or a Combined Work as defined below.
+
+ An "Application" is any work that makes use of an interface provided
+by the Library, but which is not otherwise based on the Library.
+Defining a subclass of a class defined by the Library is deemed a mode
+of using an interface provided by the Library.
+
+ A "Combined Work" is a work produced by combining or linking an
+Application with the Library. The particular version of the Library
+with which the Combined Work was made is also called the "Linked
+Version".
+
+ The "Minimal Corresponding Source" for a Combined Work means the
+Corresponding Source for the Combined Work, excluding any source code
+for portions of the Combined Work that, considered in isolation, are
+based on the Application, and not on the Linked Version.
+
+ The "Corresponding Application Code" for a Combined Work means the
+object code and/or source code for the Application, including any data
+and utility programs needed for reproducing the Combined Work from the
+Application, but excluding the System Libraries of the Combined Work.
+
+ 1. Exception to Section 3 of the GNU GPL.
+
+ You may convey a covered work under sections 3 and 4 of this License
+without being bound by section 3 of the GNU GPL.
+
+ 2. Conveying Modified Versions.
+
+ If you modify a copy of the Library, and, in your modifications, a
+facility refers to a function or data to be supplied by an Application
+that uses the facility (other than as an argument passed when the
+facility is invoked), then you may convey a copy of the modified
+version:
+
+ a) under this License, provided that you make a good faith effort to
+ ensure that, in the event an Application does not supply the
+ function or data, the facility still operates, and performs
+ whatever part of its purpose remains meaningful, or
+
+ b) under the GNU GPL, with none of the additional permissions of
+ this License applicable to that copy.
+
+ 3. Object Code Incorporating Material from Library Header Files.
+
+ The object code form of an Application may incorporate material from
+a header file that is part of the Library. You may convey such object
+code under terms of your choice, provided that, if the incorporated
+material is not limited to numerical parameters, data structure
+layouts and accessors, or small macros, inline functions and templates
+(ten or fewer lines in length), you do both of the following:
+
+ a) Give prominent notice with each copy of the object code that the
+ Library is used in it and that the Library and its use are
+ covered by this License.
+
+ b) Accompany the object code with a copy of the GNU GPL and this license
+ document.
+
+ 4. Combined Works.
+
+ You may convey a Combined Work under terms of your choice that,
+taken together, effectively do not restrict modification of the
+portions of the Library contained in the Combined Work and reverse
+engineering for debugging such modifications, if you also do each of
+the following:
+
+ a) Give prominent notice with each copy of the Combined Work that
+ the Library is used in it and that the Library and its use are
+ covered by this License.
+
+ b) Accompany the Combined Work with a copy of the GNU GPL and this license
+ document.
+
+ c) For a Combined Work that displays copyright notices during
+ execution, include the copyright notice for the Library among
+ these notices, as well as a reference directing the user to the
+ copies of the GNU GPL and this license document.
+
+ d) Do one of the following:
+
+ 0) Convey the Minimal Corresponding Source under the terms of this
+ License, and the Corresponding Application Code in a form
+ suitable for, and under terms that permit, the user to
+ recombine or relink the Application with a modified version of
+ the Linked Version to produce a modified Combined Work, in the
+ manner specified by section 6 of the GNU GPL for conveying
+ Corresponding Source.
+
+ 1) Use a suitable shared library mechanism for linking with the
+ Library. A suitable mechanism is one that (a) uses at run time
+ a copy of the Library already present on the user's computer
+ system, and (b) will operate properly with a modified version
+ of the Library that is interface-compatible with the Linked
+ Version.
+
+ e) Provide Installation Information, but only if you would otherwise
+ be required to provide such information under section 6 of the
+ GNU GPL, and only to the extent that such information is
+ necessary to install and execute a modified version of the
+ Combined Work produced by recombining or relinking the
+ Application with a modified version of the Linked Version. (If
+ you use option 4d0, the Installation Information must accompany
+ the Minimal Corresponding Source and Corresponding Application
+ Code. If you use option 4d1, you must provide the Installation
+ Information in the manner specified by section 6 of the GNU GPL
+ for conveying Corresponding Source.)
+
+ 5. Combined Libraries.
+
+ You may place library facilities that are a work based on the
+Library side by side in a single library together with other library
+facilities that are not Applications and are not covered by this
+License, and convey such a combined library under terms of your
+choice, if you do both of the following:
+
+ a) Accompany the combined library with a copy of the same work based
+ on the Library, uncombined with any other library facilities,
+ conveyed under the terms of this License.
+
+ b) Give prominent notice with the combined library that part of it
+ is a work based on the Library, and explaining where to find the
+ accompanying uncombined form of the same work.
+
+ 6. Revised Versions of the GNU Lesser General Public License.
+
+ The Free Software Foundation may publish revised and/or new versions
+of the GNU Lesser General Public License from time to time. Such new
+versions will be similar in spirit to the present version, but may
+differ in detail to address new problems or concerns.
+
+ Each version is given a distinguishing version number. If the
+Library as you received it specifies that a certain numbered version
+of the GNU Lesser General Public License "or any later version"
+applies to it, you have the option of following the terms and
+conditions either of that published version or of any later version
+published by the Free Software Foundation. If the Library as you
+received it does not specify a version number of the GNU Lesser
+General Public License, you may choose any version of the GNU Lesser
+General Public License ever published by the Free Software Foundation.
+
+ If the Library as you received it specifies that a proxy can decide
+whether future versions of the GNU Lesser General Public License shall
+apply, that proxy's public statement of acceptance of any version is
+permanent authorization for you to choose that version for the
+Library.
diff --git a/README.md b/README.md
new file mode 100644
--- /dev/null
+++ b/README.md
@@ -0,0 +1,70 @@
+## dynobud - your dynamic optimization buddy
+
+This library has a few distinct features, which may later be broken into separate packages:
+* high-level, strongly-typed interface to CasADi
+* NLP modeling/solving (examples/Basic.hs, examples/BasicJ.hs)
+* monadic NLP modeling DSL (examples/StaticExample.hs)
+* OCP modeling/solving (examles/Glider.hs)
+* monadic OCP modeling DSL (examples/OcpM.hs, examples/Rocket.hs)
+* live plotter for OCP solving (examples/Plotter.hs)
+
+This package is built on top of CasADi (www.casadi.org).
+You will have to install the CasADi C++ libraries and the casadi-bindings haskell package.
+See http://hackage.haskell.org/package/casadi-bindings for instructions.
+Installing ipopt is also highly recommended if you want to solve NLPs (`apt-get install coinor-libipopt-dev` if you're lucky)
+
+Please keep in mind that this library is continually evolving as my PhD progresses and I expect it to be very unstable.
+The API is also very messy as the library is evolving fast and it's unclear which parts are internal and external.
+Specifically, the matrix and vector views (J and M) aren't polished enough and don't yet work without leaking internals.
+Nevertheless, I have started making hackage releases so that my few users have some snapshots to version-constrain against.
+
+To install:
+
+ >> cabal update
+ >> cabal install dynobud
+
+casadi-bindings will probably fail, re-read casadi-bindings instructions
+
+To install dependencies, you may need to do something like this:
+
+ >> cabal install alex
+ >> cabal install happy
+ >> cabal install gtk2hs-buildtools
+
+ >> sudo apt-get install coinor-libipopt-dev
+ >> sudo apt-get install liblapack-dev
+ >> sudo apt-get install libblas-dev
+ >> sudo apt-get install libglpk-dev
+ >> sudo apt-get install libgl1-mesa-dev
+ >> sudo apt-get install libglu1-mesa-dev
+ >> sudo apt-get install freeglut3-dev
+ >> sudo apt-get install libzmq3-dev
+ >> sudo apt-get install libglib2.0-dev
+ >> sudo apt-get install libcairo2-dev
+ >> sudo apt-get install libpango1.0-dev
+ >> sudo apt-get install libgtk2.0-dev
+ >> sudo apt-get install libgsl0-dev
+
+To build dynobud from source
+
+ >> git clone git://github.com:ghorn/dynobud.git
+ >> cd dynobud
+ >> cabal install --only-dependencies # without examples
+ >> cabal install --only-dependencies -fexamples # with examples
+ >> cabal configure
+ >> cabal build
+
+Try running the examples in dynobud/examples.
+
+ >> cabal configure -fexamples
+ >> cabal build
+ >> dist/build/rocket/rocket
+
+Known issues:
+
+ "user error: out of memory"
+
+If you get this ^ error on OSX while using the plotting tools, your
+cairo/pango/gtk may be linked to an XQuartz library.
+Add "extra-lib-dirs=/usr/local/lib" (or wherever the correct libraries are)
+to your .cabal/config and re-install haskell bindings to cairo/pango/gtk/etc
diff --git a/Setup.hs b/Setup.hs
new file mode 100644
--- /dev/null
+++ b/Setup.hs
@@ -0,0 +1,2 @@
+import Distribution.Simple
+main = defaultMain
diff --git a/dynobud.cabal b/dynobud.cabal
new file mode 100644
--- /dev/null
+++ b/dynobud.cabal
@@ -0,0 +1,396 @@
+name: dynobud
+version: 1.0.0.0
+synopsis: your dynamic optimization buddy
+description: See readme at
+license: LGPL-3
+license-file: LICENSE
+author: Greg Horn
+maintainer: gregmainland@gmail.com
+copyright: (c) Greg Horn 2013-2015
+category: Science
+build-type: Simple
+cabal-version: >=1.10
+extra-source-files: README.md
+stability: Experimental
+
+source-repository head
+ type: git
+ location: git://github.com/ghorn/dynobud.git
+
+library
+ exposed-modules: Dyno.LagrangePolynomials
+ Dyno.TypeVecs
+ Dyno.Nats
+ Dyno.MultipleShooting
+ Dyno.Dae
+ Dyno.Models.Aircraft
+ Dyno.Models.AeroCoeffs
+ Dyno.Models.Betty
+ Dyno.Ocp
+ Dyno.DirectCollocation
+ Dyno.DirectCollocation.Dynamic
+ Dyno.DirectCollocation.Export
+ Dyno.DirectCollocation.Formulate
+ Dyno.DirectCollocation.Integrate
+ Dyno.DirectCollocation.Profile
+ Dyno.DirectCollocation.Quadratures
+ Dyno.DirectCollocation.Reify
+ Dyno.DirectCollocation.Robust
+ Dyno.DirectCollocation.Types
+ Dyno.Cov
+ Dyno.SXElement
+ Dyno.View
+ Dyno.View.CasadiMat
+ Dyno.View.CustomFunction
+ Dyno.View.Fun
+ Dyno.View.FunJac
+ Dyno.View.HList
+ Dyno.View.JV
+ Dyno.View.M
+ Dyno.View.NumInstances
+ Dyno.View.Scheme
+ Dyno.View.Symbolic
+ Dyno.View.View
+ Dyno.View.Viewable
+ Dyno.Vectorize
+ Dyno.Nlp
+ Dyno.NlpMonad
+ Dyno.NlpScaling
+ Dyno.NlpSolver
+ Dyno.OcpMonad
+ Dyno.Interface.LogsAndErrors
+ Dyno.Interface.Types
+ Dyno.Solvers
+-- Dyno.Sqp.Sqp
+-- Dyno.Sqp.LineSearch
+ Dyno.Server.Accessors
+ Dyno.Server.GraphWidget
+ Dyno.Server.PlotChart
+ Dyno.Server.PlotTypes
+ Dyno.Server.Server
+
+ other-modules:
+ build-depends: base >=4.6 && < 5,
+ vector >=0.10,
+ data-default,
+ mtl >=2.2.1,
+ containers >=0.5,
+ jacobi-roots >=0.2 && <0.3,
+ hmatrix,
+ unordered-containers >=0.2,
+ casadi-bindings-internal,
+ casadi-bindings-core >= 2.2.0.2,
+ casadi-bindings >= 2.2.0.2,
+ transformers >=0.3,
+ primitive >=0.5 && <0.6,
+ ghc-prim >=0.3 && <0.4,
+ linear >= 1.3.1.1,
+ spatial-math >= 0.2.1.0,
+ tagged >= 0.6,
+ reflection >= 1.3.2,
+ lens,
+ cereal,
+ glib,
+ time,
+ stm,
+ gtk >= 0.13,
+ Chart-cairo >= 1.3.3,
+ cairo,
+ Chart >= 1.3.3,
+ data-default-class,
+ distributive,
+ text,
+ process
+-- cplex
+ hs-source-dirs: src
+ default-language: Haskell2010
+ ghc-options: -O2 -Wall
+-- ghc-options: -O2 -rtsopts
+ ghc-prof-options: -O2 -Wall -prof -fprof-auto -fprof-cafs -fprof-auto-calls
+
+flag examples
+ description: build the examples
+ default: False
+
+executable multiple_shooting
+ if flag(examples)
+ Buildable: True
+ else
+ Buildable: False
+ hs-source-dirs: examples
+ main-is: MultipleShooting.hs
+ default-language: Haskell2010
+ build-depends: dynobud
+ , base >=4.6 && < 5
+ , vector
+ , linear
+ , Chart >= 1.3.3
+ , Chart-gtk >= 1.3.3
+ , lens
+ , colour
+ , data-default-class
+ ghc-options: -O2
+
+
+executable plotSofa
+ if flag(examples)
+ Buildable: True
+ else
+ Buildable: False
+ hs-source-dirs: examples
+ main-is: PlotSofa.hs
+ default-language: Haskell2010
+ build-depends: dynobud,
+ base >=4.6 && < 5,
+ not-gloss >= 0.7.0.0,
+ stm,
+ containers,
+ cereal,
+ linear,
+ bytestring,
+ zeromq4-haskell,
+ vector
+ ghc-options: -O2 -threaded
+
+executable sofaTime
+ if flag(examples)
+ Buildable: True
+ else
+ Buildable: False
+ hs-source-dirs: examples
+ main-is: Sofa.hs
+ default-language: Haskell2010
+ build-depends: dynobud,
+ vector,
+ casadi-bindings-core,
+ zeromq4-haskell,
+ bytestring,
+ cereal,
+ linear,
+ base >= 4.6 && < 5
+ ghc-options: -threaded -O2
+
+executable homotopy
+ if flag(examples)
+ Buildable: True
+ else
+ Buildable: False
+ hs-source-dirs: examples
+ main-is: Homotopy.hs
+ default-language: Haskell2010
+ build-depends: dynobud,
+ vector,
+ base >= 4.6 && < 5
+ ghc-options: -threaded -O2
+
+executable vec
+ if flag(examples)
+ Buildable: True
+ else
+ Buildable: False
+ hs-source-dirs: examples
+ main-is: Vec.hs
+ default-language: Haskell2010
+ build-depends: dynobud,
+ vector >=0.10,
+ base >=4.6 && < 5
+
+ ghc-options: -threaded -O2
+
+executable ocpMonad
+ if flag(examples)
+ Buildable: True
+ else
+ Buildable: False
+ hs-source-dirs: examples
+ main-is: OcpM.hs
+ default-language: Haskell2010
+ build-depends: dynobud,
+ vector >=0.10,
+ base >=4.6 && < 5,
+ zeromq4-haskell,
+ cereal,
+ bytestring
+
+ ghc-options: -threaded -O2
+
+executable rocket
+ if flag(examples)
+ Buildable: True
+ else
+ Buildable: False
+ hs-source-dirs: examples
+ main-is: Rocket.hs
+ default-language: Haskell2010
+ build-depends: dynobud,
+ vector >=0.10,
+ base >=4.6 && < 5,
+ zeromq4-haskell,
+ cereal,
+ bytestring
+
+ ghc-options: -threaded -O2
+
+executable staticExample
+ if flag(examples)
+ Buildable: True
+ else
+ Buildable: False
+ hs-source-dirs: examples
+ main-is: StaticExample.hs
+ default-language: Haskell2010
+ build-depends: dynobud,
+ vector >=0.10,
+ base >=4.6 && < 5
+ ghc-options: -threaded -O2
+
+executable basic
+ if flag(examples)
+ Buildable: True
+ else
+ Buildable: False
+ hs-source-dirs: examples
+ main-is: Basic.hs
+ default-language: Haskell2010
+ build-depends: dynobud,
+ vector >=0.10,
+ base >=4.6 && < 5
+ ghc-options: -threaded -O2
+
+executable basicJ
+ if flag(examples)
+ Buildable: True
+ else
+ Buildable: False
+ hs-source-dirs: examples
+ main-is: BasicJ.hs
+ default-language: Haskell2010
+ build-depends: dynobud,
+ vector >=0.10,
+ base >=4.6 && < 5
+ ghc-options: -threaded -O2
+
+executable daeColl
+ if flag(examples)
+ Buildable: True
+ else
+ Buildable: False
+ hs-source-dirs: examples
+ main-is: DaeColl.hs
+ default-language: Haskell2010
+ build-depends: dynobud,
+ base >=4.6 && < 5,
+ vector
+ ghc-options: -threaded -O2
+
+executable glider
+ if flag(examples)
+ Buildable: True
+ else
+ Buildable: False
+ hs-source-dirs: examples
+ main-is: Glider.hs
+ default-language: Haskell2010
+ build-depends: dynobud,
+ base >=4.6 && < 5,
+ containers,
+ linear,
+ bytestring,
+ cereal,
+ vector,
+ zeromq4-haskell
+ ghc-options: -threaded -O2
+
+executable sailboat
+ if flag(examples)
+ Buildable: True
+ else
+ Buildable: False
+ hs-source-dirs: examples
+ main-is: Sailboat.hs
+ default-language: Haskell2010
+ build-depends: dynobud,
+ base >=4.6 && < 5,
+ containers,
+ linear,
+ bytestring,
+ cereal,
+ vector,
+ semigroups,
+ zeromq4-haskell
+ ghc-options: -threaded -O2
+
+executable dynoplot
+ if flag(examples)
+ Buildable: True
+ else
+ Buildable: False
+ hs-source-dirs: examples
+ main-is: Plotter.hs
+ default-language: Haskell2010
+ build-depends: dynobud,
+ base >=4.6 && < 5,
+ containers,
+ vector,
+ cereal,
+ bytestring,
+ zeromq4-haskell,
+ cmdargs
+ ghc-options: -O2
+
+--test-suite lp_tests
+-- type: exitcode-stdio-1.0
+-- hs-source-dirs: tests
+-- main-is: OldTests.hs
+-- default-language: Haskell2010
+-- build-depends: dynobud,
+-- vector,
+-- linear,
+-- hmatrix-glpk,
+-- QuickCheck >= 2,
+-- HUnit,
+-- test-framework,
+-- test-framework-hunit,
+-- test-framework-quickcheck2,
+-- base >=4.6 && < 5
+-- ghc-options: -O2
+
+--test-suite old-unit-tests
+-- type: exitcode-stdio-1.0
+-- hs-source-dirs: tests
+-- main-is: UnitTests.hs
+-- default-language: Haskell2010
+-- build-depends: dynobud,
+-- QuickCheck >= 2,
+-- HUnit,
+-- test-framework,
+-- test-framework-hunit,
+-- test-framework-quickcheck2,
+-- hmatrix,
+-- hmatrix-glpk,
+-- vector,
+-- linear,
+-- MemoTrie,
+-- base >=4.6 && < 5
+---- ghc-options: -O2
+
+test-suite unit-tests
+ type: exitcode-stdio-1.0
+ hs-source-dirs: tests
+ main-is: NewUnitTests.hs
+ other-modules: VectorizeTests
+ ViewTests
+ Utils
+ default-language: Haskell2010
+ build-depends: dynobud,
+ QuickCheck >= 2,
+ HUnit,
+ test-framework,
+ test-framework-hunit,
+ test-framework-quickcheck2,
+ vector,
+ linear,
+ casadi-bindings,
+ hmatrix,
+ base >=4.6 && < 5
+ ghc-options: -O2
diff --git a/examples/Basic.hs b/examples/Basic.hs
new file mode 100644
--- /dev/null
+++ b/examples/Basic.hs
@@ -0,0 +1,57 @@
+-- | Minimize the Rosenbrock function (plus a trivial constraint) using
+-- the basic NLP interface.
+
+{-# OPTIONS_GHC -Wall #-}
+{-# Language DeriveFunctor #-}
+{-# Language DeriveGeneric #-}
+
+module Main where
+
+import Dyno.Vectorize
+import Dyno.Nlp
+import Dyno.NlpSolver
+import Dyno.Solvers
+
+data X a = X a a deriving (Functor, Generic1, Show)
+data G a = G a deriving (Functor, Generic1, Show)
+
+instance Vectorize X
+instance Vectorize G
+
+myNlp :: Nlp X None G SXElement
+myNlp = Nlp { nlpFG = fg
+ , nlpBX = bx
+ , nlpBG = bg
+ , nlpX0 = x0
+ , nlpP = None
+ , nlpLamX0 = Nothing
+ , nlpLamG0 = Nothing
+ , nlpScaleF = Nothing
+ , nlpScaleX = Nothing
+ , nlpScaleG = Nothing
+ }
+ where
+ x0 :: X Double
+ x0 = X (-8) (-8)
+
+ bx :: X Bounds
+ bx = X (Just (-21), Just 0.5)
+ (Just (-2), Just 2)
+
+ bg :: G Bounds
+ bg = G (Just (-10), Just 10)
+
+ fg :: X SXElement -> None SXElement -> (SXElement, G SXElement)
+ fg (X x y) _ = (f, g)
+ where
+ f = (1-x)**2 + 100*(y - x**2)**2
+ g = G x
+
+solver :: NlpSolverStuff
+solver = ipoptSolver
+--solver = snoptSolver
+
+main :: IO ()
+main = do
+ opt <- solveNlp solver myNlp Nothing
+ print opt
diff --git a/examples/BasicJ.hs b/examples/BasicJ.hs
new file mode 100644
--- /dev/null
+++ b/examples/BasicJ.hs
@@ -0,0 +1,60 @@
+-- | Minimize the Rosenbrock function (plus a trivial constraint) using
+-- the more complicated NLP' interface.
+
+{-# OPTIONS_GHC -Wall #-}
+{-# Language DeriveGeneric #-}
+
+module Main where
+
+import GHC.Generics ( Generic )
+import Data.Vector ( Vector )
+import qualified Data.Vector as V
+
+import Dyno.View
+import Dyno.Nlp
+import Dyno.NlpSolver
+import Dyno.Solvers
+
+
+data X a = X (J S a) (J S a) deriving (Generic, Show)
+data G a = G (J S a) deriving (Generic, Show)
+
+instance View X
+instance View G
+
+myNlp :: Nlp' X JNone G MX
+myNlp = Nlp' { nlpFG' = fg
+ , nlpBX' = bx
+ , nlpBG' = bg
+ , nlpX0' = x0
+ , nlpP' = cat JNone
+ , nlpLamX0' = Nothing
+ , nlpLamG0' = Nothing
+ , nlpScaleF' = Nothing
+ , nlpScaleX' = Nothing
+ , nlpScaleG' = Nothing
+ }
+ where
+ x0 :: J X (V.Vector Double)
+ x0 = cat $ X (-8) (-8)
+
+ bx :: J X (Vector Bounds)
+ bx = mkJ $
+ V.fromList [ (Just (-21), Just 0.5)
+ , (Just (-2), Just 2)
+ ]
+ bg :: J G (Vector Bounds)
+ bg = mkJ $ (V.singleton (Just (-10), Just 10))
+
+ fg :: J X MX -> J JNone MX -> (J S MX, J G MX)
+ fg xy _ = (f, cat g)
+ where
+ f = (1-x)**2 + 100*(y - x**2)**2
+ g = G x
+
+ X x y = split xy
+
+main :: IO ()
+main = do
+ opt <- solveNlp' ipoptSolver myNlp Nothing
+ print opt
diff --git a/examples/DaeColl.hs b/examples/DaeColl.hs
new file mode 100644
--- /dev/null
+++ b/examples/DaeColl.hs
@@ -0,0 +1,139 @@
+{-# OPTIONS_GHC -Wall #-}
+{-# Language FlexibleInstances #-}
+{-# Language DeriveFunctor #-}
+{-# Language DeriveGeneric #-}
+
+module Main where
+
+import Data.Vector ( Vector )
+
+import Dyno.Vectorize
+import Dyno.View
+import Dyno.TypeVecs
+import Dyno.Nats
+import Dyno.Solvers
+--import Dyno.Sqp.Sqp
+--import Dyno.Sqp.LineSearch
+import Dyno.Nlp
+import Dyno.NlpSolver
+import Dyno.Server.Accessors
+
+import Dyno.Ocp
+import Dyno.DirectCollocation
+
+data PendX a = PendX { pX :: a
+ , pY :: a
+ , pVx :: a
+ , pVy :: a
+ } deriving (Functor, Generic, Generic1, Show)
+data PendZ a = PendZ { pTau :: a} deriving (Functor, Generic, Generic1, Show)
+data PendU a = PendU { pTorque :: a } deriving (Functor, Generic, Generic1, Show)
+data PendP a = PendP { pMass :: a } deriving (Functor, Generic, Generic1, Show)
+data PendR a = PendR a a a a a deriving (Functor, Generic, Generic1, Show)
+data PendO a = PendO deriving (Functor, Generic, Generic1, Show)
+
+instance Vectorize PendX
+instance Vectorize PendZ
+instance Vectorize PendU
+instance Vectorize PendP
+instance Vectorize PendR
+instance Vectorize PendO
+
+instance Lookup (PendX ())
+instance Lookup (PendZ ())
+instance Lookup (PendU ())
+
+mayer :: Num a => t -> PendX a -> PendX a -> a
+mayer _ _ _ = 0
+
+lagrange :: Floating a => PendX a -> PendZ a -> PendU a -> PendP a -> PendO a -> a -> a -> a
+lagrange x _ u _ _ _ _ = vx*vx + vy*vy + 1e-4*torque**2
+ where
+ PendX _ _ vx vy = x
+ PendU torque = u
+
+r :: Floating a => a
+r = 0.3
+
+pendDae :: Floating a => PendX a -> PendX a -> PendZ a -> PendU a -> PendP a -> a -> (PendR a, PendO a)
+pendDae (PendX x' y' vx' vy') (PendX x y vx vy) (PendZ tau) (PendU torque) (PendP m) _ =
+ (PendR (x' - vx) (y' - vy)
+ (m*vx' + x*tau - fx)
+ (m*vy' + y*tau - fy)
+ (x*vx' + y*vy' + (vx*vx + vy*vy))
+ , PendO
+ )
+ where
+ fx = torque*y
+ fy = -torque*x + m*9.8
+
+pendOcp :: OcpPhase PendX PendZ PendU PendP PendR PendO (Vec D8) None
+pendOcp = OcpPhase { ocpMayer = mayer
+ , ocpLagrange = lagrange
+ , ocpDae = pendDae
+ , ocpBc = bc
+ , ocpPathC = pathc
+ , ocpPathCBnds = None
+ , ocpBcBnds = fill (Just 0, Just 0)
+ , ocpXbnd = xbnd
+ , ocpUbnd = ubnd
+ , ocpZbnd = fill (Nothing, Nothing)
+ , ocpPbnd = PendP (Just 0.3, Just 0.3)
+ , ocpTbnd = (Just 4, Just 10)
+ , ocpObjScale = Nothing
+ , ocpTScale = Nothing
+ , ocpXScale = Nothing
+ , ocpZScale = Nothing
+ , ocpUScale = Nothing
+ , ocpPScale = Nothing
+ , ocpResidualScale = Nothing
+ , ocpBcScale = Nothing
+ , ocpPathCScale = Nothing
+ }
+
+pathc :: Floating a => PendX a -> PendZ a -> PendU a -> PendP a -> PendO a -> a -> None a
+pathc _ _ _ _ _ _ = None
+
+xbnd :: PendX Bounds
+xbnd = PendX { pX = (Just (-10), Just 10)
+ , pY = (Just (-10), Just 10)
+ , pVx = (Just (-10), Just 10)
+ , pVy = (Just (-10), Just 10)
+ }
+
+ubnd :: PendU Bounds
+ubnd = PendU (Just (-40), Just 40)
+
+bc :: Floating a => PendX a -> PendX a -> Vec D8 a
+bc (PendX x0 y0 vx0 vy0) (PendX xf yf vxf vyf) =
+ mkVec'
+ [ x0
+ , y0 + r
+ , vx0
+ , vy0
+ , xf
+ , yf - r
+ , vxf
+ , vyf
+ ]
+
+type NCollStages = D80
+type CollDeg = D3
+
+guess :: J (CollTraj PendX PendZ PendU PendP NCollStages CollDeg) (Vector Double)
+guess = jfill 1
+
+solver :: NlpSolverStuff
+solver = ipoptSolver
+
+solver2 :: NlpSolverStuff
+solver2 = ipoptSolver { options = [("expand", Opt True)] }
+
+
+main :: IO ()
+main = do
+ cp <- makeCollProblem pendOcp
+ let nlp = cpNlp cp
+ _ <- solveNlp' solver (nlp { nlpX0' = guess }) Nothing
+-- _ <- solveNlp solver2 (nlp { nlpX0' = guess }) Nothing
+ return ()
diff --git a/examples/Glider.hs b/examples/Glider.hs
new file mode 100644
--- /dev/null
+++ b/examples/Glider.hs
@@ -0,0 +1,143 @@
+{-# OPTIONS_GHC -Wall #-}
+
+module Main ( main ) where
+
+import Linear
+import Data.Vector ( Vector )
+
+import Dyno.Vectorize
+import Dyno.View
+import Dyno.Solvers
+--import Dyno.Sqp.Sqp
+--import Dyno.Sqp.LineSearch
+import Dyno.Nlp
+import Dyno.NlpSolver
+
+import Dyno.Ocp
+import Dyno.DirectCollocation
+import Dyno.DirectCollocation.Dynamic ( toMeta )
+
+import Dyno.Models.Aircraft
+import Dyno.Models.AeroCoeffs
+import Dyno.Models.Betty
+import Dyno.Nats
+
+import GliderShared
+import ServerSender ( withCallback )
+
+type NCollStages = D100
+type CollDeg = D2
+
+mayer :: Floating a => a -> AcX a -> AcX a -> a
+mayer _ _ _ = 0
+
+lagrange :: Floating a => AcX a -> None a -> AcU a -> None a -> None a -> a -> a -> a
+lagrange (AcX _ _ _ _ (AcU surfs)) _ (AcU surfs') _ _ _ _ =
+ elev**2 + rudd**2 + ail**2 + flaps**2 +
+ 100*(elev'**2 + rudd'**2 + ail'**2 + flaps'**2)
+ where
+ elev = csElev surfs
+ rudd = csElev surfs
+ ail = csElev surfs
+ flaps = csFlaps surfs
+
+ elev' = csElev surfs'
+ rudd' = csElev surfs'
+ ail' = csElev surfs'
+ flaps' = csFlaps surfs'
+
+dae :: Floating a => AcX a -> AcX a -> None a -> AcU a -> None a -> a -> (AcX a, None a)
+dae x' x _ u _ _ = (aircraftDae (mass, inertia) fcs mcs refs x' x u, None)
+ where
+ mass = bettyMass
+ inertia = bettyInertia
+ fcs = bettyFc
+ mcs = bettyMc
+ refs = bettyRefs
+
+ocp :: OcpPhase AcX None AcU None AcX None AcX None
+ocp = OcpPhase { ocpMayer = mayer
+ , ocpLagrange = lagrange
+ , ocpDae = dae
+ , ocpBc = bc
+ , ocpPathC = pathc
+ , ocpPathCBnds = None
+ , ocpBcBnds = fill (Just 0, Just 0)
+ , ocpXbnd = xbnd
+ , ocpUbnd = ubnd
+ , ocpZbnd = None
+ , ocpPbnd = None
+ , ocpTbnd = (Just 0.5, Just 0.5)
+-- , ocpTbnd = (Just 4, Just 4)
+ , ocpObjScale = Nothing
+ , ocpTScale = Nothing
+ , ocpXScale = Nothing
+ , ocpZScale = Nothing
+ , ocpUScale = Nothing
+ , ocpPScale = Nothing
+ , ocpResidualScale = Nothing
+ , ocpBcScale = Nothing
+ , ocpPathCScale = Nothing
+ }
+
+pathc :: x a -> z a -> u a -> p a -> None a -> a -> None a
+pathc _ _ _ _ _ _ = None
+
+xbnd :: AcX (Maybe Double, Maybe Double)
+xbnd = AcX { ac_r_n2b_n = fill (Nothing, Nothing)
+ , ac_v_bn_b = fill (Nothing,Nothing)
+ , ac_R_n2b = fill $ fill (Just (-1.2), Just 1.2)
+ , ac_w_bn_b = fill (Just (-8*2*pi), Just (8*2*pi))
+ , ac_u = ubnd
+ }
+
+d2r :: Floating a => a -> a
+d2r d = d*pi/180
+
+ubnd :: AcU (Maybe Double, Maybe Double)
+ubnd =
+ AcU
+ ControlSurfaces { csElev = (Just (d2r (-10)), Just (d2r 10))
+ , csRudder = (Just (d2r (-10)), Just (d2r 10))
+ , csAil = (Just (d2r (-10)), Just (d2r 10))
+ , csFlaps = (Just (d2r (-0.01)), Just (d2r 0.01))
+ }
+
+bc :: Floating a => AcX a -> AcX a -> AcX a
+bc (AcX x0 v0 dcm0 w0 cs) _ = AcX x0 (v0 - V3 30 0 0) (dcm0 - eye3) w0 cs
+
+main :: IO ()
+main = do
+ putStrLn $ "using ip \""++gliderUrl++"\""
+ putStrLn $ "using channel \""++gliderChannelName++"\""
+
+ cp <- makeCollProblem ocp
+ let nlp = cpNlp cp
+ toDyn = cpCallback cp
+ withCallback gliderUrl gliderChannelName $ \cb -> do
+ let guess = jfill 1
+
+ cb' :: J (CollTraj AcX None AcU None NCollStages CollDeg) (Vector Double) -> IO Bool
+ cb' traj = do
+ (dyn,_) <- toDyn traj
+ let proxy :: Proxy (CollTraj AcX None AcU None NCollStages CollDeg)
+ proxy = Proxy
+ cb ([dyn], toMeta (cpRoots cp) (Proxy :: Proxy None) proxy)
+
+ (msg,opt') <- solveNlp' ipoptSolver (nlp { nlpX0' = guess }) (Just cb')
+ opt <- case msg of Left msg' -> error msg'
+ Right _ -> return opt'
+-- let xopt = xOpt opt
+-- lambda = lambdaOpt opt
+--
+-- snoptOpt' <- solveNlp snoptSolver (nlp {nlpX0 = xopt}) (Just cb) (Just lambda)
+-- snoptOpt <- case snoptOpt' of Left msg -> error msg
+-- Right opt'' -> return opt''
+-- let xopt' = xOpt snoptOpt
+-- lambda' = lambdaOpt opt
+-- lambdax' = vectorize $ lambdaX lambda'
+-- lambdag' = vectorize $ lambdaG lambda'
+-- _ <- solveSqp (nlp {nlpX0 = xopt}) fullStep
+-- _ <- solveSqp (nlp {nlpX0 = xopt}) armilloSearch
+
+ return ()
diff --git a/examples/Homotopy.hs b/examples/Homotopy.hs
new file mode 100644
--- /dev/null
+++ b/examples/Homotopy.hs
@@ -0,0 +1,83 @@
+-- | Minimize the Rosenbrock function (plus a trivial constraint) using
+-- the more complicated NLP' interface.
+
+{-# OPTIONS_GHC -Wall #-}
+{-# Language DeriveGeneric #-}
+
+module Main where
+
+import GHC.Generics ( Generic )
+import Data.Vector ( Vector )
+import qualified Data.Vector as V
+import Text.Printf ( printf )
+
+import Dyno.View
+import Dyno.Nlp
+import Dyno.NlpSolver
+import Dyno.Solvers
+
+
+data P a = P (J S a) (J S a) deriving (Generic, Show)
+data X a = X (J S a) (J S a) deriving (Generic, Show)
+data G a = G (J S a)-- (J S a)
+ deriving (Generic, Show)
+
+instance View X
+instance View G
+instance View P
+
+myNlp :: Nlp' X P G MX
+myNlp = Nlp' { nlpFG' = fg
+ , nlpBX' = bx
+ , nlpBG' = bg
+ , nlpX0' = x0
+ , nlpP' = cat $ P (-2) 0
+ , nlpLamX0' = Nothing
+ , nlpLamG0' = Nothing
+ , nlpScaleF' = Nothing
+ , nlpScaleX' = Nothing
+ , nlpScaleG' = Nothing
+ }
+ where
+ x0 :: J X (V.Vector Double)
+ x0 = cat $ X (-8) (-8)
+
+ bx :: J X (Vector Bounds)
+ bx = mkJ $
+ V.fromList [ (Just (-3), Just 3)
+ , (Just (-3), Just 3)
+ ]
+ bg :: J G (Vector Bounds)
+ bg = mkJ $ (V.singleton (Nothing, Just 0))
+
+ fg :: J X MX -> J P MX -> (J S MX, J G MX)
+ fg xy pxy = (f, cat g)
+ where
+ X x y = split xy
+ P px py = split pxy
+ f = (1-x)**2 + 100*(y - x**2)**2
+-- g = G x
+-- f = (x - px)**2 + (y - py)**2
+
+ g = G (x - px)
+
+--solver = ipoptSolver {options = [ --("max_iter", Opt (5 :: Int))
+-- ("print_level", Opt (0 :: Int))
+-- , ("print_time", Opt False)
+-- ]}
+solver = snoptSolver {options = [ ("print_time", Opt False)
+ , ("_isumm", Opt (0 :: Int))
+-- , ("max_iter", Opt (5 :: Int))
+-- , ("_start", Opt "Warm")
+ ]}
+main :: IO ()
+main = do
+ let cbp :: J X (Vector Double) -> J P (Vector Double) -> Double -> IO ()
+ cbp xy pxy alpha = do
+ let X x y = split xy
+ P px py = split pxy
+ --printf "X: (%.3f,%.3f), P: (%.3f, %.3f), a: %.4f\n"
+ -- (V.head (unJ x)) (V.head (unJ y)) (V.head (unJ px)) (V.head (unJ py)) alpha
+ return ()
+ opt <- solveNlpHomotopy' 1e-3 (0.6, 2, 10, 20) solver myNlp (cat (P (2) (0))) Nothing (Just cbp)
+ print opt
diff --git a/examples/MultipleShooting.hs b/examples/MultipleShooting.hs
new file mode 100644
--- /dev/null
+++ b/examples/MultipleShooting.hs
@@ -0,0 +1,105 @@
+{-# OPTIONS_GHC -Wall #-}
+{-# Language ScopedTypeVariables #-}
+{-# Language DeriveGeneric #-}
+{-# Language DeriveFunctor #-}
+
+module Main
+ ( main
+ ) where
+
+import GHC.Generics ( Generic )
+import qualified Data.Vector as V
+import qualified Data.Foldable as F
+import Control.Applicative ( Applicative(..) )
+import Linear
+
+import Graphics.Rendering.Chart hiding ( x0 )
+import Graphics.Rendering.Chart.Gtk
+import Data.Default.Class
+import Data.Colour
+import Data.Colour.Names
+import Control.Lens
+
+import Dyno.View.View
+import Dyno.View.JV
+import Dyno.Nlp
+import Dyno.NlpSolver
+import Dyno.Solvers
+import Dyno.Vectorize
+import Dyno.View.CasadiMat ( MX )
+import Dyno.Nats
+import Dyno.MultipleShooting
+
+-- state/control/parameter definitions
+data X a = X a a deriving (Functor, Generic, Generic1, Show)
+data U a = U a deriving (Functor, Generic, Generic1, Show)
+data P a = P deriving (Functor, Generic, Generic1, Show)
+
+-- boilerplate
+instance Vectorize X
+instance Vectorize U
+instance Vectorize P
+instance Applicative X where
+ pure = fill
+ x0 <*> x1 = devectorize (V.zipWith id (vectorize x0) (vectorize x1))
+instance Applicative U where
+ pure = fill
+ x0 <*> x1 = devectorize (V.zipWith id (vectorize x0) (vectorize x1))
+instance Additive X where
+ zero = fill 0
+instance Additive U where
+ zero = fill 0
+
+-- ocp specification
+ocp :: MsOcp X U P
+ocp =
+ MsOcp
+ { msOde = ode
+ , msEndTime = 10
+ , msXBnds = X (Just (-2), Just 2) (Just (-2), Just 2)
+ , msUBnds = U (Just (-3), Just 3)
+ , msPBnds = P
+ , msMayer = \_ -> 0
+ , msLagrangeSum = \(X p v) (U u) -> p*p + v*v + u*u
+ , msX0 = X (Just 0) (Just 0)
+ , msXF = X (Just 1) (Just 1)
+ , msNumRk4Steps = Just 10
+ }
+
+-- dynamics
+ode :: Floating a => X a -> U a -> P a -> a -> X a
+ode (X x v) (U u) _p _t = X v (-x -0.1*v + u)
+
+-- run the thing
+main :: IO ()
+main = do
+ myNlp <- makeMsNlp ocp :: IO (Nlp' (MsDvs X U P D40) JNone (MsConstraints X D40) MX)
+ (msg,opt') <- solveNlp' ipoptSolver myNlp Nothing
+ opt <- case msg of
+ Left err -> error err
+ Right _ -> return opt'
+ let xopt = split $ xOpt' opt
+ splitXU xu = (splitJV x, splitJV u)
+ where
+ JTuple x u = split xu
+ (xs', us) = unzip $ map splitXU $ F.toList $ unJVec $ split (dvXus xopt)
+ xf = splitJV (dvXf xopt)
+ xs = xs' ++ [xf]
+ renderableToWindow (chart [ ("u", (map (\(U u) -> u) us) ++ [0])
+ , ("p", map (\(X p _) -> p) xs)
+ , ("v", map (\(X _ v) -> v) xs)
+ ]) 600 600
+
+chart :: [(String, [Double])] -> Renderable ()
+chart vals = toRenderable layout
+ where
+ points :: (String, [Double]) -> PlotPoints Double Double
+ points (name, ys) = plot_points_style .~ filledCircles 2 (opaque red)
+ $ plot_points_values .~ (zip [0..] ys)
+ $ plot_points_title .~ name
+ $ def
+
+ layout :: Layout Double Double
+ layout = layout_title .~ "a plot"
+ $ layout_plots .~ (map (toPlot . points) vals)
+ $ def
diff --git a/examples/OcpM.hs b/examples/OcpM.hs
new file mode 100644
--- /dev/null
+++ b/examples/OcpM.hs
@@ -0,0 +1,72 @@
+{-# OPTIONS_GHC -Wall #-}
+
+module Main ( main ) where
+
+import Control.Monad ( void )
+
+import Dyno.OcpMonad
+import Dyno.Solvers
+import ServerSender
+import GliderShared
+
+myDae :: SXElement -> DaeMonad ()
+myDae time = do
+ (p,p') <- diffState "p"
+ (v,v') <- diffState "v"
+ u <- control "u"
+
+ let k = 4
+ b = 0.3
+
+ force = u - k * p - b * v
+ obj = p**2 + v**2 + u**2
+ output "force" force
+ output "obj" obj
+
+ p' === v
+ v' === force + 0.1 * sin time
+
+boundaryConditions :: (String -> BCMonad SXElement) -> (String -> BCMonad SXElement) -> BCMonad ()
+boundaryConditions get0 getF = do
+ p0 <- get0 "p"
+ v0 <- get0 "v"
+
+ pF <- getF "p"
+ vF <- getF "v"
+
+ p0 === 0
+ v0 === 0
+
+-- p0 + 4 <== pF -- inequalities missing for now
+-- v0 === vF
+ pF === 1
+ vF === 0
+
+mayer :: (Floating a, Monad m) => a -> (String -> m a) -> (String -> m a) -> m a
+mayer endTime get0 getF = do
+ p <- getF "p"
+ v <- getF "v"
+
+ return (p**2 + v**2 + endTime/1000)
+
+myOcp :: SXElement -> (String -> OcpMonad SXElement) -> OcpMonad ()
+myOcp time get = do
+ p <- get "p"
+ v <- get "v"
+ u <- get "u"
+ force <- get "force"
+ obj <- get "obj"
+
+ v**2 + u**2 <== 4 + time/100
+
+ lagrangeTerm obj
+
+main :: IO ()
+main = void $ withCallback gliderUrl gliderChannelName go
+ where
+ n = 100
+ deg = 3
+ tbnds = (Just 4, Just 4)
+ go cb = solveStaticOcp ipoptSolver myDae mayer boundaryConditions myOcp tbnds n deg (Just cb')
+ where
+ cb' meta x = cb (x, meta)
diff --git a/examples/PlotSofa.hs b/examples/PlotSofa.hs
new file mode 100644
--- /dev/null
+++ b/examples/PlotSofa.hs
@@ -0,0 +1,171 @@
+{-# OPTIONS_GHC -Wall #-}
+{-# Language CPP #-}
+
+module Main ( main ) where
+
+import qualified Data.Foldable as F
+import Linear.V3 ( V3(..) )
+import Linear.Quaternion ( Quaternion(..) )
+import Control.Monad ( when, forever )
+import Data.ByteString.Char8 ( pack )
+import Data.Serialize
+import qualified System.ZMQ4 as ZMQ
+import qualified Control.Concurrent.STM as STM
+import qualified Control.Concurrent as CC
+import Text.Printf
+
+import Vis
+
+import SofaShared
+
+--type M22 = ((Double,Double),(Double,Double))
+
+
+--sub :: ((DynCollTraj (Vector Double), CollTrajMeta, [M22], M22) -> IO ()) -> IO ()
+--sub writeChan = ZMQ.withContext $ \context ->
+-- ZMQ.withSocket context ZMQ.Sub $ \subscriber -> do
+-- ZMQ.connect subscriber url
+-- ZMQ.subscribe subscriber (pack channelName)
+-- forever $ do
+-- _ <- ZMQ.receive subscriber
+-- mre <- ZMQ.moreToReceive subscriber
+-- when mre $ do
+
+sub :: (SofaMessage -> IO ()) -> IO ()
+sub writeChan = ZMQ.withContext $ \context ->
+ ZMQ.withSocket context ZMQ.Sub $ \subscriber -> do
+ ZMQ.connect subscriber url
+ ZMQ.subscribe subscriber (pack sofaChannel)
+ forever $ do
+ _ <- ZMQ.receive subscriber
+ mre <- ZMQ.moreToReceive subscriber
+ when mre $ do
+ msg <- ZMQ.receive subscriber
+ let decoded :: SofaMessage
+ decoded = case decode msg of
+ Left err -> error err
+ Right t -> t
+ writeChan decoded
+
+main :: IO ()
+main = do
+ -- keep reading from tcp and storing results in the queue
+ trajChan <- STM.atomically STM.newTQueue
+ _ <- CC.forkIO (sub (STM.atomically . (STM.writeTQueue trajChan)))
+
+ -- keep parsing results from the queue into nice form
+ trajMVar <- CC.newMVar (VisObjects [], [], [])
+
+ let getLastValue = do
+ val <- STM.atomically (STM.readTQueue trajChan)
+ empty' <- STM.atomically (STM.isEmptyTQueue trajChan)
+ if empty' then return val else getLastValue
+
+ parserThread = do
+ sofaX <- getLastValue
+ CC.modifyMVar_ trajMVar $ \(_,_,xs) -> do
+ let (mainvis, stages) = toVisObjects sofaX
+ return (mainvis, stages, xs)
+ parserThread
+
+ _ <- CC.forkIO parserThread
+
+ animateIO (defaultOpts { optWindowName = "sofa lol" }) (animateFun trajMVar)
+
+multiplyList :: Int -> Int -> [a] -> [a]
+multiplyList _ _ [] = []
+multiplyList k 0 (_:xs) = multiplyList k k xs
+multiplyList k j xs@(x0:_) = x0 : multiplyList k (j-1) xs
+
+animateFun :: CC.MVar (VisObject Double, [VisObject Double], [VisObject Double])
+ -> Float -> IO (VisObject Double)
+animateFun mv = const $ do
+ (mainvis, stages, plotstages) <- CC.takeMVar mv
+ case plotstages of
+ (s0:ss) -> do
+ CC.putMVar mv (mainvis, stages, ss)
+ return $ VisObjects [mainvis, s0]
+ []-> do
+ let n = max 1 $ 400 `div` length stages
+ CC.putMVar mv (mainvis, stages, multiplyList n n stages)
+ return mainvis
+
+linspace :: Fractional a => a -> a -> Int -> [a]
+linspace x0 xf n = xs
+ where
+ h = (xf-x0)/(fromIntegral n - 1)
+ xs = map (\k -> x0 + h*(fromIntegral k)) (take n [(0::Int)..])
+
+qy :: Quaternion Double
+qy = Quaternion 0 $ V3 1 0 0
+
+toVisObjects :: SofaMessage -> (VisObject Double, [VisObject Double])
+toVisObjects (SofaMessage iters r points stages) =
+ ( RotQuat qy $ VisObjects [walls, txt, shape0
+ , VisObjects (allPoints (-1))
+ , Trans (V3 (-2) (-2) 0) axes
+ ]
+ , map (RotQuat qy . Trans (V3 1 1 0)) (allPoints 0)
+ )
+ where
+ walls = VisObjects
+ [ Line [ V3 (-4) 1 0
+ , V3 1 1 0
+ , V3 1 (-4) 0
+ ] (makeColor 1 1 1 1)
+ , Line [ V3 (-4) 2 0
+ , V3 2 2 0
+ , V3 2 (-4) 0
+ ] (makeColor 1 1 1 1)
+ , Line [ V3 (-4) 0 0
+ , V3 0 0 0
+ , V3 0 (-4) 0
+ ] (makeColor 1 1 1 1)
+ ]
+
+ axes = Axes (0.5, 15)
+ npoints = length points
+ nsteps = length stages
+ shape0 = Line' $
+ zipWith (\(Point x y) c -> ((V3 x y 0) - (V3 2 2 0), c))
+ (points ++ [head points])
+ (colors (npoints + 1))
+ drawOne :: [Point Double] -> Double -> Color -> VisObject Double
+ drawOne ps@(p0:_) z =
+ Line
+ (map (\(Point x y) -> (V3 x y z)) (ps ++ [p0]))
+ drawOne _ _ = const (VisObjects [])
+
+ area = 0.5 * (F.sum $ zipWithNext' cross points)
+
+ allPoints :: Double -> [VisObject Double]
+ allPoints maxheight = zipWith3 (\c so z -> drawOne (stagePoints so) z c)
+ (colors (nsteps + 1))
+ stages
+ (linspace 0 maxheight (nsteps + 1))
+
+ colors :: Int -> [Color]
+ colors k = fmap (\gamma -> makeColor 0 gamma (1 - gamma) 1) (gammas k)
+
+ gammas :: Int -> [Float]
+ gammas k = linspace 0 1 k
+
+ stagePoints :: (Point Double, Double) -> [Point Double]
+ stagePoints (mean, theta) = fmap rot points
+ where
+ rot :: Point Double -> Point Double
+ rot (Point x y) = mean + Point (x*cos(theta) + y*sin(theta)) (-x*sin(theta) + y*cos(theta))
+
+
+ messages = [ show npoints ++ " segments"
+ , show nsteps ++ " stages"
+ , printf "segment length: %.4f" r
+ , printf "area: %.4f" area
+ , "iteration: " ++ show iters
+ ]
+ txt = VisObjects $
+ zipWith (\s k -> Text2d s (30,fromIntegral $ 30*k) TimesRoman24 (makeColor 1 1 1 1))
+ messages (reverse [1..length messages])
+-- trajLine = Line (zipWith (\x y -> V3 x y 0) (concat xs0) (concat ys0)) (makeColor 1 0 0 0.4)
+-- trajDots = Points (zipWith (\x y -> V3 x y 0) xsCollPts ysCollPts) (Just 1) red
+-- trajDots' = Points (zipWith (\x y -> V3 x y 0) xsBigPts ysBigPts) (Just 2) red
diff --git a/examples/Plotter.hs b/examples/Plotter.hs
new file mode 100644
--- /dev/null
+++ b/examples/Plotter.hs
@@ -0,0 +1,56 @@
+{-# OPTIONS_GHC -Wall #-}
+{-# Language DeriveDataTypeable #-}
+
+module Main ( main ) where
+
+import qualified Control.Concurrent as CC
+import Control.Monad ( when, forever )
+import Data.ByteString.Char8 ( pack )
+import Data.Serialize
+import Data.Vector ( Vector )
+import qualified System.ZMQ4 as ZMQ
+import System.Console.CmdArgs ( (&=), Data, Typeable )
+import qualified System.Console.CmdArgs as CA
+
+import Dyno.Server.Server ( runPlotter, newChannel )
+import Dyno.DirectCollocation.Dynamic
+
+import GliderShared ( gliderUrl, gliderChannelName )
+
+sub :: String -> (([DynCollTraj (Vector Double)], CollTrajMeta) -> IO ()) -> String -> IO ()
+sub ip' writeChan name = ZMQ.withContext $ \context ->
+ ZMQ.withSocket context ZMQ.Sub $ \subscriber -> do
+ ZMQ.connect subscriber ip'
+ ZMQ.subscribe subscriber (pack name)
+ forever $ do
+ _ <- ZMQ.receive subscriber
+ mre <- ZMQ.moreToReceive subscriber
+ when mre $ do
+ msg <- ZMQ.receive subscriber
+ let decoded :: ([DynCollTraj (Vector Double)], CollTrajMeta)
+ decoded = case decode msg of
+ Left err -> error err
+ Right t -> t
+ writeChan decoded
+
+main :: IO ()
+main = do
+ args <- CA.cmdArgs (myargs &= CA.program "dynoplot")
+ let ip' = ip args
+ channel' = channel args
+ putStrLn $ "using ip \""++ip'++"\""
+ putStrLn $ "using channel \""++channel'++"\""
+
+ (c0, writeMe) <- newChannel channel'
+
+ listenerTid0 <- CC.forkIO (sub ip' writeMe channel')
+ runPlotter c0 [listenerTid0]
+
+data VisArgs = VisArgs { ip :: String
+ , channel :: String
+ } deriving (Show, Data, Typeable)
+
+myargs :: VisArgs
+myargs = VisArgs { ip = gliderUrl &= CA.help "an IP address" &= CA.typ "ADDRESS"
+ , channel = gliderChannelName &= CA.help "zmq channel name"
+ } &= CA.summary "plotter for dynobud OCPs"
diff --git a/examples/Rocket.hs b/examples/Rocket.hs
new file mode 100644
--- /dev/null
+++ b/examples/Rocket.hs
@@ -0,0 +1,91 @@
+{-# OPTIONS_GHC -Wall #-}
+
+module Main ( main ) where
+
+import Control.Monad ( void )
+--import Control.Concurrent ( threadDelay )
+
+import Dyno.OcpMonad
+import Dyno.Solvers
+
+import ServerSender
+import GliderShared
+
+myDae :: SXElement -> DaeMonad ()
+myDae time = do
+ (p,p') <- diffState "p"
+ (v,v') <- diffState "v"
+ (m,m') <- diffState "m"
+ (u,u') <- diffState "u"
+ u'' <- control "u'"
+
+ let g = 9.8
+ force = u - m*g
+
+ output "force" force
+
+ p' === v
+ v' === force/m
+ m' === -1e-2*u**2
+ u'' === u'
+
+boundaryConditions :: (String -> BCMonad SXElement) -> (String -> BCMonad SXElement) -> BCMonad ()
+boundaryConditions get0 getF = do
+ -- initial
+ p0 <- get0 "p"
+ v0 <- get0 "v"
+ m0 <- get0 "m"
+
+ p0 === 1
+ v0 === 0
+ m0 === 10
+
+ -- final
+ pF <- getF "p"
+ vF <- getF "v"
+
+ pF === 0
+ vF === 0
+
+mayer :: (Floating a, Monad m) => a -> (String -> m a) -> (String -> m a) -> m a
+mayer endTime get0 getF = do
+ m <- getF "m"
+
+ return (-m) -- endTime -- (p**2 + v**2)
+
+myOcp :: SXElement -> (String -> OcpMonad SXElement) -> OcpMonad ()
+myOcp time get = do
+ p <- get "p"
+ v <- get "v"
+ m <- get "m"
+ u <- get "u"
+ u' <- get "u'"
+
+ -200 <== u
+ u <== 200
+
+ -100 <== u'
+ u' <== 100
+
+ 0.01 <== m
+
+ 0 <== p
+
+ -10 <== v
+ v <== 0.0
+
+ lagrangeTerm (1e-4*u'*u')
+ --lagrangeTerm (1e-8*u*u + 1e-9*p*p + 1e-9*v*v + 1e-9*m*m)
+ --lagrangeTerm (1e-6*u*u + 1e-6*p*p + 1e-6*v*v + 1e-6*m*m)
+
+main :: IO ()
+main = void $ withCallback gliderUrl gliderChannelName go
+ where
+ n = 100
+ deg = 3
+ tbnds = (Just 0.2, Just 6)
+ --tbnds = (Just 1.5, Just 1.5)
+ go cb = solveStaticOcp ipoptSolver myDae mayer boundaryConditions myOcp tbnds n deg (Just cb')
+ where
+ cb' meta x = cb (x, meta)
+ --cb' meta x = threadDelay 200000 >> cb (x, meta)
diff --git a/examples/Sailboat.hs b/examples/Sailboat.hs
new file mode 100644
--- /dev/null
+++ b/examples/Sailboat.hs
@@ -0,0 +1,311 @@
+-- This example is based on Fabian Wierer's final project for
+-- Optimal Control and Estimation, 2014, taught by Prof. Moritz Diehl
+-- \Used with permission.
+
+{-# OPTIONS_GHC -Wall #-}
+{-# Language ScopedTypeVariables #-}
+{-# Language FlexibleInstances #-}
+{-# Language DeriveFunctor #-}
+{-# Language DeriveGeneric #-}
+
+module Main ( main ) where
+
+import Data.Vector ( Vector )
+
+import Dyno.Vectorize
+import Dyno.View
+import Dyno.Nats
+import Dyno.Solvers
+import Dyno.NlpSolver
+import Dyno.Server.Accessors
+import Dyno.Nlp
+import Dyno.Ocp
+import Dyno.DirectCollocation
+import Dyno.DirectCollocation.Quadratures ( QuadratureRoots(..) )
+import Dyno.DirectCollocation.Formulate ( makeGuess )
+import Dyno.DirectCollocation.Dynamic
+
+import qualified System.ZMQ4 as ZMQ
+import Linear -- ( V2(..) )
+import qualified Data.List.NonEmpty as NE
+import qualified Data.ByteString as BS
+import qualified Data.ByteString.Char8 as BS8
+import qualified Data.Serialize as Ser
+
+data SbX a = SbX { xGamma :: a
+ , xP :: V2 a
+ , xV :: V2 a
+ } deriving (Functor, Generic, Generic1, Show)
+data SbZ a = SbZ deriving (Functor, Generic, Generic1, Show)
+data SbU a = SbU { uOmega :: a
+ , uAlpha :: a
+ } deriving (Functor, Generic, Generic1, Show)
+data SbP a = SbP deriving (Functor, Generic, Generic1, Show)
+data SbR a = SbR (SbX a) deriving (Functor, Generic, Generic1, Show)
+data SbO a = SbO { oFla :: V2 a
+ , oFda :: V2 a
+ , oFlw :: V2 a
+ , oFdw :: V2 a
+ , oAirspeed :: a
+ , oWaterspeed :: a
+ , oAlphaDeg :: a
+ , oOmegaDeg :: a
+ , oGammaDeg :: a
+ } deriving (Functor, Generic, Generic1, Show)
+
+instance Vectorize SbX
+instance Vectorize SbZ
+instance Vectorize SbU
+instance Vectorize SbP
+instance Vectorize SbR
+instance Vectorize SbO
+instance Vectorize SbBc
+
+instance Lookup (SbX ())
+instance Lookup (SbZ ())
+instance Lookup (SbU ())
+instance Lookup (SbP ())
+instance Lookup (SbO ())
+
+------------------------------ zmq helpers -------------------------------------
+newtype Packed = Packed { unPacked :: BS.ByteString }
+
+encodeSerial :: Ser.Serialize a => a -> Packed
+encodeSerial = Packed . Ser.encode
+
+--------------------------------------------------------------------------
+norm2sqr :: Num a => V2 a -> a
+norm2sqr (V2 x y) = x*x + y*y
+
+clift :: Floating a => a -> a
+clift alpha = 2*pi*alpha*10/12 - exp (alpha/pi*180 - 12) + exp (-alpha/pi*180 - 12)
+
+sbDae :: forall a . Floating a => SbX a -> SbX a -> SbZ a -> SbU a -> SbP a -> a -> (SbR a, SbO a)
+sbDae
+ (SbX gamma' p'@(V2 px' pz') v'@(V2 vx' vz'))
+ (SbX gamma p@(V2 px pz) v@(V2 vx vz))
+ _
+ (SbU omega alpha)
+ _
+ _
+ = (residual, outputs)
+ where
+ residual :: SbR a
+ residual = SbR $
+ SbX
+ (gamma' - omega)
+ (p' - v)
+ (v' - (fmap (/mB) f))
+ outputs :: SbO a
+ outputs = SbO { oFla = fLa
+ , oFda = fDa
+ , oFlw = fLw
+ , oFdw = fDw
+ , oAirspeed = airspeed
+ , oWaterspeed = waterspeed
+ , oAlphaDeg = alpha * 180/pi
+ , oOmegaDeg = omega * 180/pi
+ , oGammaDeg = gamma * 180/pi
+ }
+
+ mB = 160 + 70 -- boat's mass + sailor's mass
+ w = V2 (-5) 0
+ we@(V2 wex wez) = w ^-^ v
+ rhoAir = 1.2
+ airspeed2 = norm2sqr we
+ airspeed = sqrt airspeed2
+ liftDirectionAir = (V2 wez (-wex)) ^/ airspeed
+
+ srefSail = 10 + 6 -- main + fock sail
+ clSail = clift alpha
+ cdSail = 0.01 + clSail*clSail/(10*pi)
+ dragDirectionAir = we ^/ airspeed
+ fDa = 0.5 * rhoAir * airspeed2 * cdSail * srefSail *^ dragDirectionAir
+ fLa = 0.5 * rhoAir * airspeed2 * clSail * srefSail *^ liftDirectionAir
+
+ waterspeed2 = norm2sqr v
+ waterspeed = sqrt waterspeed2
+
+ finArea = afin + ahull
+ where
+ afin = 0.3 * 0.7 -- area of fin
+ ahull = 0.2 * 5 -- submerged part of hull: Length * submerged height
+
+ rhoWater = 1000
+ clFin = clift gamma
+ cdFin = 0.01 + clFin*clFin/(10*pi)
+ liftDirectionWater :: V2 a
+ liftDirectionWater = (V2 (-vz) vx) ^/ waterspeed
+ dragDirectionWater = (-v) ^/ waterspeed
+ fLw = 0.5 * rhoWater * waterspeed2 * clFin * finArea *^ liftDirectionWater
+ fDw = 0.5 * rhoWater * waterspeed2 * cdFin * finArea *^ dragDirectionWater
+ f = fDa + fLa + fDw + fLw
+
+
+data SbBc a = SbBc { bcPeriodicGamma :: a
+ , bcPeriodicPz :: a
+ , bcPeriodicVx :: a
+ , bcPeriodicVz :: a
+ , bcP0 :: V2 a
+ }
+ deriving (Functor, Generic, Generic1, Show)
+bc :: Num a => SbX a -> SbX a -> SbBc a
+bc
+ (SbX gamma0 p0@(V2 px0 pz0) (V2 vx0 vz0))
+ (SbX gammaF (V2 pxF pzF) (V2 vxF vzF))
+ = SbBc
+ { bcPeriodicGamma = gamma0 + gammaF
+ , bcPeriodicPz = pz0 - pzF
+ , bcPeriodicVx = vx0 - vxF
+ , bcPeriodicVz = vz0 + vzF
+ , bcP0 = p0
+ }
+
+mayer :: Floating a => a -> SbX a -> SbX a -> a
+mayer tf _ (SbX _ (V2 pxF _) _) = - pxF / tf
+
+lagrange :: Floating a => SbX a -> SbZ a -> SbU a -> SbP a -> SbO a -> a -> a -> a
+lagrange _ _ (SbU omega alpha) _ _ _ _ = 1e-3*omega*omega + 1e-3*alpha*alpha
+
+ubnd :: SbU (Maybe Double, Maybe Double)
+ubnd = SbU
+ (Just (-5*pi/180), Just (5*pi/180))
+ (Just (-12*pi/180), Just (12*pi/180))
+
+xbnd :: SbX (Maybe Double, Maybe Double)
+xbnd = SbX
+ (Just (-12*pi/180), Just (12*pi/180))
+ (V2
+ (Just (-1000), Just 1000)
+ (Just (-30), Just 30))
+ (V2
+ (Just (-100), Just 100)
+ (Just (-100), Just 100))
+
+pathc :: t -> t1 -> t2 -> t3 -> t4 -> t5 -> None a
+pathc _ _ _ _ _ _ = None
+
+ocp :: OcpPhase SbX SbZ SbU SbP SbR SbO SbBc None
+ocp = OcpPhase { ocpMayer = mayer
+ , ocpLagrange = lagrange
+ , ocpDae = sbDae
+ , ocpBc = bc
+ , ocpPathC = pathc
+ , ocpPathCBnds = None
+ , ocpBcBnds = fill (Just 0, Just 0)
+ , ocpXbnd = xbnd
+ , ocpUbnd = ubnd
+ , ocpZbnd = SbZ
+ , ocpPbnd = SbP
+ , ocpTbnd = (Just 1, Just 50)
+ , ocpObjScale = Nothing
+ , ocpTScale = Nothing
+ , ocpXScale = Nothing
+ , ocpZScale = Nothing
+ , ocpUScale = Nothing
+ , ocpPScale = Nothing
+ , ocpResidualScale = Nothing
+ , ocpBcScale = Nothing
+ , ocpPathCScale = Nothing
+ }
+
+
+
+urlDynoPlot :: String
+urlDynoPlot = "tcp://127.0.0.1:5563"
+
+--urlOptTelem :: String
+--urlOptTelem = "tcp://127.0.0.1:5563"
+
+withPublisher
+ :: ZMQ.Context -> String -> ((String -> Packed -> IO ()) -> IO a) -> IO a
+withPublisher context url f =
+ ZMQ.withSocket context ZMQ.Pub $ \publisher -> do
+ ZMQ.bind publisher url
+ let send :: String -> Packed -> IO ()
+ send channel msg =
+ ZMQ.sendMulti publisher (NE.fromList [ BS8.pack channel
+ , unPacked msg
+ ])
+ f send
+
+initialGuess :: CollTraj SbX SbZ SbU SbP NCollStages CollDeg (Vector Double)
+initialGuess = makeGuess Legendre tf guessX (const SbZ) guessU SbP
+ where
+ tf = 20
+ r = 30
+
+ guessU _ = SbU 0 0
+ guessX t = SbX 0
+ (V2 (r - r*cos(w*t)) (r*sin(w*t)))
+ (V2 (w*r*sin(w*t)) (w*r*cos(w*t)))
+ where
+ w = pi/tf
+
+type NCollStages = D200
+type CollDeg = D2
+
+solver :: NlpSolverStuff
+solver = ipoptSolver
+--solver = snoptSolver { options = [("detect_linear", Opt False)] }
+
+main :: IO ()
+main = do
+ cp <- makeCollProblem ocp
+ let nlp = cpNlp cp
+ toDyn = cpCallback cp
+ ZMQ.withContext $ \context ->
+ withPublisher context urlDynoPlot $ \sendDynoPlotMsg -> do
+-- withPublisher context urlOptTelem $ \sendOptTelemMsg -> do
+ let guess = cat initialGuess
+ proxy :: Proxy (CollTraj SbX SbZ SbU SbP NCollStages CollDeg)
+ proxy = Proxy
+ meta = toMeta (cpRoots cp) (Proxy :: Proxy SbO) proxy
+
+ callback :: J (CollTraj SbX SbZ SbU SbP NCollStages CollDeg) (Vector Double)
+ -> IO Bool
+ callback traj = do
+ (dyn,_) <- toDyn traj
+ -- dynoplot
+ let dynoPlotMsg = encodeSerial ([dyn], meta)
+ sendDynoPlotMsg "glider" dynoPlotMsg
+-- -- 3d vis
+-- let CollTraj tf' _ _ stages' xf = split traj
+-- stages :: [(CollStage (JV SbX) (JV None) (JV SbU) CollDeg) (Vector Double)]
+-- stages = map split $ F.toList $ unJVec (split stages')
+--
+-- states :: [SbX Double]
+-- states = concatMap stageToXs stages ++ [jToX xf]
+--
+-- stageToXs :: CollStage (JV SbX) (JV None) (JV SbU) CollDeg (Vector Double)
+-- -> [SbX Double]
+-- stageToXs (CollStage x0 xzus) = [jToX x0]
+---- stageToXs (CollStage x0 xzus) = jToX x0 : map getX points
+---- where
+---- points :: [CollPoint (JV SbX) (JV None) (JV SbU) (Vector Double)]
+---- points = map split (F.toList (unJVec (split xzus)))
+--
+-- jToX = fmap V.head . unJV . split
+--
+-- getX :: CollPoint (JV SbX) (JV None) (JV SbU) (Vector Double) -> SbX Double
+-- getX (CollPoint x _ _) = jToX x
+--
+-- poses :: [Msg.SbPose]
+-- poses = map stateToPose states
+--
+-- stateToPose :: SbX Double -> Msg.SbPose
+-- stateToPose acX = Msg.SbPose
+-- (toXyz (ac_r_n2b_n acX))
+-- (toDcmMsg (ac_R_n2b acX))
+-- tf :: Double
+-- tf = (V.head . unS . split) tf'
+-- msgs = [printf "final time: %.2f" tf]
+-- optTelemMsg = Msg.OptTelem (S.fromList poses) (S.fromList (map PB.fromString msgs))
+--
+-- sendOptTelemMsg "opt_telem" (encodeProto optTelemMsg)
+ return True
+
+ (msg0,opt0') <- solveNlp' solver (nlp { nlpX0' = guess }) (Just callback)
+ opt0 <- case msg0 of Left msg' -> error msg'
+ Right _ -> return opt0'
+ return ()
diff --git a/examples/Sofa.hs b/examples/Sofa.hs
new file mode 100644
--- /dev/null
+++ b/examples/Sofa.hs
@@ -0,0 +1,274 @@
+-- | How big of a sofa can we get around a corner?
+
+{-# OPTIONS_GHC -Wall #-}
+{-# Language DeriveFunctor #-}
+{-# Language DeriveGeneric #-}
+{-# Language ScopedTypeVariables #-}
+
+module Main where
+
+import Data.IORef ( newIORef, readIORef, writeIORef )
+import qualified Data.Foldable as F
+import Data.Serialize
+import qualified System.ZMQ4 as ZMQ
+import Data.ByteString.Char8 ( pack )
+
+import Dyno.Vectorize
+import Dyno.Nlp
+import Dyno.NlpSolver
+import Dyno.TypeVecs ( Vec )
+import qualified Dyno.TypeVecs as TV
+import Dyno.Solvers
+import Dyno.Nats
+
+import SofaShared
+
+type NPoints = D81
+type NSteps = D61
+
+data X a =
+ X
+ { xR :: a
+ , xPoints :: Vec NPoints (Point a)
+ , xStages :: Vec NSteps (Stage a)
+ } deriving (Functor, Generic1, Show)
+
+data G a =
+ G
+ { gMin90 :: Vec NPoints a
+ , gEqualR :: Vec NPoints a
+ , g360s :: Vec NPoints a
+ , gMean0 :: Point a
+ , gStages :: Vec NSteps (StageCon a)
+ , gCloseMean :: Vec NSteps (Point a)
+ , gCloseTheta :: Vec NSteps a
+ } deriving (Functor, Generic1, Show)
+
+data Stage a =
+ Stage
+ { sTheta :: a
+ , sMean :: Point a
+ , sPhis :: Vec NPoints a
+ } deriving (Functor, Generic1, Show)
+
+data StageCon a =
+ StageCon
+ { scOuters :: Vec NPoints (Point a)
+ , scInners :: Vec NPoints a
+ } deriving (Functor, Generic1, Show)
+
+instance Vectorize X
+instance Vectorize G
+instance Vectorize Stage
+instance Vectorize StageCon
+
+npoints :: Int
+npoints = vlength (Proxy :: Proxy (Vec NPoints))
+
+nsteps :: Int
+nsteps = vlength (Proxy :: Proxy (Vec NSteps))
+
+linspace :: Fractional a => a -> a -> Int -> [a]
+linspace x0 xf n =
+ fmap
+ (\x -> x0 + (xf - x0)*(fromIntegral x / fromIntegral (n-1)))
+ $ take n [(0::Int)..]
+
+radius0 :: Fractional a => a
+radius0 = 0.3
+
+segment0 :: Floating a => a
+segment0 = 2 * radius0 * sin(pi/fromIntegral npoints)
+
+points0 :: Vec NPoints (Point Double)
+points0 = TV.mkVec' $ map (\q -> Point (radius0*cos(q)) (radius0*sin(q))) $ take npoints $ linspace 0 (2*pi) (npoints + 1)
+
+atan2' :: RealFloat a => Point a -> a
+atan2' (Point x y) = atan2 y x
+
+--data G a =
+-- G
+-- { gMin90 :: Vec NPoints a
+-- , gEqualR :: Vec NPoints a
+-- , gMean0 :: Point a
+-- , gStages :: Vec NSteps (StageCon a)
+-- , gCloseMean :: Vec NSteps (Point a)
+-- , gCloseTheta :: Vec NSteps a
+-- } deriving (Functor, Generic1, Show)
+
+--(f0,g0) = fg guess undefined
+
+----worst :: Vectorize f => f Double -> Double
+----worst = V.toList (fmap abs)
+--
+--blah :: IO ()
+--blah = do
+---- putStrLn $ "gmin90: " ++ show (minimum $ F.toList $ gMin90 g0)
+---- putStrLn $ "gmin90: " ++ show (maximum $ F.toList $ gMin90 g0)
+-- print $ gMean0 g0
+-- print $ g360 g0
+
+
+guess :: X Double
+guess =
+ X
+ { xR = segment0
+ , xPoints = points0
+ , xStages = TV.tvzipWith (\mean theta ->
+ Stage { sTheta = theta
+ , sMean = mean
+ , sPhis = fill $ min 0 (max (pi/2) (atan2' mean))
+ }) means0 thetas0
+ }
+ where
+ thetas0 :: Vec NSteps Double
+ thetas0 = TV.mkVec' $ linspace 0 0 nsteps
+
+ means0 :: Vec NSteps (Point Double)
+ means0 = TV.mkVec' $ map f (linspace (-pi/4) (3*pi/4) nsteps)
+-- means0 = TV.mkVec' $ map f (linspace 0 (pi/2) npoints)
+ where
+ f :: Double -> Point Double
+ f q
+ | q <= pi/4 = fmap (/ (2*px)) p0
+ | otherwise = fmap (/ (2*py)) p0
+ where
+ p0 = Point px py
+ px = cos q
+ py = sin q
+
+
+myNlp :: Nlp X None G SXElement
+myNlp = Nlp { nlpFG = fg
+ , nlpBX = bx
+ , nlpBG = bg
+ , nlpX0 = guess
+ , nlpP = None
+ , nlpLamX0 = Nothing
+ , nlpLamG0 = Nothing
+ , nlpScaleF = Nothing
+ , nlpScaleX = Nothing
+ , nlpScaleG = Nothing
+ }
+ where
+
+ bx :: X Bounds
+ bx = X
+ { xR = (Just (segment0/2), Nothing)
+ , xPoints = fill $ Point (Just (-5), Just 5) (Just (-5), Just 5)
+ , xStages = TV.mkVec' $ stage0 : replicate (nsteps-1) otherStages
+ }
+ where
+ stage0 =
+ Stage
+ { sTheta = (Just 0, Just 0)
+ , sMean = Point (Just (-3), Just 3) (Just (-3), Just 3)
+ , sPhis = fill (Just 0, Just (pi/2))
+ }
+ otherStages =
+ Stage
+ { sTheta = (Just (-4*pi), Just (4*pi))
+ , sMean = Point (Just (-3), Just 3) (Just (-3), Just 3)
+ , sPhis = fill (Just 0, Just (pi/2))
+ }
+
+
+ bg :: G Bounds
+ bg = G
+ { gMin90 = fill (Just 0.8, Nothing)
+ , gEqualR = fill (Just 0, Just 0)
+ , gMean0 = fill (Just 0, Just 0)
+ , g360s = TV.mkVec' $ map (\q -> (Just (q - pi), Just (q + pi)))
+ $ linspace 0 (2*pi) npoints
+ , gStages = TV.mkVec' $ stage0 : replicate (nsteps-2) midStages ++ [stageF]
+ , gCloseMean = TV.mkVec' $ replicate (nsteps - 1) (fill (Just (-deltaMean), Just deltaMean)) ++ [fill (Nothing, Nothing)]
+ , gCloseTheta = TV.mkVec' $ replicate (nsteps - 1) (Just (-deltaTheta), Just deltaTheta) ++ [(Nothing, Nothing)]
+ }
+ where
+ deltaTheta = pi / fromIntegral nsteps
+ deltaMean = 4 / fromIntegral nsteps
+ stage0 = StageCon
+ { scOuters = fill $ Point (Nothing, Just 1) (Nothing, Just 0)
+ , scInners = fill (Just 0, Nothing)
+ }
+ stageF = StageCon
+ { scOuters = fill $ Point (Nothing, Just 0) (Nothing, Just 1)
+ , scInners = fill (Just 0, Nothing)
+ }
+ midStages = StageCon
+ { scOuters = fill $ Point (Nothing, Just 1) (Nothing, Just 1)
+ , scInners = fill (Just 0, Nothing)
+ }
+
+dot :: Num a => Point a -> Point a -> a
+dot (Point x0 y0) (Point x1 y1) = x0*x1 + y0*y1
+
+fg :: forall a . Floating a => X a -> None a -> (a, G a)
+fg (X r points stages) _ = (f, g)
+ where
+ ds :: Vec NPoints (Point a)
+ ds = zipWithNext (\x0 x1 -> x1 - x0) points
+
+ curvatureRegularization = (F.sum (zipWithNext (\x0 x1 -> dot x0 x1) ds)) / (fromIntegral npoints)
+
+ f = 1*curvatureRegularization - 0.5 * (F.sum $ zipWithNext cross points)
+ g = G
+ { gMin90 = zipWithNext (\x0 x1 -> dot x0 x1 / ((norm2 x0) * (norm2 x1))) ds
+ , gEqualR = fmap (\(Point x y) -> x*x + y*y - r*r) ds
+ , gMean0 = F.sum points / (fromIntegral npoints)
+ , g360s = TV.mkVec' $
+ drop 1 $ scanl (+) 0 $
+ F.toList $
+ zipWithNext
+ (\d0 d1 -> asin ((d0 `cross` d1) / ((1e-9 + norm2 d0) * (1e-9 + norm2 d1))))
+ ds
+ , gStages = fmap stageCon stages
+ , gCloseMean = zipWithNext (\(Stage _ mean1 _) (Stage _ mean0 _) -> mean1 - mean0) stages
+ , gCloseTheta = zipWithNext (\(Stage theta1 _ _) (Stage theta0 _ _) -> theta1 - theta0) stages
+ }
+
+ stageCon :: Stage a -> StageCon a
+ stageCon (Stage theta mean phis) = StageCon { scOuters = points'
+ , scInners = TV.tvzipWith inner points' phis
+ }
+ where
+ rot :: Point a -> Point a
+ rot (Point x y) = mean + Point (x*cos(theta) + y*sin(theta)) (-x*sin(theta) + y*cos(theta))
+
+ points' :: Vec NPoints (Point a)
+ points' = fmap rot points
+
+ inner (Point xij' yij') phiij = xij'*cos(phiij) + yij'*sin(phiij)
+
+solver :: NlpSolverStuff
+solver = ipoptSolver { options = [("ma86_order", Opt "metis"), ("max_iter", Opt (1000 :: Int))]}
+--solver = snoptSolver { options = [ ("detect_linear", Opt False) ] }
+
+send :: Serialize a => ZMQ.Socket ZMQ.Pub -> String -> a -> IO ()
+send publisher chanName stuff = do
+ let bs = encode stuff
+ ZMQ.send publisher [ZMQ.SendMore] (pack chanName)
+ ZMQ.send publisher [] bs
+
+main :: IO ()
+main =
+ ZMQ.withContext $ \context ->
+ ZMQ.withSocket context ZMQ.Pub $ \publisher -> do
+ ZMQ.bind publisher url
+ putStrLn $ "# design vars: " ++ show (vlength (Proxy :: Proxy X))
+ putStrLn $ "# constraints: " ++ show (vlength (Proxy :: Proxy G))
+ iters <- newIORef 0
+ _ <- solveNlp solver myNlp $ Just $ \x -> do
+ k <- readIORef iters
+ writeIORef iters (k + 1)
+ let msg = SofaMessage
+ { smSegmentLength = xR x
+ , smIters = k
+ , smPoints = F.toList (xPoints x)
+ , smMeanThetas = map (\stg -> (sMean stg, sTheta stg)) $ F.toList (xStages x)
+ }
+ --mapM_ (\stg -> print (sMean stg, sTheta stg)) $ F.toList (xStages x)
+ send publisher sofaChannel msg
+ return True
+ return ()
+
diff --git a/examples/StaticExample.hs b/examples/StaticExample.hs
new file mode 100644
--- /dev/null
+++ b/examples/StaticExample.hs
@@ -0,0 +1,35 @@
+{-# OPTIONS_GHC -Wall #-}
+
+module Main where
+
+import Dyno.NlpMonad
+import Dyno.Solvers
+
+rosen :: NlpMonad ()
+rosen = do
+ x1 <- designVar "x1"
+ x2 <- designVar "x2"
+ x3 <- designVar "x3"
+ x4 <- designVar "x4"
+
+ 0 + x1**2 + x2**2 + x3 === 2
+ 0 + x2**4 + x4 === 4
+ 0 + 2*x1 + 4*x2 >== 0.0
+ x3 >== 0
+ x4 >== 0
+
+ minimize $ (x1 + x2 + x3)**2 + 3*x3 + 5*x4
+
+
+main :: IO ()
+main = do
+ let guess = [ ("x1", 0.1)
+ , ("x2", 0.125)
+ , ("x3", 0.666666)
+ , ("x4", 0.142857)
+ ]
+
+ (status, fopt, xopt) <- solveStaticNlp ipoptSolver rosen guess Nothing
+ print status
+ putStrLn $ "value: " ++ show fopt
+ mapM_ (\(n,v) -> putStrLn $ n ++ ": " ++ show v) xopt
diff --git a/examples/Vec.hs b/examples/Vec.hs
new file mode 100644
--- /dev/null
+++ b/examples/Vec.hs
@@ -0,0 +1,54 @@
+-- | How to use type-indexed Vectors
+
+{-# OPTIONS_GHC -Wall #-}
+{-# Language ScopedTypeVariables #-}
+{-# Language DeriveFunctor #-}
+{-# Language DeriveGeneric #-}
+
+module Main where
+
+import qualified Data.Foldable as F
+import qualified Data.Vector as V
+
+import Dyno.Vectorize
+import Dyno.TypeVecs
+import Dyno.Nats
+
+data Params a = Params a a deriving (Functor, Generic1, Show)
+data X n a = X (Vec n (Params a)) a deriving (Functor, Generic1, Show)
+
+instance Vectorize Params
+instance Dim n => Vectorize (X n)
+
+-- some random function
+obj :: forall n a . (Num a, Dim n) => X n a -> a
+obj (X vec' b) = b*b + sum lol
+ where
+ lol :: [a]
+ lol = map (\(Params x y) -> x*x + y*y) vec
+
+ vec :: [Params a]
+ vec = F.toList vec'
+
+-- you don't know the length at compile time
+unknownLength :: (Num a, Show a) => V.Vector (Params a)
+unknownLength = V.fromList [Params 1 2, Params 3 4, Params 5 6, Params 7 8]
+
+-- you do know the length at compile time
+knownLength :: (Num a, Show a) => Vec D4 (Params a)
+knownLength = mkVec unknownLength
+
+-- do something on type-safe vec data
+doSomething :: (Dim n, Num a) => Vec n (Params a) -> a
+doSomething vec = obj (X vec 5)
+
+-- apply the type-safe operation on a vector of unknown length
+doSomethingAtRuntime :: Num a => V.Vector (Params a) -> a
+doSomethingAtRuntime vec = reifyVector vec doSomething
+
+main :: IO ()
+main = do
+ print (unknownLength :: V.Vector (Params Double))
+ print (knownLength :: Vec D4 (Params Double))
+ print (doSomething knownLength :: Double)
+ print (doSomethingAtRuntime unknownLength :: Double)
diff --git a/src/Dyno/Cov.hs b/src/Dyno/Cov.hs
new file mode 100644
--- /dev/null
+++ b/src/Dyno/Cov.hs
@@ -0,0 +1,110 @@
+{-# OPTIONS_GHC -Wall -fno-cse #-}
+{-# Language ScopedTypeVariables #-}
+{-# Language KindSignatures #-}
+--{-# Language DeriveGeneric #-}
+
+module Dyno.Cov
+ ( Cov(..)
+ , toMat
+ , fromMat
+ , toMatrix
+ , toHMatrix
+ , toHMatrix'
+ , fromMatrix
+ , diag
+ , diag'
+ , nOfVecLen
+ ) where
+
+--import GHC.Generics ( Generic )
+import Data.Vector ( Vector )
+import qualified Data.Sequence as Seq
+import System.IO.Unsafe ( unsafePerformIO )
+import qualified Data.Packed.Matrix as Mat
+
+import qualified Casadi.Sparsity as Sparsity
+import Casadi.Slice ( slice' )
+import Casadi.DMatrix ( DMatrix )
+import qualified Casadi.DMatrix as DMatrix
+
+import Dyno.Vectorize ( Vectorize(..), Proxy(..) )
+import Dyno.View.View ( View(..), J, unJ, mkJ )
+import Dyno.View.CasadiMat ( CasadiMat )
+import qualified Dyno.View.CasadiMat as CM
+import Dyno.View.JV ( JV )
+import Dyno.View.Viewable ( Viewable(..) )
+import Dyno.View.M ( M(..), mkM, toHMat )
+
+newtype Cov (f :: * -> *) a = Cov a
+instance View f => View (Cov f) where
+ cat (Cov x) = mkJ x
+ split x = Cov (unJ x)
+ size = const $ (n*n + n) `div` 2
+ where
+ n = size (Proxy :: Proxy f)
+ sizes k0 = const (Seq.singleton (k0 + n))
+ where
+ n = size (Proxy :: Proxy f)
+
+nOfVecLen :: Int -> Int
+nOfVecLen m
+ | (n*n + n) `div` 2 == m = n
+ | otherwise = error $ "nOfVecLen fail: " ++ show m
+ where
+ m' = fromIntegral m :: Double
+ n = round $ sqrt (2*m' + 1/4) - 1/2
+
+---- THIS SKIPS THE DEVECTORIZE LENGTH CHECK!!
+--instance (Serialize a) => Serialize (Cov f a) where
+-- put = put . V.toList . unCov
+-- get = fmap (Cov . V.fromList) get
+
+toMat :: (View f, CasadiMat a, Viewable a) => J (Cov f) a -> M f f a
+toMat c = mkM (toMatrix c)
+{-# NOINLINE toMat #-}
+
+toMatrix :: forall f a . (View f, CasadiMat a, Viewable a) => J (Cov f) a -> a
+toMatrix c = unsafePerformIO $ do
+ let n = size (Proxy :: Proxy f)
+ m <- CM.copy (CM.zerosSp (Sparsity.upper n))
+ --CM.setNZ m (CM.dense (unJ c)) slice'
+ CM.setNZ m (unJ c) slice' -- Joel says that "dense" isn't required here
+ return (CM.triu2symm m)
+{-# NOINLINE toMatrix #-}
+
+toHMatrix :: forall f . View f => J (Cov f) DMatrix -> Mat.Matrix Double
+toHMatrix m = toHMat (toMat m)
+
+toHMatrix' :: forall f . View f => J (Cov f) (Vector Double) -> Mat.Matrix Double
+toHMatrix' v = toHMatrix $ (mkJ (DMatrix.dvector (unJ v)) :: J (Cov f) DMatrix)
+
+diag :: (View f, CasadiMat a, Viewable a) => J f a -> J (Cov f) a
+diag = fromMatrix . CM.diag . unJ
+
+diag' :: forall f . Vectorize f => f Double -> J (Cov (JV f)) (Vector Double)
+diag' x = mkJ $ DMatrix.ddata $ DMatrix.ddense $ unJ y
+ where
+ y :: J (Cov (JV f)) DMatrix
+ y = diag $ mkJ $ DMatrix.dvector $ vectorize x
+
+--data X a = X (J S a) (J S a) deriving (Generic, Show)
+--instance View X
+--xx = X (mkJ 1) (mkJ 2) :: X DMatrix
+--xx' = cat xx
+--
+--dd :: J (Cov X) DMatrix
+--dd = diag xx'
+--
+--sp :: DMatrix
+--sp = toMatrix dd
+--
+--dd2 :: J (Cov X) DMatrix
+--dd2 = fromMatrix sp
+
+-- todo CasadiMat class
+fromMat :: (View f, CasadiMat a, Viewable a) => M f f a -> J (Cov f) a
+fromMat (UnsafeM c) = fromMatrix c
+
+fromMatrix :: (View f, CasadiMat a, Viewable a) => a -> J (Cov f) a
+fromMatrix x = mkJ $ CM.getNZ (CM.triu (CM.dense x)) slice'
+--fromMatrix x = mkJ $ CM.getNZ (CM.triu x) slice'
diff --git a/src/Dyno/Dae.hs b/src/Dyno/Dae.hs
new file mode 100644
--- /dev/null
+++ b/src/Dyno/Dae.hs
@@ -0,0 +1,41 @@
+{-# OPTIONS_GHC -Wall #-}
+
+module Dyno.Dae
+ ( Dae
+ , ExplicitOde
+ , ImplicitOde
+ , SemiExplicitDae
+ , Integrator
+ , forwardEuler
+ , rk4
+ ) where
+
+import Dyno.Vectorize
+
+--type Dae x z u p r a = x a -> x a -> z a -> u a -> p a -> a -> r a
+type Dae x z u p r a = x a -> z a -> u a -> p a -> r a
+
+type ExplicitOde x u p a = Dae x None u p x a
+type ImplicitOde x u p r a = Dae x None u p r a
+type SemiExplicitDae x z u p r a = Dae x z u p (Tuple x r) a
+
+type Integrator x z u p r a = Dae x z u p r a -> a -> x a -> u a -> p a -> x a
+
+-- x0 + dx - xf == 0
+forwardEuler :: (Vectorize x, Num a) => Integrator x None u p x a
+forwardEuler f ts x0 u p = vzipWith (+) x0 deltaX
+ where
+ deltaX = fmap (*ts) xdot
+ xdot = f x0 None u p
+
+rk4 :: (Vectorize x, Fractional a) => Integrator x None u p x a
+rk4 f h x0 u p = vzipWith (+) x0 deltaX
+ where
+ deltaX = mul (h/6) (k1 `add` mul 2 k2 `add` mul 2 k3 `add` k4)
+ k1 = f x0 None u p
+ k2 = f (x0 `add` mul (h/2) k1) None u p
+ k3 = f (x0 `add` mul (h/2) k2) None u p
+ k4 = f (x0 `add` mul h k3) None u p
+
+ mul y = fmap (y*)
+ add = vzipWith (+)
diff --git a/src/Dyno/DirectCollocation.hs b/src/Dyno/DirectCollocation.hs
new file mode 100644
--- /dev/null
+++ b/src/Dyno/DirectCollocation.hs
@@ -0,0 +1,46 @@
+{-# OPTIONS_GHC -Wall #-}
+{-# Language ScopedTypeVariables #-}
+
+module Dyno.DirectCollocation
+ ( CollTraj(..)
+ , CollProblem(..)
+ , makeCollProblem
+ , solveOcp
+ ) where
+
+import Data.Proxy
+import Data.Vector ( Vector )
+
+import Dyno.View ( J, jfill )
+import Dyno.Vectorize ( Vectorize )
+import Dyno.Ocp ( OcpPhase )
+import Dyno.NlpSolver ( NlpSolverStuff, solveNlp' )
+import Dyno.Nlp ( Nlp'(..) )
+import Dyno.DirectCollocation.Formulate ( CollProblem(..), makeCollProblem )
+import Dyno.DirectCollocation.Types ( CollTraj(..) )
+import Dyno.DirectCollocation.Dynamic ( DynCollTraj )
+import qualified Dyno.TypeVecs as TV
+
+solveOcp ::
+ forall x z u p r o c h .
+ (Vectorize x, Vectorize z, Vectorize u, Vectorize p,
+ Vectorize r, Vectorize o, Vectorize c, Vectorize h)
+ => NlpSolverStuff -> Int -> Int -> Maybe ([DynCollTraj (Vector Double)] -> IO Bool)
+ -> OcpPhase x z u p r o c h
+ -> IO (Either String String)
+solveOcp solverStuff n deg cb0 ocp =
+ TV.reifyDim n $ \(Proxy :: Proxy n) ->
+ TV.reifyDim deg $ \(Proxy :: Proxy deg) -> do
+ let guess :: J (CollTraj x z u p n deg) (Vector Double)
+ guess = jfill 1
+ cp <- makeCollProblem ocp
+ let nlp = cpNlp cp
+ toDynamic = cpCallback cp
+ --_ <- solveNlp' solverStuff (nlp {nlpX0' = guess}) (fmap (. ctToDynamic) cb)
+ let cb = case cb0 of
+ Nothing -> Nothing
+ Just cb' -> Just $ \x -> do
+ (dyn,_) <- toDynamic x
+ cb' [dyn]
+ (res, _) <- solveNlp' solverStuff (nlp {nlpX0' = guess}) cb
+ return res
diff --git a/src/Dyno/DirectCollocation/Dynamic.hs b/src/Dyno/DirectCollocation/Dynamic.hs
new file mode 100644
--- /dev/null
+++ b/src/Dyno/DirectCollocation/Dynamic.hs
@@ -0,0 +1,260 @@
+{-# OPTIONS_GHC -Wall -fno-warn-orphans #-}
+{-# Language ScopedTypeVariables #-}
+{-# Language DeriveGeneric #-}
+
+module Dyno.DirectCollocation.Dynamic
+ ( DynCollTraj(..)
+ , DynPlotPoints
+ , CollTrajMeta(..)
+ , MetaTree
+ , forestFromMeta
+ , toMeta
+ , toMetaCov
+ , ctToDynamic
+ , dynPlotPoints
+ , catDynPlotPoints
+-- , toPlotTree
+ , NameTree(..)
+ ) where
+
+import Data.List ( mapAccumL, unzip5 )
+import Data.Tree ( Tree(..) )
+import Data.Vector ( Vector )
+import qualified Data.Vector as V
+import qualified Data.Foldable as F
+import qualified Data.Tree as Tree
+import Data.Serialize ( Serialize(..) )
+import GHC.Generics ( Generic )
+import Linear.V
+
+import Dyno.Server.Accessors ( AccessorTree(..), Lookup(..), accessors )
+import Dyno.Vectorize
+import Dyno.View.JV
+import Dyno.View.View
+import qualified Dyno.TypeVecs as TV
+import Dyno.TypeVecs ( Vec )
+
+import Dyno.DirectCollocation.Types
+import Dyno.DirectCollocation.Quadratures ( QuadratureRoots, mkTaus, interpolate )
+import Dyno.DirectCollocation.Reify ( reifyCollTraj )
+
+
+data DynPlotPoints a = DynPlotPoints
+ [[(a, Vector a)]]
+ [[(a, Vector a)]]
+ [[(a, Vector a)]]
+ [[(a, Vector a)]]
+ [[(a, Vector a)]]
+ deriving Show
+
+catDynPlotPoints :: [DynPlotPoints a] -> DynPlotPoints a
+catDynPlotPoints pps =
+ DynPlotPoints
+ (concatMap (\(DynPlotPoints x _ _ _ _) -> x) pps)
+ (concatMap (\(DynPlotPoints _ x _ _ _) -> x) pps)
+ (concatMap (\(DynPlotPoints _ _ x _ _) -> x) pps)
+ (concatMap (\(DynPlotPoints _ _ _ x _) -> x) pps)
+ (concatMap (\(DynPlotPoints _ _ _ _ x) -> x) pps)
+
+data D a
+data DynCollTraj a = DynCollTraj (J (CollTraj D D D D () ()) a) (Vec () (Vec () (J D a, J D a)))
+ deriving (Generic, Show)
+instance Serialize a => Serialize (DynCollTraj a)
+instance Serialize a => Serialize (V.Vector a) where
+ put = put . V.toList
+ get = fmap V.fromList get
+
+dynPlotPoints ::
+ forall a .
+ (Real a, Fractional a, Show a)
+ => DynCollTraj (Vector a) -> CollTrajMeta -> DynPlotPoints a
+dynPlotPoints (DynCollTraj traj outputs) meta =
+ reifyCollTraj (nx,nz,nu,np,no,n,deg) traj outputs foo
+ where
+ quadratureRoots = ctmQuadRoots meta
+ nx = ctmNx meta
+ nz = ctmNz meta
+ nu = ctmNu meta
+ np = ctmNp meta
+ no = ctmNo meta
+ n = ctmN meta
+ deg = ctmDeg meta
+
+ foo :: (Vectorize x, Vectorize z, Vectorize u, Vectorize p, Vectorize o, Dim deg, Dim n)
+ => J (CollTraj x z u p n deg) (Vector a)
+ -> Vec n (Vec deg (J (JV o) (Vector a), J (JV x) (Vector a)))
+ -> DynPlotPoints a
+ foo ct outs = plotPoints quadratureRoots (split ct) outs
+
+
+-- a safe, point maker which is difficult to work with
+-- first stage in making a list
+plotPoints ::
+ forall x z u p o n deg a .
+ (Dim n, Dim deg, Real a, Fractional a, Show a,
+ Vectorize x, Vectorize z, Vectorize u, Vectorize o, Vectorize p)
+ => QuadratureRoots -> CollTraj x z u p n deg (Vector a)
+ -> Vec n (Vec deg (J (JV o) (Vector a), J (JV x) (Vector a)))
+ -> DynPlotPoints a
+plotPoints quadratureRoots (CollTraj (UnsafeJ tf') _ stages' xf) outputs =
+ DynPlotPoints (xss++[[(tf,unJ xf)]]) zss uss oss xdss
+ where
+ nStages = size (Proxy :: Proxy (JVec n S))
+ tf,h :: a
+ tf = V.head tf'
+ h = tf / fromIntegral nStages
+
+ taus :: Vec deg a
+ taus = mkTaus quadratureRoots
+
+ stages :: Vec n (CollStage (JV x) (JV z) (JV u) deg (Vector a))
+ stages = fmap split (unJVec (split stages'))
+ (xss,zss,uss,oss,xdss) = unzip5 $ F.toList $ f 0 $ zip (F.toList stages) (F.toList outputs)
+
+
+ -- todo: check this final time against expected tf
+ f :: a
+ -> [( CollStage (JV x) (JV z) (JV u) deg (Vector a)
+ , Vec deg (J (JV o) (Vector a), J (JV x) (Vector a))
+ )]
+ -> [( [(a,Vector a)]
+ , [(a,Vector a)]
+ , [(a,Vector a)]
+ , [(a,Vector a)]
+ , [(a,Vector a)]
+ )]
+ f _ [] = []
+ f t0 ((CollStage x0 xzus', xdos') : css) = (xs,zs,us,os,xds) : f tnext css
+ where
+ tnext = t0 + h
+ xzus0 = fmap split (unJVec (split xzus')) :: Vec deg (CollPoint (JV x) (JV z) (JV u) (Vector a))
+ xnext = interpolate taus x0 (fmap getX xzus0)
+
+ getX (CollPoint x _ _) = x
+
+ xs :: [(a,Vector a)]
+ xs = (t0,unJ x0):xs'++[(tnext,unJ xnext)]
+
+ xs',zs,us,os,xds :: [(a,Vector a)]
+ (xs',zs,us,os,xds) = unzip5 $ F.toList $ TV.tvzipWith3 g xzus0 xdos' taus
+
+ g (CollPoint x z u) (o,x') tau = ( (t,unJ' "x" x), (t,unJ' "z" z), (t,unJ' "u" u), (t,unJ' "o" o), (t,unJ' "x'" x') )
+ where
+ t = t0 + h*tau
+
+--toPlotTree :: forall x z u .
+-- (Lookup (x Double), Lookup (z Double), Lookup (u Double),
+-- Vectorize x, Vectorize z, Vectorize u) =>
+-- Tree (String, String, Maybe (PlotPointsL x z u Double -> [[(Double, Double)]]))
+--toPlotTree = Node ("trajectory", "trajectory", Nothing) [xtree, ztree, utree]
+-- where
+-- xtree :: Tree ( String, String, Maybe (PlotPointsL x z u Double -> [[(Double, Double)]]))
+-- xtree = toGetterTree (\(PlotPointsL x _ _) -> x) "differential states" $ accessors (fill 0)
+--
+-- ztree :: Tree ( String, String, Maybe (PlotPointsL x z u Double -> [[(Double, Double)]]))
+-- ztree = toGetterTree (\(PlotPointsL _ z _) -> z) "algebraic variables" $ accessors (fill 0)
+--
+-- utree :: Tree ( String, String, Maybe (PlotPointsL x z u Double -> [[(Double, Double)]]))
+-- utree = toGetterTree (\(PlotPointsL _ _ u) -> u) "controls" $ accessors (fill 0)
+--
+-- toGetterTree toXs name (Getter f) = Node (name, name, Just g) []
+-- where
+-- g = map (map (second f)) . toXs
+-- toGetterTree toXs name (Data (_,name') children) =
+-- Node (name, name', Nothing) $ map (uncurry (toGetterTree toXs)) children
+
+
+data NameTree = NameTreeNode (String,String) [(String,NameTree)]
+ | NameTreeLeaf Int
+ deriving (Show, Eq, Generic)
+instance Serialize NameTree
+
+data CollTrajMeta = CollTrajMeta { ctmX :: NameTree
+ , ctmZ :: NameTree
+ , ctmU :: NameTree
+ , ctmP :: NameTree
+ , ctmO :: NameTree
+ , ctmNx :: Int
+ , ctmNz :: Int
+ , ctmNu :: Int
+ , ctmNp :: Int
+ , ctmNo :: Int
+ , ctmNsx :: Int
+ , ctmN :: Int
+ , ctmDeg :: Int
+ , ctmQuadRoots :: QuadratureRoots
+ } deriving (Eq, Generic, Show)
+instance Serialize CollTrajMeta
+
+namesFromAccTree :: AccessorTree a -> NameTree
+namesFromAccTree x = (\(_,(_,y)) -> y) $ namesFromAccTree' 0 ("",x)
+
+namesFromAccTree' :: Int -> (String, AccessorTree a) -> (Int, (String, NameTree))
+namesFromAccTree' k (nm, Getter _) = (k+1, (nm, NameTreeLeaf k))
+namesFromAccTree' k0 (nm, Data names ats) = (k, (nm, NameTreeNode names children))
+ where
+ (k, children) = mapAccumL namesFromAccTree' k0 ats
+
+
+type MetaTree a = Tree.Forest (String, String, Maybe (DynPlotPoints a -> [[(a,a)]]))
+
+forestFromMeta :: CollTrajMeta -> MetaTree Double
+forestFromMeta meta = [xTree,zTree,uTree,oTree,xdTree]
+ where
+ xTree = blah (\(DynPlotPoints x _ _ _ _ ) -> x) "differential states" (ctmX meta)
+ zTree = blah (\(DynPlotPoints _ z _ _ _ ) -> z) "algebraic variables" (ctmZ meta)
+ uTree = blah (\(DynPlotPoints _ _ u _ _ ) -> u) "controls" (ctmU meta)
+ oTree = blah (\(DynPlotPoints _ _ _ o _ ) -> o) "outputs" (ctmO meta)
+ xdTree = blah (\(DynPlotPoints _ _ _ _ xd) -> xd) "diff state derivatives" (ctmX meta)
+
+ blah :: (c -> [[(t, V.Vector t)]]) -> String -> NameTree ->
+ Tree (String, String, Maybe (c -> [[(t, t)]]))
+ blah f myname (NameTreeNode (nm1,_) children) =
+ Tree.Node (myname,nm1,Nothing) $ map (uncurry (blah f)) children
+ blah f myname (NameTreeLeaf k) = Tree.Node (myname,"",Just (woo . f)) []
+ where
+ woo = map (map (\(t,x) -> (t, x V.! k)))
+
+
+toMeta :: forall x z u p o n deg .
+ (Lookup (x ()), Lookup (z ()), Lookup (u ()), Lookup (p ()), Lookup (o ()),
+ Vectorize x, Vectorize z, Vectorize u, Vectorize p, Vectorize o,
+ Dim n, Dim deg)
+ => QuadratureRoots -> Proxy o -> Proxy (CollTraj x z u p n deg) -> CollTrajMeta
+toMeta roots _ _ =
+ CollTrajMeta { ctmX = namesFromAccTree $ accessors (jfill () :: J (JV x) (Vector ()))
+ , ctmZ = namesFromAccTree $ accessors (jfill () :: J (JV z) (Vector ()))
+ , ctmU = namesFromAccTree $ accessors (jfill () :: J (JV u) (Vector ()))
+ , ctmP = namesFromAccTree $ accessors (jfill () :: J (JV p) (Vector ()))
+ , ctmO = namesFromAccTree $ accessors (jfill () :: J (JV o) (Vector ()))
+ , ctmNx = size (Proxy :: Proxy (JV x))
+ , ctmNz = size (Proxy :: Proxy (JV z))
+ , ctmNu = size (Proxy :: Proxy (JV u))
+ , ctmNp = size (Proxy :: Proxy (JV p))
+ , ctmNo = size (Proxy :: Proxy (JV o))
+ , ctmNsx = 0
+ , ctmN = reflectDim (Proxy :: Proxy n)
+ , ctmDeg = reflectDim (Proxy :: Proxy deg)
+ , ctmQuadRoots = roots
+ }
+
+toMetaCov :: forall sx x z u p o n deg .
+ (Lookup (x ()), Lookup (z ()), Lookup (u ()), Lookup (p ()), Lookup (o ()),
+ Vectorize x, Vectorize z, Vectorize u, Vectorize p, Vectorize o,
+ Vectorize sx,
+ Dim n, Dim deg)
+ => QuadratureRoots -> Proxy o -> Proxy (CollTrajCov sx x z u p n deg) -> CollTrajMeta
+toMetaCov roots _ _ = meta0 { ctmNsx = size (Proxy :: Proxy (JV sx)) }
+ where
+ meta0 = toMeta roots (Proxy :: Proxy o) (Proxy :: Proxy (CollTraj x z u p n deg))
+
+ctToDynamic :: forall x z u p o n deg a .
+ (Vectorize x, Vectorize z, Vectorize u, Vectorize p) =>
+ J (CollTraj x z u p n deg) a -> Vec n (Vec deg (J (JV o) a, J (JV x) a)) -> DynCollTraj a
+ctToDynamic (UnsafeJ x) os = DynCollTraj (UnsafeJ x) (castO os) -- this should be totally safe
+ where
+ castO :: Vec n (Vec deg (J (JV o) a, J (JV x) a)) -> Vec () (Vec () (J D a, J D a))
+ castO = TV.mkUnit . fmap (TV.mkUnit . fmap cast)
+
+ cast :: (J (JV o) a, J (JV x) a) -> (J D a, J D a)
+ cast (UnsafeJ o, UnsafeJ x') = (UnsafeJ o, UnsafeJ x')
diff --git a/src/Dyno/DirectCollocation/Export.hs b/src/Dyno/DirectCollocation/Export.hs
new file mode 100644
--- /dev/null
+++ b/src/Dyno/DirectCollocation/Export.hs
@@ -0,0 +1,106 @@
+{-# OPTIONS_GHC -Wall #-}
+{-# Language ScopedTypeVariables #-}
+
+module Dyno.DirectCollocation.Export
+ ( toMatlab
+ ) where
+
+import Linear.V ( Dim(..) )
+import Data.Vector ( Vector )
+import qualified Data.Vector as V
+import qualified Data.Foldable as F
+
+import Dyno.Server.Accessors ( Lookup, flatten, accessors )
+import Dyno.TypeVecs ( Vec )
+import Dyno.Vectorize ( Vectorize, Proxy(..), fill )
+import Dyno.View ( View(..), JV, JVec(..), unJ, splitJV )
+import Dyno.DirectCollocation.Formulate ( CollProblem(..) )
+import Dyno.DirectCollocation.Types ( CollTraj(..), CollStage(..), CollPoint(..) )
+import Dyno.DirectCollocation.Quadratures ( timesFromTaus )
+
+toMatlab ::
+ forall x z u p r c h o n deg
+ . ( Lookup (x Double), Vectorize x
+ , Lookup (z Double), Vectorize z
+ , Lookup (u Double), Vectorize u
+ , Lookup (o Double), Vectorize o
+ , Lookup (p Double), Vectorize p
+ , Dim deg
+ , Dim n
+ )
+ => CollProblem x z u p r c h o n deg
+ -> CollTraj x z u p n deg (Vector Double)
+ -> IO String
+toMatlab cp ct@(CollTraj tf' p' stages' xf) = do
+ outs <- fmap snd $ cpCallback cp (cat ct)
+
+ let taus :: Vec deg Double
+ taus = cpTaus cp
+ tf = V.head (unJ tf')
+
+ n = reflectDim (Proxy :: Proxy n)
+
+ times :: Vec n (Double, Vec deg Double)
+ times = timesFromTaus 0 taus dt
+ where
+ dt = tf / fromIntegral n
+
+ xTimes = concatMap (\(t0,ts) -> t0 : F.toList ts) (F.toList times) ++ [tf]
+ zuoTimes = concatMap (\(_,ts) -> F.toList ts) (F.toList times)
+
+ stages :: [CollStage (JV x) (JV z) (JV u) deg (Vector Double)]
+ stages = map split $ F.toList $ unJVec $ split stages'
+
+ xs :: [x Double]
+ xs = concatMap getXs stages ++ [splitJV xf]
+
+ zs :: [z Double]
+ zs = concatMap getZs stages
+
+ us :: [u Double]
+ us = concatMap getUs stages
+
+ os :: [o Double]
+ xdots :: [x Double]
+ (os, xdots) = unzip $ F.concatMap F.toList outs
+
+ getXs (CollStage x0 xzus) = splitJV x0 : map (getX . split) (F.toList (unJVec (split xzus)))
+ getZs (CollStage _ xzus) = map (getZ . split) (F.toList (unJVec (split xzus)))
+ getUs (CollStage _ xzus) = map (getU . split) (F.toList (unJVec (split xzus)))
+
+ getX :: CollPoint (JV x) (JV z) (JV u) (Vector Double) -> x Double
+ getX (CollPoint x _ _) = splitJV x
+
+ getZ :: CollPoint (JV x) (JV z) (JV u) (Vector Double) -> z Double
+ getZ (CollPoint _ z _) = splitJV z
+
+ getU :: CollPoint (JV x) (JV z) (JV u) (Vector Double) -> u Double
+ getU (CollPoint _ _ u) = splitJV u
+
+ at :: (Vectorize xzu, Lookup (xzu Double)) => [(String, xzu Double -> Double)]
+ at = flatten $ accessors (fill 0)
+
+ p = splitJV p'
+
+ woo :: String -> [xzu Double] -> String -> (xzu Double -> Double) -> String
+ woo topName xzus name get = topName ++ "." ++ name ++ " = " ++ show (map get xzus) ++ ";"
+
+ wooP :: String -> (p Double -> Double) -> String
+ wooP name get = "params." ++ name ++ " = " ++ show (get p) ++ ";"
+
+ ret :: String
+ ret = init $ unlines $
+ map (uncurry (woo "ret.diffStates" xs)) at ++
+ map (uncurry (woo "ret.diffStateDerivs" xdots)) at ++
+ map (uncurry (woo "ret.algVars" zs)) at ++
+ map (uncurry (woo "ret.controls" us)) at ++
+ map (uncurry (woo "ret.outputs" os)) at ++
+ map (uncurry wooP) at ++
+ [ ""
+ , "ret.tx = " ++ show xTimes
+ , "ret.tzuo = " ++ show zuoTimes
+ , "ret.N = " ++ show n
+ , "ret.deg = " ++ show (reflectDim (Proxy :: Proxy deg))
+ , "ret.collocationRoots = '" ++ show (cpRoots cp) ++ "'"
+ ]
+ return ret
diff --git a/src/Dyno/DirectCollocation/Formulate.hs b/src/Dyno/DirectCollocation/Formulate.hs
new file mode 100644
--- /dev/null
+++ b/src/Dyno/DirectCollocation/Formulate.hs
@@ -0,0 +1,865 @@
+{-# OPTIONS_GHC -Wall #-}
+--{-# OPTIONS_GHC -fdefer-type-errors #-}
+{-# Language DeriveGeneric #-}
+{-# Language ScopedTypeVariables #-}
+{-# Language TypeOperators #-}
+{-# Language FlexibleContexts #-}
+
+module Dyno.DirectCollocation.Formulate
+ ( CovTraj(..)
+ , CollProblem(..)
+ , CollCovProblem(..)
+ , makeCollProblem
+ , makeCollCovProblem
+ , mkTaus
+ , interpolate
+ , makeGuess
+ , makeGuessSim
+ ) where
+
+import GHC.Generics ( Generic )
+import Data.Maybe ( fromMaybe )
+import Data.Proxy ( Proxy(..) )
+import Data.Vector ( Vector )
+import qualified Data.Vector as V
+import qualified Data.Foldable as F
+import qualified Data.Traversable as T
+import qualified Data.Packed.Matrix as Mat
+import qualified Numeric.LinearAlgebra.Algorithms as LA
+import Linear.Matrix hiding ( trace )
+import Linear.V
+
+import Casadi.DMatrix ( dvector, ddata, ddense )
+
+import Dyno.SXElement ( sxToSXElement, sxElementToSX )
+import Dyno.View.CasadiMat hiding ( solve )
+import Dyno.Cov
+import Dyno.View.View
+import Dyno.View.JV ( JV, sxCatJV, sxSplitJV, catJV, catJV' )
+import Dyno.View.HList ( (:*:)(..) )
+import Dyno.View.Fun
+import Dyno.View.Viewable ( Viewable )
+import Dyno.View.Scheme ( Scheme )
+import Dyno.Vectorize ( Vectorize(..), fill, vlength, vzipWith )
+import Dyno.TypeVecs ( Vec )
+import qualified Dyno.TypeVecs as TV
+import Dyno.LagrangePolynomials ( lagrangeDerivCoeffs )
+import Dyno.Nlp ( Nlp'(..), Bounds )
+import Dyno.Ocp ( OcpPhase(..), OcpPhaseWithCov(..) )
+
+import Dyno.DirectCollocation.Types
+import Dyno.DirectCollocation.Dynamic ( DynCollTraj, ctToDynamic )
+import Dyno.DirectCollocation.Quadratures ( QuadratureRoots(..), mkTaus, interpolate, timesFromTaus )
+import Dyno.DirectCollocation.Robust
+
+data CollProblem x z u p r c h o n deg =
+ CollProblem
+ { cpNlp :: Nlp' (CollTraj x z u p n deg) JNone (CollOcpConstraints n deg x r c h) MX
+ , cpOcp :: OcpPhase x z u p r o c h
+ , cpCallback :: J (CollTraj x z u p n deg) (Vector Double)
+ -> IO (DynCollTraj (Vector Double), Vec n (Vec deg (o Double, x Double)))
+ , cpTaus :: Vec deg Double
+ , cpRoots :: QuadratureRoots
+ }
+
+makeCollProblem ::
+ forall x z u p r o c h deg n .
+ (Dim deg, Dim n, Vectorize x, Vectorize p, Vectorize u, Vectorize z,
+ Vectorize r, Vectorize o, Vectorize h, Vectorize c)
+ => OcpPhase x z u p r o c h
+ -> IO (CollProblem x z u p r c h o n deg)
+makeCollProblem ocp = do
+ let -- the collocation points
+ roots :: QuadratureRoots
+ roots = Legendre
+
+ taus :: Vec deg Double
+ taus = mkTaus roots
+
+ n = reflectDim (Proxy :: Proxy n)
+
+ -- coefficients for getting xdot by lagrange interpolating polynomials
+ cijs :: Vec (TV.Succ deg) (Vec (TV.Succ deg) Double)
+ cijs = lagrangeDerivCoeffs (0 TV.<| taus)
+
+ bcFun <- toSXFun "bc" $ \(x0:*:x1) -> sxCatJV $ ocpBc ocp (sxSplitJV x0) (sxSplitJV x1)
+ mayerFun <- toSXFun "mayer" $ \(x0:*:x1:*:x2) ->
+ mkJ $ sxElementToSX $ ocpMayer ocp (sxToSXElement (unJ x0)) (sxSplitJV x1) (sxSplitJV x2)
+ lagrangeFun <- toSXFun "lagrange" $ \(x0:*:x1:*:x2:*:x3:*:x4:*:x5:*:x6) ->
+ mkJ $ sxElementToSX $ ocpLagrange ocp (sxSplitJV x0) (sxSplitJV x1) (sxSplitJV x2) (sxSplitJV x3) (sxSplitJV x4) (sxToSXElement (unJ x5)) (sxToSXElement (unJ x6))
+ quadFun <- toMXFun "quadratures" $ evaluateQuadraturesFunction lagrangeFun cijs taus n
+-- let callQuadFun = call quadFun
+ callQuadFun <- fmap call (expandMXFun quadFun)
+
+ dynFun <- toSXFun "dynamics" $ dynamicsFunction $
+ \x0 x1 x2 x3 x4 x5 ->
+ let (r,o) = ocpDae ocp (sxSplitJV x0) (sxSplitJV x1) (sxSplitJV x2) (sxSplitJV x3) (sxSplitJV x4) (sxToSXElement (unJ x5))
+ in (sxCatJV r, sxCatJV o)
+
+ pathConFun <- toSXFun "pathConstraints" $ pathConFunction $
+ \x0 x1 x2 x3 x4 x5 -> sxCatJV $ ocpPathC ocp (sxSplitJV x0) (sxSplitJV x1) (sxSplitJV x2) (sxSplitJV x3) (sxSplitJV x4) (sxToSXElement (unJ x5))
+ pathStageConFun <- toMXFun "pathStageCon" (pathStageConstraints pathConFun)
+
+ dynStageConFun <- toMXFun "dynamicsStageCon" (dynStageConstraints cijs taus dynFun)
+
+ stageFun <- toMXFun "stageFunction" $ stageFunction pathStageConFun (call dynStageConFun)
+-- let callStageFun = call stageFun
+ callStageFun <- fmap call (expandMXFun stageFun)
+
+ outputFun <- toMXFun "stageOutputs" $ outputFunction cijs taus dynFun
+
+ -- prepare callbacks
+ let nlpX0 = jfill 0 :: J (CollTraj x z u p n deg) (Vector Double)
+
+ f :: J (JV o) DMatrix -> J (JV x) DMatrix
+ -> (J (JV o) (Vector Double), J (JV x) (Vector Double))
+ f o' x' = (mkJ (ddata (ddense (unJ o'))), mkJ (ddata (ddense (unJ x'))))
+
+ dmToDv :: J a (Vector Double) -> J a DMatrix
+ dmToDv (UnsafeJ v) = UnsafeJ (dvector v)
+
+ callOutputFun :: J (JV p) (Vector Double)
+ -> J S (Vector Double)
+ -> J (CollStage (JV x) (JV z) (JV u) deg) (Vector Double)
+ -> J S (Vector Double)
+ -> IO (Vec deg (J (JV o) (Vector Double), J (JV x) (Vector Double)))
+ callOutputFun p h stage k = do
+ (_ :*: xdot :*: out) <- eval outputFun $
+ (dmToDv stage) :*: (dmToDv p) :*: (dmToDv h) :*: (dmToDv k)
+ let outs0 = unJVec (split out) :: Vec deg (J (JV o) DMatrix)
+ xdots0 = unJVec (split xdot) :: Vec deg (J (JV x) DMatrix)
+ return (TV.tvzipWith f outs0 xdots0)
+
+ mapOutputFun :: J (CollTraj x z u p n deg) (Vector Double)
+ -> IO (Vec n (Vec deg (J (JV o) (Vector Double), J (JV x) (Vector Double))))
+ mapOutputFun ct = do
+ let CollTraj tf p stages _ = split ct
+ h = tf / fromIntegral n
+
+ vstages = unJVec (split stages)
+ :: Vec n (J (CollStage (JV x) (JV z) (JV u) deg) (Vector Double))
+ ks :: Vec n (J S (Vector Double))
+ ks = TV.mkVec' $ map (mkJ . V.singleton . realToFrac) (take n [(0::Int)..])
+
+ T.sequence $ TV.tvzipWith (callOutputFun p h) vstages ks
+
+ callback :: J (CollTraj x z u p n deg) (Vector Double)
+ -> IO (DynCollTraj (Vector Double), Vec n (Vec deg (o Double, x Double)))
+ callback traj = do
+ outputs <- mapOutputFun traj
+ let -- devectorize outputs
+ devec :: (J (JV o) (Vector Double), J (JV x) (Vector Double)) -> (o Double, x Double)
+ devec (UnsafeJ os, UnsafeJ xds) = (devectorize os, devectorize xds)
+ return (ctToDynamic traj outputs, fmap (fmap devec) outputs)
+
+ let nlp = Nlp' {
+ nlpFG' =
+ getFg taus
+ (bcFun :: SXFun (J (JV x) :*: J (JV x)) (J (JV c)))
+ (mayerFun :: SXFun (J S :*: (J (JV x) :*: (J (JV x)))) (J S))
+ (callQuadFun :: (J (JV p) :*: J (JVec deg (CollPoint (JV x) (JV z) (JV u))) :*: J (JVec deg (JV o)) :*: J S :*: J (JVec deg S)) MX
+ -> J S MX)
+ (callStageFun :: (J S :*: J (JV p) :*: J (JVec deg S) :*: J (JV x) :*: J (JVec deg (JTuple (JV x) (JV z))) :*: J (JVec deg (JV u))) MX
+ -> (J (JVec deg (JV r)) :*: J (JVec deg (JV o)) :*: J (JVec deg (JV h)) :*: J (JV x)) MX)
+ , nlpBX' = cat $ fillCollTraj
+ (ocpXbnd ocp)
+ (ocpZbnd ocp)
+ (ocpUbnd ocp)
+ (ocpPbnd ocp)
+ (ocpTbnd ocp)
+ , nlpBG' = cat (getBg ocp)
+ , nlpX0' = nlpX0
+ , nlpP' = cat JNone
+ , nlpLamX0' = Nothing
+ , nlpLamG0' = Nothing
+ , nlpScaleF' = ocpObjScale ocp
+ , nlpScaleX' = Just $ cat $ fillCollTraj
+ (fromMaybe (fill 1) (ocpXScale ocp))
+ (fromMaybe (fill 1) (ocpZScale ocp))
+ (fromMaybe (fill 1) (ocpUScale ocp))
+ (fromMaybe (fill 1) (ocpPScale ocp))
+ (fromMaybe 1 (ocpTScale ocp))
+
+ , nlpScaleG' = Just $ cat $ fillCollConstraints
+ (fromMaybe (fill 1) (ocpXScale ocp))
+ (fromMaybe (fill 1) (ocpResidualScale ocp))
+ (fromMaybe (fill 1) (ocpBcScale ocp))
+ (fromMaybe (fill 1) (ocpPathCScale ocp))
+ }
+ return $ CollProblem { cpNlp = nlp
+ , cpOcp = ocp
+ , cpCallback = callback
+ , cpTaus = taus
+ , cpRoots = roots
+ }
+
+
+data CollCovProblem x z u p r o c h n deg sx sw sh shr sc =
+ CollCovProblem
+ { ccpNlp :: Nlp'
+ (CollTrajCov sx x z u p n deg)
+ JNone
+ (CollOcpCovConstraints n deg x r c h sh shr sc) MX
+ , ccpCallback ::
+ J (CollTrajCov sx x z u p n deg) (Vector Double)
+ -> IO ( DynCollTraj (Vector Double), Vec n (Vec deg (o Double, x Double))
+ , Vec n (J (Cov (JV sx)) (Vector Double)), J (Cov (JV sx)) (Vector Double)
+ )
+ , ccpSensitivities :: MXFun
+ (J (CollTraj x z u p n deg))
+ (CovarianceSensitivities (JV sx) (JV sw) n)
+ , ccpCovariances :: MXFun
+ (J (CollTrajCov sx x z u p n deg)) (J (CovTraj sx n))
+ , ccpRoots :: QuadratureRoots
+ }
+
+makeCollCovProblem ::
+ forall x z u p r o c h sx sz sw sr sh shr sc deg n .
+ (Dim deg, Dim n, Vectorize x, Vectorize p, Vectorize u, Vectorize z,
+ Vectorize sr, Vectorize sw, Vectorize sz, Vectorize sx,
+ Vectorize r, Vectorize o, Vectorize h, Vectorize c,
+ View sh, Vectorize shr, View sc)
+ => OcpPhase x z u p r o c h
+ -> OcpPhaseWithCov (OcpPhase x z u p r o c h) sx sz sw sr sh shr sc
+ -> IO (CollCovProblem x z u p r o c h n deg sx sw sh shr sc)
+makeCollCovProblem ocp ocpCov = do
+ let -- the collocation points
+ roots = Legendre
+
+ taus :: Vec deg Double
+ taus = mkTaus roots
+
+ computeSensitivities <- mkComputeSensitivities roots (ocpCovDae ocpCov)
+ computeCovariances <- mkComputeCovariances continuousToDiscreetNoiseApprox
+ (computeSensitivities) (ocpCovSq ocpCov)
+
+ sbcFun <- toSXFun "sbc" $ \(x0:*:x1) -> ocpCovSbc ocpCov x0 x1
+ shFun <- toSXFun "sh" $ \(x0:*:x1) -> ocpCovSh ocpCov (sxSplitJV x0) x1
+ mayerFun <- toSXFun "cov mayer" $ \(x0:*:x1:*:x2:*:x3:*:x4) ->
+ mkJ $ sxElementToSX $ ocpCovMayer ocpCov (sxToSXElement (unJ x0)) (sxSplitJV x1) (sxSplitJV x2) x3 x4
+ lagrangeFun <- toSXFun "cov lagrange" $ \(x0:*:x1:*:x2:*:x3) ->
+ mkJ $ sxElementToSX $ ocpCovLagrange ocpCov (sxToSXElement (unJ x0)) (sxSplitJV x1) x2 (sxToSXElement (unJ x3))
+
+ cp0 <- makeCollProblem ocp
+
+ robustify <- mkRobustifyFunction (ocpCovProjection ocpCov) (ocpCovRobustifyPathC ocpCov)
+
+ let nlp0 = cpNlp cp0
+ callback0 = cpCallback cp0
+ gammas' = ocpCovGammas ocpCov :: shr Double
+
+ gammas :: J (JV shr) MX
+ gammas = catJV' (fmap realToFrac gammas')
+
+ rpathCUb :: shr Bounds
+ rpathCUb = fill (Nothing, Just 0)
+
+ robustPathCUb :: J (JV shr) (Vector Bounds)
+ robustPathCUb = catJV rpathCUb
+
+ -- the NLP
+ fg :: J (CollTrajCov sx x z u p n deg) MX
+ -> J JNone MX
+ -> (J S MX, J (CollOcpCovConstraints n deg x r c h sh shr sc) MX)
+ fg = getFgCov taus
+ computeCovariances
+ gammas
+ (robustify :: (J (JV shr) MX -> J (JV p) MX -> J (JV x) MX -> J (Cov (JV sx)) MX -> J (JV shr) MX))
+ (sbcFun :: SXFun (J (Cov (JV sx)) :*: J (Cov (JV sx))) (J sc))
+ (shFun :: SXFun (J (JV x) :*: J (Cov (JV sx))) (J sh))
+ (lagrangeFun :: SXFun (J S :*: J (JV x) :*: J (Cov (JV sx)) :*: J S) (J S))
+ (mayerFun :: SXFun (J S :*: (J (JV x) :*: (J (JV x) :*: (J (Cov (JV sx)) :*: J (Cov (JV sx)))))) (J S))
+ (nlpFG' nlp0)
+
+ computeCovariancesFun' <- toMXFun "compute covariances" computeCovariances
+ -- callbacks
+ let dmToDv :: J a (Vector Double) -> J a DMatrix
+ dmToDv (UnsafeJ v) = UnsafeJ (dvector v)
+
+ --dvToDm :: View a => J a DMatrix -> J a (Vector Double)
+ --dvToDm v = mkJ (ddata (ddense (unJ v)))
+ dvToDm :: J a DMatrix -> J a (Vector Double)
+ dvToDm (UnsafeJ v) = UnsafeJ (ddata (ddense v))
+
+ callback collTrajCov = do
+ let CollTrajCov _ collTraj = split collTrajCov
+ (dynCollTraj, outputs) <- callback0 collTraj
+ covTraj <- fmap split $ eval computeCovariancesFun' (dmToDv collTrajCov)
+ let covs' = ctAllButLast covTraj
+ pF = ctLast covTraj
+ let covs = unJVec (split covs') :: Vec n (J (Cov (JV sx)) DMatrix)
+ return (dynCollTraj, outputs, fmap dvToDm covs, dvToDm pF)
+
+ nlp =
+ Nlp'
+ { nlpFG' = fg
+ , nlpBX' = cat $ CollTrajCov (ocpCovS0bnd ocpCov) (nlpBX' nlp0)
+ , nlpBG' = cat $ CollOcpCovConstraints
+ { cocNormal = nlpBG' nlp0
+ , cocCovPathC = jreplicate (ocpCovShBnds ocpCov)
+ , cocCovRobustPathC = jreplicate robustPathCUb
+ , cocSbc = ocpCovSbcBnds ocpCov
+ }
+ , nlpX0' = cat $ CollTrajCov (jfill 0) (nlpX0' nlp0)
+ , nlpP' = cat JNone
+ , nlpLamX0' = Nothing
+ , nlpLamG0' = Nothing
+ , nlpScaleF' = ocpObjScale ocp
+ , nlpScaleX' = Just $ cat $
+ CollTrajCov (fromMaybe (jfill 1) (ocpCovSScale ocpCov)) $
+ cat $ fillCollTraj
+ (fromMaybe (fill 1) (ocpXScale ocp))
+ (fromMaybe (fill 1) (ocpZScale ocp))
+ (fromMaybe (fill 1) (ocpUScale ocp))
+ (fromMaybe (fill 1) (ocpPScale ocp))
+ (fromMaybe 1 (ocpTScale ocp))
+
+ , nlpScaleG' = Just $ cat $ CollOcpCovConstraints
+ { cocNormal = cat $ fillCollConstraints
+ (fromMaybe (fill 1) (ocpXScale ocp))
+ (fromMaybe (fill 1) (ocpResidualScale ocp))
+ (fromMaybe (fill 1) (ocpBcScale ocp))
+ (fromMaybe (fill 1) (ocpPathCScale ocp))
+ , cocCovPathC = jreplicate (fromMaybe (jfill 1) (ocpCovPathCScale ocpCov))
+ , cocCovRobustPathC = jreplicate $
+ fromMaybe (jfill 1) $
+ fmap catJV (ocpCovRobustPathCScale ocpCov)
+ , cocSbc = fromMaybe (jfill 1) (ocpCovSbcScale ocpCov)
+ }
+ }
+ computeSensitivitiesFun' <- toMXFun "compute sensitivities" computeSensitivities
+ return $ CollCovProblem { ccpNlp = nlp
+ , ccpCallback = callback
+ , ccpSensitivities = computeSensitivitiesFun'
+ , ccpCovariances = computeCovariancesFun'
+ , ccpRoots = roots
+ }
+
+getFg ::
+ forall z x u p r o c h n deg .
+ (Dim deg, Dim n, Vectorize x, Vectorize z, Vectorize u, Vectorize p,
+ Vectorize r, Vectorize o, Vectorize c, Vectorize h)
+ => Vec deg Double
+ -> SXFun (J (JV x) :*: J (JV x)) (J (JV c))
+ -> SXFun
+ (J S :*: J (JV x) :*: J (JV x)) (J S)
+ -> ((J (JV p) :*: J (JVec deg (CollPoint (JV x) (JV z) (JV u))) :*: J (JVec deg (JV o)) :*: J S :*: J (JVec deg S)) MX ->
+ (J S) MX)
+ -> ((J S :*: J (JV p) :*: J (JVec deg S) :*: J (JV x) :*: J (JVec deg (JTuple (JV x) (JV z))) :*: J (JVec deg (JV u))) MX -> (J (JVec deg (JV r)) :*: J (JVec deg (JV o)) :*: J (JVec deg (JV h)) :*: J (JV x)) MX)
+ -> J (CollTraj x z u p n deg) MX
+ -> J JNone MX
+ -> (J S MX, J (CollOcpConstraints n deg x r c h) MX)
+getFg taus bcFun mayerFun quadFun stageFun collTraj _ = (obj, cat g)
+ where
+ -- split up the design vars
+ CollTraj tf parm stages' xf = split collTraj
+ stages = unJVec (split stages') :: Vec n (J (CollStage (JV x) (JV z) (JV u) deg) MX)
+ spstages = fmap split stages :: Vec n (CollStage (JV x) (JV z) (JV u) deg MX)
+
+ spstagesPoints :: Vec n (J (JVec deg (CollPoint (JV x) (JV z) (JV u))) MX)
+ spstagesPoints = fmap (\(CollStage _ cps) -> cps) spstages
+
+ obj = objLagrange + objMayer
+
+ objMayer = call mayerFun (tf :*: x0 :*: xf)
+
+ objLagrange :: J S MX
+ objLagrange = F.sum $ TV.tvzipWith3 oneStage spstagesPoints outputs times'
+ oneStage :: J (JVec deg (CollPoint (JV x) (JV z) (JV u))) MX -> J (JVec deg (JV o)) MX -> J (JVec deg S) MX
+ -> J S MX
+ oneStage stagePoints stageOutputs stageTimes =
+ quadFun (parm :*: stagePoints :*: stageOutputs :*: dt :*: stageTimes)
+
+ -- timestep
+ dt = tf / fromIntegral n
+ n = reflectDim (Proxy :: Proxy n)
+
+ -- times at each collocation point
+ times :: Vec n (Vec deg (J S MX))
+ times = fmap snd $ timesFromTaus 0 (fmap realToFrac taus) dt
+
+ times' :: Vec n (J (JVec deg S) MX)
+ times' = fmap (cat . JVec) times
+
+ -- initial point at each stage
+ x0s :: Vec n (J (JV x) MX)
+ x0s = fmap (\(CollStage x0' _) -> x0') spstages
+
+ -- final point at each stage (for matching constraint)
+ xfs :: Vec n (J (JV x) MX)
+ xfs = TV.tvshiftl x0s xf
+
+ x0 = (\(CollStage x0' _) -> x0') (TV.tvhead spstages)
+ g = CollOcpConstraints
+ { coCollPoints = cat $ JVec dcs
+ , coContinuity = cat $ JVec integratorMatchingConstraints
+ , coPathC = cat $ JVec hs
+ , coBc = call bcFun (x0 :*: xf)
+ }
+
+ integratorMatchingConstraints :: Vec n (J (JV x) MX) -- THIS SHOULD BE A NONLINEAR FUNCTION
+ integratorMatchingConstraints = vzipWith (-) interpolatedXs xfs
+
+ dcs :: Vec n (J (JVec deg (JV r)) MX)
+ outputs :: Vec n (J (JVec deg (JV o)) MX)
+ hs :: Vec n (J (JVec deg (JV h)) MX)
+ interpolatedXs :: Vec n (J (JV x) MX)
+ (dcs, outputs, hs, interpolatedXs) = TV.tvunzip4 $ fmap fff $ TV.tvzip spstages times'
+ fff :: (CollStage (JV x) (JV z) (JV u) deg MX, J (JVec deg S) MX) ->
+ (J (JVec deg (JV r)) MX, J (JVec deg (JV o)) MX, J (JVec deg (JV h)) MX, J (JV x) MX)
+ fff (CollStage x0' xzus, stageTimes) = (dc, output, stageHs, interpolatedX')
+ where
+ dc :*: output :*: stageHs :*: interpolatedX' =
+ stageFun (dt :*: parm :*: stageTimes :*: x0' :*: xzs :*: us)
+
+ xzs = cat (JVec xzs') :: J (JVec deg (JTuple (JV x) (JV z))) MX
+ us = cat (JVec us') :: J (JVec deg (JV u)) MX
+ (xzs', us') = TV.tvunzip $ fmap toTuple $ unJVec (split xzus)
+ toTuple xzu = (cat (JTuple x z), u)
+ where
+ CollPoint x z u = split xzu
+
+
+getFgCov ::
+ forall z x u p r c h sx sh shr sc n deg .
+ (Dim deg, Dim n, Vectorize x, Vectorize z, Vectorize u, Vectorize p,
+ Vectorize h, Vectorize c, Vectorize r,
+ Vectorize sx, View sc, View sh, Vectorize shr)
+ -- taus
+ => Vec deg Double
+ -> (J (CollTrajCov sx x z u p n deg) MX -> J (CovTraj sx n) MX)
+ -- gammas
+ -> J (JV shr) MX
+ -- robustify
+ -> (J (JV shr) MX -> J (JV p) MX -> J (JV x) MX -> J (Cov (JV sx)) MX -> J (JV shr) MX)
+ -- sbcFun
+ -> SXFun (J (Cov (JV sx)) :*: J (Cov (JV sx))) (J sc)
+ -- shFun
+ -> SXFun (J (JV x) :*: J (Cov (JV sx))) (J sh)
+ -- lagrangeFun
+ -> SXFun
+ (J S :*: J (JV x) :*: J (Cov (JV sx)) :*: J S) (J S)
+ -- mayerFun
+ -> SXFun
+ (J S :*: J (JV x) :*: J (JV x) :*: J (Cov (JV sx)) :*: J (Cov (JV sx))) (J S)
+ -> (J (CollTraj x z u p n deg) MX -> J JNone MX -> (J S MX, J (CollOcpConstraints n deg x r c h) MX)
+ )
+ -> J (CollTrajCov sx x z u p n deg) MX
+ -> J JNone MX
+ -> (J S MX, J (CollOcpCovConstraints n deg x r c h sh shr sc) MX)
+getFgCov
+ taus computeCovariances
+ gammas robustify sbcFun shFun lagrangeFun mayerFun
+ normalFG collTrajCov nlpParams =
+ (obj0 + objectiveLagrangeCov + objectiveMayerCov, cat g)
+ where
+ CollTrajCov p0 collTraj = split collTrajCov
+ (obj0, g0) = normalFG collTraj nlpParams
+
+ g = CollOcpCovConstraints
+ { cocNormal = g0
+ , cocCovPathC = cat (JVec covPathConstraints)
+ , cocCovRobustPathC = cat (JVec robustifiedPathC)
+ , cocSbc = call sbcFun (p0 :*: pF)
+ }
+ -- split up the design vars
+ CollTraj tf parm stages' xf = split collTraj
+ stages = unJVec (split stages') :: Vec n (J (CollStage (JV x) (JV z) (JV u) deg) MX)
+ spstages = fmap split stages :: Vec n (CollStage (JV x) (JV z) (JV u) deg MX)
+
+ objectiveMayerCov = call mayerFun (tf :*: x0 :*: xf :*: p0 :*: pF)
+
+ -- timestep
+ dt = tf / fromIntegral n
+ n = reflectDim (Proxy :: Proxy n)
+
+ -- times at each collocation point
+ t0s :: Vec n (J S MX)
+ (t0s, _) = TV.tvunzip $ timesFromTaus 0 (fmap realToFrac taus) dt
+
+ -- initial point at each stage
+ x0s :: Vec n (J (JV x) MX)
+ x0s = fmap (\(CollStage x0' _) -> x0') spstages
+
+ x0 = (\(CollStage x0' _) -> x0') (TV.tvhead spstages)
+
+-- sensitivities = call computeSensitivities collTraj
+
+ covs :: Vec n (J (Cov (JV sx)) MX)
+ covs = unJVec (split covs')
+
+ covs' :: J (JVec n (Cov (JV sx))) MX -- all but last covariance
+ pF :: J (Cov (JV sx)) MX -- last covariances
+ CovTraj covs' pF = split (computeCovariances collTrajCov)
+
+ -- lagrange term
+ objectiveLagrangeCov = (lagrangeF + lagrange0s) / fromIntegral n
+ where
+ lagrangeF = call lagrangeFun (tf :*: xf :*: pF :*: tf)
+ lagrange0s =
+ sum $ F.toList $
+ TV.tvzipWith3 (\tk xk pk -> call lagrangeFun (tk :*: xk :*: pk :*: tf)) t0s x0s covs
+
+ covPathConstraints :: Vec n (J sh MX)
+ covPathConstraints = TV.tvzipWith (\xk pk -> call shFun (xk:*:pk)) x0s covs
+
+ robustifiedPathC :: Vec n (J (JV shr) MX)
+ robustifiedPathC = TV.tvzipWith (robustify gammas parm) x0s covs
+
+
+
+
+
+getBg :: forall x z u p r o c h deg n .
+ (Dim n, Dim deg, Vectorize x, Vectorize r, Vectorize c, Vectorize h)
+ => OcpPhase x z u p r o c h
+ -> CollOcpConstraints n deg x r c h (Vector Bounds)
+getBg ocp =
+ CollOcpConstraints
+ { coCollPoints = jreplicate (jfill (Just 0, Just 0)) -- dae residual constraint
+ , coContinuity = jreplicate (jfill (Just 0, Just 0)) -- continuity constraint
+ , coPathC = jreplicate (jreplicate hbnds)
+ , coBc = mkJ $ vectorize $ ocpBcBnds ocp
+ }
+ where
+ hbnds = mkJ $ vectorize $ ocpPathCBnds ocp
+
+evaluateQuadraturesFunction ::
+ forall x z u p o deg .
+ (Dim deg, View x, View z, View u, View o, View p)
+ => SXFun (J x :*: J z :*: J u :*: J p :*: J o :*: J S :*: J S) (J S)
+ -> Vec (TV.Succ deg) (Vec (TV.Succ deg) Double)
+ -> Vec deg Double
+ -> Int
+ -> (J p :*: J (JVec deg (CollPoint x z u)) :*: J (JVec deg o) :*: J S :*: J (JVec deg S)) MX
+ -> J S MX
+evaluateQuadraturesFunction f cijs' taus n (p :*: stage' :*: outputs' :*: dt :*: stageTimes') =
+ dt * qnext
+ where
+ tf = dt * fromIntegral n
+
+ stage :: Vec deg (CollPoint x z u MX)
+ stage = fmap split $ unJVec $ split stage'
+
+ outputs :: Vec deg (J o MX)
+ outputs = unJVec (split outputs')
+
+ stageTimes :: Vec deg (J S MX)
+ stageTimes = unJVec (split stageTimes')
+
+ qnext :: J S MX
+ qnext = interpolate taus 0 qs
+
+ qdots :: Vec deg (J S MX)
+ qdots = TV.tvzipWith3 (\(CollPoint x z u) o t -> call f (x:*:z:*:u:*:p:*:o:*:t:*:tf)) stage outputs stageTimes
+
+ qs = cijInvFr !* qdots
+
+ cijs :: Vec deg (Vec deg Double)
+ cijs = TV.tvtail $ fmap TV.tvtail cijs'
+
+ cijMat :: Mat.Matrix Double
+ cijMat = Mat.fromLists $ F.toList $ fmap F.toList cijs
+
+ cijInv' :: Mat.Matrix Double
+ cijInv' = LA.inv cijMat
+
+ cijInv :: Vec deg (Vec deg Double)
+ cijInv = TV.mkVec' (map TV.mkVec' (Mat.toLists cijInv'))
+
+ cijInvFr :: Vec deg (Vec deg (J S MX))
+ cijInvFr = fmap (fmap realToFrac) cijInv
+
+dot :: forall x deg a b. (Fractional (J x a), Real b) => Vec deg b -> Vec deg (J x a) -> J x a
+dot cks xs = F.sum $ TV.unSeq elemwise
+ where
+ elemwise :: Vec deg (J x a)
+ elemwise = TV.tvzipWith smul cks xs
+
+ smul :: b -> J x a -> J x a
+ smul x y = realToFrac x * y
+
+
+interpolateXDots' :: (Real b, Fractional (J x a)) => Vec deg (Vec deg b) -> Vec deg (J x a) -> Vec deg (J x a)
+interpolateXDots' cjks xs = fmap (`dot` xs) cjks
+
+interpolateXDots ::
+ (Real b, Dim deg, Fractional (J x a)) =>
+ Vec (TV.Succ deg) (Vec (TV.Succ deg) b)
+ -> Vec (TV.Succ deg) (J x a)
+ -> Vec deg (J x a)
+interpolateXDots cjks xs = TV.tvtail $ interpolateXDots' cjks xs
+
+
+-- dynamics residual and outputs
+dynamicsFunction ::
+ forall x z u p r o a . (View x, View z, View u, View r, View o, Viewable a)
+ => (J x a -> J x a -> J z a -> J u a -> J p a -> J S a -> (J r a, J o a))
+ -> (J S :*: J p :*: J x :*: J (CollPoint x z u)) a
+ -> (J r :*: J o) a
+dynamicsFunction dae (t :*: parm :*: x' :*: collPoint) =
+ r :*: o
+ where
+ CollPoint x z u = split collPoint
+ (r,o) = dae x' x z u parm t
+
+-- path constraints
+pathConFunction ::
+ forall x z u p o h a . (View x, View z, View u, View o, View h, Viewable a)
+ => (J x a -> J z a -> J u a -> J p a -> J o a -> J S a -> J h a)
+ -> (J S :*: J p :*: J o :*: J (CollPoint x z u)) a
+ -> J h a
+pathConFunction pathC (t :*: parm :*: o :*: collPoint) =
+ pathC x z u parm o t
+ where
+ CollPoint x z u = split collPoint
+
+-- return dynamics constraints, outputs, and interpolated state
+dynStageConstraints ::
+ forall x z u p r o deg . (Dim deg, View x, View z, View u, View p, View r, View o)
+ => Vec (TV.Succ deg) (Vec (TV.Succ deg) Double) -> Vec deg Double
+ -> SXFun (J S :*: J p :*: J x :*: J (CollPoint x z u))
+ (J r :*: J o)
+ -> (J x :*: J (JVec deg (JTuple x z)) :*: J (JVec deg u) :*: J S :*: J p :*: J (JVec deg S)) MX
+ -> (J (JVec deg r) :*: J x :*: J (JVec deg o)) MX
+dynStageConstraints cijs taus dynFun (x0 :*: xzs' :*: us' :*: UnsafeJ h :*: p :*: stageTimes') =
+ cat (JVec dynConstrs) :*: xnext :*: cat (JVec outputs)
+ where
+ xzs = fmap split (unJVec (split xzs')) :: Vec deg (JTuple x z MX)
+ us = unJVec (split us') :: Vec deg (J u MX)
+
+ -- interpolated final state
+ xnext :: J x MX
+ xnext = interpolate taus x0 xs
+
+ stageTimes = unJVec $ split stageTimes'
+
+ -- dae constraints (dynamics)
+ dynConstrs :: Vec deg (J r MX)
+ outputs :: Vec deg (J o MX)
+ (dynConstrs, outputs) = TV.tvunzip $ TV.tvzipWith4 applyDae xdots xzs us stageTimes
+
+ applyDae :: J x MX -> JTuple x z MX -> J u MX -> J S MX -> (J r MX, J o MX)
+ applyDae x' (JTuple x z) u t = (r, o)
+ where
+ r :*: o = call dynFun (t :*: p :*: x' :*: collPoint)
+ collPoint = cat (CollPoint x z u)
+
+ -- state derivatives, maybe these could be useful as outputs
+ xdots :: Vec deg (J x MX)
+ xdots = fmap (/ UnsafeJ h) $ interpolateXDots cijs (x0 TV.<| xs)
+
+ xs :: Vec deg (J x MX)
+ xs = fmap (\(JTuple x _) -> x) xzs
+
+
+data ErrorIn0 x z u p deg a =
+ ErrorIn0 (J x a) (J (JVec deg (CollPoint x z u)) a) (J S a) (J p a) (J (JVec deg S) a)
+ deriving Generic
+data ErrorInD sx sw sz deg a =
+ ErrorInD (J sx a) (J sw a) (J (JVec deg (JTuple sx sz)) a)
+ deriving Generic
+data ErrorOut sr sx deg a =
+ ErrorOut (J (JVec deg sr) a) (J sx a)
+ deriving Generic
+
+instance (View x, View z, View u, View p, Dim deg) => Scheme (ErrorIn0 x z u p deg)
+instance (View sx, View sw, View sz, Dim deg) => View (ErrorInD sx sw sz deg)
+instance (View sr, View sx, Dim deg) => View (ErrorOut sr sx deg)
+
+
+
+-- outputs
+outputFunction ::
+ forall x z u p r o deg . (Dim deg, View x, View z, View u, View p, View r, View o)
+ => Vec (TV.Succ deg) (Vec (TV.Succ deg) Double) -> Vec deg Double
+ -> SXFun (J S :*: J p :*: J x :*: J (CollPoint x z u))
+ (J r :*: J o)
+ -> (J (CollStage x z u deg) :*: J p :*: J S :*: J S) MX
+ -> (J (JVec deg r) :*: J (JVec deg x) :*: J (JVec deg o)) MX
+outputFunction cijs taus dynFun (collStage :*: p :*: h'@(UnsafeJ h) :*: k) =
+ cat (JVec dynConstrs) :*: cat (JVec xdots) :*: cat (JVec outputs)
+ where
+ xzus = unJVec (split xzus') :: Vec deg (J (CollPoint x z u) MX)
+ CollStage x0 xzus' = split collStage
+ -- times at each collocation point
+ stageTimes :: Vec deg (J S MX)
+ stageTimes = fmap (\tau -> t0 + realToFrac tau * h') taus
+ t0 = k*h'
+
+ -- dae constraints (dynamics)
+ dynConstrs :: Vec deg (J r MX)
+ outputs :: Vec deg (J o MX)
+ (dynConstrs, outputs) = TV.tvunzip $ TV.tvzipWith3 applyDae xdots xzus stageTimes
+
+ applyDae :: J x MX -> J (CollPoint x z u) MX -> J S MX -> (J r MX, J o MX)
+ applyDae x' xzu t = (r, o)
+ where
+ r :*: o = call dynFun (t :*: p :*: x' :*: xzu)
+
+ -- state derivatives, maybe these could be useful as outputs
+ xdots :: Vec deg (J x MX)
+ xdots = fmap (/ UnsafeJ h) $ interpolateXDots cijs (x0 TV.<| xs)
+
+ xs :: Vec deg (J x MX)
+ xs = fmap ((\(CollPoint x _ _) -> x) . split) xzus
+
+
+
+
+-- return dynamics constraints, outputs, and interpolated state
+pathStageConstraints ::
+ forall x z u p o h deg . (Dim deg, View x, View z, View u, View p, View o, View h)
+ => SXFun (J S :*: J p :*: J o :*: J (CollPoint x z u))
+ (J h)
+ -> (J p :*: J (JVec deg S) :*: J (JVec deg o) :*: J (JVec deg (CollPoint x z u))) MX
+ -> J (JVec deg h) MX
+pathStageConstraints pathCFun
+ (p :*: stageTimes' :*: outputs :*: collPoints) =
+ cat (JVec hs)
+ where
+ stageTimes = unJVec $ split stageTimes'
+ cps = fmap split (unJVec (split collPoints)) :: Vec deg (CollPoint x z u MX)
+
+ -- dae constraints (dynamics)
+ hs :: Vec deg (J h MX)
+ hs = TV.tvzipWith3 applyH cps stageTimes (unJVec (split outputs))
+
+ applyH :: CollPoint x z u MX -> J S MX -> J o MX -> J h MX
+ applyH (CollPoint x z u) t o = pathc'
+ where
+ pathc' = call pathCFun (t :*: p :*: o :*: collPoint)
+ collPoint = cat (CollPoint x z u)
+
+
+stageFunction ::
+ forall x z u p o r h deg . (Dim deg, View x, View z, View u, View p, View r, View o, View h)
+ => MXFun (J p :*: J (JVec deg S) :*: J (JVec deg o) :*: J (JVec deg (CollPoint x z u)))
+ (J (JVec deg h))
+ -> ((J x :*: J (JVec deg (JTuple x z)) :*: J (JVec deg u) :*: J S :*: J p :*: J (JVec deg S)) MX
+ -> (J (JVec deg r) :*: J x :*: J (JVec deg o)) MX)
+ -> (J S :*: J p :*: J (JVec deg S) :*: J x :*: J (JVec deg (JTuple x z)) :*: J (JVec deg u)) MX
+ -> (J (JVec deg r) :*: J (JVec deg o) :*: J (JVec deg h) :*: J x) MX
+stageFunction pathConStageFun dynStageCon
+ (dt :*: parm :*: stageTimes :*: x0' :*: xzs' :*: us) =
+ dynConstrs :*: outputs :*: hs :*: interpolatedX
+ where
+ collPoints = cat $ JVec $ TV.tvzipWith catXzu (unJVec (split xzs')) (unJVec (split us))
+
+ catXzu :: J (JTuple x z) MX -> J u MX -> J (CollPoint x z u) MX
+ catXzu xz u = cat $ CollPoint x z u
+ where
+ JTuple x z = split xz
+
+ dynConstrs :: J (JVec deg r) MX
+ outputs :: J (JVec deg o) MX
+ interpolatedX :: J x MX
+ (dynConstrs :*: interpolatedX :*: outputs) =
+ dynStageCon (x0' :*: xzs' :*: us :*: dt :*: parm :*: stageTimes)
+
+ hs :: J (JVec deg h) MX
+ hs = call pathConStageFun (parm :*: stageTimes :*: outputs :*: collPoints)
+
+
+-- | make an initial guess
+makeGuess ::
+ forall x z u p deg n .
+ (Dim n, Dim deg, Vectorize x, Vectorize z, Vectorize u, Vectorize p)
+ => QuadratureRoots
+ -> Double -> (Double -> x Double) -> (Double -> z Double) -> (Double -> u Double)
+ -> p Double
+ -> CollTraj x z u p n deg (Vector Double)
+makeGuess quadratureRoots tf guessX guessZ guessU parm =
+ CollTraj (jfill tf) (v2j parm) guesses (v2j (guessX tf))
+ where
+ -- timestep
+ dt = tf / fromIntegral n
+ n = vlength (Proxy :: Proxy (Vec n))
+
+ -- initial time at each collocation stage
+ t0s :: Vec n Double
+ t0s = TV.mkVec' $ take n [dt * fromIntegral k | k <- [(0::Int)..]]
+
+ -- times at each collocation point
+ times :: Vec n (Double, Vec deg Double)
+ times = fmap (\t0 -> (t0, fmap (\tau -> t0 + tau*dt) taus)) t0s
+
+ mkGuess' :: (Double, Vec deg Double) -> CollStage (JV x) (JV z) (JV u) deg (Vector Double)
+ mkGuess' (t,ts) =
+ CollStage (v2j (guessX t)) $
+ cat $ JVec $ fmap (\t' -> cat (CollPoint (v2j (guessX t')) (v2j (guessZ t')) (v2j (guessU t')))) ts
+
+ guesses :: J (JVec n (CollStage (JV x) (JV z) (JV u) deg)) (Vector Double)
+ guesses = cat $ JVec $ fmap (cat . mkGuess') times
+
+ -- the collocation points
+ taus :: Vec deg Double
+ taus = mkTaus quadratureRoots
+
+
+ v2j :: Vectorize v => v Double -> J (JV v) (Vector Double)
+ v2j = mkJ . vectorize
+
+
+-- | make an initial guess
+makeGuessSim ::
+ forall x z u p deg n .
+ (Dim n, Dim deg, Vectorize x, Vectorize z, Vectorize u, Vectorize p)
+ => QuadratureRoots
+ -> Double
+ -> x Double
+ -> (x Double -> u Double -> x Double)
+ -> (x Double -> Double -> u Double)
+ -> p Double
+ -> CollTraj x z u p n deg (Vector Double)
+makeGuessSim quadratureRoots tf x00 ode guessU p =
+ CollTraj (jfill tf) (v2j p) (cat (JVec stages)) (v2j xf)
+ where
+ -- timestep
+ dt = tf / fromIntegral n
+ n = vlength (Proxy :: Proxy (Vec n))
+
+ -- initial time at each collocation stage
+ t0s :: Vec n Double
+ t0s = TV.mkVec' $ take n [dt * fromIntegral k | k <- [(0::Int)..]]
+
+ xf :: x Double
+ stages :: Vec n (J (CollStage (JV x) (JV z) (JV u) deg) (Vector Double))
+ (xf, stages) = T.mapAccumL stageGuess x00 t0s
+
+ stageGuess :: x Double -> Double
+ -> (x Double, J (CollStage (JV x) (JV z) (JV u) deg) (Vector Double))
+ stageGuess x0 t0 = (integrate 1, cat (CollStage (v2j x0) points))
+ where
+ points = cat $ JVec $ fmap (toCollPoint . integrate) taus
+ u = guessU x0 t0
+ f x = ode x u
+ toCollPoint x = cat $ CollPoint (v2j x) (v2j (fill 0 :: z Double)) (v2j u)
+ integrate localTau = rk4 f (localTau * dt) x0
+
+ -- the collocation points
+ taus :: Vec deg Double
+ taus = mkTaus quadratureRoots
+
+ v2j :: Vectorize v => v Double -> J (JV v) (Vector Double)
+ v2j = mkJ . vectorize
+
+ rk4 :: (x Double -> x Double) -> Double -> x Double -> x Double
+ rk4 f h x0 = x0 ^+^ ((k1 ^+^ (2 *^ k2) ^+^ (2 *^ k3) ^+^ k4) ^/ 6)
+ where
+ k1 = (f x0) ^* h
+ k2 = (f (x0 ^+^ (k1^/2))) ^* h
+ k3 = (f (x0 ^+^ (k2^/2))) ^* h
+ k4 = (f (x0 ^+^ k3)) ^* h
+
+ (^+^) :: x Double -> x Double -> x Double
+ y0 ^+^ y1 = devectorize $ V.zipWith (+) (vectorize y0) (vectorize y1)
+
+ (*^) :: Double -> x Double -> x Double
+ y0 *^ y1 = devectorize $ V.map (y0 *) (vectorize y1)
+
+ (^*) :: x Double -> Double -> x Double
+ y0 ^* y1 = devectorize $ V.map (* y1) (vectorize y0)
+
+ (^/) :: x Double -> Double -> x Double
+ y0 ^/ y1 = devectorize $ V.map (/ y1) (vectorize y0)
diff --git a/src/Dyno/DirectCollocation/Integrate.hs b/src/Dyno/DirectCollocation/Integrate.hs
new file mode 100644
--- /dev/null
+++ b/src/Dyno/DirectCollocation/Integrate.hs
@@ -0,0 +1,315 @@
+{-# OPTIONS_GHC -Wall #-}
+{-# Language ScopedTypeVariables #-}
+{-# Language TypeOperators #-}
+{-# Language DeriveGeneric #-}
+{-# Language FlexibleContexts #-}
+
+module Dyno.DirectCollocation.Integrate
+ ( withIntegrator
+ ) where
+
+import qualified Control.Concurrent as CC
+import Control.Monad ( void, forever )
+import GHC.Generics ( Generic )
+import Data.Proxy ( Proxy(..) )
+import Data.Vector ( Vector )
+import qualified Data.Vector as V
+import qualified Data.Foldable as F
+import Linear.V
+
+import Dyno.SXElement ( SXElement, sxToSXElement )
+import Dyno.View
+import Dyno.Vectorize ( Vectorize(..), vzipWith )
+import Dyno.TypeVecs ( Vec )
+import qualified Dyno.TypeVecs as TV
+import Dyno.LagrangePolynomials ( lagrangeDerivCoeffs )
+import Dyno.NlpSolver ( NlpSolverStuff, runNlpSolver, liftIO, solve
+ , setX0, setLbg, setUbg, setP, setLbx, setUbx, getX )
+import Dyno.DirectCollocation.Types ( CollStage(..), CollPoint(..) )
+import Dyno.DirectCollocation.Quadratures ( QuadratureRoots(..), mkTaus, interpolate, timesFromTaus )
+
+
+
+data IntegratorX x z n deg a =
+ IntegratorX
+ { ixStages :: J (JVec n (CollStage (JV x) (JV z) JNone deg)) a
+ , ixXf :: J (JV x) a
+ } deriving (Generic)
+data IntegratorP u p n deg a =
+ IntegratorP
+ { ipTf :: J S a
+ , ipParm :: J (JV p) a
+ , ipU :: J (JVec n (JVec deg (JV u))) a
+ } deriving (Generic)
+data IntegratorG x r n deg a =
+ IntegratorG
+ { igCollPoints :: J (JVec n (JVec deg (JV r))) a
+ , igContinuity :: J (JVec n (JV x)) a
+ } deriving (Generic)
+
+
+instance (Vectorize x, Vectorize z, Dim n, Dim deg)
+ => View (IntegratorX x z n deg)
+instance (Vectorize u, Vectorize p, Dim n, Dim deg)
+ => View (IntegratorP u p n deg)
+instance (Vectorize x, Vectorize r, Dim n, Dim deg)
+ => View (IntegratorG x r n deg)
+
+
+dot :: forall x deg a b. (Fractional (J x a), Real b) => Vec deg b -> Vec deg (J x a) -> J x a
+dot cks xs = F.sum $ TV.unSeq elemwise
+ where
+ elemwise :: Vec deg (J x a)
+ elemwise = TV.tvzipWith smul cks xs
+
+ smul :: b -> J x a -> J x a
+ smul x y = realToFrac x * y
+
+
+interpolateXDots' :: (Real b, Fractional (J x a)) => Vec deg (Vec deg b) -> Vec deg (J x a) -> Vec deg (J x a)
+interpolateXDots' cjks xs = fmap (`dot` xs) cjks
+
+interpolateXDots ::
+ (Real b, Dim deg, Fractional (J x a)) =>
+ Vec (TV.Succ deg) (Vec (TV.Succ deg) b)
+ -> Vec (TV.Succ deg) (J x a)
+ -> Vec deg (J x a)
+interpolateXDots cjks xs = TV.tvtail $ interpolateXDots' cjks xs
+
+
+-- return dynamics constraints, outputs, and interpolated state
+dynStageConstraints' ::
+ forall x z u p r deg . (Dim deg, View x, View z, View u, View p, View r)
+ => Vec (TV.Succ deg) (Vec (TV.Succ deg) Double) -> Vec deg Double
+ -> SXFun (J S :*: J p :*: J x :*: J (CollPoint x z u)) (J r)
+ -> (J x :*: J (JVec deg (JTuple x z)) :*: J (JVec deg u) :*: J S :*: J p :*: J (JVec deg S)) MX
+ -> (J (JVec deg r) :*: J x) MX
+dynStageConstraints' cijs taus dynFun (x0 :*: xzs' :*: us' :*: UnsafeJ h :*: p :*: stageTimes') =
+ cat (JVec dynConstrs) :*: xnext
+ where
+ xzs = fmap split (unJVec (split xzs')) :: Vec deg (JTuple x z MX)
+ us = unJVec (split us') :: Vec deg (J u MX)
+
+ -- interpolated final state
+ xnext :: J x MX
+ xnext = interpolate taus x0 xs
+
+ stageTimes = unJVec $ split stageTimes'
+
+ -- dae constraints (dynamics)
+ dynConstrs :: Vec deg (J r MX)
+ dynConstrs = TV.tvzipWith4 applyDae xdots xzs us stageTimes
+
+ applyDae :: J x MX -> JTuple x z MX -> J u MX -> J S MX -> J r MX
+ applyDae x' (JTuple x z) u t = r
+ where
+ r = call dynFun (t :*: p :*: x' :*: collPoint)
+ collPoint = cat (CollPoint x z u)
+
+ -- state derivatives, maybe these could be useful as outputs
+ xdots :: Vec deg (J x MX)
+ xdots = fmap (/ UnsafeJ h) $ interpolateXDots cijs (x0 TV.<| xs)
+
+ xs :: Vec deg (J x MX)
+ xs = fmap (\(JTuple x _) -> x) xzs
+
+
+-- dynamics residual and outputs
+dynamicsFunction' ::
+ forall x z u p r a . (View x, View z, View u, View r, Viewable a)
+ => (J x a -> J x a -> J z a -> J u a -> J p a -> J S a -> J r a)
+ -> (J S :*: J p :*: J x :*: J (CollPoint x z u)) a
+ -> J r a
+dynamicsFunction' dae (t :*: parm :*: x' :*: collPoint) = dae x' x z u parm t
+ where
+ CollPoint x z u = split collPoint
+
+type Sxe = SXElement
+
+
+withIntegrator ::
+ forall x z u p r deg n b .
+ (Dim n, Dim deg, Vectorize x, Vectorize p, Vectorize u, Vectorize z, Vectorize r)
+ => Proxy (n, deg)
+ -> x Double
+ -> (x Sxe -> x Sxe -> z Sxe -> u Sxe -> p Sxe -> Sxe -> r Sxe)
+ -> NlpSolverStuff
+ -> ((x Double -> Either (u Double) (Vec n (Vec deg (u Double))) -> p Double -> Double -> IO (x Double)) -> IO b)
+ -> IO b
+withIntegrator _ initialX dae solver userFun = do
+ let -- the collocation points
+ roots :: QuadratureRoots
+ roots = Legendre
+
+ taus :: Vec deg Double
+ taus = mkTaus roots
+
+ n = reflectDim (Proxy :: Proxy n)
+
+ -- coefficients for getting xdot by lagrange interpolating polynomials
+ cijs :: Vec (TV.Succ deg) (Vec (TV.Succ deg) Double)
+ cijs = lagrangeDerivCoeffs (0 TV.<| taus)
+
+ dynFun <- toSXFun "dynamics" $ dynamicsFunction' $
+ \x0 x1 x2 x3 x4 x5 ->
+ let r = dae (sxSplitJV x0) (sxSplitJV x1) (sxSplitJV x2) (sxSplitJV x3)
+ (sxSplitJV x4) (sxToSXElement (unJ x5))
+ in sxCatJV r
+
+ dynStageConFun <- toMXFun "dynamicsStageCon" (dynStageConstraints' cijs taus dynFun)
+-- let callDynStageConFun = call dynStageConFun
+ callDynStageConFun <- fmap call (expandMXFun dynStageConFun)
+
+ let fg :: J (IntegratorX x z n deg) MX
+ -> J (IntegratorP u p n deg) MX
+ -> (J S MX, J (IntegratorG x r n deg) MX)
+ fg = getFgIntegrator taus callDynStageConFun
+
+ scaleX = Nothing
+ scaleG = Nothing
+-- , nlpScaleX' = Just $ cat $ fillCollTraj
+-- (fromMaybe (fill 1) (ocpXScale ocp))
+-- (fromMaybe (fill 1) (ocpZScale ocp))
+-- (fromMaybe (fill 1) (ocpUScale ocp))
+-- (fromMaybe (fill 1) (ocpPScale ocp))
+-- (fromMaybe 1 (ocpTScale ocp))
+--
+-- , nlpScaleG' = Just $ cat $ fillCollConstraints
+-- (fromMaybe (fill 1) (ocpXScale ocp))
+-- (fromMaybe (fill 1) (ocpResidualScale ocp))
+
+ inputMVar <- CC.newEmptyMVar
+ outputMVar <- CC.newEmptyMVar
+
+ let toParams :: Either (u Double) (Vec n (Vec deg (u Double)))
+ -> p Double
+ -> Double
+ -> J (IntegratorP u p n deg) (Vector Double)
+ toParams us p tf =
+ cat $
+ IntegratorP
+ { ipTf = mkJ (V.singleton tf)
+ , ipParm = catJV p
+ , ipU = case us of
+ Left u -> jreplicate (jreplicate (catJV u))
+ Right us' -> cat $ JVec $ fmap (cat . JVec . fmap catJV) us'
+ }
+
+ let toBounds :: x Double -> J (IntegratorX x z n deg) (Vector (Maybe Double))
+ toBounds x0 =
+ cat $
+ IntegratorX
+ { ixStages = cat $ JVec $ TV.mkVec' $ take n $ xs0 : repeat (jfill Nothing)
+ , ixXf = jfill Nothing
+ }
+ where
+ xs0 :: J (CollStage (JV x) (JV z) JNone deg) (Vector (Maybe Double))
+ xs0 = cat $ CollStage (catJV (fmap Just x0)) (jfill Nothing)
+
+ let solverThread = do
+ let initialX' :: J (JV x) (Vector Double)
+ initialX' = catJV initialX
+
+ setX0 $ cat $
+ IntegratorX
+ { ixStages = jreplicate $ cat $
+ CollStage initialX' $ jreplicate $ cat $ CollPoint initialX' (jfill 0) (jfill 0)
+ , ixXf = initialX'
+ }
+ setLbg (jfill (Just 0))
+ setUbg (jfill (Just 0))
+
+ void $ forever $ do
+ (x0, us, p, tf) <- liftIO $ CC.takeMVar inputMVar
+ let bx = toBounds x0
+-- liftIO $ putStrLn "\n\nsolving optimization problem"
+-- liftIO $ printf "lnba, uba: %.3f, %.3f\n" lba uba
+-- liftIO $ print (snd woo)
+
+ setP (toParams us p tf)
+ setLbx bx
+ setUbx bx
+
+ ret <- solve
+ xtopt <- case ret of
+ Left msg -> error $ "failed solving with error: \"" ++ msg ++ "\""
+ Right _ -> getX
+ setX0 xtopt
+
+ liftIO $ CC.putMVar outputMVar (splitJV (ixXf (split xtopt)))
+
+ _ <- CC.forkIO $ runNlpSolver solver fg scaleX scaleG Nothing Nothing solverThread
+
+ let getNextValue :: x Double -> Either (u Double) (Vec n (Vec deg (u Double))) -> p Double -> Double -> IO (x Double)
+ getNextValue x us p tf = do
+ CC.putMVar inputMVar (x, us, p, tf)
+ CC.takeMVar outputMVar
+ userFun getNextValue
+
+
+getFgIntegrator ::
+ forall x z u p r n deg .
+ (Dim deg, Dim n, Vectorize x, Vectorize z, Vectorize u, Vectorize p, Vectorize r)
+ => Vec deg Double
+ -> ((J (JV x) :*: J (JVec deg (JTuple (JV x) (JV z))) :*: J (JVec deg (JV u)) :*: J S :*: J (JV p) :*: J (JVec deg S)) MX -> (J (JVec deg (JV r)) :*: J (JV x)) MX)
+ -> J (IntegratorX x z n deg) MX
+ -> J (IntegratorP u p n deg) MX
+ -> (J S MX, J (IntegratorG x r n deg) MX)
+getFgIntegrator taus stageFun ix' ip' = (0, cat g)
+ where
+ ix = split ix'
+ ip = split ip'
+
+ xf = ixXf ix
+ tf = ipTf ip
+ parm = ipParm ip
+ stages = unJVec (split (ixStages ix)) :: Vec n (J (CollStage (JV x) (JV z) JNone deg) MX)
+
+ spstages :: Vec n (CollStage (JV x) (JV z) JNone deg MX)
+ spstages = fmap split stages
+
+ us :: Vec n (J (JVec deg (JV u)) MX)
+ us = unJVec $ split $ ipU ip
+
+ -- timestep
+ dt = tf / fromIntegral n
+ n = reflectDim (Proxy :: Proxy n)
+
+ -- times at each collocation point
+ times :: Vec n (Vec deg (J S MX))
+ times = fmap snd $ timesFromTaus 0 (fmap realToFrac taus) dt
+
+ times' :: Vec n (J (JVec deg S) MX)
+ times' = fmap (cat . JVec) times
+
+ -- initial point at each stage
+ x0s :: Vec n (J (JV x) MX)
+ x0s = fmap (\(CollStage x0' _) -> x0') spstages
+
+ -- final point at each stage (for matching constraint)
+ xfs :: Vec n (J (JV x) MX)
+ xfs = TV.tvshiftl x0s xf
+
+ g = IntegratorG
+ { igCollPoints = cat $ JVec dcs
+ , igContinuity = cat $ JVec integratorMatchingConstraints
+ }
+ integratorMatchingConstraints :: Vec n (J (JV x) MX) -- THIS SHOULD BE A NONLINEAR FUNCTION
+ integratorMatchingConstraints = vzipWith (-) interpolatedXs xfs
+
+ dcs :: Vec n (J (JVec deg (JV r)) MX)
+ interpolatedXs :: Vec n (J (JV x) MX)
+ (dcs, interpolatedXs) = TV.tvunzip $ TV.tvzipWith3 fff spstages us times'
+ fff :: CollStage (JV x) (JV z) JNone deg MX
+ -> J (JVec deg (JV u)) MX
+ -> J (JVec deg S) MX
+ -> (J (JVec deg (JV r)) MX, J (JV x) MX)
+ fff (CollStage x0' xzs') us' stageTimes = (dc, interpolatedX')
+ where
+ dc :*: interpolatedX' = stageFun (x0' :*: xzs :*: us' :*: dt :*: parm :*: stageTimes)
+
+ xzs :: J (JVec deg (JTuple (JV x) (JV z))) MX
+ xzs = cat $ JVec $ fmap toTuple $ unJVec $ split xzs'
+ toTuple xzu = cat (JTuple x z)
+ where
+ CollPoint x z _ = split xzu
diff --git a/src/Dyno/DirectCollocation/Profile.hs b/src/Dyno/DirectCollocation/Profile.hs
new file mode 100644
--- /dev/null
+++ b/src/Dyno/DirectCollocation/Profile.hs
@@ -0,0 +1,63 @@
+{-# OPTIONS_GHC -Wall #-}
+{-# Language ScopedTypeVariables #-}
+{-# Language RankNTypes #-}
+
+module Dyno.DirectCollocation.Profile
+ ( ProfileReport(..)
+ , profile
+ ) where
+
+import Data.Vector ( Vector )
+import Linear.V ( Dim(..) )
+
+import Dyno.View.View ( J )
+import Dyno.Vectorize ( Vectorize, Proxy(..) )
+import Dyno.Ocp ( OcpPhase )
+import Dyno.Solvers ( NlpSolverStuff )
+import Dyno.DirectCollocation.Types ( CollTraj, CollOcpConstraints )
+import Dyno.DirectCollocation.Formulate ( CollProblem(..), makeCollProblem )
+import qualified Dyno.TypeVecs as TV
+import Dyno.NlpSolver ( solveNlp' )
+import Dyno.Nlp ( Nlp'(..), NlpOut'(..) )
+
+data ProfileReport =
+ ProfileReport
+ {
+ }
+
+toProfileReport ::
+ Either String String
+ -> NlpOut' (CollTraj x z u p n deg) (CollOcpConstraints n deg x r c h) (Vector Double)
+ -> IO ProfileReport
+toProfileReport _ _ = return ProfileReport
+
+profile :: forall x z u p r o c h .
+ (Vectorize x, Vectorize z, Vectorize u, Vectorize p,
+ Vectorize r, Vectorize o, Vectorize c, Vectorize h)
+ => OcpPhase x z u p r o c h
+ -> (forall deg n . (Dim deg, Dim n) => J (CollTraj x z u p n deg) (Vector Double))
+ -> NlpSolverStuff
+ -> [(Int,Int)]
+ -> IO [ProfileReport]
+profile ocp guess solver range = do
+ let go :: (Int,Int) -> IO ProfileReport
+ go (n,deg) =
+ TV.reifyDim n $ \(Proxy :: Proxy n ) ->
+ TV.reifyDim deg $ \(Proxy :: Proxy deg) ->
+ profileOne ocp (guess :: J (CollTraj x z u p n deg) (Vector Double)) solver
+ mapM go range
+
+profileOne ::
+ forall x z u p r o c h n deg .
+ (Vectorize x, Vectorize z, Vectorize u, Vectorize p,
+ Vectorize r, Vectorize o, Vectorize c, Vectorize h,
+ Dim n, Dim deg)
+ => OcpPhase x z u p r o c h
+ -> J (CollTraj x z u p n deg) (Vector Double)
+ -> NlpSolverStuff
+ -> IO ProfileReport
+profileOne ocp guess solver = do
+ cp <- makeCollProblem ocp
+ let nlp = cpNlp cp
+ x <- solveNlp' solver (nlp { nlpX0' = guess }) Nothing
+ uncurry toProfileReport x
diff --git a/src/Dyno/DirectCollocation/Quadratures.hs b/src/Dyno/DirectCollocation/Quadratures.hs
new file mode 100644
--- /dev/null
+++ b/src/Dyno/DirectCollocation/Quadratures.hs
@@ -0,0 +1,81 @@
+{-# OPTIONS_GHC -Wall #-}
+{-# Language ScopedTypeVariables #-}
+{-# Language FlexibleContexts #-}
+{-# Language DeriveGeneric #-}
+
+module Dyno.DirectCollocation.Quadratures
+ ( QuadratureRoots(..)
+ , mkTaus
+ , interpolate
+ , timesFromTaus
+ , collocationTimes
+ ) where
+
+import GHC.Generics ( Generic )
+import Data.Proxy ( Proxy(..) )
+import qualified Data.Vector as V
+import qualified Data.Foldable as F
+import Data.Serialize ( Serialize(..) )
+import Linear.V
+
+import JacobiRoots ( shiftedLegendreRoots ) --, shiftedRadauRoots )
+
+import Dyno.View
+import Dyno.TypeVecs ( Vec )
+import qualified Dyno.TypeVecs as TV
+import Dyno.LagrangePolynomials ( lagrangeXis )
+
+data QuadratureRoots = Legendre | Radau deriving (Show, Eq, Ord, Enum, Generic)
+instance Serialize QuadratureRoots
+
+mkTaus ::
+ forall deg a
+ . (Dim deg, Fractional a)
+ => QuadratureRoots -> Vec deg a
+mkTaus quadratureRoots = case taus of
+ Just taus' -> TV.mkVec $ V.map (fromRational . toRational) taus'
+ Nothing -> error "makeTaus: too high degree"
+ where
+ deg = reflectDim (Proxy :: Proxy deg)
+ taus = case quadratureRoots of
+ Legendre -> shiftedLegendreRoots deg
+ Radau -> error "radau not yet supported" -- shiftedRadauRoots (deg-1) ++ [1.0]
+
+
+dot :: forall x deg a b. (Fractional (J x a), Real b) => Vec deg b -> Vec deg (J x a) -> J x a
+dot cks xs = F.sum $ TV.unSeq elemwise
+ where
+ elemwise :: Vec deg (J x a)
+ elemwise = TV.tvzipWith smul cks xs
+
+ smul :: b -> J x a -> J x a
+ smul x y = realToFrac x * y
+
+
+interpolate :: (Dim deg, Real b, Fractional b, Fractional (J x a), View x) =>
+ Vec deg b -> J x a -> Vec deg (J x a) -> J x a
+interpolate taus x0 xs = dot (TV.mkVec' xis) (x0 TV.<| xs)
+ where
+ xis = map (lagrangeXis (0 : F.toList taus) 1) [0..deg]
+ deg = TV.tvlength taus
+
+
+timesFromTaus ::
+ forall n deg a
+ . (Num a, Dim n, Dim deg)
+ => a -> Vec deg a -> a -> Vec n (a, Vec deg a)
+timesFromTaus t0 taus dt = times
+ where
+ n = reflectDim (Proxy :: Proxy n)
+
+ -- initial time at each collocation stage
+ t0s :: Vec n a
+ t0s = TV.mkVec' $ take n [t0 + (dt * fromIntegral k) | k <- [(0::Int)..]]
+
+ -- times at each collocation point
+ times :: Vec n (a, Vec deg a)
+ times = fmap (\t0' -> (t0', fmap (\tau -> t0' + tau * dt) taus)) t0s
+
+collocationTimes ::
+ (Dim n, Dim deg, Fractional a) => a -> QuadratureRoots -> a -> Vec n (a, Vec deg a)
+collocationTimes t0 qr dt = timesFromTaus t0 (mkTaus qr) dt
diff --git a/src/Dyno/DirectCollocation/Reify.hs b/src/Dyno/DirectCollocation/Reify.hs
new file mode 100644
--- /dev/null
+++ b/src/Dyno/DirectCollocation/Reify.hs
@@ -0,0 +1,104 @@
+{-# OPTIONS_GHC -Wall #-}
+{-# Language RankNTypes #-}
+{-# Language ScopedTypeVariables #-}
+
+module Dyno.DirectCollocation.Reify
+ ( reifyCollTraj
+ , reifyCollTrajCov
+ ) where
+
+import Linear.V ( Dim )
+
+import Dyno.Vectorize
+import Dyno.TypeVecs ( Vec )
+import Dyno.View.JV
+import Dyno.View.View
+import Dyno.View.Viewable
+import qualified Dyno.TypeVecs as TV
+import Dyno.DirectCollocation.Types
+
+-- TODO: re-enable the check on output dimension
+reifyCollTraj
+ :: forall a r x' z' u' p' o' .
+ Viewable a
+ => (Int,Int,Int,Int,Int,Int,Int)
+ -> J (CollTraj x' z' u' p' () ()) a
+ -> Vec () (Vec () (J o' a, J x' a))
+ -> (forall x z u p o n deg .
+ (Vectorize x, Vectorize z, Vectorize u, Vectorize p, Vectorize o, Dim n, Dim deg)
+ => J (CollTraj x z u p n deg) a -> Vec n (Vec deg (J (JV o) a, J (JV x) a)) -> r)
+ -> r
+reifyCollTraj (nx,nz,nu,np,no,n,deg) (UnsafeJ x) outputs f
+ | ntotal /= ntotal' =
+ error $ "reifyCollTraj stages dimension mismatch, " ++
+ "expected: " ++ show ntotal ++
+ "actual : " ++ show ntotal'
+-- | nOutsTotal /= nOutsTotal' =
+-- error $ "reifyCollTraj outputs dimension mismatch, " ++
+-- "expected: " ++ show nOutsTotal ++
+-- "actual : " ++ show nOutsTotal'
+ | otherwise =
+ TV.reifyDim nx $ \(Proxy :: Proxy nx) ->
+ TV.reifyDim nz $ \(Proxy :: Proxy nz) ->
+ TV.reifyDim nu $ \(Proxy :: Proxy nu) ->
+ TV.reifyDim np $ \(Proxy :: Proxy np) ->
+ TV.reifyDim no $ \(Proxy :: Proxy no) ->
+ TV.reifyDim n $ \(Proxy :: Proxy n) ->
+ TV.reifyDim deg $ \(Proxy :: Proxy deg) ->
+ f
+ (mkJ x :: J (CollTraj (Vec nx) (Vec nz) (Vec nu) (Vec np) n deg) a)
+ (unsafeCastDim (fmap (unsafeCastDim . fmap (\(o,x') -> (unsafeToVec o, unsafeToVec x'))) outputs)
+ :: Vec n (Vec deg (J (JV (Vec no)) a, J (JV (Vec nx)) a)))
+ where
+ ntotal = 1 + np + n*(nx + deg*(nx + nz + nu)) + nx
+ ntotal' = vsize1 x
+
+-- nOutsTotal = n*deg*no :: Int
+-- nOutsTotal' = :: Int --vsize1 outs
+
+-- TODO: re-enable the check on output dimension
+reifyCollTrajCov
+ :: forall a r x' z' u' p' o' sx' .
+ Viewable a
+ => (Int,Int,Int,Int,Int,Int,Int,Int)
+ -> J (CollTrajCov sx' x' z' u' p' () ()) a
+ -> Vec () (Vec () (J o' a, J x' a))
+ -> (forall x z u p o sx n deg .
+ (Vectorize x, Vectorize z, Vectorize u, Vectorize p, Vectorize o, Vectorize sx, Dim n, Dim deg)
+ => J (CollTrajCov sx x z u p n deg) a -> Vec n (Vec deg (J (JV o) a, J (JV x) a)) -> r)
+ -> r
+reifyCollTrajCov (nsx,nx,nz,nu,np,no,n,deg) (UnsafeJ x) outputs f
+ | ntotal /= ntotal' =
+ error $ "reifyCollTraj stages dimension mismatch, " ++
+ "expected: " ++ show ntotal ++
+ "actual : " ++ show ntotal'
+-- | nOutsTotal /= nOutsTotal' =
+-- error $ "reifyCollTraj outputs dimension mismatch, " ++
+-- "expected: " ++ show nOutsTotal ++
+-- "actual : " ++ show nOutsTotal'
+ | otherwise =
+ TV.reifyDim nx $ \(Proxy :: Proxy nx) ->
+ TV.reifyDim nz $ \(Proxy :: Proxy nz) ->
+ TV.reifyDim nu $ \(Proxy :: Proxy nu) ->
+ TV.reifyDim np $ \(Proxy :: Proxy np) ->
+ TV.reifyDim no $ \(Proxy :: Proxy no) ->
+ TV.reifyDim nsx $ \(Proxy :: Proxy nsx) ->
+ TV.reifyDim n $ \(Proxy :: Proxy n) ->
+ TV.reifyDim deg $ \(Proxy :: Proxy deg) ->
+ f
+ (mkJ x :: J (CollTrajCov (Vec nsx) (Vec nx) (Vec nz) (Vec nu) (Vec np) n deg) a)
+ (unsafeCastDim (fmap (unsafeCastDim . fmap (\(o,x') -> (unsafeToVec o, unsafeToVec x'))) outputs)
+ :: Vec n (Vec deg (J (JV (Vec no)) a, J (JV (Vec nx)) a)))
+ where
+ ncov = (nsx*nsx + nsx) `div` 2
+ ntotal = 1 + ncov + np + n*(nx + deg*(nx + nz + nu)) + nx
+ ntotal' = vsize1 x
+
+-- nOutsTotal = n*deg*no :: Int
+-- nOutsTotal' = :: Int --vsize1 outs
+
+unsafeToVec :: (Viewable a, Dim no) => J dummy a -> J (JV (Vec no)) a
+unsafeToVec (UnsafeJ x) = mkJ x
+
+unsafeCastDim :: Dim no => Vec () a -> Vec no a
+unsafeCastDim = TV.mkSeq . TV.unSeq
diff --git a/src/Dyno/DirectCollocation/Robust.hs b/src/Dyno/DirectCollocation/Robust.hs
new file mode 100644
--- /dev/null
+++ b/src/Dyno/DirectCollocation/Robust.hs
@@ -0,0 +1,474 @@
+{-# OPTIONS_GHC -Wall #-}
+{-# Language ScopedTypeVariables #-}
+{-# Language TypeOperators #-}
+{-# Language DeriveGeneric #-}
+{-# Language FlexibleContexts #-}
+
+module Dyno.DirectCollocation.Robust
+ ( CovarianceSensitivities(..)
+ , CovTraj(..)
+ , mkComputeSensitivities
+ , mkComputeCovariances
+ , mkRobustifyFunction
+ , continuousToDiscreetNoiseApprox
+ ) where
+
+import GHC.Generics ( Generic, Generic1 )
+import Data.Proxy ( Proxy(..) )
+import qualified Data.Foldable as F
+import qualified Data.Traversable as T
+import Linear.V
+
+import Casadi.MX ( d2m )
+
+import Dyno.SXElement ( SXElement, sxToSXElement )
+import Dyno.View.CasadiMat as CM
+import Dyno.Cov
+import Dyno.View.View
+import Dyno.View.JV ( JV(..), sxSplitJV, sxCatJV )
+import Dyno.View.HList ( (:*:)(..) )
+import Dyno.View.Fun
+import Dyno.View.Viewable ( Viewable )
+import qualified Dyno.View.M as M
+import Dyno.View.M ( M )
+import Dyno.View.FunJac
+import Dyno.View.Scheme ( Scheme, blockSplit )
+import Dyno.Vectorize ( Vectorize(..), Id, vzipWith4 )
+import Dyno.TypeVecs ( Vec )
+import qualified Dyno.TypeVecs as TV
+import Dyno.LagrangePolynomials ( lagrangeDerivCoeffs )
+
+import Dyno.DirectCollocation.Types
+import Dyno.DirectCollocation.Quadratures ( QuadratureRoots(..), mkTaus, interpolate )
+
+data CovTraj sx n a =
+ CovTraj
+ { ctAllButLast :: J (JVec n (Cov (JV sx))) a
+ , ctLast :: J (Cov (JV sx)) a
+ } deriving (Eq, Show, Generic, Generic1)
+instance (Vectorize sx, Dim n) => View (CovTraj sx n)
+
+
+data CovarianceSensitivities xe we n a =
+ CovarianceSensitivities
+ { csFs :: M (JVec n xe) xe a
+ , csWs :: M (JVec n xe) we a
+ } deriving (Eq, Show, Generic, Generic1)
+instance (View xe, View we, Dim n) => Scheme (CovarianceSensitivities xe we n)
+
+type Sxe = SXElement
+
+mkComputeSensitivities ::
+ forall x z u p sx sz sw sr deg n .
+ (Dim deg, Dim n, Vectorize x, Vectorize p, Vectorize u, Vectorize z,
+ Vectorize sr, Vectorize sw, Vectorize sz, Vectorize sx)
+ => QuadratureRoots
+ -> (x Sxe -> x Sxe -> z Sxe -> u Sxe -> p Sxe -> Sxe
+ -> sx Sxe -> sx Sxe -> sz Sxe -> sw Sxe
+ -> sr Sxe)
+ -> IO (J (CollTraj x z u p n deg) MX -> CovarianceSensitivities (JV sx) (JV sw) n MX)
+mkComputeSensitivities roots covDae = do
+ let -- the collocation points
+ taus :: Vec deg Double
+ taus = mkTaus roots
+
+ -- coefficients for getting xdot by lagrange interpolating polynomials
+ cijs :: Vec (TV.Succ deg) (Vec (TV.Succ deg) Double)
+ cijs = lagrangeDerivCoeffs (0 TV.<| taus)
+
+ errorDynFun <- toSXFun "error dynamics" $ errorDynamicsFunction $
+ \x0 x1 x2 x3 x4 x5 x6 x7 x8 x9 ->
+ let r = covDae
+ (sxSplitJV x0) (sxSplitJV x1) (sxSplitJV x2) (sxSplitJV x3) (sxSplitJV x4)
+ (sxToSXElement (unJ x5)) (sxSplitJV x6) (sxSplitJV x7) (sxSplitJV x8) (sxSplitJV x9)
+ in sxCatJV r
+
+ edscf <- toMXFun "errorDynamicsStageCon" (errorDynStageConstraints cijs taus errorDynFun)
+ errorDynStageConFunJac <- toFunJac edscf
+
+ sensitivityStageFun' <- toMXFun "sensitivity stage function" $
+ sensitivityStageFunction (call errorDynStageConFunJac)
+ sensitivityStageFun <- expandMXFun sensitivityStageFun'
+ let sens :: J S MX
+ -> J (JV p) MX
+ -> J (JVec deg S) MX
+ -> J (JV x) MX
+ -> J (JVec deg (CollPoint (JV x) (JV z) (JV u))) MX
+ -> (M (JV sx) (JV sx) MX, M (JV sx) (JV sw) MX)
+ sens dt p stagetimes x0 xzus = (y0,y1)
+ where
+ y0 :*: y1 = call sensitivityStageFun (dt :*: p :*: stagetimes :*: x0 :*: xzus)
+
+ let computeAllSensitivities :: J (CollTraj x z u p n deg) MX
+ -> CovarianceSensitivities (JV sx) (JV sw) n MX
+ computeAllSensitivities collTraj = CovarianceSensitivities (M.vcat' fs) (M.vcat' ws)
+ where
+ -- split up the design vars
+ CollTraj tf parm stages' _ = split collTraj
+ stages = unJVec (split stages') :: Vec n (J (CollStage (JV x) (JV z) (JV u) deg) MX)
+ spstages = fmap split stages :: Vec n (CollStage (JV x) (JV z) (JV u) deg MX)
+
+ -- timestep
+ dt = tf / fromIntegral n
+ n = reflectDim (Proxy :: Proxy n)
+
+ -- initial time at each collocation stage
+ t0s :: Vec n (J S MX)
+ t0s = TV.mkVec' $ take n [dt * fromIntegral k | k <- [(0::Int)..]]
+
+ -- times at each collocation point
+ times :: Vec n (Vec deg (J S MX))
+ times = fmap (\t0 -> fmap (\tau -> t0 + realToFrac tau * dt) taus) t0s
+
+ times' :: Vec n (J (JVec deg S) MX)
+ times' = fmap (cat . JVec) times
+
+ fs :: Vec n (M (JV sx) (JV sx) MX)
+ ws :: Vec n (M (JV sx) (JV sw) MX)
+ (fs, ws) = TV.tvunzip $ TV.tvzipWith mkFw times' spstages
+ mkFw stagetimes (CollStage x0' xzus') = sens dt parm stagetimes x0' xzus'
+
+ return computeAllSensitivities
+-- toMXFun "compute all sensitivities" computeAllSensitivities
+
+
+-- todo: calculate by first multiplying all the Fs
+mkComputeCovariances ::
+ forall z x u p sx sw n deg .
+ (Dim deg, Dim n, Vectorize x, Vectorize z, Vectorize u, Vectorize p,
+ Vectorize sx, Vectorize sw)
+ => (M (JV sx) (JV sx) MX -> M (JV sx) (JV sw) MX -> J (Cov (JV sw)) MX -> J S MX
+ -> M (JV sx) (JV sx) MX)
+ -> (J (CollTraj x z u p n deg) MX -> CovarianceSensitivities (JV sx) (JV sw) n MX)
+ -> J (Cov (JV sw)) DMatrix
+ -> IO (J (CollTrajCov sx x z u p n deg) MX -> J (CovTraj sx n) MX)
+mkComputeCovariances c2d computeSens qc' = do
+ propOneCovFun <- toMXFun "propogate one covariance" (propOneCov c2d)
+
+ let computeCovs :: J (CollTrajCov sx x z u p n deg) MX -> J (CovTraj sx n) MX
+ computeCovs collTrajCov = cat covTraj
+ where
+ CollTrajCov p0 collTraj = split collTrajCov
+
+ sensitivities = computeSens collTraj
+
+ covTraj =
+ CovTraj
+ { ctAllButLast = cat (JVec covs)
+ , ctLast = pF
+ }
+
+ covs :: Vec n (J (Cov (JV sx)) MX) -- all but last covariances
+ pF :: J (Cov (JV sx)) MX -- last covariances
+ (pF, covs) = T.mapAccumL ffs p0 $
+ TV.tvzip (M.vsplit' (csFs sensitivities)) (M.vsplit' (csWs sensitivities))
+
+ qc = mkJ (d2m (unJ qc'))
+
+ ffs :: J (Cov (JV sx)) MX
+ -> (M (JV sx) (JV sx) MX, M (JV sx) (JV sw) MX)
+ -> (J (Cov (JV sx)) MX, J (Cov (JV sx)) MX)
+ ffs p0' (f, g) = (p1', p0')
+ where
+ p1' = call propOneCovFun (f :*: g :*: p0' :*: qc :*: dt)
+
+ -- split up the design vars
+ CollTraj tf _ _ _ = split collTraj
+
+ -- timestep
+ dt = tf / fromIntegral n
+ n = reflectDim (Proxy :: Proxy n)
+
+ return computeCovs
+-- toMXFun "compute all covariances" computeCovs
+
+dot :: forall x deg a b. (Fractional (J x a), Real b) => Vec deg b -> Vec deg (J x a) -> J x a
+dot cks xs = F.sum $ TV.unSeq elemwise
+ where
+ elemwise :: Vec deg (J x a)
+ elemwise = TV.tvzipWith smul cks xs
+
+ smul :: b -> J x a -> J x a
+ smul x y = realToFrac x * y
+
+
+interpolateXDots' :: (Real b, Fractional (J x a)) => Vec deg (Vec deg b) -> Vec deg (J x a) -> Vec deg (J x a)
+interpolateXDots' cjks xs = fmap (`dot` xs) cjks
+
+interpolateXDots ::
+ (Real b, Dim deg, Fractional (J x a)) =>
+ Vec (TV.Succ deg) (Vec (TV.Succ deg) b)
+ -> Vec (TV.Succ deg) (J x a)
+ -> Vec deg (J x a)
+interpolateXDots cjks xs = TV.tvtail $ interpolateXDots' cjks xs
+
+
+-- dynamics residual and outputs
+errorDynamicsFunction ::
+ forall x z u p r sx sz sw a .
+ (View x, View z, View u, View r, View sx, View sz, View sw, Viewable a)
+ => (J x a -> J x a -> J z a -> J u a -> J p a -> J S a
+ -> J sx a -> J sx a -> J sz a -> J sw a -> J r a)
+ -> (J S :*: J p :*: J x :*: J (CollPoint x z u) :*: J sx :*: J sx :*: J sz :*: J sw) a
+ -> J r a
+errorDynamicsFunction dae (t :*: parm :*: x' :*: collPoint :*: sx' :*: sx :*: sz :*: sw) =
+ r
+ where
+ CollPoint x z u = split collPoint
+ r = dae x' x z u parm t sx' sx sz sw
+
+
+data ErrorIn0 x z u p deg a =
+ ErrorIn0 (J x a) (J (JVec deg (CollPoint x z u)) a) (J S a) (J p a) (J (JVec deg S) a)
+ deriving Generic
+data ErrorInD sx sw sz deg a =
+ ErrorInD (J sx a) (J sw a) (J (JVec deg (JTuple sx sz)) a)
+ deriving Generic
+data ErrorOut sr sx deg a =
+ ErrorOut (J (JVec deg sr) a) (J sx a)
+ deriving Generic
+
+instance (View x, View z, View u, View p, Dim deg) => Scheme (ErrorIn0 x z u p deg)
+instance (View sx, View sw, View sz, Dim deg) => View (ErrorInD sx sw sz deg)
+instance (View sr, View sx, Dim deg) => View (ErrorOut sr sx deg)
+
+-- return error dynamics constraints and interpolated state
+errorDynStageConstraints ::
+ forall x z u p sx sz sw sr deg .
+ (Dim deg, View x, View z, View u, View p,
+ View sr, View sw, View sz, View sx)
+ => Vec (TV.Succ deg) (Vec (TV.Succ deg) Double)
+ -> Vec deg Double
+ -> SXFun (J S :*: J p :*: J x :*: J (CollPoint x z u) :*: J sx :*: J sx :*: J sz :*: J sw)
+ (J sr)
+ -> JacIn (ErrorInD sx sw sz deg) (ErrorIn0 x z u p deg) MX
+ -> JacOut (ErrorOut sr sx deg) (J JNone) MX
+errorDynStageConstraints cijs taus dynFun
+ (JacIn errorInD (ErrorIn0 x0 xzus' (UnsafeJ h) p stageTimes'))
+ = JacOut (cat (ErrorOut (cat (JVec dynConstrs)) sxnext)) (cat JNone)
+ where
+ ErrorInD sx0 sw0 sxzs' = split errorInD
+
+ xzus = unJVec (split xzus')
+
+ xs :: Vec deg (J x MX)
+ xs = fmap ((\(CollPoint x _ _) -> x) . split) xzus
+
+ xdots :: Vec deg (J x MX)
+ xdots = fmap (/ UnsafeJ h) $ interpolateXDots cijs (x0 TV.<| xs)
+
+-- -- interpolated final state
+-- xnext :: J x MX
+-- xnext = interpolate taus x0 xs
+
+ -- interpolated final state
+ sxnext :: J sx MX
+ sxnext = interpolate taus sx0 sxs
+
+ stageTimes = unJVec $ split stageTimes'
+
+ -- dae constraints (dynamics)
+ dynConstrs :: Vec deg (J sr MX)
+ dynConstrs = TV.tvzipWith6 applyDae sxdots sxs szs xdots xzus stageTimes
+
+ applyDae
+ :: J sx MX -> J sx MX -> J sz MX
+ -> J x MX -> J (CollPoint x z u) MX -> J S MX
+ -> J sr MX
+ applyDae sx' sx sz x' xzu t =
+ call dynFun
+ (t :*: p :*: x' :*: xzu :*: sx' :*: sx :*: sz :*: sw0)
+
+ -- error state derivatives
+ sxdots :: Vec deg (J sx MX)
+ sxdots = fmap (/ UnsafeJ h) $ interpolateXDots cijs (sx0 TV.<| sxs)
+
+ sxs :: Vec deg (J sx MX)
+ szs :: Vec deg (J sz MX)
+ (sxs, szs) = TV.tvunzip
+ $ fmap ((\(JTuple sx sz) -> (sx,sz)) . split)
+ $ unJVec $ split sxzs'
+
+
+continuousToDiscreetNoiseApprox :: (View sx, View sw)
+ => M sx sx MX -> M sx sw MX -> J (Cov sw) MX -> J S MX -> M sx sx MX
+continuousToDiscreetNoiseApprox _dsx1_dsx0 dsx1_dsw0 qs h = qd
+ where
+ -- Qs' = G * Qs * G.T
+ qs' = dsx1_dsw0 `M.mm` (toMat qs) `M.mm` M.trans dsx1_dsw0
+
+ qd = qs' `M.ms` (1/h)
+-- + (dsx1_dsx0 `M.mm` qs' + qs' `M.mm` (M.trans dsx1_dsx0)) `M.ms` (h*h/2)
+-- + (dsx1_dsx0 `M.mm` qs' `M.mm` (M.trans dsx1_dsx0)) `M.ms` (h*h*h/3)
+
+
+propOneCov ::
+ forall sx sw
+ . (View sx, View sw)
+ => (M sx sx MX -> M sx sw MX -> J (Cov sw) MX -> J S MX -> M sx sx MX)
+ -> (M sx sx :*: M sx sw :*: J (Cov sx) :*: J (Cov sw) :*: J S) MX
+ -> J (Cov sx) MX
+propOneCov c2d (dsx1_dsx0 :*: dsx1_dsw0 :*: p0 :*: qs :*: h) = fromMat p1
+ where
+ qd = c2d dsx1_dsx0 dsx1_dsw0 qs h
+
+ p1 :: M sx sx MX
+ p1 = dsx1_dsx0 `M.mm` (toMat p0) `M.mm` M.trans dsx1_dsx0 + qd
+
+
+sensitivityStageFunction ::
+ forall x z u p sx sz sw deg sr
+ . (Dim deg, View x, View z, View u, View p, View sx, View sz, View sw, View sr)
+ => (JacIn (ErrorInD sx sw sz deg) (ErrorIn0 x z u p deg) MX
+ -> Jac (ErrorInD sx sw sz deg) (ErrorOut sr sx deg) (J JNone) MX)
+ -> (J S :*: J p :*: J (JVec deg S) :*: J x :*: J (JVec deg (CollPoint x z u))) MX
+ -> (M sx sx :*: M sx sw) MX
+sensitivityStageFunction dynStageConJac
+ (dt :*: parm :*: stageTimes :*: x0' :*: xzus') = dsx1_dsx0 :*: dsx1_dsw0
+ where
+ sx0 :: J sx MX
+ sx0 = M.uncol M.zeros
+ sw0 :: J sw MX
+ sw0 = M.uncol M.zeros
+ sxzs :: J (JVec deg (JTuple sx sz)) MX
+ sxzs = M.uncol M.zeros
+
+ mat :: M.M (ErrorOut sr sx deg) (ErrorInD sx sw sz deg) MX
+ Jac mat _ _ =
+ dynStageConJac $
+ JacIn (cat (ErrorInD sx0 sw0 sxzs)) (ErrorIn0 x0' xzus' dt parm stageTimes)
+
+ df_dsx0 :: M (JVec deg sr) sx MX
+ df_dsw0 :: M (JVec deg sr) sw MX
+ df_dsxz :: M (JVec deg sr) (JVec deg (JTuple sx sz)) MX
+ dg_dsx0 :: M sx sx MX
+ dg_dsw0 :: M sx sw MX
+ dg_dsxz :: M sx (JVec deg (JTuple sx sz)) MX
+ ((df_dsx0, df_dsw0, df_dsxz), (dg_dsx0, dg_dsw0, dg_dsxz)) =
+ case fmap F.toList (F.toList (blockSplit mat)) of
+ [[x00,x01,x02],[x10,x11,x12]] -> ((M.mkM x00, M.mkM x01, M.mkM x02),
+ (M.mkM x10, M.mkM x11, M.mkM x12))
+ _ -> error "stageFunction: got wrong number of elements in jacobian"
+
+ -- TODO: this should be much simpler for radau
+
+ -- TODO: check these next 4 lines
+ dsxz_dsx0 = - (M.solve df_dsxz df_dsx0) :: M (JVec deg (JTuple sx sz)) sx MX
+ dsxz_dsw0 = - (M.solve df_dsxz df_dsw0) :: M (JVec deg (JTuple sx sz)) sw MX
+
+ dsx1_dsx0 = dg_dsx0 + dg_dsxz `M.mm` dsxz_dsx0 :: M sx sx MX
+ dsx1_dsw0 = dg_dsw0 + dg_dsxz `M.mm` dsxz_dsw0 :: M sx sw MX
+
+
+mkRobustifyFunction ::
+ forall x sx shr p .
+ (Vectorize x, Vectorize sx, Vectorize shr, Vectorize p)
+ => (x Sxe -> sx Sxe -> x Sxe)
+ -> (x Sxe -> sx Sxe -> p Sxe -> shr Sxe)
+ -> IO (J (JV shr) MX -> J (JV p) MX -> J (JV x) MX -> J (Cov (JV sx)) MX -> J (JV shr) MX)
+mkRobustifyFunction project robustifyPathC = do
+ proj <- toSXFun "errorSpaceProjection" $
+ \(JacIn x0 x1) -> JacOut (sxCatJV (project (sxSplitJV x1) (sxSplitJV x0))) (cat JNone)
+ let _ = proj :: SXFun
+ (JacIn (JV sx) (J (JV x)))
+ (JacOut (JV x) (J JNone))
+
+ projJac <- toFunJac proj
+ let _ = projJac :: SXFun
+ (JacIn (JV sx) (J (JV x)))
+ (Jac (JV sx) (JV x) (J JNone))
+
+ let zerosx = (M.uncol M.zeros) :: J (JV sx) SX
+ simplifiedPropJac <- toSXFun "simplified error space projection jacobian" $
+ \x0 -> (\(Jac j0 _ _) -> j0) (callSX projJac (JacIn zerosx x0))
+ let _ = simplifiedPropJac :: SXFun
+ (J (JV x))
+ (M.M (JV x) (JV sx))
+
+ let rpc (JacIn xe parm) = JacOut (sxCatJV lol) (cat JNone)
+ where
+ lol = robustifyPathC (sxSplitJV x) (sxSplitJV e) (sxSplitJV parm)
+ JTuple x e = split xe
+ robustH <- toSXFun "robust constraint" rpc
+ let _ = robustH :: SXFun
+ (JacIn (JTuple (JV x) (JV sx)) (J (JV p)))
+ (JacOut (JV shr) (J JNone))
+ robustHJac <- toFunJac robustH
+ let _ = robustHJac :: SXFun
+ (JacIn (JTuple (JV x) (JV sx)) (J (JV p)))
+ (Jac (JTuple (JV x) (JV sx)) (JV shr) (J JNone))
+
+ srh :: (J (JV x) :*: J (JV p)) SX -> Jac (JTuple (JV x) (JV sx)) (JV shr) (J JNone) SX
+ srh (x :*: p) = ret
+ where
+
+ xe = M.uncol M.zeros :: J (JV sx) SX
+ xxe = cat (JTuple x xe) :: J (JTuple (JV x) (JV sx)) SX
+
+ ret :: Jac (JTuple (JV x) (JV sx)) (JV shr) (J JNone) SX
+ ret = callSX robustHJac (JacIn xxe p)
+
+ simplifiedHJac <- toSXFun "simplified robust constraint jacobian" srh
+ let _ = simplifiedHJac :: SXFun
+ (J (JV x) :*: J (JV p))
+ (Jac (JTuple (JV x) (JV sx)) (JV shr) (J JNone))
+
+ let gogo :: J (JV shr) MX -> J (JV p) MX -> J (JV x) MX -> J (Cov (JV sx)) MX -> J (JV shr) MX
+ gogo gammas' theta x pe' = rcs'
+ where
+ gammas = fmap mkJ (unJV (split gammas')) :: shr (J (JV Id) MX)
+
+ jHx :: M (JV shr) (JV x) MX
+ jHe :: M (JV shr) (JV sx) MX
+ (jHx, jHe) = M.hsplitTup jacH'
+
+ jacH' :: M (JV shr) (JTuple (JV x) (JV sx)) MX
+ h0vec :: J (JV shr) MX
+ Jac jacH' h0vec _ = call simplifiedHJac (x :*: theta)
+
+ f :: M.M (JV x) (JV sx) MX
+ f = call simplifiedPropJac x
+
+ pe :: M.M (JV sx) (JV sx) MX
+ pe = toMat pe'
+
+ fpef :: M.M (JV x) (JV x) MX
+ fpef = fpe `M.mm` (M.trans f)
+
+ fpe :: M.M (JV x) (JV sx) MX
+ fpe = f `M.mm` pe
+
+ jHxs :: shr (M.M (JV Id) (JV x) MX)
+ jHxs = M.vsplit jHx
+
+ jHes :: shr (M.M (JV Id) (JV sx) MX)
+ jHes = M.vsplit jHe
+
+ shr' = fmap mkJ (unJV (split h0vec)) :: shr (J (JV Id) MX)
+
+ rcs' :: J (JV shr) MX
+ rcs' = cat $ JV $ fmap unsafeUnJ rcs
+
+ rcs :: shr (J (JV Id) MX)
+ rcs = vzipWith4 robustify gammas shr' jHxs jHes
+
+ robustify :: J (JV Id) MX
+ -> J (JV Id) MX
+ -> M.M (JV Id) (JV x) MX
+ -> M.M (JV Id) (JV sx) MX
+ -> J (JV Id) MX
+ robustify gamma h0 gHx gHe = h0 + gamma * sqrt sigma2
+ where
+ sigma2 :: J (JV Id) MX
+ sigma2 = mkJ sigma2'
+
+ M.UnsafeM sigma2' =
+ gHx `M.mm` fpef `M.mm` (M.trans gHx) +
+ 2 * gHx `M.mm` fpe `M.mm` (M.trans gHe) +
+ gHe `M.mm` pe `M.mm` (M.trans gHe)
+ :: M.M (JV Id) (JV Id) MX
+
+ retFun <- toMXFun "robust constraint violations"
+ (\(x0 :*: x1 :*: x2 :*: x3) -> gogo x0 x1 x2 x3) -- >>= expandMXFun
+
+ return (\x y z w -> call retFun (x :*: y :*: z :*: w))
diff --git a/src/Dyno/DirectCollocation/Types.hs b/src/Dyno/DirectCollocation/Types.hs
new file mode 100644
--- /dev/null
+++ b/src/Dyno/DirectCollocation/Types.hs
@@ -0,0 +1,204 @@
+{-# OPTIONS_GHC -Wall #-}
+{-# Language ScopedTypeVariables #-}
+{-# Language FlexibleContexts #-}
+{-# Language DeriveGeneric #-}
+
+module Dyno.DirectCollocation.Types
+ ( CollTraj(..)
+ , CollStage(..)
+ , CollPoint(..)
+ , CollStageConstraints(..)
+ , CollOcpConstraints(..)
+ , CollTrajCov(..)
+ , CollOcpCovConstraints(..)
+ , fillCollTraj
+ , fmapCollTraj
+ , fmapStage
+ , fmapCollPoint
+ , fillCollConstraints
+ , getXzus
+ ) where
+
+import qualified Data.Foldable as F
+import Data.Serialize ( Serialize )
+import GHC.Generics ( Generic )
+import Linear.V ( Dim(..) )
+import Data.Vector ( Vector )
+
+import Dyno.View ( View(..), J, JVec(..), S, mkJ, unJ, jfill, jreplicate )
+import Dyno.View.JV ( JV, splitJV )
+import Dyno.Vectorize ( Vectorize(..) )
+import Dyno.Cov ( Cov )
+
+
+-- design variables
+data CollTraj x z u p n deg a =
+ CollTraj (J S a) (J (JV p) a) (J (JVec n (CollStage (JV x) (JV z) (JV u) deg)) a) (J (JV x) a)
+ deriving (Eq, Generic, Show)
+ -- endtime, params, coll stages, xf
+
+-- design variables
+data CollTrajCov sx x z u p n deg a =
+ CollTrajCov (J (Cov (JV sx)) a) (J (CollTraj x z u p n deg) a)
+ deriving (Eq, Generic, Show)
+ -- endtime, params, coll stages, xf
+
+data CollStage x z u deg a = CollStage (J x a) (J (JVec deg (CollPoint x z u)) a)
+ deriving (Eq, Generic, Show)
+
+data CollPoint x z u a = CollPoint (J x a) (J z a) (J u a)
+ deriving (Eq, Generic, Show)
+
+-- constraints
+data CollStageConstraints x deg r a =
+ CollStageConstraints (J (JVec deg (JV r)) a) (J (JV x) a)
+ deriving (Eq, Generic, Show)
+
+data CollOcpConstraints n deg x r c h a =
+ CollOcpConstraints
+ { coCollPoints :: J (JVec n (JVec deg (JV r))) a
+ , coContinuity :: J (JVec n (JV x)) a
+ , coPathC :: J (JVec n (JVec deg (JV h))) a
+ , coBc :: J (JV c) a
+ } deriving (Eq, Generic, Show)
+
+data CollOcpCovConstraints n deg x r c h sh shr sc a =
+ CollOcpCovConstraints
+ { cocNormal :: J (CollOcpConstraints n deg x r c h) a
+ , cocCovPathC :: J (JVec n sh) a
+ , cocCovRobustPathC :: J (JVec n (JV shr)) a
+ , cocSbc :: J sc a
+ } deriving (Eq, Generic, Show)
+
+-- serialize instances
+instance Serialize a => Serialize (CollPoint x z u a)
+instance Serialize a => Serialize (CollStage x z u deg a)
+instance Serialize a => Serialize (CollTraj x z u p n deg a)
+instance Serialize a => Serialize (CollTrajCov sx x z u p n deg a)
+
+-- View instances
+instance (View x, View z, View u) => View (CollPoint x z u)
+instance (View x, View z, View u, Dim deg) => View (CollStage x z u deg)
+instance (Vectorize x, Vectorize z, Vectorize u, Vectorize p, Dim n, Dim deg) =>
+ View (CollTraj x z u p n deg)
+instance (Vectorize sx, Vectorize x, Vectorize z, Vectorize u, Vectorize p, Dim n, Dim deg) =>
+ View (CollTrajCov sx x z u p n deg)
+
+instance (Vectorize x, Vectorize r, Dim deg) => View (CollStageConstraints x deg r)
+instance (Vectorize x, Vectorize r, Dim n, Dim deg, Vectorize c, Vectorize h) =>
+ View (CollOcpConstraints n deg x r c h)
+instance ( Vectorize x, Vectorize r, Dim n, Dim deg, Vectorize c, Vectorize h
+ , View sh, Vectorize shr, View sc
+ ) => View (CollOcpCovConstraints n deg x r c h sh shr sc)
+
+
+getXzus ::
+ (Vectorize x, Vectorize z, Vectorize u, Dim n, Dim deg)
+ => CollTraj x z u p n deg (Vector a) -> ([[x a]], [[z a]], [[u a]])
+getXzus (CollTraj _ _ stages xf) = (xs ++ [[splitJV xf]], zs, us)
+ where
+ (xs, zs, us) = unzip3 $ map (getXzus' . split) (F.toList (unJVec (split stages)))
+
+getXzus' :: (Vectorize x, Vectorize z, Vectorize u, Dim deg)
+ => CollStage (JV x) (JV z) (JV u) deg (Vector a) -> ([x a], [z a], [u a])
+getXzus' (CollStage x0 xzus) = (splitJV x0 : xs, zs, us)
+ where
+ (xs, zs, us) = unzip3 $ map (f . split) (F.toList (unJVec (split xzus)))
+ f (CollPoint x z u) = (splitJV x, splitJV z, splitJV u)
+
+fillCollConstraints ::
+ forall x r c h n deg a .
+ (Vectorize x, Vectorize r, Vectorize h, Vectorize c,
+ Dim n, Dim deg, Show a)
+ => x a -> r a -> c a -> h a -> CollOcpConstraints n deg x r c h (Vector a)
+fillCollConstraints x r c h =
+ CollOcpConstraints
+ { coCollPoints = jreplicate $ jreplicate $ mkJ (vectorize r)
+ , coContinuity = jreplicate $ mkJ (vectorize x)
+ , coPathC = jreplicate $ jreplicate $ mkJ (vectorize h)
+ , coBc = mkJ (vectorize c)
+ }
+
+
+fillCollTraj ::
+ forall x z u p n deg a .
+ (Vectorize x, Vectorize z, Vectorize u, Vectorize p,
+ Dim n, Dim deg, Show a)
+ => x a -> z a -> u a -> p a -> a -> CollTraj x z u p n deg (Vector a)
+fillCollTraj x z u p t =
+ fmapCollTraj
+ (const x)
+ (const z)
+ (const u)
+ (const p)
+ (const t)
+ (split (jfill () :: J (CollTraj x z u p n deg) (Vector ())))
+
+fmapCollTraj ::
+ forall x1 x2 z1 z2 u1 u2 p1 p2 n deg a b .
+ ( Vectorize x1, Vectorize x2
+ , Vectorize z1, Vectorize z2
+ , Vectorize u1, Vectorize u2
+ , Vectorize p1, Vectorize p2
+ , Dim n, Dim deg
+ , Show a, Show b )
+ => (x1 a -> x2 b)
+ -> (z1 a -> z2 b)
+ -> (u1 a -> u2 b)
+ -> (p1 a -> p2 b)
+ -> (a -> b)
+ -> CollTraj x1 z1 u1 p1 n deg (Vector a)
+ -> CollTraj x2 z2 u2 p2 n deg (Vector b)
+fmapCollTraj fx fz fu fp ft (CollTraj tf1 p stages1 xf) = CollTraj tf2 (fj fp p) stages2 (fj fx xf)
+ where
+ tf2 :: J S (Vector b)
+ tf2 = mkJ $ fmap ft (unJ tf1)
+ stages2 = cat $ fmapJVec (fmapStage fx fz fu) (split stages1)
+
+ fj :: (Vectorize f1, Vectorize f2)
+ => (f1 a -> f2 b)
+ -> J (JV f1) (Vector a) -> J (JV f2) (Vector b)
+ fj f = mkJ . vectorize . f . devectorize . unJ
+
+fmapJVec :: (View f, View g, Show a, Show b)
+ => (f (Vector a) -> g (Vector b)) -> JVec deg f (Vector a) -> JVec deg g (Vector b)
+fmapJVec f = JVec . fmap (cat . f . split) . unJVec
+
+fmapStage :: forall x1 x2 z1 z2 u1 u2 deg a b .
+ ( Vectorize x1, Vectorize x2
+ , Vectorize z1, Vectorize z2
+ , Vectorize u1, Vectorize u2
+ , Dim deg
+ , Show a, Show b )
+ => (x1 a -> x2 b)
+ -> (z1 a -> z2 b)
+ -> (u1 a -> u2 b)
+ -> CollStage (JV x1) (JV z1) (JV u1) deg (Vector a)
+ -> CollStage (JV x2) (JV z2) (JV u2) deg (Vector b)
+fmapStage fx fz fu (CollStage x0 points0) = CollStage (fj fx x0) points1
+ where
+ points1 = cat $ fmapJVec (fmapCollPoint fx fz fu) (split points0)
+
+ fj :: (Vectorize f1, Vectorize f2)
+ => (f1 a -> f2 b)
+ -> J (JV f1) (Vector a)
+ -> J (JV f2) (Vector b)
+ fj f = mkJ . vectorize . f . devectorize . unJ
+
+fmapCollPoint :: forall x1 x2 z1 z2 u1 u2 a b .
+ ( Vectorize x1, Vectorize x2
+ , Vectorize z1, Vectorize z2
+ , Vectorize u1, Vectorize u2
+ , Show a, Show b )
+ => (x1 a -> x2 b)
+ -> (z1 a -> z2 b)
+ -> (u1 a -> u2 b)
+ -> CollPoint (JV x1) (JV z1) (JV u1) (Vector a)
+ -> CollPoint (JV x2) (JV z2) (JV u2) (Vector b)
+fmapCollPoint fx fz fu (CollPoint x z u) = CollPoint (fj fx x) (fj fz z) (fj fu u)
+ where
+ fj :: (Vectorize f1, Vectorize f2)
+ => (f1 a -> f2 b)
+ -> J (JV f1) (Vector a)
+ -> J (JV f2) (Vector b)
+ fj f = mkJ . vectorize . f . devectorize . unJ
diff --git a/src/Dyno/Interface/LogsAndErrors.hs b/src/Dyno/Interface/LogsAndErrors.hs
new file mode 100644
--- /dev/null
+++ b/src/Dyno/Interface/LogsAndErrors.hs
@@ -0,0 +1,58 @@
+{-# OPTIONS_GHC -Wall #-}
+{-# Language PackageImports #-}
+{-# Language FlexibleContexts #-}
+
+module Dyno.Interface.LogsAndErrors
+ ( ErrorMessage (..)
+ , LogMessage (..)
+ , countLogs
+ , debug
+ , warn
+ , err
+ , impossible
+ ) where
+
+import "mtl" Control.Monad.Except ( MonadError, throwError )
+import "mtl" Control.Monad.Writer ( MonadWriter, tell )
+
+data LogMessage = Debug String
+ | Warning String
+ | Error String
+ | Impossible String
+
+instance Show LogMessage where
+ show (Debug x) = "Debug: " ++ x
+ show (Warning x) = "Warning: " ++ x
+ show (Error x) = "Error: " ++ x
+ show (Impossible x) = "\"Impossible\" Error: " ++ x
+
+countLogs' :: (Int,Int,Int,Int) -> [LogMessage] -> (Int,Int,Int,Int)
+countLogs' x [] = x
+countLogs' (a,b,c,d) (Debug _:xs) = countLogs' (a+1, b, c, d) xs
+countLogs' (a,b,c,d) (Warning _:xs) = countLogs' ( a, b+1, c, d) xs
+countLogs' (a,b,c,d) (Error _:xs) = countLogs' ( a, b, c+1, d) xs
+countLogs' (a,b,c,d) (Impossible _:xs) = countLogs' ( a, b, c, d+1) xs
+
+countLogs :: [LogMessage] -> (Int,Int,Int,Int)
+countLogs = countLogs' (0,0,0,0)
+
+newtype ErrorMessage = ErrorMessage String -- deriving Error
+instance Show ErrorMessage where
+ show (ErrorMessage msg) = msg
+
+logMessage :: MonadWriter [t] m => t -> m ()
+logMessage x = tell [x]
+
+debug :: MonadWriter [LogMessage] m => String -> m ()
+debug = logMessage . Debug
+
+warn :: MonadWriter [LogMessage] m => String -> m ()
+warn = logMessage . Warning
+
+err :: (MonadError ErrorMessage m, MonadWriter [LogMessage] m) =>
+ String -> m a
+err x = logMessage (Error x) >> throwError (ErrorMessage x)
+
+impossible :: (MonadError ErrorMessage m, MonadWriter [LogMessage] m) =>
+ String -> m b
+impossible x = logMessage (Impossible x) >> throwError (ErrorMessage ("\"impossible error\": " ++ x))
diff --git a/src/Dyno/Interface/Types.hs b/src/Dyno/Interface/Types.hs
new file mode 100644
--- /dev/null
+++ b/src/Dyno/Interface/Types.hs
@@ -0,0 +1,86 @@
+{-# OPTIONS_GHC -Wall -ddump-splices #-}
+
+module Dyno.Interface.Types
+ ( Constraint(..)
+ , Objective(..)
+ , HomotopyParam(..)
+ , NlpMonadState(..)
+ , OcpState(..)
+ , DaeState(..)
+ , daeX
+ , daeXDot
+ , daeZ
+ , daeU
+ , daeP
+ , daeO
+ ) where
+
+import qualified Data.HashSet as HS
+import qualified Data.Sequence as S
+import qualified Data.Map as M
+import Control.Lens
+import Data.Functor ( (<$>) )
+
+import Dyno.SXElement ( SXElement )
+
+data Constraint a = Eq2 a a
+ | Ineq2 a a
+ | Ineq3 a (Double, Double)
+
+data Objective a = ObjectiveUnset | Objective a
+data HomotopyParam a = HomotopyParamUnset | HomotopyParam a
+
+data NlpMonadState =
+ NlpMonadState
+ { nlpX :: S.Seq (String, SXElement)
+ , nlpXSet :: HS.HashSet String
+ , nlpConstraints :: S.Seq (Constraint SXElement)
+ , nlpObj :: Objective SXElement
+ , nlpHomoParam :: HomotopyParam SXElement
+ }
+
+data OcpState = OcpState { ocpPathConstraints :: S.Seq (Constraint SXElement)
+ , ocpLagrangeObj :: Objective SXElement
+ , ocpHomoParam :: HomotopyParam SXElement
+ }
+
+data DaeState = DaeState { _daeXDot :: S.Seq (String, SXElement)
+ , _daeX :: S.Seq (String, SXElement)
+ , _daeZ :: S.Seq (String, SXElement)
+ , _daeU :: S.Seq (String, SXElement)
+ , _daeP :: S.Seq (String, SXElement)
+ , _daeO :: M.Map String SXElement
+ , daeNameSet :: HS.HashSet String
+ , daeConstraints :: S.Seq (SXElement, SXElement)
+ }
+
+--makeLenses ''DaeState
+daeXDot :: Lens' DaeState (S.Seq (String, SXElement))
+daeXDot f (DaeState xdot' x z u p o ss c) =
+ (\xdot -> DaeState xdot x z u p o ss c) <$> f xdot'
+{-# INLINE daeXDot #-}
+
+daeX :: Lens' DaeState (S.Seq (String, SXElement))
+daeX f (DaeState xdot x' z u p o ss c) =
+ (\x -> DaeState xdot x z u p o ss c) <$> f x'
+{-# INLINE daeX #-}
+
+daeZ :: Lens' DaeState (S.Seq (String, SXElement))
+daeZ f (DaeState xdot x z' u p o ss c) =
+ (\z -> DaeState xdot x z u p o ss c) <$> f z'
+{-# INLINE daeZ #-}
+
+daeU :: Lens' DaeState (S.Seq (String, SXElement))
+daeU f (DaeState xdot x z u' p o ss c) =
+ (\u -> DaeState xdot x z u p o ss c) <$> f u'
+{-# INLINE daeU #-}
+
+daeP :: Lens' DaeState (S.Seq (String, SXElement))
+daeP f (DaeState xdot x z u p' o ss c) =
+ (\p -> DaeState xdot x z u p o ss c) <$> f p'
+{-# INLINE daeP #-}
+
+daeO :: Lens' DaeState (M.Map String SXElement)
+daeO f (DaeState xdot x z u p o' ss c) =
+ (\o -> DaeState xdot x z u p o ss c) <$> f o'
+{-# INLINE daeO #-}
diff --git a/src/Dyno/LagrangePolynomials.lhs b/src/Dyno/LagrangePolynomials.lhs
new file mode 100644
--- /dev/null
+++ b/src/Dyno/LagrangePolynomials.lhs
@@ -0,0 +1,300 @@
+\documentclass[a4paper,12pt]{article}
+%\documentclass[printer]{gOMS2e}
+%\usepackage{indentfirst}
+%\usepackage[T1]{fontenc}
+\usepackage{amsfonts}
+\usepackage{amsmath,url}
+\usepackage[left=2cm,right=2cm,top=2.5cm]{geometry}
+\usepackage{graphicx}
+\usepackage{algorithmic}
+\usepackage{algorithm}
+
+\usepackage{listings}
+\lstloadlanguages{Haskell}
+\lstnewenvironment{code}
+ {\lstset{}%
+ \csname lst@SetFirstLabel\endcsname}
+ {\csname lst@SaveFirstLabel\endcsname}
+ \lstset{
+ basicstyle=\small\ttfamily,
+ flexiblecolumns=false,
+ basewidth={0.5em,0.45em},
+ literate={+}{{$+$}}1 {/}{{$/$}}1 {*}{{$*$}}1 {=}{{$=$}}1
+ {>}{{$>$}}1 {<}{{$<$}}1 {\\}{{$\lambda$}}1
+ {\\\\}{{\char`\\\char`\\}}1
+ {->}{{$\rightarrow$}}2 {>=}{{$\geq$}}2 {<-}{{$\leftarrow$}}2
+ {<=}{{$\leq$}}2 {=>}{{$\Rightarrow$}}2
+ {\ .}{{$\circ$}}2 {\ .\ }{{$\circ$}}2
+ {>>}{{>>}}2 {>>=}{{>>=}}2
+ {|}{{$\mid$}}1
+ }
+
+%\newcommand{\qed}{\hfill \qedbox $\quad\quad$\\[1ex]}%
+%\newcommand{\beq}{\begin{equation}}
+%\newcommand{\eeq}{\end{equation}}
+%\newcommand{\barr}{\begin{array}}
+%\newcommand{\earr}{\end{array}}
+%\newcommand{\bvec}{ \left[ \!\! \barr{cccccccccccc} }
+%\newcommand{\evec}{ \earr \!\! \right] }
+%\newcommand{\bmat}{ \left( \!\! \barr{ccccccc} }
+%\newcommand{\emat}{ \earr \!\! \right) }
+%\newcommand{\e}{\mathbf{e}}
+%\renewcommand{\AA}{\mathbb{A}}
+%\newcommand{\BB}{\mathbb{B}}
+%\newcommand{\R}{\mathbb{R}}
+%\newcommand{\N}{\mathbb{N}}
+%\newcommand{\EE}{{\mathbb{E}}}
+%\newcommand{\LL}{{\mathcal{L}}}
+%\newcommand{\FF}{{\mathbb{F}}}
+%\newcommand{\XX}{{\mathbb{X}}}
+%\newcommand{\Id}{\mathbb{I}}
+%\newcommand{\conv}{\mathbf{conv}}
+%\newcommand{\nonneg}{\mathbf{nonneg}}
+%\newcommand{\quadand}{\quad\mbox{ and }\quad}
+
+\newcommand{\dpartial}[2]{\frac{\partial {#1}}{\partial {#2}}}
+\newcommand{\dtotal}[2]{\frac{d {#1}}{d {#2}}}
+
+\newcommand{\myl}{\xi}
+\newcommand{\myldot}{\myl'}
+%\newcommand{\pos}{\vec{r}}
+%\newcommand{\rad}{l}
+\newcommand{\matr}[2]{\left[\begin{array}{#1}#2\end{array}\right]}
+%\newcommand{\dpartial}[2]{\frac{\partial#1}{\partial #2}}
+\newcommand{\refeq}[1]{Eq.~(\ref{#1})}
+\newcommand{\refsec}[1]{Sect.~\ref{#1}}
+\newcommand{\reffig}[1]{Fig.~\ref{#1}}
+%\newcommand{\reftab}[1]{Table \ref{#1}}
+%\usepackage{epstopdf}
+%\usepackage{amsmath}
+%\usepackage{amssymb}
+
+\newcommand{\taus}[3]{\frac{\tau_{#1}-\tau_{#2}}{\tau_{#3}-\tau_{#2}}}
+\newcommand{\taud}[3]{\frac{1}{\tau_{#3}-\tau_{#2}}}
+
+\begin{document}
+\sffamily
+\begin{center}
+\begin{LARGE}
+{\bf Direct Collocation}\\
+\vspace*{0.3cm}
+\end{LARGE}\end{center}
+\begin{center}
+\begin{Large}
+Greg Horn
+\end{Large}
+
+\end{center}
+\vspace*{1cm}
+
+\section{Lagrange Interpolation}
+The Lagrange interpolating polynomial of degree $D$, defined by nodes $x_0..x_D$ at timepoints $\tau_0..\tau_D$ is
+\begin{equation}
+\begin{aligned}
+x(\tau) &= \sum_{j=0}^D \myl_j(\tau) x_j
+\end{aligned}
+\label{eq:lagrange_interp_poly}
+\end{equation}
+where
+\begin{equation}
+\myl_j(\tau) = \prod_{k=0,k \neq j}^D\frac{\tau-\tau_k}{\tau_j-\tau_k}
+\end{equation}
+
+\begin{code}
+{-# OPTIONS_GHC -Wall #-}
+{-# Language FlexibleContexts #-}
+{-# Language GADTs #-}
+
+module Dyno.LagrangePolynomials ( lagrangeDerivCoeffs, lagrangeXis, runComparison ) where
+
+import qualified Data.Vector as V
+
+import Casadi.SXFunction ( sxFunction )
+import Casadi.Function ( evalDMatrix )
+import Casadi.SharedObject ( soInit )
+import Casadi.SX ( SX, ssym, sgradient )
+import Casadi.DMatrix ( DMatrix, ddata, ddense )
+
+import Dyno.TypeVecs
+
+lagrangeXis :: Fractional a => [a] -> a -> Int -> a
+lagrangeXis taus tau j =
+ product [(tau - tk) / (tj - tk) | k <- [0..deg]
+ , k /= j
+ , let tk = taus !! k
+ tj = taus !! j
+ ]
+ where
+ deg = length taus - 1
+
+\end{code}
+
+The derivative of this polynomial on an intermediate point is given by
+\begin{equation}
+x'(\tau) = \sum_{j=0}^D \myldot_j(\tau) x_j
+\label{eq:lagrange_interp_poly_deriv}
+\end{equation}
+
+Written out for $D=3$, this looks like
+
+\begin{align}
+x(\tau) = & x_0 \taus{}{1}{0} \taus{}{2}{0} \taus{}{3}{0} + \\
+ & x_1 \taus{}{0}{1} \taus{}{2}{1} \taus{}{3}{1} + \\
+ & x_2 \taus{}{0}{2} \taus{}{1}{2} \taus{}{3}{2} + \\
+ & x_3 \taus{}{0}{3} \taus{}{1}{3} \taus{}{2}{3}
+\end{align}
+
+Evaluating this at an interpolation node $\tau_m$ eliminates all terms except $m=k$, giving
+\begin{equation}
+x(\tau_m) = x_m
+\end{equation}
+
+The derivative for $D=3$ is
+
+\begin{align}
+x'(\tau) = &x_0 (\taud{}{1}{0} \taus{}{2}{0} \taus{}{3}{0}
+ + \taus{}{1}{0} \taud{}{2}{0} \taus{}{3}{0}
+ + \taus{}{1}{0} \taus{}{2}{0} \taud{}{3}{0}) + \\
+ &x_1 (\taud{}{0}{1} \taus{}{2}{1} \taus{}{3}{1}
+ + \taus{}{0}{1} \taud{}{2}{1} \taus{}{3}{1}
+ + \taus{}{0}{1} \taus{}{2}{1} \taud{}{3}{1}) + \\
+ &x_2 (\taud{}{0}{2} \taus{}{1}{2} \taus{}{3}{2}
+ + \taus{}{0}{2} \taud{}{1}{2} \taus{}{3}{2}
+ + \taus{}{0}{2} \taus{}{1}{2} \taud{}{3}{2}) + \\
+ &x_3 (\taud{}{0}{3} \taus{}{1}{3} \taus{}{2}{3}
+ + \taus{}{0}{3} \taud{}{1}{3} \taus{}{2}{3}
+ + \taus{}{0}{3} \taus{}{1}{3} \taud{}{2}{3})
+\end{align}
+
+evaluating this at $\tau_0$ gives:
+
+\begin{align}
+x'(\tau_0) = &x_0 (\taud{0}{1}{0} \taus{0}{2}{0} \taus{0}{3}{0}
+ + \taus{0}{1}{0} \taud{0}{2}{0} \taus{0}{3}{0}
+ + \taus{0}{1}{0} \taus{0}{2}{0} \taud{0}{3}{0}) +\\
+ &x_1 (\taud{0}{0}{1} \taus{0}{2}{1} \taus{0}{3}{1}
+ + \taus{0}{0}{1} \taud{0}{2}{1} \taus{0}{3}{1}
+ + \taus{0}{0}{1} \taus{0}{2}{1} \taud{0}{3}{1}) +\\
+ &x_2 (\taud{0}{0}{2} \taus{0}{1}{2} \taus{0}{3}{2}
+ + \taus{0}{0}{2} \taud{0}{1}{2} \taus{0}{3}{2}
+ + \taus{0}{0}{2} \taus{0}{1}{2} \taud{0}{3}{2}) +\\
+ &x_3 (\taud{0}{0}{3} \taus{0}{1}{3} \taus{0}{2}{3}
+ + \taus{0}{0}{3} \taud{0}{1}{3} \taus{0}{2}{3}
+ + \taus{0}{0}{3} \taus{0}{1}{3} \taud{0}{2}{3})
+\end{align}
+which simplifies to
+\begin{align}
+x'(\tau_0) = &x_0 (\taud{0}{1}{0}
+ + \taud{0}{2}{0}
+ + \taud{0}{3}{0}) +\\
+ &x_1 (\taud{0}{0}{1} \taus{0}{2}{1} \taus{0}{3}{1}) +\\
+ &x_2 (\taud{0}{0}{2} \taus{0}{1}{2} \taus{0}{3}{2}) +\\
+ &x_3 (\taud{0}{0}{3} \taus{0}{1}{3} \taus{0}{2}{3}) \\
+= & x_0 C_{0,0} + x_1 C_{0,1} + x_2 C_{0,2} + x_3 C_{0,3}
+\end{align}
+
+evaluating this at $\tau_1$ gives:
+\begin{align}
+x'(\tau_0) = &x_0 (\taud{1}{1}{0} \taus{1}{2}{0} \taus{1}{3}{0}) +\\
+ &x_1 (\taud{1}{0}{1}
+ + \taud{1}{2}{1}
+ + \taud{1}{3}{1}) +\\
+ &x_2 (\taus{1}{0}{2} \taud{1}{1}{2} \taus{1}{3}{2}) +\\
+ &x_3 ( \taus{1}{0}{3} \taud{1}{1}{3} \taus{1}{2}{3}) \\
+= & x_0 C_{1,0} + x_1 C_{1,1} + x_2 C_{1,2} + x_3 C_{1,3}
+\end{align}
+
+evaluating this at $\tau_2$ gives:
+\begin{align}
+x'(\tau_2) = &x_0 (\taus{2}{1}{0} \taud{2}{2}{0} \taus{2}{3}{0}) + \\
+ &x_1 (\taus{2}{0}{1} \taud{2}{2}{1} \taus{2}{3}{1}) + \\
+ &x_2 (\taud{2}{0}{2}
+ + \taud{2}{1}{2}
+ + \taud{2}{3}{2}) + \\
+ &x_3 (\taus{2}{0}{3} \taus{2}{1}{3} \taud{2}{2}{3}) \\
+= & x_0 C_{2,0} + x_1 C_{2,1} + x_2 C_{2,2} + x_3 C_{2,3}
+\end{align}
+
+evaluating this at $\tau_3$ gives:
+\begin{align}
+x'(\tau_3) = &x_0 (\taus{3}{1}{0} \taus{3}{2}{0} \taud{3}{3}{0}) + \\
+ &x_1 (\taus{3}{0}{1} \taus{3}{2}{1} \taud{3}{3}{1}) + \\
+ &x_2 (\taus{3}{0}{2} \taus{3}{1}{2} \taud{3}{3}{2}) + \\
+ &x_3 (\taud{3}{0}{3}
+ + \taud{3}{1}{3}
+ + \taud{3}{2}{3}) \\
+= & x_0 C_{3,0} + x_1 C_{3,1} + x_2 C_{3,2} + x_3 C_{3,3}
+\end{align}
+
+The general formula for $C_{j,k}$ is
+\begin{equation}
+C_{j,k} =
+\begin{cases}
+\sum_{i=0,i\ne k}^D{\frac{1}{\tau_k-\tau_i}} & j=k \\
+\frac{1}{\tau_k-\tau_j} \prod_{i=0,i\ne j,i\ne k}^D{\frac{\tau_j-\tau_i}{\tau_k-\tau_i}} & j \ne k
+\end{cases}
+\end{equation}
+
+\begin{code}
+lagrangeDerivCoeffs :: (Dim deg, Fractional a) => Vec deg a -> Vec deg (Vec deg a)
+lagrangeDerivCoeffs taus' = mkVec' [mkVec' [cjk j k | k <- [0..deg]] | j <- [0..deg]]
+ where
+ taus = unVec taus'
+ deg = V.length taus - 1
+
+ cjk j k
+ | j == k = sum [ 1/(tau_k - taus V.! i) | i <- [0..deg], i /= k ]
+ | otherwise =
+ 1 / (tau_k - tau_j) *
+ product [ (tau_j - tau_i)/(tau_k - tau_i)
+ | i <- [0..deg], i /= j, i /= k, let tau_i = taus V.! i
+ ]
+ where
+ tau_k = taus V.! k
+ tau_j = taus V.! j
+\end{code}
+
+Testing code:
+\begin{code}
+ssyms :: String -> Int -> IO [SX]
+ssyms name k = mapM (ssym . (name ++) . show) $ take k [(0::Int)..]
+
+runComparison :: IO ()
+runComparison = do
+ let deg = 6
+ sampleTaus = take (deg+1) [0.1,0.2..]
+ sampleTaus' = map realToFrac sampleTaus
+
+ tau <- ssym "t"
+ taus <- ssyms "t" (deg + 1)
+
+ let zs :: [SX]
+ zs = map (lagrangeXis taus tau) [0..deg]
+ inputs = tau : taus
+ zdot = map (`sgradient` tau) zs
+ zdotAlg <- sxFunction (V.fromList inputs) (V.fromList zdot)
+ soInit zdotAlg
+
+ --mapM_ print zdot'
+
+ putStrLn "numeric:"
+ vals' <- V.mapM (\tau_i -> evalDMatrix zdotAlg (V.fromList (tau_i : sampleTaus'))) (V.fromList sampleTaus')
+ let d2d :: DMatrix -> Double
+ d2d x = case V.toList (ddata (ddense x)) of
+ [y] -> y
+ ys -> error $ "d2d: need length 1, got length " ++ show (length ys)
+
+ vals = fmap (fmap d2d) vals'
+ V.mapM_ print vals
+
+ putStrLn "\nnumeric difference:"
+ let cmp :: V.Vector Double -> V.Vector Double -> IO ()
+ cmp v1s v2s = print $ V.zipWith (-) v1s v2s
+ f :: Dim n => Vec n Double -> IO ()
+ f st = V.zipWithM_ cmp vals $ fmap unVec $ unVec $ lagrangeDerivCoeffs st
+ reifyVector (V.fromList sampleTaus) f
+ return ()
+
+\end{code}
+
+\end{document}
diff --git a/src/Dyno/Models/AeroCoeffs.hs b/src/Dyno/Models/AeroCoeffs.hs
new file mode 100644
--- /dev/null
+++ b/src/Dyno/Models/AeroCoeffs.hs
@@ -0,0 +1,271 @@
+{-# OPTIONS_GHC -Wall #-}
+{-# Language DeriveFunctor #-}
+{-# Language DeriveFoldable #-}
+{-# Language DeriveGeneric #-}
+
+module Dyno.Models.AeroCoeffs where
+
+import Data.Foldable ( Foldable )
+import GHC.Generics
+import Linear
+
+import Dyno.Server.Accessors ( Lookup(..) )
+
+import Dyno.Vectorize
+
+atan2' :: Floating a => a -> a -> a
+atan2' y x = 2 * atan (y / (sqrt(x*x + y*y + 1e-15) + x) )
+
+data ControlSurfaces a =
+ ControlSurfaces { csElev :: a
+ , csRudder :: a
+ , csAil :: a
+ , csFlaps :: a
+ } deriving (Eq, Functor, Foldable, Generic, Generic1, Show)
+instance Vectorize ControlSurfaces
+instance (Lookup a, Generic a) => Lookup (ControlSurfaces a)
+
+data AeroForceCoeffs a =
+ AeroForceCoeffs { af_cL0 :: a
+ , af_cL_A :: a
+ , af_cL_elev :: a
+ , af_cL_flaps :: a
+
+ , af_cD0 :: a
+ , af_cD_A :: a
+ , af_cD_A2 :: a
+ , af_cD_B2 :: a
+
+ , af_cD_elev :: a
+ , af_cD_elev2 :: a
+ , af_cD_A_elev :: a
+
+ , af_cD_flaps :: a
+ , af_cD_flaps2 :: a
+ , af_cD_A_flaps :: a
+
+ , af_cD_rudder :: a
+ , af_cD_rudder2 :: a
+ , af_cD_B_rudder :: a
+
+ , af_cD_ail :: a
+ , af_cD_ail2 :: a
+ , af_cD_B_ail :: a
+
+ , af_cY_B :: a
+ , af_cY_rudder :: a
+ } deriving (Functor, Generic, Generic1, Show)
+instance Vectorize AeroForceCoeffs
+
+data AeroMomentCoeffs a =
+ AeroMomentCoeffs { am_cm0 :: a
+
+ , am_cl_p :: a
+ , am_cl_q :: a
+ , am_cl_r :: a
+
+ , am_cm_p :: a
+ , am_cm_q :: a
+ , am_cm_r :: a
+
+ , am_cn_p :: a
+ , am_cn_q :: a
+ , am_cn_r :: a
+
+ , am_cl_B :: a
+ , am_cl_AB :: a
+ , am_cm_A :: a
+ , am_cn_B :: a
+ , am_cn_AB :: a
+
+ , am_cl_ail :: a
+ , am_cm_elev
+ , am_cm_flaps :: a
+ , am_cn_rudder :: a
+ } deriving (Functor, Generic, Generic1, Show)
+instance Vectorize AeroMomentCoeffs
+
+data AeroRefs a =
+ AeroRefs { ar_sref :: a
+ , ar_bref :: a
+ , ar_cref :: a
+ } deriving (Functor, Generic, Generic1, Show)
+instance Vectorize AeroRefs
+
+
+-- | Compute aerodynamic forces/moments in the body frame.
+-- Parameters:
+-- dcm_n2b: rotation matrix rotating vectors expressed in NED to vectors expressed in body
+-- v_bw_b: body velocity in the wind frame, expressed in the body frame
+-- w_bn_b: body angular velocity w.r.t. NED
+aeroForcesMoments :: Floating a => AeroForceCoeffs a -> AeroMomentCoeffs a -> AeroRefs a ->
+ V3 a -> V3 a -> ControlSurfaces a -> (V3 a, V3 a)
+aeroForcesMoments forceCoeffs momentCoeffs refs v_bw_b w_bn_b controlSurfaces = (forces, moments)
+ where
+ V3 cL cD cY = aeroForceCoeffs alpha beta controlSurfaces forceCoeffs
+ c_lmn = aeroMomentCoeffs alpha beta airspeed w_bn_b controlSurfaces momentCoeffs refs
+
+ -- alpha/beta
+ alpha = atan2' v_bw_b_z v_bw_b_x
+ beta = asin (v_bw_b_y / airspeed)
+ V3 v_bw_b_x v_bw_b_y v_bw_b_z = v_bw_b
+
+ airspeedSquared = quadrance v_bw_b
+ airspeed = sqrt airspeedSquared
+
+ moments = rho_sref_v2*^(V3 bref cref bref)*c_lmn
+ forces = dragForce + liftForce + sideForce
+
+ dragForce = (-rho_sref_v*cD) *^ v_bw_b
+ liftForce = rho_sref_v*cL *^ e_b2L_b_v
+ sideForce = rho_sref*cY *^ e_b2Y_b_v2
+
+ -- y axis of aircraft expressed in body frame
+ e_b2y_b = V3 0 1 0
+
+ -- lift axis normalized to airspeed
+ e_b2L_b_v = cross e_b2y_b v_bw_b
+
+ -- sideforces axis normalized to airspeed^2
+ e_b2Y_b_v2 = cross e_b2L_b_v (-v_bw_b)
+
+ rho_sref = 0.5*rho*sref
+ rho_sref_v2 = rho_sref*airspeedSquared
+ rho_sref_v = rho_sref*airspeed
+
+ sref = ar_sref refs
+ bref = ar_bref refs
+ cref = ar_cref refs
+
+ rho = 1.23
+
+aeroForceCoeffs :: Num a => a -> a -> ControlSurfaces a -> AeroForceCoeffs a -> V3 a
+aeroForceCoeffs alpha beta controlSurfaces coeffs = V3 cL cD cY
+ where
+ cL_wing = cL_A'*alpha + cL0'
+ cD_wing = cD_A'*alpha + cD_A2'*alpha*alpha + cD_B2'*beta*beta + cD0'
+ cY_wing = cY_B'*beta
+
+ cL_elev = cL_elev' * elev
+ cD_elev = cD_elev2' * elev * elev + cD_A_elev' * elev * alpha + cD_elev' * elev
+
+ cD_ail = cD_ail2'*ail*ail + cD_B_ail'*beta*ail + cD_ail'*ail
+
+ cL_flaps = cL_flaps'*flaps
+ cD_flaps = cD_flaps2'*flaps*flaps + cD_A_flaps'*alpha*flaps + cD_flaps'*flaps
+
+ cY_rudder = cY_rudder'*rudder
+ cD_rudder = cD_rudder2'*rudder*rudder + cD_B_rudder'*beta*rudder + cD_rudder'*rudder
+
+ cL = cL_wing + cL_elev + cL_flaps
+ cD = cD_wing + cD_elev + cD_ail + cD_flaps + cD_rudder
+ cY = cY_wing + cY_rudder
+
+ -- inputs
+ elev = csElev controlSurfaces
+ rudder = csRudder controlSurfaces
+ ail = csAil controlSurfaces
+ flaps = csFlaps controlSurfaces
+
+ -- unpack aero coeffs
+ cL_A' = af_cL_A coeffs
+ cL0' = af_cL0 coeffs
+ cD_A' = af_cD_A coeffs
+ cD_A2' = af_cD_A2 coeffs
+ cD_B2' = af_cD_B2 coeffs
+ cD0' = af_cD0 coeffs
+ cY_rudder' = af_cY_rudder coeffs
+ cD_rudder2' = af_cD_rudder2 coeffs
+ cD_flaps2' = af_cD_flaps2 coeffs
+ cD_elev2' = af_cD_elev2 coeffs
+ cD_flaps' = af_cD_flaps coeffs
+ cD_A_flaps' = af_cD_A_flaps coeffs
+ cD_A_elev' = af_cD_A_elev coeffs
+ cD_elev' = af_cD_elev coeffs
+ cD_ail2' = af_cD_ail2 coeffs
+ cD_ail' = af_cD_ail coeffs
+ cD_B_ail' = af_cD_B_ail coeffs
+ cD_B_rudder' = af_cD_B_rudder coeffs
+ cD_rudder' = af_cD_rudder coeffs
+ cL_elev' = af_cL_elev coeffs
+ cL_flaps' = af_cL_flaps coeffs
+ cY_B' = af_cY_B coeffs
+
+
+aeroMomentCoeffs :: Fractional a => a -> a -> a -> V3 a -> ControlSurfaces a -> AeroMomentCoeffs a -> AeroRefs a -> V3 a
+aeroMomentCoeffs alpha beta airspeed w_bn_b controlSurfaces coeffs refs =
+ momentCoeffs0 + momentCoeffs_pqr + momentCoeffs_AB + momentCoeffs_surf
+ where
+ elev = csElev controlSurfaces
+ rudder = csRudder controlSurfaces
+ ail = csAil controlSurfaces
+ flaps = csFlaps controlSurfaces
+
+ w_bn_b_hat = (V3 bref cref bref) * w_bn_b ^* (0.5/airspeed)
+
+ momentCoeffs0 = V3 cm0 0 0
+
+ momentCoeffs_pqr =
+ (V3
+ (V3 cl_p cl_q cl_r)
+ (V3 cm_p cm_q cm_r)
+ (V3 cn_p cn_q cn_r)) !* w_bn_b_hat
+
+ momentCoeffs_AB =
+ (V3
+ (V3 0 cl_B cl_AB)
+ (V3 cm_A 0 0)
+ (V3 0 cn_B cn_AB)) !* (V3 alpha beta (alpha*beta))
+
+ momentCoeffs_surf =
+ V3
+ (cl_ail * ail)
+ (cm_elev * elev + cm_flaps * flaps)
+ (cn_rudder * rudder)
+
+ -- unpack aero coeffs
+ cm0 = am_cm0 coeffs
+
+ cl_p = am_cl_p coeffs
+ cl_q = am_cl_q coeffs
+ cl_r = am_cl_r coeffs
+
+ cm_p = am_cm_p coeffs
+ cm_q = am_cm_q coeffs
+ cm_r = am_cm_r coeffs
+
+ cn_p = am_cn_p coeffs
+ cn_q = am_cn_q coeffs
+ cn_r = am_cn_r coeffs
+
+ cl_B = am_cl_B coeffs
+ cl_AB = am_cl_AB coeffs
+ cm_A = am_cm_A coeffs
+ cn_B = am_cn_B coeffs
+ cn_AB = am_cn_AB coeffs
+
+ cl_ail = am_cl_ail coeffs
+ cm_elev = am_cm_elev coeffs
+ cm_flaps = am_cm_flaps coeffs
+ cn_rudder = am_cn_rudder coeffs
+
+ bref = ar_bref refs
+ cref = ar_cref refs
+
+trans :: V3 (V3 a) -> V3 (V3 a)
+trans (V3
+ (V3 e11 e12 e13)
+ (V3 e21 e22 e23)
+ (V3 e31 e32 e33))
+ =
+ V3
+ (V3 e11 e21 e31)
+ (V3 e12 e22 e32)
+ (V3 e13 e23 e33)
+
+skew :: Num a => V3 a -> V3 (V3 a)
+skew (V3 x y z) =
+ V3
+ (V3 0 (-z) y )
+ (V3 z 0 (-x))
+ (V3 (-y) x 0 )
diff --git a/src/Dyno/Models/Aircraft.hs b/src/Dyno/Models/Aircraft.hs
new file mode 100644
--- /dev/null
+++ b/src/Dyno/Models/Aircraft.hs
@@ -0,0 +1,65 @@
+{-# OPTIONS_GHC -Wall -fno-warn-orphans #-}
+{-# Language ScopedTypeVariables #-}
+{-# Language DeriveFunctor #-}
+{-# Language DeriveGeneric #-}
+
+module Dyno.Models.Aircraft ( AcX(..), AcU(..), aircraftDae ) where
+
+import GHC.Generics
+import Linear
+
+import Dyno.Vectorize
+import Dyno.Server.Accessors ( Lookup(..) )
+import Dyno.Models.AeroCoeffs
+
+data AcX a = AcX { ac_r_n2b_n :: V3 a
+ , ac_v_bn_b :: V3 a
+ , ac_R_n2b :: M33 a
+ , ac_w_bn_b :: V3 a
+ , ac_u :: AcU a
+ } deriving (Eq, Functor, Generic, Generic1, Show)
+data AcU a = AcU { acSurfaces :: ControlSurfaces a
+ } deriving (Eq, Functor, Generic, Generic1, Show)
+newtype AcZ a = AcZ (None a) deriving (Eq, Functor, Generic, Generic1, Show)
+newtype AcR a = AcR (AcX a) deriving (Eq, Functor, Generic, Generic1, Show)
+newtype AcP a = AcP (None a) deriving (Eq, Functor, Generic, Generic1, Show)
+
+instance Vectorize AcX
+instance Vectorize AcZ
+instance Vectorize AcU
+instance Vectorize AcP
+instance Vectorize AcR
+
+instance (Lookup a, Generic a) => Lookup (AcX a)
+instance (Lookup a, Generic a) => Lookup (AcZ a)
+instance (Lookup a, Generic a) => Lookup (AcU a)
+instance (Lookup a, Generic a) => Lookup (AcP a)
+instance (Lookup a, Generic a) => Lookup (AcR a)
+
+subCs :: Num a => ControlSurfaces a -> ControlSurfaces a -> ControlSurfaces a
+subCs (ControlSurfaces x0 x1 x2 x3) (ControlSurfaces y0 y1 y2 y3) =
+ ControlSurfaces (x0-y0) (x1-y1) (x2-y2) (x3-y3)
+
+aircraftDae :: forall a. Floating a =>
+ (a, M33 a) -> AeroForceCoeffs a -> AeroMomentCoeffs a -> AeroRefs a ->
+ AcX a -> AcX a -> AcU a -> AcX a
+aircraftDae
+ (mass, inertia)
+ forceCoeffs
+ momentCoeffs
+ refs
+ (AcX r_n2b_n' v_bn_b' dcm_n2b' w_bn_b' (AcU controlSurfaces'))
+ (AcX _ v_bn_b dcm_n2b w_bn_b (AcU controlSurfaces))
+ (AcU controlSurfaces'') = daeResidual
+ where
+ v_bw_b = v_bn_b -- no relative wind
+ (aero_forces_body, moments_body) = aeroForcesMoments forceCoeffs momentCoeffs refs v_bw_b w_bn_b controlSurfaces
+ forces_body = aero_forces_body + dcm_n2b !* (V3 0 0 (9.81*mass))
+
+ daeResidual =
+ AcX { ac_r_n2b_n = (trans dcm_n2b) !* v_bn_b - r_n2b_n'
+ , ac_v_bn_b = v_bn_b' + cross w_bn_b v_bn_b - forces_body ^/ mass
+ , ac_R_n2b = (trans (skew w_bn_b)) !*! dcm_n2b - dcm_n2b'
+ , ac_w_bn_b = inertia !* w_bn_b' + cross w_bn_b (inertia !* w_bn_b) - moments_body
+ , ac_u = AcU $ subCs controlSurfaces'' controlSurfaces'
+ }
diff --git a/src/Dyno/Models/Betty.hs b/src/Dyno/Models/Betty.hs
new file mode 100644
--- /dev/null
+++ b/src/Dyno/Models/Betty.hs
@@ -0,0 +1,72 @@
+{-# OPTIONS_GHC -Wall #-}
+
+module Dyno.Models.Betty
+ ( bettyFc
+ , bettyMc
+ , bettyRefs
+ , bettyInertia
+ , bettyMass
+ ) where
+
+import Linear
+
+import Dyno.Models.AeroCoeffs
+
+bettyFc :: Floating a => AeroForceCoeffs a
+bettyFc = AeroForceCoeffs
+ { af_cL0 = 0.203530
+ , af_cL_A = 5.786876
+
+ , af_cD_A = 0.018751
+ , af_cD_A2 = 1.529989
+ , af_cD_B2 = -0.16247
+ , af_cD0 = 0.008767
+
+ , af_cY_B = -0.239789
+
+ -- control surface forces
+ , af_cL_elev = -0.0105*180/pi
+ , af_cL_flaps = 0.0184*180/pi
+ , af_cY_rudder = 0.0035*180/pi
+ , af_cD_flaps2 = 3.03874e-05, af_cD_A_flaps = 0.000101404, af_cD_flaps = 0.000208995
+ , af_cD_elev2 = 4.19816e-05, af_cD_A_elev = -9.79647e-05, af_cD_elev = 4.52856e-05
+ , af_cD_ail2 = 5.60583e-05, af_cD_B_ail = -6.73139e-06, af_cD_ail = 0
+ , af_cD_rudder2 = 2.03105e-05, af_cD_B_rudder = 5.55453e-05, af_cD_rudder = 0
+ }
+
+bettyMc :: Floating a => AeroMomentCoeffs a
+bettyMc = AeroMomentCoeffs
+ { am_cl_p = -0.576, am_cl_q = 0.0, am_cl_r = 0.0707
+ , am_cm_p = 0.0, am_cm_q = -15.5, am_cm_r = 0.0
+ , am_cn_p = -0.036, am_cn_q = 0.0, am_cn_r = -0.0667
+
+ , am_cl_B = -0.051808
+ , am_cl_AB = -0.208344
+ , am_cm_A = -0.450643
+ -- cm0 valid for CG/bridle location 0.1 meters behind main wing leading edge
+ , am_cm0 = 0.028980
+ , am_cn_B = 0.037183
+ , am_cn_AB = -0.028933
+
+ -- control surface moments
+ , am_cl_ail = 0.0073*180/pi
+ , am_cm_elev = 0.0352*180/pi
+ , am_cm_flaps = 0.0026*180/pi
+ , am_cn_rudder = 0.001176*180/pi
+ }
+
+bettyRefs :: Fractional a => AeroRefs a
+bettyRefs = AeroRefs { ar_sref = 0.684
+ , ar_bref = 2.904 -- sqrt(sref*AR),
+ , ar_cref = 0.2512 -- sqrt(sref/AR),
+ }
+
+bettyInertia :: Fractional a => M33 a
+bettyInertia =
+ V3
+ (V3 0.565 0 0)
+ (V3 0 0.161 0)
+ (V3 0 0 0.723)
+
+bettyMass :: Fractional a => a
+bettyMass = 7.5
diff --git a/src/Dyno/MultipleShooting.hs b/src/Dyno/MultipleShooting.hs
new file mode 100644
--- /dev/null
+++ b/src/Dyno/MultipleShooting.hs
@@ -0,0 +1,173 @@
+{-# OPTIONS_GHC -Wall #-}
+{-# Language ScopedTypeVariables #-}
+{-# Language DeriveGeneric #-}
+
+module Dyno.MultipleShooting
+ ( MsOcp(..)
+ , MsDvs(..)
+ , MsConstraints(..)
+ , makeMsNlp
+ ) where
+
+import GHC.Generics ( Generic )
+import Data.Vector ( Vector )
+import Data.Maybe ( fromMaybe )
+import qualified Data.Vector as V
+import Linear
+import qualified Data.Foldable as F
+
+import Dyno.TypeVecs
+import Dyno.View
+import Dyno.View.Scheme
+import Dyno.Vectorize
+import Dyno.Nlp
+
+
+data IntegratorIn x u p a = IntegratorIn (J (JV x) a) (J (JV u) a) (J (JV p) a)
+ deriving (Generic, Generic1)
+data IntegratorOut x a = IntegratorOut (J (JV x) a)
+ deriving (Generic, Generic1)
+instance (Vectorize x, Vectorize u, Vectorize p) => Scheme (IntegratorIn x u p)
+instance Vectorize x => Scheme (IntegratorOut x)
+
+type Ode x u p a = x a -> u a -> p a -> a -> x a
+
+-- problem specification
+data MsOcp x u p =
+ MsOcp
+ { msOde :: Ode x u p (J (JV Id) MX)
+ , msMayer :: x (J (JV Id) MX) -> J (JV Id) MX
+ , msLagrangeSum :: x (J (JV Id) MX) -> u (J (JV Id) MX) -> J (JV Id) MX
+ , msX0 :: x (Maybe Double)
+ , msXF :: x (Maybe Double)
+ , msXBnds :: x Bounds
+ , msUBnds :: u Bounds
+ , msPBnds :: p Bounds
+ , msEndTime :: Double
+ , msNumRk4Steps :: Maybe Int
+ }
+
+-- design variables
+data MsDvs x u p n a =
+ MsDvs
+ { dvXus :: J (JVec n (JTuple (JV x) (JV u))) a
+ , dvXf :: J (JV x) a
+ , dvP :: J (JV p) a
+ } deriving (Generic, Generic1)
+instance (Vectorize x, Vectorize u, Vectorize p, Dim n) => View (MsDvs x u p n)
+
+-- constraints
+data MsConstraints x n a =
+ MsConstraints
+ { gContinuity :: J (JVec n (JV x)) a
+ } deriving (Generic, Generic1)
+instance (Vectorize x, Dim n) => View (MsConstraints x n)
+
+rk4 :: (Floating a, Additive x) => (x a -> u a -> p a -> a -> x a) -> x a -> u a -> p a -> a -> a -> x a
+rk4 f x0 u p t h = x0 ^+^ h/6*^(k1 ^+^ 2 *^ k2 ^+^ 2 *^ k3 ^+^ k4)
+ where
+ k1 = f x0 u p t
+ k2 = f (x0 ^+^ h/2 *^ k1) u p (t+h/2)
+ k3 = f (x0 ^+^ h/2 *^ k2) u p (t+h/2)
+ k4 = f (x0 ^+^ h *^ k2) u p (t+h)
+
+simulate :: (Floating a, Additive x) => Int -> Ode x u p a -> x a -> u a -> p a -> a -> a -> x a
+simulate n ode x0' u p t h = xf
+ where
+ dt' = h/ fromIntegral n
+
+ xf = foldl sim x0' [ t+fromIntegral i*dt' | i <- [0..(n-1)] ]
+
+ sim x0'' t' = rk4 ode x0'' u p t' dt'
+
+makeMsNlp ::
+ forall x u p n
+ . (Dim n, Vectorize x, Vectorize u, Vectorize p, Additive x)
+ => MsOcp x u p -> IO (Nlp' (MsDvs x u p n) JNone (MsConstraints x n) MX)
+makeMsNlp msOcp = do
+ let n = reflectDim (Proxy :: Proxy n)
+ integrate (IntegratorIn x0 u p) = IntegratorOut (catJV' (simulate nsteps ode x0' u' p' 0 dt))
+ where
+ endTime = msEndTime msOcp
+ dt = (realToFrac endTime) / fromIntegral n
+ ode = msOde msOcp
+ nsteps = fromMaybe 1 (msNumRk4Steps msOcp)
+ x0' = splitJV' x0
+ u' = splitJV' u
+ p' = splitJV' p
+ integrator <- toMXFun "my integrator" integrate
+ let _ = integrator :: MXFun (IntegratorIn x u p) (IntegratorOut x) -- just for type signature
+
+ let nlp =
+ Nlp'
+ { nlpFG' = fg
+ , nlpBX' = bx
+ , nlpBG' = bg
+ , nlpX0' = x0
+ , nlpP' = cat JNone
+ , nlpLamX0' = Nothing
+ , nlpLamG0' = Nothing
+ , nlpScaleF' = Nothing
+ , nlpScaleX' = Nothing
+ , nlpScaleG' = Nothing
+ }
+
+ x0 :: J (MsDvs x u p n) (V.Vector Double)
+ x0 = jfill 0
+
+ boundsX0 = catJV (fmap (\x -> (x,x)) (msX0 msOcp)) :: J (JV x) (Vector Bounds)
+
+ boundsX = catJV (msXBnds msOcp) :: J (JV x) (Vector Bounds)
+ boundsU = catJV (msUBnds msOcp) :: J (JV u) (Vector Bounds)
+
+ boundsX0u = JTuple boundsX0 boundsU :: JTuple (JV x) (JV u) (Vector Bounds)
+ boundsXu = JTuple boundsX boundsU :: JTuple (JV x) (JV u) (Vector Bounds)
+ boundsXF = catJV (fmap (\x -> (x,x)) (msXF msOcp)) :: J (JV x) (Vector Bounds)
+
+ boundsXus :: (J (JVec n (JTuple (JV x) (JV u))) (Vector Bounds))
+ boundsXus = cat $ JVec $ mkVec' ( cat boundsX0u : replicate (n-1) (cat boundsXu))
+
+ bx :: J (MsDvs x u p n) (Vector Bounds)
+ bx = cat MsDvs
+ { dvXus = boundsXus
+ , dvXf = boundsXF
+ , dvP = catJV (msPBnds msOcp)
+ }
+
+ bg :: J (MsConstraints x n) (Vector Bounds)
+ bg = cat MsConstraints { gContinuity = jfill (Just 0, Just 0) }
+
+ fg :: J (MsDvs x u p n) MX -> J JNone MX -> (J S MX, J (MsConstraints x n) MX)
+ fg dvs _ = (f, cat g)
+ where
+ MsDvs xus xf p = split dvs
+ x1s :: Vec n (J (JV x) MX)
+ x1s = fmap (callIntegrate . split) $ unJVec $ split xus
+ callIntegrate (JTuple x0' u) = x1
+ where
+ IntegratorOut x1 = call integrator (IntegratorIn x0' u p)
+
+ lagrangeSum = F.sum $ fmap callLagrangeSum (unJVec (split xus))
+ where
+ callLagrangeSum xu = msLagrangeSum msOcp (splitJV' x) (splitJV' u)
+ where
+ JTuple x u = split xu
+
+ mayer = msMayer msOcp (splitJV' xf)
+
+ f :: J S MX
+ f = mkJ $ unJ $ mayer + lagrangeSum
+
+
+ x0s' = fmap (extractx . split) $ unJVec $ split xus :: Vec n (J (JV x) MX)
+ extractx (JTuple x0'' _) = x0''
+
+ x0s = tvtail (x0s' |> xf) :: Vec n (J (JV x) MX)
+
+ gaps:: Vec n (J (JV x) MX)
+ gaps = tvzipWith (-) x1s x0s
+
+ g :: MsConstraints x n MX
+ g = MsConstraints { gContinuity = cat $ JVec gaps }
+
+ return nlp
diff --git a/src/Dyno/Nats.hs b/src/Dyno/Nats.hs
new file mode 100644
--- /dev/null
+++ b/src/Dyno/Nats.hs
@@ -0,0 +1,625 @@
+{-# OPTIONS_GHC -Wall #-}
+
+module Dyno.Nats where
+
+import qualified Data.Reflection
+import Data.Reflection ( reflect )
+import Linear.V ( Dim(..) )
+import Data.Proxy
+
+data D0
+instance Dim D0 where
+ reflectDim _ = reflect (Proxy :: Proxy Data.Reflection.Z)
+data D1
+instance Dim D1 where
+ reflectDim _ = reflect (Proxy :: Proxy (Data.Reflection.SD Data.Reflection.Z))
+data D2
+instance Dim D2 where
+ reflectDim _ = reflect (Proxy :: Proxy (Data.Reflection.D (Data.Reflection.SD Data.Reflection.Z)))
+data D3
+instance Dim D3 where
+ reflectDim _ = reflect (Proxy :: Proxy (Data.Reflection.SD (Data.Reflection.SD Data.Reflection.Z)))
+data D4
+instance Dim D4 where
+ reflectDim _ = reflect (Proxy :: Proxy (Data.Reflection.D (Data.Reflection.D (Data.Reflection.SD Data.Reflection.Z))))
+data D5
+instance Dim D5 where
+ reflectDim _ = reflect (Proxy :: Proxy (Data.Reflection.SD (Data.Reflection.D (Data.Reflection.SD Data.Reflection.Z))))
+data D6
+instance Dim D6 where
+ reflectDim _ = reflect (Proxy :: Proxy (Data.Reflection.D (Data.Reflection.SD (Data.Reflection.SD Data.Reflection.Z))))
+data D7
+instance Dim D7 where
+ reflectDim _ = reflect (Proxy :: Proxy (Data.Reflection.SD (Data.Reflection.SD (Data.Reflection.SD Data.Reflection.Z))))
+data D8
+instance Dim D8 where
+ reflectDim _ = reflect (Proxy :: Proxy (Data.Reflection.D (Data.Reflection.D (Data.Reflection.D (Data.Reflection.SD Data.Reflection.Z)))))
+data D9
+instance Dim D9 where
+ reflectDim _ = reflect (Proxy :: Proxy (Data.Reflection.SD (Data.Reflection.D (Data.Reflection.D (Data.Reflection.SD Data.Reflection.Z)))))
+data D10
+instance Dim D10 where
+ reflectDim _ = reflect (Proxy :: Proxy (Data.Reflection.D (Data.Reflection.SD (Data.Reflection.D (Data.Reflection.SD Data.Reflection.Z)))))
+data D11
+instance Dim D11 where
+ reflectDim _ = reflect (Proxy :: Proxy (Data.Reflection.SD (Data.Reflection.SD (Data.Reflection.D (Data.Reflection.SD Data.Reflection.Z)))))
+data D12
+instance Dim D12 where
+ reflectDim _ = reflect (Proxy :: Proxy (Data.Reflection.D (Data.Reflection.D (Data.Reflection.SD (Data.Reflection.SD Data.Reflection.Z)))))
+data D13
+instance Dim D13 where
+ reflectDim _ = reflect (Proxy :: Proxy (Data.Reflection.SD (Data.Reflection.D (Data.Reflection.SD (Data.Reflection.SD Data.Reflection.Z)))))
+data D14
+instance Dim D14 where
+ reflectDim _ = reflect (Proxy :: Proxy (Data.Reflection.D (Data.Reflection.SD (Data.Reflection.SD (Data.Reflection.SD Data.Reflection.Z)))))
+data D15
+instance Dim D15 where
+ reflectDim _ = reflect (Proxy :: Proxy (Data.Reflection.SD (Data.Reflection.SD (Data.Reflection.SD (Data.Reflection.SD Data.Reflection.Z)))))
+data D16
+instance Dim D16 where
+ reflectDim _ = reflect (Proxy :: Proxy (Data.Reflection.D (Data.Reflection.D (Data.Reflection.D (Data.Reflection.D (Data.Reflection.SD Data.Reflection.Z))))))
+data D17
+instance Dim D17 where
+ reflectDim _ = reflect (Proxy :: Proxy (Data.Reflection.SD (Data.Reflection.D (Data.Reflection.D (Data.Reflection.D (Data.Reflection.SD Data.Reflection.Z))))))
+data D18
+instance Dim D18 where
+ reflectDim _ = reflect (Proxy :: Proxy (Data.Reflection.D (Data.Reflection.SD (Data.Reflection.D (Data.Reflection.D (Data.Reflection.SD Data.Reflection.Z))))))
+data D19
+instance Dim D19 where
+ reflectDim _ = reflect (Proxy :: Proxy (Data.Reflection.SD (Data.Reflection.SD (Data.Reflection.D (Data.Reflection.D (Data.Reflection.SD Data.Reflection.Z))))))
+data D20
+instance Dim D20 where
+ reflectDim _ = reflect (Proxy :: Proxy (Data.Reflection.D (Data.Reflection.D (Data.Reflection.SD (Data.Reflection.D (Data.Reflection.SD Data.Reflection.Z))))))
+data D21
+instance Dim D21 where
+ reflectDim _ = reflect (Proxy :: Proxy (Data.Reflection.SD (Data.Reflection.D (Data.Reflection.SD (Data.Reflection.D (Data.Reflection.SD Data.Reflection.Z))))))
+data D22
+instance Dim D22 where
+ reflectDim _ = reflect (Proxy :: Proxy (Data.Reflection.D (Data.Reflection.SD (Data.Reflection.SD (Data.Reflection.D (Data.Reflection.SD Data.Reflection.Z))))))
+data D23
+instance Dim D23 where
+ reflectDim _ = reflect (Proxy :: Proxy (Data.Reflection.SD (Data.Reflection.SD (Data.Reflection.SD (Data.Reflection.D (Data.Reflection.SD Data.Reflection.Z))))))
+data D24
+instance Dim D24 where
+ reflectDim _ = reflect (Proxy :: Proxy (Data.Reflection.D (Data.Reflection.D (Data.Reflection.D (Data.Reflection.SD (Data.Reflection.SD Data.Reflection.Z))))))
+data D25
+instance Dim D25 where
+ reflectDim _ = reflect (Proxy :: Proxy (Data.Reflection.SD (Data.Reflection.D (Data.Reflection.D (Data.Reflection.SD (Data.Reflection.SD Data.Reflection.Z))))))
+data D26
+instance Dim D26 where
+ reflectDim _ = reflect (Proxy :: Proxy (Data.Reflection.D (Data.Reflection.SD (Data.Reflection.D (Data.Reflection.SD (Data.Reflection.SD Data.Reflection.Z))))))
+data D27
+instance Dim D27 where
+ reflectDim _ = reflect (Proxy :: Proxy (Data.Reflection.SD (Data.Reflection.SD (Data.Reflection.D (Data.Reflection.SD (Data.Reflection.SD Data.Reflection.Z))))))
+data D28
+instance Dim D28 where
+ reflectDim _ = reflect (Proxy :: Proxy (Data.Reflection.D (Data.Reflection.D (Data.Reflection.SD (Data.Reflection.SD (Data.Reflection.SD Data.Reflection.Z))))))
+data D29
+instance Dim D29 where
+ reflectDim _ = reflect (Proxy :: Proxy (Data.Reflection.SD (Data.Reflection.D (Data.Reflection.SD (Data.Reflection.SD (Data.Reflection.SD Data.Reflection.Z))))))
+data D30
+instance Dim D30 where
+ reflectDim _ = reflect (Proxy :: Proxy (Data.Reflection.D (Data.Reflection.SD (Data.Reflection.SD (Data.Reflection.SD (Data.Reflection.SD Data.Reflection.Z))))))
+data D31
+instance Dim D31 where
+ reflectDim _ = reflect (Proxy :: Proxy (Data.Reflection.SD (Data.Reflection.SD (Data.Reflection.SD (Data.Reflection.SD (Data.Reflection.SD Data.Reflection.Z))))))
+data D32
+instance Dim D32 where
+ reflectDim _ = reflect (Proxy :: Proxy (Data.Reflection.D (Data.Reflection.D (Data.Reflection.D (Data.Reflection.D (Data.Reflection.D (Data.Reflection.SD Data.Reflection.Z)))))))
+data D33
+instance Dim D33 where
+ reflectDim _ = reflect (Proxy :: Proxy (Data.Reflection.SD (Data.Reflection.D (Data.Reflection.D (Data.Reflection.D (Data.Reflection.D (Data.Reflection.SD Data.Reflection.Z)))))))
+data D34
+instance Dim D34 where
+ reflectDim _ = reflect (Proxy :: Proxy (Data.Reflection.D (Data.Reflection.SD (Data.Reflection.D (Data.Reflection.D (Data.Reflection.D (Data.Reflection.SD Data.Reflection.Z)))))))
+data D35
+instance Dim D35 where
+ reflectDim _ = reflect (Proxy :: Proxy (Data.Reflection.SD (Data.Reflection.SD (Data.Reflection.D (Data.Reflection.D (Data.Reflection.D (Data.Reflection.SD Data.Reflection.Z)))))))
+data D36
+instance Dim D36 where
+ reflectDim _ = reflect (Proxy :: Proxy (Data.Reflection.D (Data.Reflection.D (Data.Reflection.SD (Data.Reflection.D (Data.Reflection.D (Data.Reflection.SD Data.Reflection.Z)))))))
+data D37
+instance Dim D37 where
+ reflectDim _ = reflect (Proxy :: Proxy (Data.Reflection.SD (Data.Reflection.D (Data.Reflection.SD (Data.Reflection.D (Data.Reflection.D (Data.Reflection.SD Data.Reflection.Z)))))))
+data D38
+instance Dim D38 where
+ reflectDim _ = reflect (Proxy :: Proxy (Data.Reflection.D (Data.Reflection.SD (Data.Reflection.SD (Data.Reflection.D (Data.Reflection.D (Data.Reflection.SD Data.Reflection.Z)))))))
+data D39
+instance Dim D39 where
+ reflectDim _ = reflect (Proxy :: Proxy (Data.Reflection.SD (Data.Reflection.SD (Data.Reflection.SD (Data.Reflection.D (Data.Reflection.D (Data.Reflection.SD Data.Reflection.Z)))))))
+data D40
+instance Dim D40 where
+ reflectDim _ = reflect (Proxy :: Proxy (Data.Reflection.D (Data.Reflection.D (Data.Reflection.D (Data.Reflection.SD (Data.Reflection.D (Data.Reflection.SD Data.Reflection.Z)))))))
+data D41
+instance Dim D41 where
+ reflectDim _ = reflect (Proxy :: Proxy (Data.Reflection.SD (Data.Reflection.D (Data.Reflection.D (Data.Reflection.SD (Data.Reflection.D (Data.Reflection.SD Data.Reflection.Z)))))))
+data D42
+instance Dim D42 where
+ reflectDim _ = reflect (Proxy :: Proxy (Data.Reflection.D (Data.Reflection.SD (Data.Reflection.D (Data.Reflection.SD (Data.Reflection.D (Data.Reflection.SD Data.Reflection.Z)))))))
+data D43
+instance Dim D43 where
+ reflectDim _ = reflect (Proxy :: Proxy (Data.Reflection.SD (Data.Reflection.SD (Data.Reflection.D (Data.Reflection.SD (Data.Reflection.D (Data.Reflection.SD Data.Reflection.Z)))))))
+data D44
+instance Dim D44 where
+ reflectDim _ = reflect (Proxy :: Proxy (Data.Reflection.D (Data.Reflection.D (Data.Reflection.SD (Data.Reflection.SD (Data.Reflection.D (Data.Reflection.SD Data.Reflection.Z)))))))
+data D45
+instance Dim D45 where
+ reflectDim _ = reflect (Proxy :: Proxy (Data.Reflection.SD (Data.Reflection.D (Data.Reflection.SD (Data.Reflection.SD (Data.Reflection.D (Data.Reflection.SD Data.Reflection.Z)))))))
+data D46
+instance Dim D46 where
+ reflectDim _ = reflect (Proxy :: Proxy (Data.Reflection.D (Data.Reflection.SD (Data.Reflection.SD (Data.Reflection.SD (Data.Reflection.D (Data.Reflection.SD Data.Reflection.Z)))))))
+data D47
+instance Dim D47 where
+ reflectDim _ = reflect (Proxy :: Proxy (Data.Reflection.SD (Data.Reflection.SD (Data.Reflection.SD (Data.Reflection.SD (Data.Reflection.D (Data.Reflection.SD Data.Reflection.Z)))))))
+data D48
+instance Dim D48 where
+ reflectDim _ = reflect (Proxy :: Proxy (Data.Reflection.D (Data.Reflection.D (Data.Reflection.D (Data.Reflection.D (Data.Reflection.SD (Data.Reflection.SD Data.Reflection.Z)))))))
+data D49
+instance Dim D49 where
+ reflectDim _ = reflect (Proxy :: Proxy (Data.Reflection.SD (Data.Reflection.D (Data.Reflection.D (Data.Reflection.D (Data.Reflection.SD (Data.Reflection.SD Data.Reflection.Z)))))))
+data D50
+instance Dim D50 where
+ reflectDim _ = reflect (Proxy :: Proxy (Data.Reflection.D (Data.Reflection.SD (Data.Reflection.D (Data.Reflection.D (Data.Reflection.SD (Data.Reflection.SD Data.Reflection.Z)))))))
+data D51
+instance Dim D51 where
+ reflectDim _ = reflect (Proxy :: Proxy (Data.Reflection.SD (Data.Reflection.SD (Data.Reflection.D (Data.Reflection.D (Data.Reflection.SD (Data.Reflection.SD Data.Reflection.Z)))))))
+data D52
+instance Dim D52 where
+ reflectDim _ = reflect (Proxy :: Proxy (Data.Reflection.D (Data.Reflection.D (Data.Reflection.SD (Data.Reflection.D (Data.Reflection.SD (Data.Reflection.SD Data.Reflection.Z)))))))
+data D53
+instance Dim D53 where
+ reflectDim _ = reflect (Proxy :: Proxy (Data.Reflection.SD (Data.Reflection.D (Data.Reflection.SD (Data.Reflection.D (Data.Reflection.SD (Data.Reflection.SD Data.Reflection.Z)))))))
+data D54
+instance Dim D54 where
+ reflectDim _ = reflect (Proxy :: Proxy (Data.Reflection.D (Data.Reflection.SD (Data.Reflection.SD (Data.Reflection.D (Data.Reflection.SD (Data.Reflection.SD Data.Reflection.Z)))))))
+data D55
+instance Dim D55 where
+ reflectDim _ = reflect (Proxy :: Proxy (Data.Reflection.SD (Data.Reflection.SD (Data.Reflection.SD (Data.Reflection.D (Data.Reflection.SD (Data.Reflection.SD Data.Reflection.Z)))))))
+data D56
+instance Dim D56 where
+ reflectDim _ = reflect (Proxy :: Proxy (Data.Reflection.D (Data.Reflection.D (Data.Reflection.D (Data.Reflection.SD (Data.Reflection.SD (Data.Reflection.SD Data.Reflection.Z)))))))
+data D57
+instance Dim D57 where
+ reflectDim _ = reflect (Proxy :: Proxy (Data.Reflection.SD (Data.Reflection.D (Data.Reflection.D (Data.Reflection.SD (Data.Reflection.SD (Data.Reflection.SD Data.Reflection.Z)))))))
+data D58
+instance Dim D58 where
+ reflectDim _ = reflect (Proxy :: Proxy (Data.Reflection.D (Data.Reflection.SD (Data.Reflection.D (Data.Reflection.SD (Data.Reflection.SD (Data.Reflection.SD Data.Reflection.Z)))))))
+data D59
+instance Dim D59 where
+ reflectDim _ = reflect (Proxy :: Proxy (Data.Reflection.SD (Data.Reflection.SD (Data.Reflection.D (Data.Reflection.SD (Data.Reflection.SD (Data.Reflection.SD Data.Reflection.Z)))))))
+data D60
+instance Dim D60 where
+ reflectDim _ = reflect (Proxy :: Proxy (Data.Reflection.D (Data.Reflection.D (Data.Reflection.SD (Data.Reflection.SD (Data.Reflection.SD (Data.Reflection.SD Data.Reflection.Z)))))))
+data D61
+instance Dim D61 where
+ reflectDim _ = reflect (Proxy :: Proxy (Data.Reflection.SD (Data.Reflection.D (Data.Reflection.SD (Data.Reflection.SD (Data.Reflection.SD (Data.Reflection.SD Data.Reflection.Z)))))))
+data D62
+instance Dim D62 where
+ reflectDim _ = reflect (Proxy :: Proxy (Data.Reflection.D (Data.Reflection.SD (Data.Reflection.SD (Data.Reflection.SD (Data.Reflection.SD (Data.Reflection.SD Data.Reflection.Z)))))))
+data D63
+instance Dim D63 where
+ reflectDim _ = reflect (Proxy :: Proxy (Data.Reflection.SD (Data.Reflection.SD (Data.Reflection.SD (Data.Reflection.SD (Data.Reflection.SD (Data.Reflection.SD Data.Reflection.Z)))))))
+data D64
+instance Dim D64 where
+ reflectDim _ = reflect (Proxy :: Proxy (Data.Reflection.D (Data.Reflection.D (Data.Reflection.D (Data.Reflection.D (Data.Reflection.D (Data.Reflection.D (Data.Reflection.SD Data.Reflection.Z))))))))
+data D65
+instance Dim D65 where
+ reflectDim _ = reflect (Proxy :: Proxy (Data.Reflection.SD (Data.Reflection.D (Data.Reflection.D (Data.Reflection.D (Data.Reflection.D (Data.Reflection.D (Data.Reflection.SD Data.Reflection.Z))))))))
+data D66
+instance Dim D66 where
+ reflectDim _ = reflect (Proxy :: Proxy (Data.Reflection.D (Data.Reflection.SD (Data.Reflection.D (Data.Reflection.D (Data.Reflection.D (Data.Reflection.D (Data.Reflection.SD Data.Reflection.Z))))))))
+data D67
+instance Dim D67 where
+ reflectDim _ = reflect (Proxy :: Proxy (Data.Reflection.SD (Data.Reflection.SD (Data.Reflection.D (Data.Reflection.D (Data.Reflection.D (Data.Reflection.D (Data.Reflection.SD Data.Reflection.Z))))))))
+data D68
+instance Dim D68 where
+ reflectDim _ = reflect (Proxy :: Proxy (Data.Reflection.D (Data.Reflection.D (Data.Reflection.SD (Data.Reflection.D (Data.Reflection.D (Data.Reflection.D (Data.Reflection.SD Data.Reflection.Z))))))))
+data D69
+instance Dim D69 where
+ reflectDim _ = reflect (Proxy :: Proxy (Data.Reflection.SD (Data.Reflection.D (Data.Reflection.SD (Data.Reflection.D (Data.Reflection.D (Data.Reflection.D (Data.Reflection.SD Data.Reflection.Z))))))))
+data D70
+instance Dim D70 where
+ reflectDim _ = reflect (Proxy :: Proxy (Data.Reflection.D (Data.Reflection.SD (Data.Reflection.SD (Data.Reflection.D (Data.Reflection.D (Data.Reflection.D (Data.Reflection.SD Data.Reflection.Z))))))))
+data D71
+instance Dim D71 where
+ reflectDim _ = reflect (Proxy :: Proxy (Data.Reflection.SD (Data.Reflection.SD (Data.Reflection.SD (Data.Reflection.D (Data.Reflection.D (Data.Reflection.D (Data.Reflection.SD Data.Reflection.Z))))))))
+data D72
+instance Dim D72 where
+ reflectDim _ = reflect (Proxy :: Proxy (Data.Reflection.D (Data.Reflection.D (Data.Reflection.D (Data.Reflection.SD (Data.Reflection.D (Data.Reflection.D (Data.Reflection.SD Data.Reflection.Z))))))))
+data D73
+instance Dim D73 where
+ reflectDim _ = reflect (Proxy :: Proxy (Data.Reflection.SD (Data.Reflection.D (Data.Reflection.D (Data.Reflection.SD (Data.Reflection.D (Data.Reflection.D (Data.Reflection.SD Data.Reflection.Z))))))))
+data D74
+instance Dim D74 where
+ reflectDim _ = reflect (Proxy :: Proxy (Data.Reflection.D (Data.Reflection.SD (Data.Reflection.D (Data.Reflection.SD (Data.Reflection.D (Data.Reflection.D (Data.Reflection.SD Data.Reflection.Z))))))))
+data D75
+instance Dim D75 where
+ reflectDim _ = reflect (Proxy :: Proxy (Data.Reflection.SD (Data.Reflection.SD (Data.Reflection.D (Data.Reflection.SD (Data.Reflection.D (Data.Reflection.D (Data.Reflection.SD Data.Reflection.Z))))))))
+data D76
+instance Dim D76 where
+ reflectDim _ = reflect (Proxy :: Proxy (Data.Reflection.D (Data.Reflection.D (Data.Reflection.SD (Data.Reflection.SD (Data.Reflection.D (Data.Reflection.D (Data.Reflection.SD Data.Reflection.Z))))))))
+data D77
+instance Dim D77 where
+ reflectDim _ = reflect (Proxy :: Proxy (Data.Reflection.SD (Data.Reflection.D (Data.Reflection.SD (Data.Reflection.SD (Data.Reflection.D (Data.Reflection.D (Data.Reflection.SD Data.Reflection.Z))))))))
+data D78
+instance Dim D78 where
+ reflectDim _ = reflect (Proxy :: Proxy (Data.Reflection.D (Data.Reflection.SD (Data.Reflection.SD (Data.Reflection.SD (Data.Reflection.D (Data.Reflection.D (Data.Reflection.SD Data.Reflection.Z))))))))
+data D79
+instance Dim D79 where
+ reflectDim _ = reflect (Proxy :: Proxy (Data.Reflection.SD (Data.Reflection.SD (Data.Reflection.SD (Data.Reflection.SD (Data.Reflection.D (Data.Reflection.D (Data.Reflection.SD Data.Reflection.Z))))))))
+data D80
+instance Dim D80 where
+ reflectDim _ = reflect (Proxy :: Proxy (Data.Reflection.D (Data.Reflection.D (Data.Reflection.D (Data.Reflection.D (Data.Reflection.SD (Data.Reflection.D (Data.Reflection.SD Data.Reflection.Z))))))))
+data D81
+instance Dim D81 where
+ reflectDim _ = reflect (Proxy :: Proxy (Data.Reflection.SD (Data.Reflection.D (Data.Reflection.D (Data.Reflection.D (Data.Reflection.SD (Data.Reflection.D (Data.Reflection.SD Data.Reflection.Z))))))))
+data D82
+instance Dim D82 where
+ reflectDim _ = reflect (Proxy :: Proxy (Data.Reflection.D (Data.Reflection.SD (Data.Reflection.D (Data.Reflection.D (Data.Reflection.SD (Data.Reflection.D (Data.Reflection.SD Data.Reflection.Z))))))))
+data D83
+instance Dim D83 where
+ reflectDim _ = reflect (Proxy :: Proxy (Data.Reflection.SD (Data.Reflection.SD (Data.Reflection.D (Data.Reflection.D (Data.Reflection.SD (Data.Reflection.D (Data.Reflection.SD Data.Reflection.Z))))))))
+data D84
+instance Dim D84 where
+ reflectDim _ = reflect (Proxy :: Proxy (Data.Reflection.D (Data.Reflection.D (Data.Reflection.SD (Data.Reflection.D (Data.Reflection.SD (Data.Reflection.D (Data.Reflection.SD Data.Reflection.Z))))))))
+data D85
+instance Dim D85 where
+ reflectDim _ = reflect (Proxy :: Proxy (Data.Reflection.SD (Data.Reflection.D (Data.Reflection.SD (Data.Reflection.D (Data.Reflection.SD (Data.Reflection.D (Data.Reflection.SD Data.Reflection.Z))))))))
+data D86
+instance Dim D86 where
+ reflectDim _ = reflect (Proxy :: Proxy (Data.Reflection.D (Data.Reflection.SD (Data.Reflection.SD (Data.Reflection.D (Data.Reflection.SD (Data.Reflection.D (Data.Reflection.SD Data.Reflection.Z))))))))
+data D87
+instance Dim D87 where
+ reflectDim _ = reflect (Proxy :: Proxy (Data.Reflection.SD (Data.Reflection.SD (Data.Reflection.SD (Data.Reflection.D (Data.Reflection.SD (Data.Reflection.D (Data.Reflection.SD Data.Reflection.Z))))))))
+data D88
+instance Dim D88 where
+ reflectDim _ = reflect (Proxy :: Proxy (Data.Reflection.D (Data.Reflection.D (Data.Reflection.D (Data.Reflection.SD (Data.Reflection.SD (Data.Reflection.D (Data.Reflection.SD Data.Reflection.Z))))))))
+data D89
+instance Dim D89 where
+ reflectDim _ = reflect (Proxy :: Proxy (Data.Reflection.SD (Data.Reflection.D (Data.Reflection.D (Data.Reflection.SD (Data.Reflection.SD (Data.Reflection.D (Data.Reflection.SD Data.Reflection.Z))))))))
+data D90
+instance Dim D90 where
+ reflectDim _ = reflect (Proxy :: Proxy (Data.Reflection.D (Data.Reflection.SD (Data.Reflection.D (Data.Reflection.SD (Data.Reflection.SD (Data.Reflection.D (Data.Reflection.SD Data.Reflection.Z))))))))
+data D91
+instance Dim D91 where
+ reflectDim _ = reflect (Proxy :: Proxy (Data.Reflection.SD (Data.Reflection.SD (Data.Reflection.D (Data.Reflection.SD (Data.Reflection.SD (Data.Reflection.D (Data.Reflection.SD Data.Reflection.Z))))))))
+data D92
+instance Dim D92 where
+ reflectDim _ = reflect (Proxy :: Proxy (Data.Reflection.D (Data.Reflection.D (Data.Reflection.SD (Data.Reflection.SD (Data.Reflection.SD (Data.Reflection.D (Data.Reflection.SD Data.Reflection.Z))))))))
+data D93
+instance Dim D93 where
+ reflectDim _ = reflect (Proxy :: Proxy (Data.Reflection.SD (Data.Reflection.D (Data.Reflection.SD (Data.Reflection.SD (Data.Reflection.SD (Data.Reflection.D (Data.Reflection.SD Data.Reflection.Z))))))))
+data D94
+instance Dim D94 where
+ reflectDim _ = reflect (Proxy :: Proxy (Data.Reflection.D (Data.Reflection.SD (Data.Reflection.SD (Data.Reflection.SD (Data.Reflection.SD (Data.Reflection.D (Data.Reflection.SD Data.Reflection.Z))))))))
+data D95
+instance Dim D95 where
+ reflectDim _ = reflect (Proxy :: Proxy (Data.Reflection.SD (Data.Reflection.SD (Data.Reflection.SD (Data.Reflection.SD (Data.Reflection.SD (Data.Reflection.D (Data.Reflection.SD Data.Reflection.Z))))))))
+data D96
+instance Dim D96 where
+ reflectDim _ = reflect (Proxy :: Proxy (Data.Reflection.D (Data.Reflection.D (Data.Reflection.D (Data.Reflection.D (Data.Reflection.D (Data.Reflection.SD (Data.Reflection.SD Data.Reflection.Z))))))))
+data D97
+instance Dim D97 where
+ reflectDim _ = reflect (Proxy :: Proxy (Data.Reflection.SD (Data.Reflection.D (Data.Reflection.D (Data.Reflection.D (Data.Reflection.D (Data.Reflection.SD (Data.Reflection.SD Data.Reflection.Z))))))))
+data D98
+instance Dim D98 where
+ reflectDim _ = reflect (Proxy :: Proxy (Data.Reflection.D (Data.Reflection.SD (Data.Reflection.D (Data.Reflection.D (Data.Reflection.D (Data.Reflection.SD (Data.Reflection.SD Data.Reflection.Z))))))))
+data D99
+instance Dim D99 where
+ reflectDim _ = reflect (Proxy :: Proxy (Data.Reflection.SD (Data.Reflection.SD (Data.Reflection.D (Data.Reflection.D (Data.Reflection.D (Data.Reflection.SD (Data.Reflection.SD Data.Reflection.Z))))))))
+data D100
+instance Dim D100 where
+ reflectDim _ = reflect (Proxy :: Proxy (Data.Reflection.D (Data.Reflection.D (Data.Reflection.SD (Data.Reflection.D (Data.Reflection.D (Data.Reflection.SD (Data.Reflection.SD Data.Reflection.Z))))))))
+data D101
+instance Dim D101 where
+ reflectDim _ = reflect (Proxy :: Proxy (Data.Reflection.SD (Data.Reflection.D (Data.Reflection.SD (Data.Reflection.D (Data.Reflection.D (Data.Reflection.SD (Data.Reflection.SD Data.Reflection.Z))))))))
+data D102
+instance Dim D102 where
+ reflectDim _ = reflect (Proxy :: Proxy (Data.Reflection.D (Data.Reflection.SD (Data.Reflection.SD (Data.Reflection.D (Data.Reflection.D (Data.Reflection.SD (Data.Reflection.SD Data.Reflection.Z))))))))
+data D103
+instance Dim D103 where
+ reflectDim _ = reflect (Proxy :: Proxy (Data.Reflection.SD (Data.Reflection.SD (Data.Reflection.SD (Data.Reflection.D (Data.Reflection.D (Data.Reflection.SD (Data.Reflection.SD Data.Reflection.Z))))))))
+data D104
+instance Dim D104 where
+ reflectDim _ = reflect (Proxy :: Proxy (Data.Reflection.D (Data.Reflection.D (Data.Reflection.D (Data.Reflection.SD (Data.Reflection.D (Data.Reflection.SD (Data.Reflection.SD Data.Reflection.Z))))))))
+data D105
+instance Dim D105 where
+ reflectDim _ = reflect (Proxy :: Proxy (Data.Reflection.SD (Data.Reflection.D (Data.Reflection.D (Data.Reflection.SD (Data.Reflection.D (Data.Reflection.SD (Data.Reflection.SD Data.Reflection.Z))))))))
+data D106
+instance Dim D106 where
+ reflectDim _ = reflect (Proxy :: Proxy (Data.Reflection.D (Data.Reflection.SD (Data.Reflection.D (Data.Reflection.SD (Data.Reflection.D (Data.Reflection.SD (Data.Reflection.SD Data.Reflection.Z))))))))
+data D107
+instance Dim D107 where
+ reflectDim _ = reflect (Proxy :: Proxy (Data.Reflection.SD (Data.Reflection.SD (Data.Reflection.D (Data.Reflection.SD (Data.Reflection.D (Data.Reflection.SD (Data.Reflection.SD Data.Reflection.Z))))))))
+data D108
+instance Dim D108 where
+ reflectDim _ = reflect (Proxy :: Proxy (Data.Reflection.D (Data.Reflection.D (Data.Reflection.SD (Data.Reflection.SD (Data.Reflection.D (Data.Reflection.SD (Data.Reflection.SD Data.Reflection.Z))))))))
+data D109
+instance Dim D109 where
+ reflectDim _ = reflect (Proxy :: Proxy (Data.Reflection.SD (Data.Reflection.D (Data.Reflection.SD (Data.Reflection.SD (Data.Reflection.D (Data.Reflection.SD (Data.Reflection.SD Data.Reflection.Z))))))))
+data D110
+instance Dim D110 where
+ reflectDim _ = reflect (Proxy :: Proxy (Data.Reflection.D (Data.Reflection.SD (Data.Reflection.SD (Data.Reflection.SD (Data.Reflection.D (Data.Reflection.SD (Data.Reflection.SD Data.Reflection.Z))))))))
+data D111
+instance Dim D111 where
+ reflectDim _ = reflect (Proxy :: Proxy (Data.Reflection.SD (Data.Reflection.SD (Data.Reflection.SD (Data.Reflection.SD (Data.Reflection.D (Data.Reflection.SD (Data.Reflection.SD Data.Reflection.Z))))))))
+data D112
+instance Dim D112 where
+ reflectDim _ = reflect (Proxy :: Proxy (Data.Reflection.D (Data.Reflection.D (Data.Reflection.D (Data.Reflection.D (Data.Reflection.SD (Data.Reflection.SD (Data.Reflection.SD Data.Reflection.Z))))))))
+data D113
+instance Dim D113 where
+ reflectDim _ = reflect (Proxy :: Proxy (Data.Reflection.SD (Data.Reflection.D (Data.Reflection.D (Data.Reflection.D (Data.Reflection.SD (Data.Reflection.SD (Data.Reflection.SD Data.Reflection.Z))))))))
+data D114
+instance Dim D114 where
+ reflectDim _ = reflect (Proxy :: Proxy (Data.Reflection.D (Data.Reflection.SD (Data.Reflection.D (Data.Reflection.D (Data.Reflection.SD (Data.Reflection.SD (Data.Reflection.SD Data.Reflection.Z))))))))
+data D115
+instance Dim D115 where
+ reflectDim _ = reflect (Proxy :: Proxy (Data.Reflection.SD (Data.Reflection.SD (Data.Reflection.D (Data.Reflection.D (Data.Reflection.SD (Data.Reflection.SD (Data.Reflection.SD Data.Reflection.Z))))))))
+data D116
+instance Dim D116 where
+ reflectDim _ = reflect (Proxy :: Proxy (Data.Reflection.D (Data.Reflection.D (Data.Reflection.SD (Data.Reflection.D (Data.Reflection.SD (Data.Reflection.SD (Data.Reflection.SD Data.Reflection.Z))))))))
+data D117
+instance Dim D117 where
+ reflectDim _ = reflect (Proxy :: Proxy (Data.Reflection.SD (Data.Reflection.D (Data.Reflection.SD (Data.Reflection.D (Data.Reflection.SD (Data.Reflection.SD (Data.Reflection.SD Data.Reflection.Z))))))))
+data D118
+instance Dim D118 where
+ reflectDim _ = reflect (Proxy :: Proxy (Data.Reflection.D (Data.Reflection.SD (Data.Reflection.SD (Data.Reflection.D (Data.Reflection.SD (Data.Reflection.SD (Data.Reflection.SD Data.Reflection.Z))))))))
+data D119
+instance Dim D119 where
+ reflectDim _ = reflect (Proxy :: Proxy (Data.Reflection.SD (Data.Reflection.SD (Data.Reflection.SD (Data.Reflection.D (Data.Reflection.SD (Data.Reflection.SD (Data.Reflection.SD Data.Reflection.Z))))))))
+data D120
+instance Dim D120 where
+ reflectDim _ = reflect (Proxy :: Proxy (Data.Reflection.D (Data.Reflection.D (Data.Reflection.D (Data.Reflection.SD (Data.Reflection.SD (Data.Reflection.SD (Data.Reflection.SD Data.Reflection.Z))))))))
+data D121
+instance Dim D121 where
+ reflectDim _ = reflect (Proxy :: Proxy (Data.Reflection.SD (Data.Reflection.D (Data.Reflection.D (Data.Reflection.SD (Data.Reflection.SD (Data.Reflection.SD (Data.Reflection.SD Data.Reflection.Z))))))))
+data D122
+instance Dim D122 where
+ reflectDim _ = reflect (Proxy :: Proxy (Data.Reflection.D (Data.Reflection.SD (Data.Reflection.D (Data.Reflection.SD (Data.Reflection.SD (Data.Reflection.SD (Data.Reflection.SD Data.Reflection.Z))))))))
+data D123
+instance Dim D123 where
+ reflectDim _ = reflect (Proxy :: Proxy (Data.Reflection.SD (Data.Reflection.SD (Data.Reflection.D (Data.Reflection.SD (Data.Reflection.SD (Data.Reflection.SD (Data.Reflection.SD Data.Reflection.Z))))))))
+data D124
+instance Dim D124 where
+ reflectDim _ = reflect (Proxy :: Proxy (Data.Reflection.D (Data.Reflection.D (Data.Reflection.SD (Data.Reflection.SD (Data.Reflection.SD (Data.Reflection.SD (Data.Reflection.SD Data.Reflection.Z))))))))
+data D125
+instance Dim D125 where
+ reflectDim _ = reflect (Proxy :: Proxy (Data.Reflection.SD (Data.Reflection.D (Data.Reflection.SD (Data.Reflection.SD (Data.Reflection.SD (Data.Reflection.SD (Data.Reflection.SD Data.Reflection.Z))))))))
+data D126
+instance Dim D126 where
+ reflectDim _ = reflect (Proxy :: Proxy (Data.Reflection.D (Data.Reflection.SD (Data.Reflection.SD (Data.Reflection.SD (Data.Reflection.SD (Data.Reflection.SD (Data.Reflection.SD Data.Reflection.Z))))))))
+data D127
+instance Dim D127 where
+ reflectDim _ = reflect (Proxy :: Proxy (Data.Reflection.SD (Data.Reflection.SD (Data.Reflection.SD (Data.Reflection.SD (Data.Reflection.SD (Data.Reflection.SD (Data.Reflection.SD Data.Reflection.Z))))))))
+data D128
+instance Dim D128 where
+ reflectDim _ = reflect (Proxy :: Proxy (Data.Reflection.D (Data.Reflection.D (Data.Reflection.D (Data.Reflection.D (Data.Reflection.D (Data.Reflection.D (Data.Reflection.D (Data.Reflection.SD Data.Reflection.Z)))))))))
+data D129
+instance Dim D129 where
+ reflectDim _ = reflect (Proxy :: Proxy (Data.Reflection.SD (Data.Reflection.D (Data.Reflection.D (Data.Reflection.D (Data.Reflection.D (Data.Reflection.D (Data.Reflection.D (Data.Reflection.SD Data.Reflection.Z)))))))))
+data D130
+instance Dim D130 where
+ reflectDim _ = reflect (Proxy :: Proxy (Data.Reflection.D (Data.Reflection.SD (Data.Reflection.D (Data.Reflection.D (Data.Reflection.D (Data.Reflection.D (Data.Reflection.D (Data.Reflection.SD Data.Reflection.Z)))))))))
+data D131
+instance Dim D131 where
+ reflectDim _ = reflect (Proxy :: Proxy (Data.Reflection.SD (Data.Reflection.SD (Data.Reflection.D (Data.Reflection.D (Data.Reflection.D (Data.Reflection.D (Data.Reflection.D (Data.Reflection.SD Data.Reflection.Z)))))))))
+data D132
+instance Dim D132 where
+ reflectDim _ = reflect (Proxy :: Proxy (Data.Reflection.D (Data.Reflection.D (Data.Reflection.SD (Data.Reflection.D (Data.Reflection.D (Data.Reflection.D (Data.Reflection.D (Data.Reflection.SD Data.Reflection.Z)))))))))
+data D133
+instance Dim D133 where
+ reflectDim _ = reflect (Proxy :: Proxy (Data.Reflection.SD (Data.Reflection.D (Data.Reflection.SD (Data.Reflection.D (Data.Reflection.D (Data.Reflection.D (Data.Reflection.D (Data.Reflection.SD Data.Reflection.Z)))))))))
+data D134
+instance Dim D134 where
+ reflectDim _ = reflect (Proxy :: Proxy (Data.Reflection.D (Data.Reflection.SD (Data.Reflection.SD (Data.Reflection.D (Data.Reflection.D (Data.Reflection.D (Data.Reflection.D (Data.Reflection.SD Data.Reflection.Z)))))))))
+data D135
+instance Dim D135 where
+ reflectDim _ = reflect (Proxy :: Proxy (Data.Reflection.SD (Data.Reflection.SD (Data.Reflection.SD (Data.Reflection.D (Data.Reflection.D (Data.Reflection.D (Data.Reflection.D (Data.Reflection.SD Data.Reflection.Z)))))))))
+data D136
+instance Dim D136 where
+ reflectDim _ = reflect (Proxy :: Proxy (Data.Reflection.D (Data.Reflection.D (Data.Reflection.D (Data.Reflection.SD (Data.Reflection.D (Data.Reflection.D (Data.Reflection.D (Data.Reflection.SD Data.Reflection.Z)))))))))
+data D137
+instance Dim D137 where
+ reflectDim _ = reflect (Proxy :: Proxy (Data.Reflection.SD (Data.Reflection.D (Data.Reflection.D (Data.Reflection.SD (Data.Reflection.D (Data.Reflection.D (Data.Reflection.D (Data.Reflection.SD Data.Reflection.Z)))))))))
+data D138
+instance Dim D138 where
+ reflectDim _ = reflect (Proxy :: Proxy (Data.Reflection.D (Data.Reflection.SD (Data.Reflection.D (Data.Reflection.SD (Data.Reflection.D (Data.Reflection.D (Data.Reflection.D (Data.Reflection.SD Data.Reflection.Z)))))))))
+data D139
+instance Dim D139 where
+ reflectDim _ = reflect (Proxy :: Proxy (Data.Reflection.SD (Data.Reflection.SD (Data.Reflection.D (Data.Reflection.SD (Data.Reflection.D (Data.Reflection.D (Data.Reflection.D (Data.Reflection.SD Data.Reflection.Z)))))))))
+data D140
+instance Dim D140 where
+ reflectDim _ = reflect (Proxy :: Proxy (Data.Reflection.D (Data.Reflection.D (Data.Reflection.SD (Data.Reflection.SD (Data.Reflection.D (Data.Reflection.D (Data.Reflection.D (Data.Reflection.SD Data.Reflection.Z)))))))))
+data D141
+instance Dim D141 where
+ reflectDim _ = reflect (Proxy :: Proxy (Data.Reflection.SD (Data.Reflection.D (Data.Reflection.SD (Data.Reflection.SD (Data.Reflection.D (Data.Reflection.D (Data.Reflection.D (Data.Reflection.SD Data.Reflection.Z)))))))))
+data D142
+instance Dim D142 where
+ reflectDim _ = reflect (Proxy :: Proxy (Data.Reflection.D (Data.Reflection.SD (Data.Reflection.SD (Data.Reflection.SD (Data.Reflection.D (Data.Reflection.D (Data.Reflection.D (Data.Reflection.SD Data.Reflection.Z)))))))))
+data D143
+instance Dim D143 where
+ reflectDim _ = reflect (Proxy :: Proxy (Data.Reflection.SD (Data.Reflection.SD (Data.Reflection.SD (Data.Reflection.SD (Data.Reflection.D (Data.Reflection.D (Data.Reflection.D (Data.Reflection.SD Data.Reflection.Z)))))))))
+data D144
+instance Dim D144 where
+ reflectDim _ = reflect (Proxy :: Proxy (Data.Reflection.D (Data.Reflection.D (Data.Reflection.D (Data.Reflection.D (Data.Reflection.SD (Data.Reflection.D (Data.Reflection.D (Data.Reflection.SD Data.Reflection.Z)))))))))
+data D145
+instance Dim D145 where
+ reflectDim _ = reflect (Proxy :: Proxy (Data.Reflection.SD (Data.Reflection.D (Data.Reflection.D (Data.Reflection.D (Data.Reflection.SD (Data.Reflection.D (Data.Reflection.D (Data.Reflection.SD Data.Reflection.Z)))))))))
+data D146
+instance Dim D146 where
+ reflectDim _ = reflect (Proxy :: Proxy (Data.Reflection.D (Data.Reflection.SD (Data.Reflection.D (Data.Reflection.D (Data.Reflection.SD (Data.Reflection.D (Data.Reflection.D (Data.Reflection.SD Data.Reflection.Z)))))))))
+data D147
+instance Dim D147 where
+ reflectDim _ = reflect (Proxy :: Proxy (Data.Reflection.SD (Data.Reflection.SD (Data.Reflection.D (Data.Reflection.D (Data.Reflection.SD (Data.Reflection.D (Data.Reflection.D (Data.Reflection.SD Data.Reflection.Z)))))))))
+data D148
+instance Dim D148 where
+ reflectDim _ = reflect (Proxy :: Proxy (Data.Reflection.D (Data.Reflection.D (Data.Reflection.SD (Data.Reflection.D (Data.Reflection.SD (Data.Reflection.D (Data.Reflection.D (Data.Reflection.SD Data.Reflection.Z)))))))))
+data D149
+instance Dim D149 where
+ reflectDim _ = reflect (Proxy :: Proxy (Data.Reflection.SD (Data.Reflection.D (Data.Reflection.SD (Data.Reflection.D (Data.Reflection.SD (Data.Reflection.D (Data.Reflection.D (Data.Reflection.SD Data.Reflection.Z)))))))))
+data D150
+instance Dim D150 where
+ reflectDim _ = reflect (Proxy :: Proxy (Data.Reflection.D (Data.Reflection.SD (Data.Reflection.SD (Data.Reflection.D (Data.Reflection.SD (Data.Reflection.D (Data.Reflection.D (Data.Reflection.SD Data.Reflection.Z)))))))))
+data D151
+instance Dim D151 where
+ reflectDim _ = reflect (Proxy :: Proxy (Data.Reflection.SD (Data.Reflection.SD (Data.Reflection.SD (Data.Reflection.D (Data.Reflection.SD (Data.Reflection.D (Data.Reflection.D (Data.Reflection.SD Data.Reflection.Z)))))))))
+data D152
+instance Dim D152 where
+ reflectDim _ = reflect (Proxy :: Proxy (Data.Reflection.D (Data.Reflection.D (Data.Reflection.D (Data.Reflection.SD (Data.Reflection.SD (Data.Reflection.D (Data.Reflection.D (Data.Reflection.SD Data.Reflection.Z)))))))))
+data D153
+instance Dim D153 where
+ reflectDim _ = reflect (Proxy :: Proxy (Data.Reflection.SD (Data.Reflection.D (Data.Reflection.D (Data.Reflection.SD (Data.Reflection.SD (Data.Reflection.D (Data.Reflection.D (Data.Reflection.SD Data.Reflection.Z)))))))))
+data D154
+instance Dim D154 where
+ reflectDim _ = reflect (Proxy :: Proxy (Data.Reflection.D (Data.Reflection.SD (Data.Reflection.D (Data.Reflection.SD (Data.Reflection.SD (Data.Reflection.D (Data.Reflection.D (Data.Reflection.SD Data.Reflection.Z)))))))))
+data D155
+instance Dim D155 where
+ reflectDim _ = reflect (Proxy :: Proxy (Data.Reflection.SD (Data.Reflection.SD (Data.Reflection.D (Data.Reflection.SD (Data.Reflection.SD (Data.Reflection.D (Data.Reflection.D (Data.Reflection.SD Data.Reflection.Z)))))))))
+data D156
+instance Dim D156 where
+ reflectDim _ = reflect (Proxy :: Proxy (Data.Reflection.D (Data.Reflection.D (Data.Reflection.SD (Data.Reflection.SD (Data.Reflection.SD (Data.Reflection.D (Data.Reflection.D (Data.Reflection.SD Data.Reflection.Z)))))))))
+data D157
+instance Dim D157 where
+ reflectDim _ = reflect (Proxy :: Proxy (Data.Reflection.SD (Data.Reflection.D (Data.Reflection.SD (Data.Reflection.SD (Data.Reflection.SD (Data.Reflection.D (Data.Reflection.D (Data.Reflection.SD Data.Reflection.Z)))))))))
+data D158
+instance Dim D158 where
+ reflectDim _ = reflect (Proxy :: Proxy (Data.Reflection.D (Data.Reflection.SD (Data.Reflection.SD (Data.Reflection.SD (Data.Reflection.SD (Data.Reflection.D (Data.Reflection.D (Data.Reflection.SD Data.Reflection.Z)))))))))
+data D159
+instance Dim D159 where
+ reflectDim _ = reflect (Proxy :: Proxy (Data.Reflection.SD (Data.Reflection.SD (Data.Reflection.SD (Data.Reflection.SD (Data.Reflection.SD (Data.Reflection.D (Data.Reflection.D (Data.Reflection.SD Data.Reflection.Z)))))))))
+data D160
+instance Dim D160 where
+ reflectDim _ = reflect (Proxy :: Proxy (Data.Reflection.D (Data.Reflection.D (Data.Reflection.D (Data.Reflection.D (Data.Reflection.D (Data.Reflection.SD (Data.Reflection.D (Data.Reflection.SD Data.Reflection.Z)))))))))
+data D161
+instance Dim D161 where
+ reflectDim _ = reflect (Proxy :: Proxy (Data.Reflection.SD (Data.Reflection.D (Data.Reflection.D (Data.Reflection.D (Data.Reflection.D (Data.Reflection.SD (Data.Reflection.D (Data.Reflection.SD Data.Reflection.Z)))))))))
+data D162
+instance Dim D162 where
+ reflectDim _ = reflect (Proxy :: Proxy (Data.Reflection.D (Data.Reflection.SD (Data.Reflection.D (Data.Reflection.D (Data.Reflection.D (Data.Reflection.SD (Data.Reflection.D (Data.Reflection.SD Data.Reflection.Z)))))))))
+data D163
+instance Dim D163 where
+ reflectDim _ = reflect (Proxy :: Proxy (Data.Reflection.SD (Data.Reflection.SD (Data.Reflection.D (Data.Reflection.D (Data.Reflection.D (Data.Reflection.SD (Data.Reflection.D (Data.Reflection.SD Data.Reflection.Z)))))))))
+data D164
+instance Dim D164 where
+ reflectDim _ = reflect (Proxy :: Proxy (Data.Reflection.D (Data.Reflection.D (Data.Reflection.SD (Data.Reflection.D (Data.Reflection.D (Data.Reflection.SD (Data.Reflection.D (Data.Reflection.SD Data.Reflection.Z)))))))))
+data D165
+instance Dim D165 where
+ reflectDim _ = reflect (Proxy :: Proxy (Data.Reflection.SD (Data.Reflection.D (Data.Reflection.SD (Data.Reflection.D (Data.Reflection.D (Data.Reflection.SD (Data.Reflection.D (Data.Reflection.SD Data.Reflection.Z)))))))))
+data D166
+instance Dim D166 where
+ reflectDim _ = reflect (Proxy :: Proxy (Data.Reflection.D (Data.Reflection.SD (Data.Reflection.SD (Data.Reflection.D (Data.Reflection.D (Data.Reflection.SD (Data.Reflection.D (Data.Reflection.SD Data.Reflection.Z)))))))))
+data D167
+instance Dim D167 where
+ reflectDim _ = reflect (Proxy :: Proxy (Data.Reflection.SD (Data.Reflection.SD (Data.Reflection.SD (Data.Reflection.D (Data.Reflection.D (Data.Reflection.SD (Data.Reflection.D (Data.Reflection.SD Data.Reflection.Z)))))))))
+data D168
+instance Dim D168 where
+ reflectDim _ = reflect (Proxy :: Proxy (Data.Reflection.D (Data.Reflection.D (Data.Reflection.D (Data.Reflection.SD (Data.Reflection.D (Data.Reflection.SD (Data.Reflection.D (Data.Reflection.SD Data.Reflection.Z)))))))))
+data D169
+instance Dim D169 where
+ reflectDim _ = reflect (Proxy :: Proxy (Data.Reflection.SD (Data.Reflection.D (Data.Reflection.D (Data.Reflection.SD (Data.Reflection.D (Data.Reflection.SD (Data.Reflection.D (Data.Reflection.SD Data.Reflection.Z)))))))))
+data D170
+instance Dim D170 where
+ reflectDim _ = reflect (Proxy :: Proxy (Data.Reflection.D (Data.Reflection.SD (Data.Reflection.D (Data.Reflection.SD (Data.Reflection.D (Data.Reflection.SD (Data.Reflection.D (Data.Reflection.SD Data.Reflection.Z)))))))))
+data D171
+instance Dim D171 where
+ reflectDim _ = reflect (Proxy :: Proxy (Data.Reflection.SD (Data.Reflection.SD (Data.Reflection.D (Data.Reflection.SD (Data.Reflection.D (Data.Reflection.SD (Data.Reflection.D (Data.Reflection.SD Data.Reflection.Z)))))))))
+data D172
+instance Dim D172 where
+ reflectDim _ = reflect (Proxy :: Proxy (Data.Reflection.D (Data.Reflection.D (Data.Reflection.SD (Data.Reflection.SD (Data.Reflection.D (Data.Reflection.SD (Data.Reflection.D (Data.Reflection.SD Data.Reflection.Z)))))))))
+data D173
+instance Dim D173 where
+ reflectDim _ = reflect (Proxy :: Proxy (Data.Reflection.SD (Data.Reflection.D (Data.Reflection.SD (Data.Reflection.SD (Data.Reflection.D (Data.Reflection.SD (Data.Reflection.D (Data.Reflection.SD Data.Reflection.Z)))))))))
+data D174
+instance Dim D174 where
+ reflectDim _ = reflect (Proxy :: Proxy (Data.Reflection.D (Data.Reflection.SD (Data.Reflection.SD (Data.Reflection.SD (Data.Reflection.D (Data.Reflection.SD (Data.Reflection.D (Data.Reflection.SD Data.Reflection.Z)))))))))
+data D175
+instance Dim D175 where
+ reflectDim _ = reflect (Proxy :: Proxy (Data.Reflection.SD (Data.Reflection.SD (Data.Reflection.SD (Data.Reflection.SD (Data.Reflection.D (Data.Reflection.SD (Data.Reflection.D (Data.Reflection.SD Data.Reflection.Z)))))))))
+data D176
+instance Dim D176 where
+ reflectDim _ = reflect (Proxy :: Proxy (Data.Reflection.D (Data.Reflection.D (Data.Reflection.D (Data.Reflection.D (Data.Reflection.SD (Data.Reflection.SD (Data.Reflection.D (Data.Reflection.SD Data.Reflection.Z)))))))))
+data D177
+instance Dim D177 where
+ reflectDim _ = reflect (Proxy :: Proxy (Data.Reflection.SD (Data.Reflection.D (Data.Reflection.D (Data.Reflection.D (Data.Reflection.SD (Data.Reflection.SD (Data.Reflection.D (Data.Reflection.SD Data.Reflection.Z)))))))))
+data D178
+instance Dim D178 where
+ reflectDim _ = reflect (Proxy :: Proxy (Data.Reflection.D (Data.Reflection.SD (Data.Reflection.D (Data.Reflection.D (Data.Reflection.SD (Data.Reflection.SD (Data.Reflection.D (Data.Reflection.SD Data.Reflection.Z)))))))))
+data D179
+instance Dim D179 where
+ reflectDim _ = reflect (Proxy :: Proxy (Data.Reflection.SD (Data.Reflection.SD (Data.Reflection.D (Data.Reflection.D (Data.Reflection.SD (Data.Reflection.SD (Data.Reflection.D (Data.Reflection.SD Data.Reflection.Z)))))))))
+data D180
+instance Dim D180 where
+ reflectDim _ = reflect (Proxy :: Proxy (Data.Reflection.D (Data.Reflection.D (Data.Reflection.SD (Data.Reflection.D (Data.Reflection.SD (Data.Reflection.SD (Data.Reflection.D (Data.Reflection.SD Data.Reflection.Z)))))))))
+data D181
+instance Dim D181 where
+ reflectDim _ = reflect (Proxy :: Proxy (Data.Reflection.SD (Data.Reflection.D (Data.Reflection.SD (Data.Reflection.D (Data.Reflection.SD (Data.Reflection.SD (Data.Reflection.D (Data.Reflection.SD Data.Reflection.Z)))))))))
+data D182
+instance Dim D182 where
+ reflectDim _ = reflect (Proxy :: Proxy (Data.Reflection.D (Data.Reflection.SD (Data.Reflection.SD (Data.Reflection.D (Data.Reflection.SD (Data.Reflection.SD (Data.Reflection.D (Data.Reflection.SD Data.Reflection.Z)))))))))
+data D183
+instance Dim D183 where
+ reflectDim _ = reflect (Proxy :: Proxy (Data.Reflection.SD (Data.Reflection.SD (Data.Reflection.SD (Data.Reflection.D (Data.Reflection.SD (Data.Reflection.SD (Data.Reflection.D (Data.Reflection.SD Data.Reflection.Z)))))))))
+data D184
+instance Dim D184 where
+ reflectDim _ = reflect (Proxy :: Proxy (Data.Reflection.D (Data.Reflection.D (Data.Reflection.D (Data.Reflection.SD (Data.Reflection.SD (Data.Reflection.SD (Data.Reflection.D (Data.Reflection.SD Data.Reflection.Z)))))))))
+data D185
+instance Dim D185 where
+ reflectDim _ = reflect (Proxy :: Proxy (Data.Reflection.SD (Data.Reflection.D (Data.Reflection.D (Data.Reflection.SD (Data.Reflection.SD (Data.Reflection.SD (Data.Reflection.D (Data.Reflection.SD Data.Reflection.Z)))))))))
+data D186
+instance Dim D186 where
+ reflectDim _ = reflect (Proxy :: Proxy (Data.Reflection.D (Data.Reflection.SD (Data.Reflection.D (Data.Reflection.SD (Data.Reflection.SD (Data.Reflection.SD (Data.Reflection.D (Data.Reflection.SD Data.Reflection.Z)))))))))
+data D187
+instance Dim D187 where
+ reflectDim _ = reflect (Proxy :: Proxy (Data.Reflection.SD (Data.Reflection.SD (Data.Reflection.D (Data.Reflection.SD (Data.Reflection.SD (Data.Reflection.SD (Data.Reflection.D (Data.Reflection.SD Data.Reflection.Z)))))))))
+data D188
+instance Dim D188 where
+ reflectDim _ = reflect (Proxy :: Proxy (Data.Reflection.D (Data.Reflection.D (Data.Reflection.SD (Data.Reflection.SD (Data.Reflection.SD (Data.Reflection.SD (Data.Reflection.D (Data.Reflection.SD Data.Reflection.Z)))))))))
+data D189
+instance Dim D189 where
+ reflectDim _ = reflect (Proxy :: Proxy (Data.Reflection.SD (Data.Reflection.D (Data.Reflection.SD (Data.Reflection.SD (Data.Reflection.SD (Data.Reflection.SD (Data.Reflection.D (Data.Reflection.SD Data.Reflection.Z)))))))))
+data D190
+instance Dim D190 where
+ reflectDim _ = reflect (Proxy :: Proxy (Data.Reflection.D (Data.Reflection.SD (Data.Reflection.SD (Data.Reflection.SD (Data.Reflection.SD (Data.Reflection.SD (Data.Reflection.D (Data.Reflection.SD Data.Reflection.Z)))))))))
+data D191
+instance Dim D191 where
+ reflectDim _ = reflect (Proxy :: Proxy (Data.Reflection.SD (Data.Reflection.SD (Data.Reflection.SD (Data.Reflection.SD (Data.Reflection.SD (Data.Reflection.SD (Data.Reflection.D (Data.Reflection.SD Data.Reflection.Z)))))))))
+data D192
+instance Dim D192 where
+ reflectDim _ = reflect (Proxy :: Proxy (Data.Reflection.D (Data.Reflection.D (Data.Reflection.D (Data.Reflection.D (Data.Reflection.D (Data.Reflection.D (Data.Reflection.SD (Data.Reflection.SD Data.Reflection.Z)))))))))
+data D193
+instance Dim D193 where
+ reflectDim _ = reflect (Proxy :: Proxy (Data.Reflection.SD (Data.Reflection.D (Data.Reflection.D (Data.Reflection.D (Data.Reflection.D (Data.Reflection.D (Data.Reflection.SD (Data.Reflection.SD Data.Reflection.Z)))))))))
+data D194
+instance Dim D194 where
+ reflectDim _ = reflect (Proxy :: Proxy (Data.Reflection.D (Data.Reflection.SD (Data.Reflection.D (Data.Reflection.D (Data.Reflection.D (Data.Reflection.D (Data.Reflection.SD (Data.Reflection.SD Data.Reflection.Z)))))))))
+data D195
+instance Dim D195 where
+ reflectDim _ = reflect (Proxy :: Proxy (Data.Reflection.SD (Data.Reflection.SD (Data.Reflection.D (Data.Reflection.D (Data.Reflection.D (Data.Reflection.D (Data.Reflection.SD (Data.Reflection.SD Data.Reflection.Z)))))))))
+data D196
+instance Dim D196 where
+ reflectDim _ = reflect (Proxy :: Proxy (Data.Reflection.D (Data.Reflection.D (Data.Reflection.SD (Data.Reflection.D (Data.Reflection.D (Data.Reflection.D (Data.Reflection.SD (Data.Reflection.SD Data.Reflection.Z)))))))))
+data D197
+instance Dim D197 where
+ reflectDim _ = reflect (Proxy :: Proxy (Data.Reflection.SD (Data.Reflection.D (Data.Reflection.SD (Data.Reflection.D (Data.Reflection.D (Data.Reflection.D (Data.Reflection.SD (Data.Reflection.SD Data.Reflection.Z)))))))))
+data D198
+instance Dim D198 where
+ reflectDim _ = reflect (Proxy :: Proxy (Data.Reflection.D (Data.Reflection.SD (Data.Reflection.SD (Data.Reflection.D (Data.Reflection.D (Data.Reflection.D (Data.Reflection.SD (Data.Reflection.SD Data.Reflection.Z)))))))))
+data D199
+instance Dim D199 where
+ reflectDim _ = reflect (Proxy :: Proxy (Data.Reflection.SD (Data.Reflection.SD (Data.Reflection.SD (Data.Reflection.D (Data.Reflection.D (Data.Reflection.D (Data.Reflection.SD (Data.Reflection.SD Data.Reflection.Z)))))))))
+data D200
+instance Dim D200 where
+ reflectDim _ = reflect (Proxy :: Proxy (Data.Reflection.D (Data.Reflection.D (Data.Reflection.D (Data.Reflection.SD (Data.Reflection.D (Data.Reflection.D (Data.Reflection.SD (Data.Reflection.SD Data.Reflection.Z)))))))))
+data D500
+instance Dim D500 where
+ reflectDim _ = reflect (Proxy :: Proxy (Data.Reflection.D (Data.Reflection.D (Data.Reflection.SD (Data.Reflection.D (Data.Reflection.SD (Data.Reflection.SD (Data.Reflection.SD (Data.Reflection.SD (Data.Reflection.SD Data.Reflection.Z))))))))))
+data D1000
+instance Dim D1000 where
+ reflectDim _ = reflect (Proxy :: Proxy (Data.Reflection.D (Data.Reflection.D (Data.Reflection.D (Data.Reflection.SD (Data.Reflection.D (Data.Reflection.SD (Data.Reflection.SD (Data.Reflection.SD (Data.Reflection.SD (Data.Reflection.SD Data.Reflection.Z)))))))))))
+data D1500
+instance Dim D1500 where
+ reflectDim _ = reflect (Proxy :: Proxy (Data.Reflection.D (Data.Reflection.D (Data.Reflection.SD (Data.Reflection.SD (Data.Reflection.SD (Data.Reflection.D (Data.Reflection.SD (Data.Reflection.SD (Data.Reflection.SD (Data.Reflection.D (Data.Reflection.SD Data.Reflection.Z))))))))))))
+data D2000
+instance Dim D2000 where
+ reflectDim _ = reflect (Proxy :: Proxy (Data.Reflection.D (Data.Reflection.D (Data.Reflection.D (Data.Reflection.D (Data.Reflection.SD (Data.Reflection.D (Data.Reflection.SD (Data.Reflection.SD (Data.Reflection.SD (Data.Reflection.SD (Data.Reflection.SD Data.Reflection.Z))))))))))))
+
diff --git a/src/Dyno/Nlp.hs b/src/Dyno/Nlp.hs
new file mode 100644
--- /dev/null
+++ b/src/Dyno/Nlp.hs
@@ -0,0 +1,85 @@
+{-# OPTIONS_GHC -Wall #-}
+{-# Language FlexibleInstances #-}
+{-# Language DeriveFunctor #-}
+{-# Language DeriveGeneric #-}
+
+module Dyno.Nlp
+ ( Bounds
+ , Nlp(..), NlpOut(..)
+ , Nlp'(..), NlpOut'(..)
+ ) where
+
+import GHC.Generics ( Generic, Generic1 )
+import qualified Data.Vector as V
+import Data.Serialize ( Serialize(..) )
+
+import Dyno.Vectorize ( Vectorize(..) )
+import Dyno.View.View ( View(..), J, S, unJ, mkJ )
+
+type Bounds = (Maybe Double, Maybe Double)
+
+-- | user-friendly NLP
+--
+-- > minimize f(x,p)
+-- > x
+-- >
+-- > subject to xlb <= x <= xub
+-- > glb <= g(x) <= gub
+--
+-- where p is some parameter
+--
+data Nlp x p g a =
+ Nlp
+ { nlpFG :: x a -> p a -> (a, g a)
+ , nlpBX :: x Bounds
+ , nlpBG :: g Bounds
+ , nlpX0 :: x Double
+ , nlpP :: p Double
+ , nlpLamX0 :: Maybe (x Double)
+ , nlpLamG0 :: Maybe (g Double)
+ , nlpScaleF :: Maybe Double
+ , nlpScaleX :: Maybe (x Double)
+ , nlpScaleG :: Maybe (g Double)
+ }
+
+data NlpOut x g a =
+ NlpOut
+ { fOpt :: a
+ , xOpt :: x a
+ , gOpt :: g a
+ , lambdaXOpt :: x a
+ , lambdaGOpt :: g a
+ } deriving (Eq, Show, Functor, Generic, Generic1)
+instance (Vectorize x, Vectorize g) => Vectorize (NlpOut x g)
+instance (Vectorize x, Vectorize g, Serialize a) => Serialize (NlpOut x g a) where
+ put = put . V.toList . vectorize
+ get = fmap (devectorize . V.fromList) get
+
+-- | NLP using Views
+data NlpOut' x g a =
+ NlpOut'
+ { fOpt' :: J S a
+ , xOpt' :: J x a
+ , gOpt' :: J g a
+ , lambdaXOpt' :: J x a
+ , lambdaGOpt' :: J g a
+ } deriving (Eq, Show, Generic)
+instance (View x, View g) => View (NlpOut' x g)
+instance (View x, View g, Serialize a) => Serialize (NlpOut' x g (V.Vector a)) where
+ put = put . V.toList . unJ . cat
+ get = fmap (split . mkJ . V.fromList) get
+
+
+data Nlp' x p g a =
+ Nlp'
+ { nlpFG' :: J x a -> J p a -> (J S a, J g a)
+ , nlpBX' :: J x (V.Vector Bounds)
+ , nlpBG' :: J g (V.Vector Bounds)
+ , nlpX0' :: J x (V.Vector Double)
+ , nlpP' :: J p (V.Vector Double)
+ , nlpLamX0' :: Maybe (J x (V.Vector Double))
+ , nlpLamG0' :: Maybe (J g (V.Vector Double))
+ , nlpScaleF' :: Maybe Double
+ , nlpScaleX' :: Maybe (J x (V.Vector Double))
+ , nlpScaleG' :: Maybe (J g (V.Vector Double))
+ }
diff --git a/src/Dyno/NlpMonad.hs b/src/Dyno/NlpMonad.hs
new file mode 100644
--- /dev/null
+++ b/src/Dyno/NlpMonad.hs
@@ -0,0 +1,231 @@
+{-# OPTIONS_GHC -Wall #-}
+{-# Language ScopedTypeVariables #-}
+{-# Language PackageImports #-}
+{-# Language GeneralizedNewtypeDeriving #-}
+{-# Language RankNTypes #-}
+
+module Dyno.NlpMonad
+ ( NlpMonad
+ , (===)
+ , (<==)
+ , (>==)
+ , bound
+ , minimize
+ , designVar
+ , solveStaticNlp
+ ) where
+
+import Control.Applicative ( Applicative )
+import Control.Monad ( when )
+import "mtl" Control.Monad.Reader ( MonadIO(..) )
+import "mtl" Control.Monad.Except ( ExceptT, MonadError, runExceptT )
+import "mtl" Control.Monad.State ( StateT, MonadState, runStateT, get, put )
+import "mtl" Control.Monad.Writer ( WriterT, MonadWriter, runWriterT )
+import qualified Data.Foldable as F
+import qualified Data.HashSet as HS
+import qualified Data.Sequence as S
+import qualified Data.Map.Strict as M
+import Data.Sequence ( (|>) )
+import Data.Vector ( Vector )
+import qualified Data.Vector as V
+import Linear.V ( Dim(..) )
+import Data.Proxy
+
+import Casadi.SharedObject ( soInit )
+import Casadi.MX ( MX )
+import Casadi.SXFunction
+import Casadi.Function
+
+import Dyno.View.CasadiMat ( veccat )
+import Dyno.SXElement ( SXElement, sxElementSym, sxElementToSX )
+import Dyno.Vectorize
+import Dyno.TypeVecs ( Vec )
+import Dyno.View.View
+import qualified Dyno.TypeVecs as TV
+import Dyno.Interface.LogsAndErrors
+import Dyno.Interface.Types
+import Dyno.NlpSolver ( NlpSolverStuff, solveNlp' )
+import Dyno.Nlp ( Nlp'(..), NlpOut'(..), Bounds)
+
+--withEllipse :: Int -> String -> String
+--withEllipse n blah
+-- | length blah <= n = blah
+-- | otherwise = take n blah ++ "..."
+
+newtype NlpMonad a =
+ NlpMonad
+ { runNlp :: ExceptT ErrorMessage (WriterT [LogMessage] (StateT NlpMonadState IO)) a
+ } deriving ( Functor
+ , Applicative
+ , Monad
+ , MonadError ErrorMessage
+ , MonadState NlpMonadState
+ , MonadWriter [LogMessage]
+ , MonadIO
+ )
+
+emptySymbolicNlp :: NlpMonadState
+emptySymbolicNlp = NlpMonadState S.empty HS.empty S.empty ObjectiveUnset HomotopyParamUnset
+
+build :: NlpMonad a -> IO (Either ErrorMessage a, [LogMessage], NlpMonadState)
+build = build' emptySymbolicNlp
+ where
+ build' :: NlpMonadState -> NlpMonad a -> IO (Either ErrorMessage a, [LogMessage], NlpMonadState)
+ build' nlp0 builder = do
+ ((result,logs),state) <- flip runStateT nlp0 . runWriterT . runExceptT . runNlp $ builder
+ return (result, logs, state)
+
+designVar :: String -> NlpMonad SXElement
+designVar name = do
+ debug $ "adding design variable \""++name++"\""
+ state0 <- get
+ let map0 = nlpXSet state0
+ sym <- liftIO (sxElementSym name)
+ when (HS.member name map0) $ err $ name ++ " already in symbol map"
+ let state1 = state0 { nlpX = nlpX state0 |> (name, sym)
+ , nlpXSet = HS.insert name map0
+ }
+ put state1
+ return sym
+
+infix 4 ===
+(===) :: SXElement -> SXElement -> NlpMonad ()
+(===) lhs rhs = do
+ debug $ "adding equality constraint: "
+-- ++ withEllipse 30 (show lhs) ++ " == " ++ withEllipse 30 (show rhs)
+ state0 <- get
+ put $ state0 { nlpConstraints = nlpConstraints state0 |> Eq2 lhs rhs }
+
+infix 4 <==
+(<==) :: SXElement -> SXElement -> NlpMonad ()
+(<==) lhs rhs = do
+ debug $ "adding inequality constraint: "
+-- ++ withEllipse 30 (show lhs) ++ " <= " ++ withEllipse 30 (show rhs)
+ state0 <- get
+ put $ state0 { nlpConstraints = nlpConstraints state0 |> Ineq2 lhs rhs }
+
+infix 4 >==
+(>==) :: SXElement -> SXElement -> NlpMonad ()
+(>==) lhs rhs = do
+ debug $ "adding inequality constraint: "
+-- ++ withEllipse 30 (show lhs) ++ " >= " ++ withEllipse 30 (show rhs)
+ state0 <- get
+ put $ state0 { nlpConstraints = nlpConstraints state0 |> Ineq2 rhs lhs }
+
+bound :: SXElement -> (Double,Double) -> NlpMonad ()
+bound mid (lhs, rhs) = do
+ debug $ "adding inequality bound: " -- ++
+-- withEllipse 30 (show lhs) ++ " <= " ++
+-- withEllipse 30 (show mid) ++ " <= " ++
+-- withEllipse 30 (show rhs)
+ state0 <- get
+ put $ state0 { nlpConstraints = nlpConstraints state0 |> Ineq3 mid (lhs, rhs) }
+
+minimize :: SXElement -> NlpMonad ()
+minimize obj = do
+ debug $ "setting objective function: " -- ++ withEllipse 30 (show obj)
+ state0 <- get
+ case nlpObj state0 of
+ Objective _x -> err $ init $ unlines
+ [ "you set the objective function twice"
+-- , " old val: " ++ show x
+-- , " new val: " ++ show obj
+ ]
+ ObjectiveUnset -> put $ state0 { nlpObj = Objective obj }
+
+
+constr :: Constraint SXElement -> (SXElement, Bounds)
+constr (Eq2 lhs rhs) = (lhs - rhs, (Just 0, Just 0))
+constr (Ineq2 lhs rhs) = (lhs - rhs, (Nothing, Just 0))
+constr (Ineq3 x (lhs,rhs)) = (x, (Just lhs, Just rhs))
+
+
+toG :: Dim ng => S.Seq (Constraint SXElement) -> Vec ng (SXElement, Bounds)
+toG nlpConstraints' = TV.mkSeq $ fmap constr nlpConstraints'
+
+buildNlp :: forall nx ng .
+ (Dim nx, Dim ng) => NlpMonadState -> IO (Nlp' (JVec nx S) JNone (JVec ng S) MX)
+buildNlp state = do
+ obj <- case nlpObj state of
+ Objective obj' -> return obj'
+ ObjectiveUnset -> error "solveNlp: objective unset"
+
+ let inputs :: Vector SXElement
+ inputs = V.fromList $ map snd $ F.toList (nlpX state)
+
+ g :: Vec ng SXElement
+ gbnd :: Vec ng Bounds
+ (g, gbnd) = TV.tvunzip $ toG (nlpConstraints state)
+
+ xbnd :: Vec nx Bounds
+ xbnd = fill (Nothing, Nothing)
+
+ svector = veccat . fmap sxElementToSX
+
+ sxfun <- sxFunction (V.fromList [svector inputs]) (V.fromList [svector (V.singleton obj), svector (TV.unVec g)])
+ soInit sxfun
+ let fg :: J (JVec nx S) MX -> J JNone MX -> (J S MX, J (JVec ng S) MX)
+ fg x _ = (mkJ (ret V.! 0), mkJ (ret V.! 1))
+ where
+ ret = callMX sxfun (V.singleton (unJ x))
+
+ return Nlp' { nlpFG' = fg
+ , nlpBX' = mkJ (TV.unVec xbnd)
+ , nlpBG' = mkJ (TV.unVec gbnd)
+ , nlpX0' = jfill 0
+ , nlpP' = cat JNone
+ , nlpScaleF' = Nothing
+ , nlpScaleX' = Nothing
+ , nlpScaleG' = Nothing
+ , nlpLamX0' = Nothing
+ , nlpLamG0' = Nothing
+ }
+
+
+reifyNlp ::
+ forall r .
+ NlpMonad () -> Maybe (Vector Double -> IO Bool) -> M.Map String Double
+ -> (forall x g . (View x, View g)
+ => Nlp' x JNone g MX -> Maybe (J x (Vector Double) -> IO Bool) -> NlpMonadState -> IO r)
+ -> IO r
+reifyNlp nlpmonad cb x0map f = do
+ (ret,logs,state) <- build nlpmonad
+ case ret of
+ Right _ -> return ()
+ Left err' -> error $ unlines $ map show logs ++ [show err']
+
+ let nx = S.length (nlpX state)
+ ng = S.length (nlpConstraints state)
+
+ lookupGuess = flip (M.findWithDefault 0) x0map
+ x0 = V.fromList $ map (lookupGuess . fst) $ F.toList (nlpX state)
+
+ TV.reifyDim nx $ \(Proxy :: Proxy nx) ->
+-- TV.reifyDim np $ \(Proxy :: Proxy np) ->
+ TV.reifyDim ng $ \(Proxy :: Proxy ng) -> do
+ nlp0 <- buildNlp state :: IO (Nlp' (JVec nx S) JNone (JVec ng S) MX)
+ let nlp = nlp0 { nlpX0' = mkJ x0 }
+ f nlp (fmap (. unJ) cb) state
+
+
+solveStaticNlp ::
+ NlpSolverStuff
+ -> NlpMonad () -> [(String,Double)] -> Maybe (Vector Double -> IO Bool)
+ -> IO (Either String String, Double, [(String,Double)])
+solveStaticNlp solverStuff nlp x0' callback = reifyNlp nlp callback x0 foo
+ where
+ x0 = M.fromListWithKey errlol x0'
+ errlol name xx yy =
+ error $ "solveStaticNlp: initial guess has variable \"" ++ name ++ "\" more than once: " ++
+ show (xx,yy)
+
+ foo ::
+ (View x, View p, View g) =>
+ Nlp' x p g MX -> Maybe (J x (Vector Double) -> IO Bool) -> NlpMonadState ->
+ IO (Either String String, Double, [(String,Double)])
+ foo nlp' cb' state = do
+ (ret,nlpOut) <- solveNlp' solverStuff nlp' cb'
+ let fopt = V.head (unJ (fOpt' nlpOut)) :: Double
+ xopt = F.toList $ unJ (xOpt' nlpOut) :: [Double]
+ xnames = map fst (F.toList (nlpX state)) :: [String]
+ return (ret, fopt, zip xnames xopt)
diff --git a/src/Dyno/NlpScaling.hs b/src/Dyno/NlpScaling.hs
new file mode 100644
--- /dev/null
+++ b/src/Dyno/NlpScaling.hs
@@ -0,0 +1,121 @@
+{-# OPTIONS_GHC -Wall #-}
+{-# Language ScopedTypeVariables #-}
+
+module Dyno.NlpScaling
+ ( ScaleFuns(..)
+ , scaledFG
+ , mkScaleFuns
+ ) where
+
+import Data.Maybe ( fromMaybe )
+import qualified Data.Vector as V
+
+import Dyno.View.View
+import Dyno.View.Viewable ( Viewable )
+import Dyno.View.CasadiMat ( CasadiMat(..) )
+
+data ScaleFuns x g a =
+ ScaleFuns
+ { fToFBar :: J S a -> J S a
+ , fbarToF :: J S a -> J S a
+ , xToXBar :: J x a -> J x a
+ , xbarToX :: J x a -> J x a
+ , gToGBar :: J g a -> J g a
+ , gbarToG :: J g a -> J g a
+ , lamXToLamXBar :: J x a -> J x a
+ , lamXBarToLamX :: J x a -> J x a
+ , lamGToLamGBar :: J g a -> J g a
+ , lamGBarToLamG :: J g a -> J g a
+ }
+
+scaledFG ::
+ forall x p g a .
+ (View x, View g, CasadiMat a, Viewable a)
+ => ScaleFuns x g a
+ -> (J x a -> J p a -> (J S a, J g a))
+ -> J x a
+ -> J p a
+ -> (J S a, J g a)
+scaledFG scaleFuns fg x p = (fToFBar scaleFuns f, gToGBar scaleFuns g)
+ where
+ (f, g) = fg (xbarToX scaleFuns x) p
+
+allPositive :: Maybe (V.Vector Double) -> Bool
+allPositive = all (> 0) . fromMaybe [] . fmap V.toList
+
+mkScaleFuns ::
+ forall x g a .
+ (View x, View g, CasadiMat a, Viewable a)
+ => Maybe (J x (V.Vector Double))
+ -> Maybe (J g (V.Vector Double))
+ -> Maybe Double
+ -> ScaleFuns x g a
+mkScaleFuns mx mg mf
+ | any (not . allPositive)
+ [ fmap unJ mx
+ , fmap unJ mg
+ , fmap V.singleton mf
+ ] = error "all scaling factors must be positive"
+ | otherwise =
+ ScaleFuns { fToFBar = divByFScale
+ , fbarToF = mulByFScale
+ , xToXBar = divByXScale
+ , xbarToX = mulByXScale
+ , gToGBar = divByGScale
+ , gbarToG = mulByGScale
+ , lamXToLamXBar = lamXToLamXBar'
+ , lamXBarToLamX = lamXBarToLamX'
+ , lamGToLamGBar = lamGToLamGBar'
+ , lamGBarToLamG = lamGBarToLamG'
+ }
+ where
+ (lamXToLamXBar', lamXBarToLamX') = case mf of
+ Nothing -> (mulByXScale, divByXScale)
+ Just fscl -> ( \lamx -> mkJ ((unJ (mulByXScale lamx)) / fs)
+ , \lamx -> mkJ ((unJ (divByXScale lamx)) * fs)
+ )
+ where
+ fs :: a
+ fs = fromDVector (V.singleton fscl)
+
+ (lamGToLamGBar', lamGBarToLamG') = case mf of
+ Nothing -> (mulByGScale, divByGScale)
+ Just fscl -> ( \lamg -> mkJ ((unJ (mulByGScale lamg)) / fs)
+ , \lamg -> mkJ ((unJ (divByGScale lamg)) * fs)
+ )
+ where
+ fs :: a
+ fs = fromDVector (V.singleton fscl)
+
+ mulByXScale :: J x a -> J x a
+ divByXScale :: J x a -> J x a
+ (mulByXScale, divByXScale) = case mx of
+ Nothing -> (id, id)
+ Just xscl -> ( \(UnsafeJ x') -> mkJ (x' * s)
+ , \(UnsafeJ x') -> mkJ (x' / s)
+ )
+ where
+ s :: a
+ s = fromDVector (unJ xscl)
+
+ mulByGScale :: J g a -> J g a
+ divByGScale :: J g a -> J g a
+ (mulByGScale, divByGScale) = case mg of
+ Nothing -> (id, id)
+ Just gscl -> ( \(UnsafeJ g') -> mkJ (g' * s)
+ , \(UnsafeJ g') -> mkJ (g' / s)
+ )
+ where
+ s :: a
+ s = fromDVector (unJ gscl)
+
+ mulByFScale :: J S a -> J S a
+ divByFScale :: J S a -> J S a
+ (mulByFScale, divByFScale) = case mf of
+ Nothing -> (id, id)
+ Just fscl -> ( \(UnsafeJ f') -> mkJ (f' * s)
+ , \(UnsafeJ f') -> mkJ (f' / s)
+ )
+ where
+ s :: a
+ s = fromDVector (V.singleton fscl)
diff --git a/src/Dyno/NlpSolver.hs b/src/Dyno/NlpSolver.hs
new file mode 100644
--- /dev/null
+++ b/src/Dyno/NlpSolver.hs
@@ -0,0 +1,595 @@
+{-# OPTIONS_GHC -Wall #-}
+{-# Language ScopedTypeVariables #-}
+{-# Language PackageImports #-}
+{-# Language KindSignatures #-}
+{-# Language GeneralizedNewtypeDeriving #-}
+{-# Language MultiWayIf #-}
+
+module Dyno.NlpSolver
+ ( NlpSolver
+ , SXElement
+ , runNlpSolver
+ -- * solve
+ , solveNlp
+ , solveNlp'
+ , solveNlpHomotopy'
+ , solve
+ , solve'
+ -- * inputs
+ , setX0
+ , setP
+ , setLbx
+ , setUbx
+ , setLbg
+ , setUbg
+ , setLamX0
+ , setLamG0
+ , getX0
+ , getP
+ , getLbx
+ , getUbx
+ , getLbg
+ , getUbg
+ , getLamX0
+ , getLamG0
+ -- * outputs
+ , getF
+ , getX
+ , getG
+ , getLamX
+ , getLamG
+ , NlpSolverStuff(..)
+ -- * options
+ , Op.Opt(..)
+ , setOption
+ , reinit
+ , liftIO
+ , generateAndCompile
+ ) where
+
+import System.Process ( callProcess, showCommandForUser )
+import Control.Exception ( AsyncException( UserInterrupt ), try )
+import Control.Concurrent ( forkIO, newEmptyMVar, takeMVar, putMVar )
+import Control.Applicative ( Applicative(..) )
+import Control.Monad ( when, void )
+import "mtl" Control.Monad.Reader ( MonadIO(..), MonadReader(..), ReaderT(..) )
+import Data.Maybe ( fromMaybe )
+import Data.IORef ( newIORef, readIORef, writeIORef )
+import Data.Vector ( Vector )
+import qualified Data.Vector as V
+import System.IO ( hFlush, stdout )
+import Text.Printf ( printf )
+
+import Casadi.Core.Enums ( InputOutputScheme(..) )
+import qualified Casadi.Core.Classes.Function as C
+import qualified Casadi.Core.Classes.NlpSolver as C
+import qualified Casadi.Core.Classes.GenericType as C
+import qualified Casadi.Core.Classes.IOInterfaceFunction as C
+
+import Casadi.Callback ( makeCallback )
+import Casadi.DMatrix
+import Casadi.SX
+import Casadi.Function ( Function, externalFunction )
+import qualified Casadi.Option as Op
+import qualified Casadi.GenericC as Gen
+import Casadi.SharedObject ( soInit )
+
+import Dyno.SXElement ( SXElement, sxElementToSX )
+import Dyno.Vectorize ( Vectorize(..) )
+import Dyno.View.JV
+import Dyno.View.View
+import Dyno.View.Symbolic
+import Dyno.View.Viewable ( Viewable )
+import Dyno.View.CasadiMat ( CasadiMat )
+import qualified Dyno.View.CasadiMat as CM
+import Dyno.Nlp ( Nlp(..), NlpOut(..), Nlp'(..), NlpOut'(..), Bounds )
+import Dyno.NlpScaling ( ScaleFuns(..), scaledFG, mkScaleFuns )
+import Data.Proxy
+
+type VD a = J a (Vector Double)
+type VMD a = J a (Vector (Maybe Double))
+
+data NlpSolverStuff =
+ NlpSolverStuff
+ { solverName :: String
+ , defaultOptions :: [(String,Op.Opt)]
+ , options :: [(String,Op.Opt)]
+ , solverInterruptCode :: Int
+ , successCodes :: [String]
+ , functionOptions :: [(String, Op.Opt)]
+ , functionCall :: C.Function -> IO ()
+ }
+
+getStat :: String -> NlpSolver x p g C.GenericType
+getStat name = do
+ nlpState <- ask
+ liftIO $ C.function_getStat (isSolver nlpState) name
+
+setInput ::
+ View xg
+ => (ScaleFuns x g DMatrix -> (J xg DMatrix -> J xg DMatrix))
+ -> (NlpState x g -> Int)
+ -> String
+ -> J xg (V.Vector Double)
+ -> NlpSolver x p g ()
+setInput scaleFun getLen name x0 = do
+ nlpState <- ask
+ let x = unJ $ scaleFun (isScale nlpState) $ mkJ $ CM.fromDVector (unJ x0)
+ let nActual = (dsize1 x, dsize2 x)
+ nTypeLevel = (getLen nlpState, 1)
+ when (nTypeLevel /= nActual) $ error $
+ name ++ " dimension mismatch, " ++ show nTypeLevel ++
+ " (type-level) /= " ++ show nActual ++ " (given)"
+ liftIO $ C.ioInterfaceFunction_setInput__0 (isSolver nlpState) x name
+ return ()
+
+setX0 :: forall x p g. View x => VD x -> NlpSolver x p g ()
+setX0 = setInput xToXBar isNx "x0"
+
+inf :: Double
+inf = read "Infinity"
+
+toLb :: View x => J x (Vector (Maybe Double)) -> J x (Vector Double)
+toLb = mkJ . V.map (fromMaybe (-inf)) . unJ
+
+toUb :: View x => J x (Vector (Maybe Double)) -> J x (Vector Double)
+toUb = mkJ . V.map (fromMaybe inf ) . unJ
+
+setLbx :: View x => VMD x -> NlpSolver x p g ()
+setLbx = setInput xToXBar isNx "lbx" . toLb
+
+setUbx :: View x => VMD x -> NlpSolver x p g ()
+setUbx = setInput xToXBar isNx "ubx" . toUb
+
+setLbg :: View g => VMD g -> NlpSolver x p g ()
+setLbg = setInput gToGBar isNg "lbg" . toLb
+
+setUbg :: View g => VMD g -> NlpSolver x p g ()
+setUbg = setInput gToGBar isNg "ubg" . toUb
+
+setP :: View p => VD p -> NlpSolver x p g ()
+setP = setInput (const id) isNp "p"
+
+setLamX0 :: View x => VD x -> NlpSolver x p g ()
+setLamX0 = setInput lamXToLamXBar isNx "lam_x0"
+
+setLamG0 :: View g => VD g -> NlpSolver x p g ()
+setLamG0 = setInput lamGToLamGBar isNg "lam_g0"
+
+getInput ::
+ View xg
+ => (ScaleFuns x g DMatrix -> (J xg DMatrix -> J xg DMatrix))
+ -> String -> NlpSolver x p g (J xg (Vector Double))
+getInput scaleFun name = do
+ nlpState <- ask
+ dmat <- liftIO $ C.ioInterfaceFunction_input__0 (isSolver nlpState) name
+ let scale = scaleFun (isScale nlpState)
+ return (mkJ $ ddata $ unJ $ scale (mkJ dmat))
+
+getX0 :: View x => NlpSolver x p g (VD x)
+getX0 = getInput xbarToX "x0"
+
+getLbx :: View x => NlpSolver x p g (VD x)
+getLbx = getInput xbarToX "lbx"
+
+getUbx :: View x => NlpSolver x p g (VD x)
+getUbx = getInput xbarToX "ubx"
+
+getLbg :: View g => NlpSolver x p g (VD g)
+getLbg = getInput gbarToG "lbg"
+
+getUbg :: View g => NlpSolver x p g (VD g)
+getUbg = getInput gbarToG "ubg"
+
+getP :: View p => NlpSolver x p g (VD p)
+getP = getInput (const id) "p"
+
+getLamX0 :: View x => NlpSolver x p g (VD x)
+getLamX0 = getInput lamXBarToLamX "lam_x0"
+
+getLamG0 :: View g => NlpSolver x p g (VD g)
+getLamG0 = getInput lamGBarToLamG "lam_g0"
+
+getOutput ::
+ View xg
+ => (ScaleFuns x g DMatrix -> (J xg DMatrix -> J xg DMatrix))
+ -> String -> NlpSolver x p g (J xg (Vector Double))
+getOutput scaleFun name = do
+ nlpState <- ask
+ dmat <- liftIO $ C.ioInterfaceFunction_output__0 (isSolver nlpState) name
+ let scale = scaleFun (isScale nlpState)
+ return (mkJ $ ddata $ unJ $ scale (mkJ dmat))
+
+getF :: NlpSolver x p g (VD S)
+getF = getOutput fbarToF "f"
+
+getX :: View x => NlpSolver x p g (VD x)
+getX = getOutput xbarToX "x"
+
+getG :: View g => NlpSolver x p g (VD g)
+getG = getOutput gbarToG "g"
+
+getLamX :: View x => NlpSolver x p g (VD x)
+getLamX = getOutput lamXBarToLamX "lam_x"
+
+getLamG :: View g => NlpSolver x p g (VD g)
+getLamG = getOutput lamGBarToLamG "lam_g"
+
+
+setOption :: Gen.GenericC a => String -> a -> NlpSolver x p g ()
+setOption name val = do
+ nlpState <- ask
+ let nlp = isSolver nlpState
+ liftIO $ Op.setOption nlp name val
+
+
+reinit :: NlpSolver x p g ()
+reinit = do
+ nlpState <- ask
+ let nlp = isSolver nlpState
+ liftIO $ soInit nlp
+
+-- | solve with current inputs, return success or failure code
+solve :: NlpSolver x p g (Either String String)
+solve = do
+ nlpState <- ask
+ let nlp = isSolver nlpState
+ solveStatus <- liftIO $ do
+
+ stop <- newEmptyMVar -- mvar that will be filled when nlp finishes
+ _ <- forkIO (C.function_evaluate nlp >> putMVar stop ())
+ -- wait until nlp finishes
+ ret <- try (takeMVar stop)
+ case ret of Right () -> return () -- no exceptions
+ Left UserInterrupt -> do -- got ctrl-C
+ isInterrupt nlpState -- tell nlp to stop iterations
+ _ <- takeMVar stop -- wait for nlp to return
+ return ()
+ Left _ -> void (takeMVar stop) -- don't handle this one
+ genericStat <- C.function_getStat nlp "return_status"
+ strStat <- Gen.fromGeneric genericStat :: IO (Maybe String)
+ intStat <- Gen.fromGeneric genericStat :: IO (Maybe Int)
+ statDescription <- Gen.getDescription genericStat
+ case strStat of
+ Just strStat' -> return strStat'
+ Nothing -> case intStat of
+ Just intStat' -> return (show intStat')
+ Nothing -> error $ "nlp solver error: return status is not {string,int}, it's " ++
+ statDescription
+
+ return $ if solveStatus `elem` isSuccessCodes nlpState
+ then Right solveStatus
+ else Left solveStatus
+
+-- | solve with current inputs, return lots of info on success, or message on failure
+solve' :: (View x, View g) => NlpSolver x p g (Either String String, NlpOut' x g (Vector Double))
+solve' = do
+ solveStatus <- solve
+ nlpOut <- getNlpOut'
+ return (solveStatus, nlpOut)
+
+getNlpOut' :: (View x, View g) => NlpSolver x p g (NlpOut' x g (Vector Double))
+getNlpOut' = do
+ fopt <- getF
+ xopt <- getX
+ gopt <- getG
+ lamXOpt <- getLamX
+ lamGOpt <- getLamG
+ let nlpOut = NlpOut' { fOpt' = fopt
+ , xOpt' = xopt
+ , gOpt' = gopt
+ , lambdaXOpt' = lamXOpt
+ , lambdaGOpt' = lamGOpt
+ }
+ return nlpOut
+
+
+data NlpState (x :: * -> *) (g :: * -> *) =
+ NlpState
+ { isNx :: Int
+ , isNg :: Int
+ , isNp :: Int
+ , isSolver :: C.NlpSolver
+ , isInterrupt :: IO ()
+ , isSuccessCodes :: [String]
+ , isScale :: ScaleFuns x g DMatrix
+ }
+newtype NlpSolver (x :: * -> *) (p :: * -> *) (g :: * -> *) a =
+ NlpSolver (ReaderT (NlpState x g) IO a)
+ deriving ( Functor
+ , Applicative
+ , Monad
+ , MonadReader (NlpState x g)
+ , MonadIO
+ )
+
+generateAndCompile :: String -> Function -> IO Function
+generateAndCompile name f = do
+ putStrLn $ "generating " ++ name ++ ".c"
+-- writeFile (name ++ ".c") (generateCode f)
+ C.function_generateCode__3 f (name ++ ".c") True
+ let cmd = "clang"
+ args = ["-fPIC","-shared","-Wall","-Wno-unused-variable",name++".c","-o",name++".so"]
+ putStrLn (showCommandForUser cmd args)
+ callProcess cmd args
+ externalFunction ("./"++name++".so")
+
+runNlpSolver ::
+ forall x p g a s .
+ (View x, View p, View g, Symbolic s)
+ => NlpSolverStuff
+ -> (J x s -> J p s -> (J S s, J g s))
+ -> Maybe (J x (Vector Double))
+ -> Maybe (J g (Vector Double))
+ -> Maybe Double
+ -> Maybe (J x (Vector Double) -> IO Bool)
+ -> NlpSolver x p g a
+ -> IO a
+--runNlpSolver solverStuff nlpFun nlpX0' callback' (NlpSolver nlpMonad) = do
+runNlpSolver solverStuff nlpFun scaleX scaleG scaleF callback' (NlpSolver nlpMonad) = do
+ inputsX <- sym "x"
+ inputsP <- sym "p"
+
+ let scale :: forall sfa . (CasadiMat sfa, Viewable sfa) => ScaleFuns x g sfa
+ scale = mkScaleFuns scaleX scaleG scaleF
+
+ let (obj, g) = scaledFG scale nlpFun inputsX inputsP
+
+ let inputsXMat = unJ inputsX
+ inputsPMat = unJ inputsP
+ objMat = unJ obj
+ gMat = unJ g
+
+ inputScheme <- mkScheme SCHEME_NLPInput [("x", inputsXMat), ("p", inputsPMat)]
+ outputScheme <- mkScheme SCHEME_NLPOutput [("f", objMat), ("g", gMat)]
+ nlp <- mkFunction "nlp" inputScheme outputScheme
+-- Op.setOption nlp "verbose" True
+ mapM_ (\(l,Op.Opt o) -> Op.setOption nlp l o) (functionOptions solverStuff)
+ soInit nlp
+
+ functionCall solverStuff nlp
+
+-- let eval 0 = error "finished"
+-- eval k = do
+-- putStrLn "setting input"
+-- ioInterfaceFunction_setInput''' nlp (unJ nlpX0') (0::Int)
+-- putStrLn $ "evaluating " ++ show k
+-- C.function_evaluate nlp
+-- eval (k-1 :: Int)
+-- eval (300::Int)
+-- casadiOptions_stopProfiling
+-- _ <- error "done"
+
+
+-- jac_sparsity <- C.function_jacSparsity nlp 0 1 True False
+-- C.sparsity_spyMatlab jac_sparsity "jac_sparsity_reorder.m"
+
+
+ solver <- C.nlpSolver__0 (solverName solverStuff) nlp
+
+ -- add callback if user provides it
+ intref <- newIORef False
+ let cb function' = do
+ callbackRet <- case callback' of
+ Nothing -> return True
+ Just callback -> do
+ xval <- fmap (mkJ . ddata . unJ . xbarToX scale . mkJ . ddense) $
+ C.ioInterfaceFunction_output__2 function' 0
+ callback xval
+ interrupt <- readIORef intref
+ return $ if callbackRet && not interrupt then 0 else fromIntegral (solverInterruptCode solverStuff)
+ casadiCallback <- makeCallback cb >>= C.genericType__0
+ Op.setOption solver "iteration_callback" casadiCallback
+-- grad_f <- gradient nlp 0 0
+-- soInit grad_f
+-- jac_g <- jacobian nlp 0 1 True False
+-- soInit jac_g
+--
+-- let eval 0 = error "finished"
+-- eval k = do
+-- putStrLn "setting input"
+-- ioInterfaceFunction_setInput''' jac_g (unJ nlpX0') (0::Int)
+-- putStrLn $ "evaluating " ++ show k
+-- C.function_evaluate jac_g
+-- eval (k-1 :: Int)
+-- eval (40::Int)
+
+-- nlp' <- generateAndCompile "nlp" nlp
+-- grad_f' <- generateAndCompile "grad_f" grad_f
+-- jac_g' <- generateAndCompile "jac_g" jac_g
+-- _ <- error "lal"
+-- Op.setOption solver "grad_f" grad_f'
+-- Op.setOption solver "jac_g" jac_g'
+
+ -- set all the user options
+ mapM_ (\(l,Op.Opt o) -> Op.setOption solver l o) (defaultOptions solverStuff ++ options solverStuff)
+ soInit solver
+
+ let nlpState = NlpState { isNx = size (proxy inputsX)
+ , isNp = size (proxy inputsP)
+ , isNg = size (proxy g)
+ , isSolver = solver
+ , isInterrupt = writeIORef intref True
+ , isSuccessCodes = successCodes solverStuff
+ , isScale = scale
+ }
+ liftIO $ runReaderT nlpMonad nlpState
+proxy :: J a b -> Proxy a
+proxy = const Proxy
+
+-- | convenience function to solve a pure Nlp
+solveNlp :: forall x p g .
+ (Vectorize x, Vectorize p, Vectorize g)
+ => NlpSolverStuff
+ -> Nlp x p g SXElement -> Maybe (x Double -> IO Bool)
+ -> IO (Either String String, NlpOut x g Double)
+solveNlp solverStuff nlp callback = do
+ let nlp' :: Nlp' (JV x) (JV p) (JV g) SX
+ nlp' = Nlp' { nlpFG' = \x' p' -> let x = sxSplitJV x' :: x SXElement
+ p = sxSplitJV p' :: p SXElement
+ (obj,g) = nlpFG nlp x p :: (SXElement, g SXElement)
+ obj' = mkJ (sxElementToSX obj) :: J S SX
+ g' = sxCatJV g :: J (JV g) SX
+ in (obj',g')
+ , nlpBX' = mkJ $ vectorize (nlpBX nlp) :: J (JV x) (V.Vector Bounds)
+ , nlpBG' = mkJ $ vectorize (nlpBG nlp) :: J (JV g) (V.Vector Bounds)
+ , nlpX0' = mkJ $ vectorize (nlpX0 nlp) :: J (JV x) (V.Vector Double)
+ , nlpP' = mkJ $ vectorize (nlpP nlp) :: J (JV p) (V.Vector Double)
+ , nlpLamX0' = fmap (mkJ . vectorize) (nlpLamX0 nlp)
+ :: Maybe (J (JV x) (V.Vector Double))
+ , nlpLamG0' = fmap (mkJ . vectorize) (nlpLamG0 nlp)
+ :: Maybe (J (JV g) (V.Vector Double))
+ , nlpScaleF' = nlpScaleF nlp
+ , nlpScaleX' = fmap (mkJ . vectorize) (nlpScaleX nlp)
+ :: Maybe (J (JV x) (V.Vector Double))
+ , nlpScaleG' = fmap (mkJ . vectorize) (nlpScaleG nlp)
+ :: Maybe (J (JV g) (V.Vector Double))
+ }
+
+ callback' :: Maybe (J (JV x) (Vector Double) -> IO Bool)
+ callback' = fmap (. devectorize . unJ) callback
+
+ (r0, r1') <- solveNlp' solverStuff nlp' callback'
+
+ let r1 :: NlpOut x g Double
+ r1 = NlpOut { fOpt = V.head $ unJ (fOpt' r1')
+ , xOpt = devectorize $ unJ (xOpt' r1')
+ , gOpt = devectorize $ unJ (gOpt' r1')
+ , lambdaXOpt = devectorize $ unJ $ lambdaXOpt' r1'
+ , lambdaGOpt = devectorize $ unJ $ lambdaGOpt' r1'
+ }
+
+ return (r0, r1)
+
+
+fmapJ :: View x => (a -> b) -> J x (Vector a) -> J x (Vector b)
+fmapJ f (UnsafeJ v) = mkJ (V.map f v)
+
+junzip :: View x => J x (Vector (a,b)) -> (J x (Vector a), J x (Vector b))
+junzip (UnsafeJ v) = (mkJ x, mkJ y)
+ where
+ (x,y) = V.unzip v
+
+-- | convenience function to solve a pure Nlp'
+solveNlp' ::
+ (View x, View p, View g, Symbolic a)
+ => NlpSolverStuff
+ -> Nlp' x p g a -> Maybe (J x (Vector Double) -> IO Bool)
+ -> IO (Either String String, NlpOut' x g (Vector Double))
+solveNlp' solverStuff nlp callback =
+-- runNlpSolver solverStuff (nlpFG' nlp) (nlpX0' nlp) callback $ do
+ runNlpSolver solverStuff (nlpFG' nlp) (nlpScaleX' nlp) (nlpScaleG' nlp) (nlpScaleF' nlp) callback $ do
+ let (lbx,ubx) = junzip (nlpBX' nlp)
+ (lbg,ubg) = junzip (nlpBG' nlp)
+
+ setX0 (nlpX0' nlp)
+ setP (nlpP' nlp)
+ setLbx lbx
+ setUbx ubx
+ setLbg lbg
+ setUbg ubg
+ case nlpLamX0' nlp of
+ Just lam -> setLamX0 lam
+ Nothing -> return ()
+ case nlpLamG0' nlp of
+ Just lam -> setLamG0 lam
+ Nothing -> return ()
+
+ solve'
+
+-- | solve a homotopy nlp
+solveNlpHomotopy' ::
+ forall x p g a .
+ (View x, View p, View g, Symbolic a)
+ => Double -> (Double, Double, Int, Int)
+ -> NlpSolverStuff
+ -> Nlp' x p g a -> J p (Vector Double) -> Maybe (J (JTuple x p) (Vector Double) -> IO Bool)
+ -> Maybe (J x (Vector Double) -> J p (Vector Double) -> Double -> IO ())
+ -> IO (Either String String, NlpOut' (JTuple x p) g (Vector Double))
+solveNlpHomotopy' userStep (reduction, increase, iterIncrease, iterDecrease)
+ solverStuff nlp (UnsafeJ pF) callback callbackP = do
+ when (reduction >= 1) $ error $ "homotopy reduction factor " ++ show reduction ++ " >= 1"
+ when (increase <= 1) $ error $ "homotopy increase factor " ++ show increase ++ " <= 1"
+ let fg :: J (JTuple x p) a -> J JNone a -> (J S a, J g a)
+ fg xp _ = nlpFG' nlp x p
+ where
+ JTuple x p = split xp
+ runNlpSolver solverStuff fg Nothing (nlpScaleG' nlp) (nlpScaleF' nlp) callback $ do
+ let (lbx,ubx) = junzip (nlpBX' nlp)
+ (lbg,ubg) = junzip (nlpBG' nlp)
+ UnsafeJ p0 = nlpP' nlp
+
+ setAlpha :: Double -> NlpSolver (JTuple x p) JNone g ()
+ setAlpha alpha = do
+ let p = mkJ $ V.zipWith (+) p0 (V.map (alpha*) (V.zipWith (-) pF p0))
+ setLbx $ cat (JTuple lbx (fmapJ Just p))
+ setUbx $ cat (JTuple ubx (fmapJ Just p))
+
+ -- initial solve
+ setX0 $ cat $ JTuple (nlpX0' nlp) (nlpP' nlp)
+ setP $ cat JNone
+ setAlpha 0
+ setLbg lbg
+ setUbg ubg
+ case nlpLamX0' nlp of
+ Just lam -> setLamX0 $ cat (JTuple lam (jfill 0))
+ Nothing -> return ()
+ case nlpLamG0' nlp of
+ Just lam -> setLamG0 lam
+ Nothing -> return ()
+ (ret0, _) <- solve'
+ case ret0 of
+ Right _ -> return ()
+ Left msg -> error $ "error: homotopy solver initial guess not good enough\n" ++ msg
+ getX >>= setX0
+ getLamX >>= setLamX0
+ getLamG >>= setLamG0
+
+ -- run the homotopy
+ let runCallback alphaTrial = case callbackP of
+ Nothing -> return ()
+ Just cbp -> do
+ xp <- getX
+ let JTuple x p = split xp
+ liftIO $ void (cbp x p alphaTrial)
+
+ tryStep :: Int -> Double -> Double
+ -> NlpSolver (JTuple x p) JNone g
+ (Either String String, NlpOut' (JTuple x p) g (Vector Double))
+ tryStep majorIter alpha0 step
+ | step < 1e-12 = do no <- getNlpOut'
+ return (Left "step size too small", no)
+ | otherwise = do
+ liftIO $ printf "%4d, alpha: %.2e, step: %.2e " majorIter alpha0 step
+ liftIO $ hFlush stdout
+ let (alphaTrial, alphaIsOne)
+ | alpha0 + step >= 1 = (1, True)
+ | otherwise = (alpha0 + step, False)
+ setAlpha alphaTrial
+ ret <- solve'
+ case ret of
+ (Left msg,_) -> do
+ liftIO $ putStrLn $ "step failed to solve: " ++ msg
+ tryStep (majorIter+1) alpha0 (reduction*step)
+ (Right _,_) -> do
+ itersStat <- getStat "iter_count"
+ mk <- liftIO (Gen.fromGeneric itersStat :: IO (Maybe Int))
+ iters <- case mk of
+ Nothing ->
+ liftIO (Gen.getDescription itersStat) >>=
+ error . ("homotopy solver: iters is not an Int, it is: " ++) . show
+ Just k' -> return k'
+ liftIO $ putStrLn $ "step successful (" ++ show iters ++ " iterations)"
+ runCallback alphaTrial
+ if alphaIsOne
+ then return ret
+ else do getX >>= setX0
+ getLamX >>= setLamX0
+ getLamG >>= setLamG0
+ if | iters < iterIncrease -> tryStep (majorIter + 1) alphaTrial (step*increase)
+ | iters < iterDecrease -> tryStep (majorIter + 1) alphaTrial step
+ | otherwise -> tryStep (majorIter + 1) alphaTrial (step*reduction)
+
+ ret <- tryStep 0 0 userStep
+ liftIO $ putStrLn "homotopy successful"
+ return ret
diff --git a/src/Dyno/Ocp.hs b/src/Dyno/Ocp.hs
new file mode 100644
--- /dev/null
+++ b/src/Dyno/Ocp.hs
@@ -0,0 +1,175 @@
+{-# OPTIONS_GHC -Wall #-}
+{-# Language TypeFamilies #-}
+{-# Language FlexibleInstances #-}
+
+module Dyno.Ocp
+ ( OcpPhase(..)
+ , OcpPhaseWithCov(..)
+ , OcpPhaseClass(..)
+ ) where
+
+import Data.Default ( Default(..) )
+import Data.Vector ( Vector )
+
+import Dyno.Vectorize ( Vectorize, None(..), fill )
+import Dyno.View.JV
+import Dyno.View.View
+import Dyno.Cov
+import Dyno.Nlp ( Bounds )
+import Dyno.SXElement ( SXElement )
+
+import Casadi.SX ( SX )
+import Casadi.DMatrix ( DMatrix )
+
+type Sx a = J a SX
+type Sxe = SXElement
+
+class OcpPhaseClass a where
+ type X a :: * -> *
+ type Z a :: * -> *
+ type U a :: * -> *
+ type P a :: * -> *
+ type R a :: * -> *
+ type O a :: * -> *
+ type C a :: * -> *
+ type H a :: * -> *
+
+instance OcpPhaseClass (OcpPhase x z u p r o c h) where
+ type X (OcpPhase x z u p r o c h) = x
+ type Z (OcpPhase x z u p r o c h) = z
+ type U (OcpPhase x z u p r o c h) = u
+ type P (OcpPhase x z u p r o c h) = p
+ type R (OcpPhase x z u p r o c h) = r
+ type O (OcpPhase x z u p r o c h) = o
+ type C (OcpPhase x z u p r o c h) = c
+ type H (OcpPhase x z u p r o c h) = h
+
+-- | One stage of an optimal control problem, solvable as a stand-alone optimal control problem.
+--
+-- > minimize Jm(x(T),T) + integrate( Jl(x(t),z(t),u(t),p,t), {t,0,T} )
+-- > x(.), z(.), u(.), p, T
+-- >
+-- > subject to:
+--
+-- bound constraints:
+--
+-- > Tlb <= T <= Tub
+-- > xlb <= x <= xub
+-- > zlb <= z <= zub
+-- > ulb <= u <= uub
+--
+-- nonlinear path constraints
+--
+-- > hlb <= h(x(t), z(t), u(t), p, t) <= hub
+--
+-- dynamics constraints:
+--
+-- > f(x'(t), x(t), z(t), u(t), p, t) == 0
+--
+-- boundary conditions:
+--
+-- > c(x(0), x(T)) == 0
+--
+-- perhaps this should be:
+--
+-- > c(x(0), 0, x(T), T) == 0
+data OcpPhase x z u p r o c h =
+ OcpPhase
+ { -- | the Mayer term @Jm(T, x(0), x(T))@
+ ocpMayer :: Sxe -> x Sxe -> x Sxe -> Sxe
+ -- | the Lagrange term @Jl(x(t),z(t),u(t),p,o,t,T)@
+ , ocpLagrange :: x Sxe -> z Sxe -> u Sxe -> p Sxe -> o Sxe -> Sxe -> Sxe -> Sxe
+ -- | fully implicit differential-algebraic equation of the form:
+ --
+ -- > f(x'(t), x(t), z(t), u(t), p, t) == 0
+ , ocpDae :: x Sxe -> x Sxe -> z Sxe -> u Sxe -> p Sxe -> Sxe -> (r Sxe, o Sxe)
+ -- | the boundary conditions @clb <= c(x(0), x(T)) <= cub@
+ , ocpBc :: x Sxe -> x Sxe -> c Sxe
+ -- | the path constraints @h(x(t), z(t), u(t), p, t)@
+ , ocpPathC :: x Sxe -> z Sxe -> u Sxe -> p Sxe -> o Sxe -> Sxe -> h Sxe
+ -- | the boundary condition bounds @clb <= c(x(0), x(T)) <= cub@
+ , ocpBcBnds :: c Bounds
+ -- | the path constraint bounds @(hlb, hub)@
+ , ocpPathCBnds :: h Bounds
+ -- | differential state bounds @(xlb, xub)@
+ , ocpXbnd :: x Bounds
+ -- | algebraic variable bounds @(zlb, zub)@
+ , ocpZbnd :: z Bounds
+ -- | control bounds @(ulb, uub)@
+ , ocpUbnd :: u Bounds
+ -- | parameter bounds @(plb, pub)@
+ , ocpPbnd :: p Bounds
+ -- | time bounds @(Tlb, Tub)@
+ , ocpTbnd :: Bounds
+ -- | scaling
+ , ocpObjScale :: Maybe Double
+ , ocpTScale :: Maybe Double
+ , ocpXScale :: Maybe (x Double)
+ , ocpZScale :: Maybe (z Double)
+ , ocpUScale :: Maybe (u Double)
+ , ocpPScale :: Maybe (p Double)
+ , ocpResidualScale :: Maybe (r Double)
+ , ocpBcScale :: Maybe (c Double)
+ , ocpPathCScale :: Maybe (h Double)
+ }
+instance (Vectorize x, Vectorize z, Vectorize u, Vectorize p)
+ => Default (OcpPhase x z u p r o None None) where
+ def =
+ OcpPhase
+ { ocpMayer = \_ _ _ -> 0
+ , ocpLagrange = \_ _ _ _ _ _ _ -> 0
+ , ocpDae = error "no default dae in OcpPhase"
+ , ocpBc = \_ _ -> None
+ , ocpPathC = \_ _ _ _ _ _ -> None
+ , ocpBcBnds = None
+ , ocpPathCBnds = None
+ , ocpXbnd = fill (Nothing, Nothing)
+ , ocpZbnd = fill (Nothing, Nothing)
+ , ocpUbnd = fill (Nothing, Nothing)
+ , ocpPbnd = fill (Nothing, Nothing)
+ , ocpTbnd = (Nothing, Nothing)
+ , ocpObjScale = Nothing
+ , ocpTScale = Nothing
+ , ocpXScale = Nothing
+ , ocpZScale = Nothing
+ , ocpUScale = Nothing
+ , ocpPScale = Nothing
+ , ocpResidualScale = Nothing
+ , ocpBcScale = Nothing
+ , ocpPathCScale = Nothing
+ }
+
+data OcpPhaseWithCov ocp sx sz sw sr sh shr sc =
+ OcpPhaseWithCov
+ { -- | the Mayer term @Jm(T, x(0), x(T), P(0), P(t))@
+ ocpCovMayer :: Sxe -> X ocp Sxe -> X ocp Sxe -> Sx (Cov (JV sx)) -> Sx (Cov (JV sx)) -> Sxe
+ -- | the Lagrange term @Jl(t, x(t), P(t), T)@
+ , ocpCovLagrange :: Sxe -> X ocp Sxe -> Sx (Cov (JV sx)) -> Sxe -> Sxe
+ -- | the system dynamics of the stage: @f(x'(t), x(t), z(t), u(t), p, t)@
+ , ocpCovDae :: X ocp Sxe -> X ocp Sxe -> Z ocp Sxe -> U ocp Sxe -> P ocp Sxe -> Sxe
+ -> sx Sxe -> sx Sxe -> sz Sxe -> sw Sxe
+ -> sr Sxe
+ -- | the projection from covariance state to full state
+ , ocpCovProjection :: X ocp Sxe -> sx Sxe -> X ocp Sxe
+ -- | constraints which (g(x) <= 0) will be satisfied with some margin defined by gamma
+ -- .
+ -- TODO: user upper and lower bounds without adding another constraint, probably impossible
+ , ocpCovRobustifyPathC :: X ocp Sxe -> sx Sxe -> P ocp Sxe -> shr Sxe
+ -- | robust factors for the robustified constraints
+ , ocpCovGammas :: shr Double
+ -- | covariance injection
+ , ocpCovSq :: J (Cov (JV sw)) DMatrix
+ -- | bounds on the initial convariance
+ , ocpCovS0bnd :: J (Cov (JV sx)) (Vector Bounds)
+ -- | the covariance boundary conditions @c(s(0), s(T))@
+ , ocpCovSbc :: Sx (Cov (JV sx)) -> Sx (Cov (JV sx)) -> Sx sc
+ , ocpCovSbcBnds :: J sc (Vector Bounds)
+ -- | the covariance path constraints @h(s)@, only applied to first n Ss
+ , ocpCovSh :: X ocp SXElement -> Sx (Cov (JV sx)) -> Sx sh
+ , ocpCovShBnds :: J sh (Vector Bounds)
+ -- | scaling
+ , ocpCovSScale :: Maybe (J (Cov (JV sx)) (Vector Double))
+ , ocpCovPathCScale :: Maybe (J sh (Vector Double))
+ , ocpCovRobustPathCScale :: Maybe (shr Double)
+ , ocpCovSbcScale :: Maybe (J sc (Vector Double))
+ }
diff --git a/src/Dyno/OcpMonad.hs b/src/Dyno/OcpMonad.hs
new file mode 100644
--- /dev/null
+++ b/src/Dyno/OcpMonad.hs
@@ -0,0 +1,496 @@
+{-# OPTIONS_GHC -Wall #-}
+{-# Language ScopedTypeVariables #-}
+{-# Language PackageImports #-}
+{-# Language MultiParamTypeClasses #-}
+{-# Language FunctionalDependencies #-}
+{-# Language GeneralizedNewtypeDeriving #-}
+{-# Language FlexibleContexts #-}
+{-# Language RankNTypes #-}
+
+module Dyno.OcpMonad
+ ( OcpMonad
+ , EqMonad(..)
+ , LeqMonad(..)
+ , DaeMonad
+ , BCMonad
+ , SXElement
+ , diffState
+ , algVar
+ , control
+ , parameter
+ , output
+ , lagrangeTerm
+ , solveStaticOcp
+ ) where
+
+import Control.Applicative ( Applicative(..) )
+import Control.Lens ( Lens', over )
+import Control.Monad ( when )
+import qualified "mtl" Control.Monad.State as State
+import "mtl" Control.Monad.Reader ( MonadIO(..) )
+import "mtl" Control.Monad.Writer ( WriterT, Writer, MonadWriter, runWriterT, runWriter )
+import "mtl" Control.Monad.State ( StateT, MonadState, runStateT )
+import "mtl" Control.Monad.Except ( ExceptT, MonadError, runExceptT )
+import qualified Data.Foldable as F
+import qualified Data.HashSet as HS
+import qualified Data.Sequence as S
+import qualified Data.Map as M
+import Data.Sequence ( (|>) )
+import Data.Vector ( Vector )
+import qualified Data.Vector as V
+import Data.Proxy ( Proxy(..) )
+
+import Casadi.Option ( setOption )
+import Casadi.SXFunction ( sxFunction )
+import Casadi.SX ( SX )
+import Casadi.Function ( callSX )
+import Casadi.SharedObject ( soInit )
+
+import qualified Dyno.View.CasadiMat as CM
+import Dyno.SXElement ( SXElement, sxElementSym, sxElementToSX, sxToSXElement )
+import Dyno.Ocp ( OcpPhase(..) )
+import Dyno.Nlp ( Bounds )
+import Dyno.Vectorize ( Vectorize(..), fill )
+import Dyno.View.View ( mkJ )
+import Dyno.View.JV ( sxSplitJV )
+import Dyno.TypeVecs ( Vec )
+import qualified Dyno.TypeVecs as TV
+import Dyno.NlpSolver ( NlpSolverStuff )
+import Dyno.DirectCollocation.Quadratures ( QuadratureRoots(..) )
+import Dyno.DirectCollocation.Dynamic ( DynCollTraj, CollTrajMeta(..), NameTree(..) )
+import Dyno.DirectCollocation ( solveOcp )
+
+import Dyno.Interface.LogsAndErrors
+import Dyno.Interface.Types
+
+--withEllipse :: Int -> String -> String
+--withEllipse n blah
+-- | length blah <= n = blah
+-- | otherwise = take n blah ++ "..."
+
+newtype OcpMonad a =
+ OcpMonad
+ { runOcp :: ExceptT ErrorMessage (WriterT [LogMessage] (StateT OcpState IO)) a
+ } deriving ( Functor
+ , Applicative
+ , Monad
+ , MonadError ErrorMessage
+ , MonadState OcpState
+ , MonadWriter [LogMessage]
+ , MonadIO
+ )
+
+newtype BCMonad a =
+ BCMonad
+ { runBc :: ExceptT ErrorMessage (WriterT [LogMessage] (StateT (S.Seq (Constraint SXElement)) IO)) a
+ } deriving ( Functor
+ , Applicative
+ , Monad
+ , MonadError ErrorMessage
+ , MonadState (S.Seq (Constraint SXElement))
+ , MonadWriter [LogMessage]
+ , MonadIO
+ )
+
+newtype DaeMonad a =
+ DaeMonad
+ { runDae :: ExceptT ErrorMessage (WriterT [LogMessage] (StateT DaeState IO)) a
+ } deriving ( Functor
+ , Applicative
+ , Monad
+ , MonadError ErrorMessage
+ , MonadState DaeState
+ , MonadWriter [LogMessage]
+ , MonadIO
+ )
+
+emptySymbolicDae :: DaeState
+emptySymbolicDae = DaeState S.empty S.empty S.empty S.empty S.empty M.empty HS.empty S.empty
+
+buildDae :: DaeMonad a -> IO (Either ErrorMessage a, [LogMessage], DaeState)
+buildDae = buildDae' emptySymbolicDae
+ where
+ buildDae' :: DaeState -> DaeMonad a -> IO (Either ErrorMessage a, [LogMessage], DaeState)
+ buildDae' nlp0 builder = do
+ ((result,logs),state) <- flip runStateT nlp0 . runWriterT . runExceptT . runDae $ builder
+ return (result, logs, state)
+
+newDaeVariable ::
+ (MonadState DaeState m, MonadError ErrorMessage m, MonadWriter [LogMessage] m, MonadIO m)
+ => String -> Lens' DaeState (S.Seq (String, SXElement)) -> String -> m SXElement
+newDaeVariable description lens name = do
+ debug $ "adding " ++ description ++ " \""++name++"\""
+ case name of [] -> err "name cannot be empty"
+ ('_':_) -> err $ "name \"" ++ name ++
+ "\" cannot have leading underscore (this is reserved for internal use)"
+ _ -> return ()
+ state0 <- State.get
+ let map0 = daeNameSet state0
+ sym <- liftIO (sxElementSym name)
+ when (HS.member name map0) $ err $ name ++ " already in name set"
+ let state1 = state0 { daeNameSet = HS.insert name map0 }
+ state2 = over lens (|> (name, sym)) state1
+ State.put state2
+ return sym
+
+svector :: Vector SXElement -> SX
+svector = CM.vertcat . fmap sxElementToSX
+
+diffState :: String -> DaeMonad (SXElement, SXElement)
+diffState name = do
+ x <- newDaeVariable "differential state" daeX name
+ xdot <- newDaeVariable "differential state derivative" daeXDot ("ddt( " ++ name ++ " )")
+ return (x, xdot)
+
+algVar :: String -> DaeMonad SXElement
+algVar = newDaeVariable "algebraic variable" daeZ
+
+control :: String -> DaeMonad SXElement
+control = newDaeVariable "control" daeU
+
+parameter :: String -> DaeMonad SXElement
+parameter = newDaeVariable "parameter" daeP
+
+output :: String -> SXElement -> DaeMonad ()
+output name expr = do
+ debug $ "adding output \""++name++"\""
+-- debug $ "adding output \""++name++"\": " ++ withEllipse 30 (show expr)
+ state0 <- State.get
+ let nameSet0 = daeNameSet state0
+ outputs0 = _daeO state0
+ when (HS.member name nameSet0) $ err $ name ++ " already in name set"
+ when (M.member name outputs0) $ impossible $ name ++ " already in output map"
+ let state1 = state0 { daeNameSet = HS.insert name nameSet0
+ , _daeO = M.insert name expr outputs0
+ }
+ State.put state1
+
+infix 4 ===
+class EqMonad m a | m -> a where
+ (===) :: a -> a -> m ()
+
+instance EqMonad DaeMonad SXElement where
+ (===) lhs rhs = do
+ debug $ "adding equality constraint: "
+-- ++ withEllipse 30 (show lhs) ++ " == " ++ withEllipse 30 (show rhs)
+ state0 <- State.get
+ State.put $ state0 { daeConstraints = daeConstraints state0 |> (lhs, rhs) }
+
+instance EqMonad OcpMonad SXElement where
+ (===) lhs rhs = do
+ debug $ "adding equality constraint: "
+-- ++ withEllipse 30 (show lhs) ++ " == " ++ withEllipse 30 (show rhs)
+ state0 <- State.get
+ State.put $ state0 { ocpPathConstraints = ocpPathConstraints state0 |> Eq2 lhs rhs }
+
+
+infix 4 <==
+class LeqMonad m where
+ (<==) :: SXElement -> SXElement -> m ()
+
+instance LeqMonad OcpMonad where
+ (<==) lhs rhs = do
+ debug $ "adding inequality constraint: "
+-- ++ withEllipse 30 (show lhs) ++ " <= " ++ withEllipse 30 (show rhs)
+ state0 <- State.get
+ State.put $ state0 { ocpPathConstraints = ocpPathConstraints state0 |> Ineq2 lhs rhs }
+
+instance EqMonad BCMonad SXElement where
+ (===) lhs rhs = do
+ debug $ "adding inequality constraint: "
+ -- ++ withEllipse 30 (show lhs) ++ " == " ++ withEllipse 30 (show rhs)
+ state0 <- State.get
+ State.put $ state0 |> Eq2 lhs rhs
+
+instance LeqMonad BCMonad where
+ (<==) lhs rhs = do
+ debug $ "adding inequality constraint: "
+-- ++ withEllipse 30 (show lhs) ++ " <= " ++ withEllipse 30 (show rhs)
+ state0 <- State.get
+ State.put $ state0 |> Ineq2 lhs rhs
+
+
+constr :: Constraint SXElement -> (SXElement, Bounds)
+constr (Eq2 lhs rhs) = (lhs - rhs, (Just 0, Just 0))
+constr (Ineq2 lhs rhs) = (lhs - rhs, (Nothing, Just 0))
+constr (Ineq3 x (lhs,rhs)) = (x, (Just lhs, Just rhs))
+
+
+
+lagrangeTerm :: SXElement -> OcpMonad ()
+lagrangeTerm obj = do
+ debug "setting lagrange term"
+ --debug $ "setting lagrange term: " ++ withEllipse 30 (show obj)
+ state0 <- State.get
+ case ocpLagrangeObj state0 of
+ Objective _x -> err $ init $ unlines
+ [ "you set the lagrange objective function twice"
+-- , " old val: " ++ withEllipse 30 (show x)
+-- , " new val: " ++ withEllipse 30 (show obj)
+ ]
+ ObjectiveUnset -> State.put $ state0 { ocpLagrangeObj = Objective obj }
+
+
+
+emptySymbolicOcp :: OcpState
+emptySymbolicOcp = OcpState S.empty ObjectiveUnset HomotopyParamUnset
+
+reifyOcpPhase ::
+ forall ret .
+ (SXElement -> DaeMonad ())
+ -> (forall a m . (Floating a, Monad m) => a -> (String -> m a) -> (String -> m a) -> m a)
+ -> ((String -> BCMonad SXElement) -> (String -> BCMonad SXElement) -> BCMonad ())
+ -> (SXElement -> (String -> OcpMonad SXElement) -> OcpMonad ())
+ -> (Maybe Double, Maybe Double)
+ -> Int -> Int
+ -> (forall x z u p r o c h .
+ (Vectorize x, Vectorize z, Vectorize u, Vectorize p, Vectorize r, Vectorize o, Vectorize c, Vectorize h)
+ => OcpPhase x z u p r o c h -> CollTrajMeta -> IO ret)
+ -> IO ret
+reifyOcpPhase daeMonad mayerMonad bcMonad ocpMonad tbnds n deg f = do
+ time <- sxElementSym "_t"
+ endT <- sxElementSym "T"
+ let time' = sxElementToSX time
+ endT' = sxElementToSX endT
+ dae' <- buildDae (daeMonad time)
+ let dae :: DaeState
+ dae = case dae' of
+ (Left errmsg, _, _) -> error $ "buildOcpPhase: buildDae failure: " ++ show errmsg
+ (_, _, daeState) -> daeState
+
+ xdotnames, xnames, znames, unames, pnames :: Vector String
+ xdots, xs, zs, us, ps :: Vector SXElement
+ (xdotnames,xdots) = V.unzip $ V.fromList $ F.toList $ _daeXDot dae
+ (xnames,xs) = V.unzip $ V.fromList $ F.toList $ _daeX dae
+ (znames,zs) = V.unzip $ V.fromList $ F.toList $ _daeZ dae
+ (unames,us) = V.unzip $ V.fromList $ F.toList $ _daeU dae
+ (pnames,ps) = V.unzip $ V.fromList $ F.toList $ _daeP dae
+
+ xdots' = svector xdots
+ xs' = svector xs
+ zs' = svector zs
+ us' = svector us
+ ps' = svector ps
+
+ daeResidual :: Vector SXElement
+ daeResidual = V.map (uncurry (-)) $ V.fromList $ F.toList $ daeConstraints dae
+
+ onames :: Vector String
+ osOut :: Vector SXElement
+ (onames, osOut) = V.unzip $ V.fromList $ M.toList $ _daeO dae
+ os <- V.mapM sxElementSym onames :: IO (Vector SXElement)
+ let os' = svector os
+
+ lookupThingy :: String -> OcpMonad SXElement
+ lookupThingy name = do
+ debug $ "ocp monad: looking up \"" ++ name ++ "\""
+ case M.lookup name varmap of
+ Nothing -> err $ "ocp monad: nothing named \"" ++ name ++ "\""
+ Just expr -> do
+ debug $ "ocp monad: found \"" ++ name ++ "\""
+ --debug $ "ocp monad: found \"" ++ name ++ "\": " ++ show expr
+ return expr
+ where
+ varmap :: M.Map String SXElement
+ varmap = M.fromList $ F.toList $ V.concat
+ [ V.zip xdotnames xdots
+ , V.zip xnames xs
+ , V.zip znames zs
+ , V.zip unames us
+ , V.zip pnames ps
+ , V.zip onames os
+ ]
+
+ ocp' <- flip runStateT emptySymbolicOcp $ runWriterT $ runExceptT (runOcp (ocpMonad time lookupThingy))
+ let ocp :: OcpState
+ ocp = case ocp' of
+ ((Left errmsg, logs),_) ->
+ error $ unlines $ ("" : map show logs) ++ ["","ocp monad failure: " ++ show errmsg]
+ ((Right _, _), ocpState) -> ocpState
+
+ obj = case ocpLagrangeObj ocp of
+ ObjectiveUnset -> 0
+ Objective obj' -> obj'
+
+ lagFunSX <- sxFunction (V.fromList [xs',zs',us',ps',os',time',endT']) (V.fromList [svector (V.singleton obj)])
+ setOption lagFunSX "name" "lagrange"
+ soInit lagFunSX
+
+ let pathConstraints :: [SXElement]
+ pathConstraintBnds :: [(Maybe Double, Maybe Double)]
+ (pathConstraints, pathConstraintBnds) = unzip $ map constr (F.toList (ocpPathConstraints ocp))
+
+ pathcFunSX <- sxFunction (V.fromList [xs',zs',us',ps',os',time'])
+ (V.singleton (svector (V.fromList pathConstraints)))
+ setOption pathcFunSX "name" "pathConstraints"
+ soInit pathcFunSX
+
+
+ daeFunSX <- sxFunction (V.fromList [xdots', xs', zs', us', ps', time'])
+ (V.fromList [svector daeResidual, svector osOut])
+ setOption pathcFunSX "name" "daeResidualAndOutputs"
+ soInit daeFunSX
+
+ -- run the mayer function
+ x0s <- mapM (sxElementSym . (++ "_0")) (F.toList xnames)
+ xFs <- mapM (sxElementSym . (++ "_F")) (F.toList xnames)
+ let lookupState :: M.Map String SXElement -> String
+ -> ExceptT ErrorMessage (Writer [LogMessage]) SXElement
+ lookupState xmap name = do
+ debug $ "mayer monad: looking up \"" ++ name ++ "\""
+ case M.lookup name xmap of
+ Nothing -> err $ "mayer monad: no state named \"" ++ name ++ "\""
+ Just expr -> do
+ debug $ "mayer monad: found \"" ++ name ++ "\""
+ return expr
+
+ xmap0 :: M.Map String SXElement
+ xmap0 = M.fromList $ zip (F.toList xnames) x0s
+
+ xmapF :: M.Map String SXElement
+ xmapF = M.fromList $ zip (F.toList xnames) xFs
+
+ mayerObj :: SXElement
+ mayerObj = case runWriter (runExceptT (mayerMonad endT (lookupState xmap0) (lookupState xmapF))) of
+ (Left errmsg, logs) ->
+ error $ unlines $ ("" : map show logs) ++ ["","mayer monad failure: " ++ show errmsg]
+ (Right ret, _) -> ret
+ mayerFunSX <- sxFunction (V.fromList [svector (V.singleton endT), svector (V.fromList x0s), svector (V.fromList xFs)])
+ (V.singleton (svector (V.singleton mayerObj)))
+ setOption mayerFunSX "name" "mayer"
+ soInit mayerFunSX
+
+
+ let lookupState0 :: String -> BCMonad SXElement
+ lookupState0 name = do
+ debug $ "boundary condition monad: looking up initial \"" ++ name ++ "\""
+ case M.lookup name xmap0 of
+ Nothing -> err $ "boundary condition monad: no state named \"" ++ name ++ "\""
+ Just expr -> do
+ debug $ "boundary condition monad: found \"" ++ name ++ "\""
+ return expr
+
+ lookupStateF :: String -> BCMonad SXElement
+ lookupStateF name = do
+ debug $ "boundary condition monad: looking up final \"" ++ name ++ "\""
+ case M.lookup name xmapF of
+ Nothing -> err $ "boundary condition monad: no state named \"" ++ name ++ "\""
+ Just expr -> do
+ debug $ "boundary condition monad: found \"" ++ name ++ "\""
+ return expr
+ bcs' <- flip runStateT S.empty $ runWriterT (runExceptT (runBc $ bcMonad lookupState0 lookupStateF))
+ let bcs :: Vector SXElement
+ bcbnds :: Vector Bounds
+ (bcs,bcbnds) = case bcs' of
+ ((Left errmsg, logs),_) ->
+ error $ unlines $ ("" : map show logs) ++ ["","boundary condition monad failure: " ++ show errmsg]
+ ((Right _,_), ret) -> V.unzip $ V.fromList $ map constr $ F.toList ret
+ bcFunSX <- sxFunction (V.fromList [svector (V.fromList x0s), svector (V.fromList xFs)])
+ (V.singleton (svector bcs))
+ setOption bcFunSX "name" "boundaryConditions"
+ soInit bcFunSX
+
+ let meta = CollTrajMeta
+ { ctmX = NameTreeNode ("", "") (zip (F.toList xnames) (map NameTreeLeaf [0..]))
+ , ctmZ = NameTreeNode ("", "") (zip (F.toList znames) (map NameTreeLeaf [0..]))
+ , ctmU = NameTreeNode ("", "") (zip (F.toList unames) (map NameTreeLeaf [0..]))
+ , ctmP = NameTreeNode ("", "") (zip (F.toList pnames) (map NameTreeLeaf [0..]))
+ , ctmO = NameTreeNode ("", "") (zip (F.toList onames) (map NameTreeLeaf [0..]))
+ , ctmN = n
+ , ctmDeg = deg
+ , ctmNx = V.length xnames
+ , ctmNz = V.length znames
+ , ctmNu = V.length unames
+ , ctmNp = V.length pnames
+ , ctmNo = V.length onames
+ , ctmNsx = 0
+ , ctmQuadRoots = Legendre -- TODO: make this an input
+ }
+ TV.reifyDim (ctmNx meta) $ \(Proxy :: Proxy nx) ->
+ TV.reifyDim (ctmNz meta) $ \(Proxy :: Proxy nz) ->
+ TV.reifyDim (ctmNu meta) $ \(Proxy :: Proxy nu) ->
+ TV.reifyDim (ctmNp meta) $ \(Proxy :: Proxy np) ->
+ TV.reifyDim (V.length daeResidual) $ \(Proxy :: Proxy nr) ->
+ TV.reifyDim (V.length onames) $ \(Proxy :: Proxy no) ->
+ TV.reifyDim (V.length bcs) $ \(Proxy :: Proxy nc) ->
+ TV.reifyDim (length pathConstraints) $ \(Proxy :: Proxy nh) -> do
+ -- TV.reifyDim ncov $ \(Proxy :: Proxy ncov) -> do
+ -- TV.reifyDim nsh $ \(Proxy :: Proxy nsh) -> do
+ -- TV.reifyDim nsc $ \(Proxy :: Proxy nsc) -> do
+
+ let daeFun :: Vec nx SXElement -> Vec nx SXElement -> Vec nz SXElement -> Vec nu SXElement
+ -> Vec np SXElement -> SXElement
+ -> (Vec nr SXElement, Vec no SXElement)
+ daeFun x' x z u p t = (devec (rets V.! 0), devec (rets V.! 1))
+ where
+ rets = callSX daeFunSX (V.fromList [vec x', vec x, vec z, vec u, vec p, sxElementToSX t])
+
+ lagrangeFun :: Vec nx SXElement -> Vec nz SXElement -> Vec nu SXElement -> Vec np SXElement -> Vec no SXElement -> SXElement -> SXElement -> SXElement
+ lagrangeFun x z u p o t tf =
+ sxToSXElement $ V.head $ callSX lagFunSX $
+ (V.fromList [vec x, vec z, vec u, vec p, vec o, sxElementToSX t, sxElementToSX tf])
+ --Left errmsg -> error $ "toOcpPhase: lagrangeFun: " ++ errmsg ++
+ -- "\ninputs: " ++ show (xnames ++ znames ++ unames ++ pnames) ++ show onames ++
+ -- "\nnumeric inputs x: " ++ show (V.length x) ++
+ -- "\nnumeric inputs z: " ++ show (V.length z) ++
+ -- "\nnumeric inputs u: " ++ show (V.length u) ++
+ -- "\nnumeric inputs p: " ++ show (V.length p) ++
+ -- "\nnumeric inputs o: " ++ show (V.length o)
+
+ pathConstraintFun :: Vec nx SXElement -> Vec nz SXElement -> Vec nu SXElement
+ -> Vec np SXElement -> Vec no SXElement -> SXElement -> Vec nh SXElement
+ pathConstraintFun x z u p o t =
+ devec $ V.head $ callSX pathcFunSX (V.fromList [vec x, vec z, vec u, vec p, vec o, sxElementToSX t])
+
+ mayerFun :: SXElement -> Vec nx SXElement -> Vec nx SXElement
+ -> SXElement
+ mayerFun endT'' x0 xF = sxToSXElement $ V.head $ callSX mayerFunSX (V.fromList [sxElementToSX endT'', vec x0, vec xF])
+
+ bcFun :: Vec nx SXElement -> Vec nx SXElement -> Vec nc SXElement
+ bcFun x0 xF = devec $ V.head $ callSX bcFunSX (V.fromList [vec x0, vec xF])
+
+ ocpPhase =
+ OcpPhase { ocpMayer = mayerFun
+ , ocpLagrange = lagrangeFun
+ , ocpDae = daeFun
+ , ocpBc = bcFun
+ , ocpBcBnds = devectorize bcbnds
+ , ocpPathC = pathConstraintFun
+ , ocpPathCBnds = devectorize (V.fromList pathConstraintBnds)
+ , ocpXbnd = fill (Nothing, Nothing)
+ , ocpZbnd = fill (Nothing, Nothing)
+ , ocpUbnd = fill (Nothing, Nothing)
+ , ocpPbnd = fill (Nothing, Nothing)
+ , ocpTbnd = tbnds
+ , ocpObjScale = Nothing
+ , ocpTScale = Nothing
+ , ocpXScale = Nothing
+ , ocpZScale = Nothing
+ , ocpUScale = Nothing
+ , ocpPScale = Nothing
+ , ocpResidualScale = Nothing
+ , ocpBcScale = Nothing
+ , ocpPathCScale = Nothing
+ }
+ f ocpPhase meta
+
+vec :: Vectorize f => f SXElement -> SX
+vec = svector . vectorize
+
+devec :: Vectorize f => SX -> f SXElement
+devec = sxSplitJV . mkJ
+
+solveStaticOcp ::
+ NlpSolverStuff
+ -> (SXElement -> DaeMonad ())
+ -> (forall a m . (Floating a, Monad m) => a -> (String -> m a) -> (String -> m a) -> m a)
+ -> ((String -> BCMonad SXElement) -> (String -> BCMonad SXElement) -> BCMonad ())
+ -> (SXElement -> (String -> OcpMonad SXElement) -> OcpMonad ())
+ -> (Maybe Double, Maybe Double)
+ -> Int -> Int
+ -> Maybe (CollTrajMeta -> [DynCollTraj (Vector Double)] -> IO Bool)
+ -> IO (Either String String)
+solveStaticOcp solverStuff dae mayer bc ocp tbnds n deg cb =
+ reifyOcpPhase dae mayer bc ocp tbnds n deg woo
+ where
+ woo ocpphase meta = solveOcp solverStuff n deg (cb <*> pure meta) ocpphase
diff --git a/src/Dyno/SXElement.hs b/src/Dyno/SXElement.hs
new file mode 100644
--- /dev/null
+++ b/src/Dyno/SXElement.hs
@@ -0,0 +1,38 @@
+{-# OPTIONS_GHC -Wall #-}
+{-# Language GeneralizedNewtypeDeriving #-}
+
+module Dyno.SXElement
+ ( SXElement(..)
+ , sxElementSym
+ , sxToSXElement
+ , sxElementToSX
+ ) where
+
+import Linear.Conjugate ( Conjugate(..) )
+
+import Casadi.SX
+import Casadi.Overloading
+
+newtype SXElement =
+ SXElement SX
+ deriving ( Num, Fractional, Floating
+ , Fmod, ArcTan2, SymOrd
+ , Show, Eq, Conjugate
+ )
+
+sxElementSym :: String -> IO SXElement
+sxElementSym = fmap SXElement . ssym
+
+sxToSXElement :: SX -> SXElement
+sxToSXElement x
+ | (1,1) == sizes = SXElement x
+ | otherwise = error $ "sxToSXElement: got non-scalar of size " ++ show sizes
+ where
+ sizes = (ssize1 x, ssize2 x)
+
+sxElementToSX :: SXElement -> SX
+sxElementToSX (SXElement x)
+ | (1,1) == sizes = x
+ | otherwise = error $ "sxElementToSX: got non-scalar of size " ++ show sizes
+ where
+ sizes = (ssize1 x, ssize2 x)
diff --git a/src/Dyno/Server/Accessors.hs b/src/Dyno/Server/Accessors.hs
new file mode 100644
--- /dev/null
+++ b/src/Dyno/Server/Accessors.hs
@@ -0,0 +1,178 @@
+{-# OPTIONS_GHC -Wall #-}
+--{-# OPTIONS_GHC -ddump-deriv #-}
+{-# LANGUAGE DefaultSignatures #-}
+{-# LANGUAGE TypeOperators #-}
+{-# LANGUAGE FlexibleContexts #-}
+{-# LANGUAGE FlexibleInstances #-}
+--{-# LANGUAGE DeriveGeneric #-} -- for example at bottom
+
+module Dyno.Server.Accessors
+ ( Generic
+ , Lookup(..)
+ , AccessorTree(..)
+ , accessors
+ , flatten
+ ) where
+
+import Data.List ( intercalate )
+import qualified Linear
+import GHC.Generics
+
+import SpatialMath ( Euler )
+import SpatialMathT ( V3T, Rot )
+
+showAccTree :: String -> AccessorTree a -> [String]
+showAccTree spaces (Getter _) = [spaces ++ "Getter {}"]
+showAccTree spaces (Data name trees) =
+ (spaces ++ "Data " ++ show name) :
+ concatMap (showChild (spaces ++ " ")) trees
+
+showChild :: String -> (String, AccessorTree a) -> [String]
+showChild spaces (name, tree) =
+ (spaces ++ name) : showAccTree (spaces ++ " ") tree
+
+instance Show (AccessorTree a) where
+ show = unlines . showAccTree ""
+
+data AccessorTree a = Data (String,String) [(String, AccessorTree a)]
+ | Getter (a -> Double)
+
+accessors :: Lookup a => a -> AccessorTree a
+accessors = flip toAccessorTree id
+
+showMsgs :: [String] -> String
+showMsgs = intercalate "."
+
+flatten :: AccessorTree a -> [(String, a -> Double)]
+flatten = flatten' []
+
+flatten' :: [String] -> AccessorTree a -> [(String, a -> Double)]
+flatten' msgs (Getter f) = [(showMsgs (reverse msgs), f)]
+flatten' msgs (Data (_,_) trees) = concatMap f trees
+ where
+ f (name,tree) = flatten' (name:msgs) tree
+
+class Lookup a where
+ toAccessorTree :: a -> (b -> a) -> AccessorTree b
+
+ default toAccessorTree :: (Generic a, GLookup (Rep a)) => a -> (b -> a) -> AccessorTree b
+ toAccessorTree x f = gtoAccessorTree (from x) (from . f)
+
+class GLookup f where
+ gtoAccessorTree :: f a -> (b -> f a) -> AccessorTree b
+
+class GLookupS f where
+ gtoAccessorTreeS :: f a -> (b -> f a) -> [(String, AccessorTree b)]
+
+-- some instance from linear
+instance (Lookup a, Generic a) => Lookup (Linear.V0 a) where
+ toAccessorTree _ _ =
+ Data ("V0", "V0") []
+instance (Lookup a, Generic a) => Lookup (Linear.V1 a) where
+ toAccessorTree xyz f =
+ Data ("V1", "V1") [ ("x", toAccessorTree (getX xyz) (getX . f))
+ ]
+ where
+ getX (Linear.V1 x) = x
+instance (Lookup a, Generic a) => Lookup (Linear.V2 a) where
+ toAccessorTree xyz f =
+ Data ("V2", "V2") [ ("x", toAccessorTree (getX xyz) (getX . f))
+ , ("y", toAccessorTree (getY xyz) (getY . f))
+ ]
+ where
+ getX (Linear.V2 x _) = x
+ getY (Linear.V2 _ y) = y
+instance (Lookup a, Generic a) => Lookup (Linear.V3 a) where
+ toAccessorTree xyz f =
+ Data ("V3", "V3") [ ("x", toAccessorTree (getX xyz) (getX . f))
+ , ("y", toAccessorTree (getY xyz) (getY . f))
+ , ("z", toAccessorTree (getZ xyz) (getZ . f))
+ ]
+ where
+ getX (Linear.V3 x _ _) = x
+ getY (Linear.V3 _ y _) = y
+ getZ (Linear.V3 _ _ z) = z
+instance (Lookup a, Generic a) => Lookup (Linear.V4 a) where
+ toAccessorTree xyz f =
+ Data ("V4", "V4") [ ("x", toAccessorTree (getX xyz) (getX . f))
+ , ("y", toAccessorTree (getY xyz) (getY . f))
+ , ("z", toAccessorTree (getZ xyz) (getZ . f))
+ , ("w", toAccessorTree (getW xyz) (getW . f))
+ ]
+ where
+ getX (Linear.V4 x _ _ _) = x
+ getY (Linear.V4 _ y _ _) = y
+ getZ (Linear.V4 _ _ z _) = z
+ getW (Linear.V4 _ _ _ w) = w
+instance (Lookup a, Generic a) => Lookup (Linear.Quaternion a) where
+ toAccessorTree xyz f =
+ Data ("Quaternion", "Quaternion")
+ [ ("q0", toAccessorTree (getQ0 xyz) (getQ0 . f))
+ , ("q1", toAccessorTree (getQ1 xyz) (getQ1 . f))
+ , ("q2", toAccessorTree (getQ2 xyz) (getQ2 . f))
+ , ("q3", toAccessorTree (getQ3 xyz) (getQ3 . f))
+ ]
+ where
+ getQ0 (Linear.Quaternion q0 _) = q0
+ getQ1 (Linear.Quaternion _ (Linear.V3 x _ _)) = x
+ getQ2 (Linear.Quaternion _ (Linear.V3 _ y _)) = y
+ getQ3 (Linear.Quaternion _ (Linear.V3 _ _ z)) = z
+
+instance (Lookup a, Generic a) => Lookup (Rot f1 f2 a)
+instance (Lookup a, Generic a) => Lookup (V3T f a)
+instance (Lookup a, Generic a) => Lookup (Euler a)
+
+instance Lookup Float where
+ toAccessorTree _ f = Getter $ realToFrac . f
+instance Lookup Double where
+ toAccessorTree _ f = Getter $ realToFrac . f
+instance Lookup Int where
+ toAccessorTree _ f = Getter $ fromIntegral . f
+instance Lookup () where -- hack to get dummy tree
+ toAccessorTree _ _ = Getter $ const 0
+
+instance (Lookup f, Generic f) => GLookup (Rec0 f) where
+ gtoAccessorTree x f = toAccessorTree (unK1 x) (unK1 . f)
+
+instance (Selector s, GLookup a) => GLookupS (S1 s a) where
+ gtoAccessorTreeS x f = [(selname, gtoAccessorTree (unM1 x) (unM1 . f))]
+ where
+ selname = case selName x of
+ [] -> "()"
+ y -> y
+
+instance GLookupS U1 where
+ gtoAccessorTreeS _ _ = []
+
+instance (GLookupS f, GLookupS g) => GLookupS (f :*: g) where
+ gtoAccessorTreeS (x :*: y) f = tf ++ tg
+ where
+ tf = gtoAccessorTreeS x $ left . f
+ tg = gtoAccessorTreeS y $ right . f
+
+ left ( x' :*: _ ) = x'
+ right ( _ :*: y' ) = y'
+
+instance (Datatype d, Constructor c, GLookupS a) => GLookup (D1 d (C1 c a)) where
+ gtoAccessorTree d@(M1 c) f = Data (datatypeName d, conName c) con
+ where
+ con = gtoAccessorTreeS (unM1 c) (unM1 . unM1 . f)
+
+--data Xyz = Xyz { xx :: Int
+-- , yy :: Double
+-- , zz :: Float
+-- , ww :: Int
+-- } deriving (Generic)
+--data One = MkOne { one :: Double } deriving (Generic)
+--data Foo = MkFoo { aaa :: Int
+-- , bbb :: Xyz
+-- , ccc :: One
+-- } deriving (Generic)
+--instance Lookup One
+--instance Lookup Xyz
+--instance Lookup Foo
+--
+--foo :: Foo
+--foo = MkFoo 2 (Xyz 6 7 8 9) (MkOne 17)
+--
+--go = accessors foo
diff --git a/src/Dyno/Server/GraphWidget.hs b/src/Dyno/Server/GraphWidget.hs
new file mode 100644
--- /dev/null
+++ b/src/Dyno/Server/GraphWidget.hs
@@ -0,0 +1,359 @@
+{-# OPTIONS_GHC -Wall #-}
+
+module Dyno.Server.GraphWidget
+ ( newGraph
+ ) where
+
+import qualified Control.Concurrent as CC
+import Control.Monad ( when, unless )
+import qualified Data.IORef as IORef
+import Data.Maybe ( isJust, fromJust )
+import qualified Data.Tree as Tree
+import Graphics.UI.Gtk ( AttrOp( (:=) ) )
+import qualified Graphics.UI.Gtk as Gtk
+import System.Glib.Signals ( on )
+import Text.Read ( readMaybe )
+import qualified Data.Text as T
+import qualified Graphics.Rendering.Chart as Chart
+
+import Dyno.Server.PlotChart ( AxisScaling(..), displayChart, chartGtkUpdateCanvas )
+import Dyno.Server.PlotTypes ( GraphInfo(..), ListViewInfo(..), Message(..) )
+import Dyno.DirectCollocation.Dynamic ( CollTrajMeta(..), DynPlotPoints, MetaTree, forestFromMeta )
+
+-- This only concerns if we should rebuild the plot tree or not.
+-- The devectorization won't break because we always use the
+-- new meta to get the plot points
+sameMeta :: Maybe CollTrajMeta -> Maybe CollTrajMeta -> Bool
+sameMeta Nothing Nothing = True
+sameMeta (Just ctm0) (Just ctm1) =
+ and [ ctmX ctm0 == ctmX ctm1
+ , ctmZ ctm0 == ctmZ ctm1
+ , ctmU ctm0 == ctmU ctm1
+ , ctmP ctm0 == ctmP ctm1
+ , ctmO ctm0 == ctmO ctm1
+ ]
+sameMeta _ _ = False
+
+
+-- make a new graph window
+newGraph :: String -> Gtk.ListStore Message -> IO Gtk.Window
+newGraph channame msgStore = do
+ win <- Gtk.windowNew
+
+ _ <- Gtk.set win [ Gtk.containerBorderWidth := 8
+ , Gtk.windowTitle := channame
+ ]
+
+ -- mvar with all the user input
+ graphInfoMVar <- CC.newMVar GraphInfo { giXScaling = LinearScaling
+ , giYScaling = LinearScaling
+ , giXRange = Nothing
+ , giYRange = Nothing
+ , giGetters = []
+ }
+
+ let makeRenderable :: IO (Chart.Renderable ())
+ makeRenderable = do
+ gi <- CC.readMVar graphInfoMVar
+ size <- Gtk.listStoreGetSize msgStore
+
+ namePcs <- if size == 0
+ then return []
+ else do
+ Message datalog _ _ _ <- Gtk.listStoreGetValue msgStore 0
+ let f (name,getter) = (name, getter datalog :: [[(Double,Double)]])
+ return (map f (giGetters gi) :: [(String, [[(Double,Double)]])])
+ return $ displayChart (giXScaling gi, giYScaling gi) (giXRange gi, giYRange gi) namePcs
+
+ -- chart drawing area
+ chartCanvas <- Gtk.drawingAreaNew
+ _ <- Gtk.widgetSetSizeRequest chartCanvas 250 250
+
+ let redraw :: IO ()
+ redraw = do
+ renderable <- makeRenderable
+ chartGtkUpdateCanvas renderable chartCanvas
+
+ _ <- Gtk.onExpose chartCanvas $ const (redraw >> return True)
+
+
+ -- the options widget
+ optionsWidget <- makeOptionsWidget graphInfoMVar redraw
+ options <- Gtk.expanderNew "options"
+ Gtk.set options [ Gtk.containerChild := optionsWidget
+ , Gtk.expanderExpanded := False
+ ]
+
+
+ -- the signal selector
+ treeview' <- newSignalSelectorArea graphInfoMVar msgStore redraw
+ treeview <- Gtk.expanderNew "signals"
+ Gtk.set treeview [ Gtk.containerChild := treeview'
+ , Gtk.expanderExpanded := True
+ ]
+
+ -- options and signal selector packed in vbox
+ vboxOptionsAndSignals <- Gtk.vBoxNew False 4
+ Gtk.set vboxOptionsAndSignals
+ [ Gtk.containerChild := options
+ , Gtk.boxChildPacking options := Gtk.PackNatural
+ , Gtk.containerChild := treeview
+ , Gtk.boxChildPacking treeview := Gtk.PackGrow
+ ]
+
+ -- hbox to hold eveything
+ hboxEverything <- Gtk.hBoxNew False 4
+ Gtk.set hboxEverything
+ [ Gtk.containerChild := vboxOptionsAndSignals
+ , Gtk.boxChildPacking vboxOptionsAndSignals := Gtk.PackNatural
+ , Gtk.containerChild := chartCanvas
+ ]
+ _ <- Gtk.set win [ Gtk.containerChild := hboxEverything ]
+
+ Gtk.widgetShowAll win
+ return win
+
+
+
+newSignalSelectorArea ::
+ CC.MVar GraphInfo -> Gtk.ListStore Message -> IO () -> IO Gtk.ScrolledWindow
+newSignalSelectorArea graphInfoMVar msgStore redraw = do
+ treeStore <- Gtk.treeStoreNew []
+ treeview <- Gtk.treeViewNewWithModel treeStore
+
+ Gtk.treeViewSetHeadersVisible treeview True
+
+ -- add some columns
+ col1 <- Gtk.treeViewColumnNew
+ col2 <- Gtk.treeViewColumnNew
+
+ Gtk.treeViewColumnSetTitle col1 "signal"
+ Gtk.treeViewColumnSetTitle col2 "visible?"
+
+ renderer1 <- Gtk.cellRendererTextNew
+ renderer2 <- Gtk.cellRendererToggleNew
+
+ Gtk.cellLayoutPackStart col1 renderer1 True
+ Gtk.cellLayoutPackStart col2 renderer2 True
+
+ let showName (Just _) name _ = name
+ showName Nothing name "" = name
+ showName Nothing name typeName = name ++ " (" ++ typeName ++ ")"
+ Gtk.cellLayoutSetAttributes col1 renderer1 treeStore $
+ \(ListViewInfo {lviName = name, lviType = typeName, lviGetter = getter}) ->
+ [ Gtk.cellText := showName getter name typeName]
+ Gtk.cellLayoutSetAttributes col2 renderer2 treeStore $ \lvi -> [ Gtk.cellToggleActive := lviMarked lvi]
+
+ _ <- Gtk.treeViewAppendColumn treeview col1
+ _ <- Gtk.treeViewAppendColumn treeview col2
+
+
+ let -- update the graph information
+ updateGraphInfo = do
+ -- first get all trees
+ let getTrees k = do
+ tree' <- Gtk.treeStoreLookup treeStore [k]
+ case tree' of Nothing -> return []
+ Just tree -> fmap (tree:) (getTrees (k+1))
+ theTrees <- getTrees 0
+ let newGetters = [ (lviName lvi, fromJust $ lviGetter lvi)
+ | lvi <- concatMap Tree.flatten theTrees
+ , lviMarked lvi
+ , isJust (lviGetter lvi)
+ ]
+ _ <- CC.modifyMVar_ graphInfoMVar (\gi0 -> return $ gi0 { giGetters = newGetters })
+ return ()
+
+ -- update which y axes are visible
+ _ <- on renderer2 Gtk.cellToggled $ \pathStr -> do
+ let treePath = Gtk.stringToTreePath pathStr
+ -- toggle the check mark
+ let g lvi@(ListViewInfo _ _ Nothing _) = lvi
+ g lvi = lvi {lviMarked = not (lviMarked lvi)}
+ ret <- Gtk.treeStoreChange treeStore treePath g
+ unless ret $ putStrLn "treeStoreChange fail"
+ updateGraphInfo
+ redraw
+
+
+ -- rebuild the signal tree
+ let rebuildSignalTree :: MetaTree Double -> IO ()
+ rebuildSignalTree meta = do
+ let mkTreeNode (name,typeName,maybeget) = ListViewInfo name typeName maybeget False
+ newTrees :: [Tree.Tree (ListViewInfo (DynPlotPoints Double))]
+ newTrees = map (fmap mkTreeNode) meta
+ Gtk.treeStoreClear treeStore
+ Gtk.treeStoreInsertForest treeStore [] 0 newTrees
+ updateGraphInfo
+
+ oldMetaRef <- IORef.newIORef Nothing
+ let maybeRebuildSignalTree newMeta = do
+ oldMeta <- IORef.readIORef oldMetaRef
+ unless (sameMeta oldMeta (Just newMeta)) $ do
+ IORef.writeIORef oldMetaRef (Just newMeta)
+ rebuildSignalTree (forestFromMeta newMeta)
+
+ -- on insert or change, rebuild the signal tree
+ _ <- on msgStore Gtk.rowChanged $ \_ changedPath -> do
+ Message _ _ _ newMeta <- Gtk.listStoreGetValue msgStore (Gtk.listStoreIterToIndex changedPath)
+ maybeRebuildSignalTree newMeta >> redraw
+ _ <- on msgStore Gtk.rowInserted $ \_ changedPath -> do
+ Message _ _ _ newMeta <- Gtk.listStoreGetValue msgStore (Gtk.listStoreIterToIndex changedPath)
+ maybeRebuildSignalTree newMeta >> redraw
+
+ -- rebuild the signal tree right now if it exists
+ size <- Gtk.listStoreGetSize msgStore
+ when (size > 0) $ do
+ Message _ _ _ newMeta <- Gtk.listStoreGetValue msgStore 0
+ maybeRebuildSignalTree newMeta >> redraw
+
+
+ scroll <- Gtk.scrolledWindowNew Nothing Nothing
+ Gtk.containerAdd scroll treeview
+ Gtk.set scroll [ Gtk.scrolledWindowHscrollbarPolicy := Gtk.PolicyNever
+ , Gtk.scrolledWindowVscrollbarPolicy := Gtk.PolicyAutomatic
+ ]
+ return scroll
+
+
+
+makeOptionsWidget :: CC.MVar GraphInfo -> IO () -> IO Gtk.VBox
+makeOptionsWidget graphInfoMVar redraw = do
+ -- user selectable range
+ xRange <- Gtk.entryNew
+ yRange <- Gtk.entryNew
+ Gtk.set xRange [ Gtk.entryEditable := False
+ , Gtk.widgetSensitive := False
+ ]
+ Gtk.set yRange [ Gtk.entryEditable := False
+ , Gtk.widgetSensitive := False
+ ]
+ xRangeBox <- labeledWidget "x range:" xRange
+ yRangeBox <- labeledWidget "y range:" yRange
+ Gtk.set xRange [Gtk.entryText := "(-10,10)"]
+ Gtk.set yRange [Gtk.entryText := "(-10,10)"]
+ let updateXRange = do
+ Gtk.set xRange [ Gtk.entryEditable := True
+ , Gtk.widgetSensitive := True
+ ]
+ txt <- Gtk.get xRange Gtk.entryText
+ gi <- CC.readMVar graphInfoMVar
+ case readMaybe txt of
+ Nothing -> do
+ putStrLn $ "invalid x range entry: " ++ txt
+ Gtk.set xRange [Gtk.entryText := "(min,max)"]
+ Just (z0,z1) -> if z0 >= z1
+ then do
+ putStrLn $ "invalid x range entry (min >= max): " ++ txt
+ Gtk.set xRange [Gtk.entryText := "(min,max)"]
+ return ()
+ else do
+ _ <- CC.swapMVar graphInfoMVar (gi {giXRange = Just (z0,z1)})
+ redraw
+ let updateYRange = do
+ Gtk.set yRange [ Gtk.entryEditable := True
+ , Gtk.widgetSensitive := True
+ ]
+ txt <- Gtk.get yRange Gtk.entryText
+ gi <- CC.readMVar graphInfoMVar
+ case readMaybe txt of
+ Nothing -> do
+ putStrLn $ "invalid y range entry: " ++ txt
+ Gtk.set yRange [Gtk.entryText := "(min,max)"]
+ Just (z0,z1) -> if z0 >= z1
+ then do
+ putStrLn $ "invalid y range entry (min >= max): " ++ txt
+ Gtk.set yRange [Gtk.entryText := "(min,max)"]
+ return ()
+ else do
+ _ <- CC.swapMVar graphInfoMVar (gi {giYRange = Just (z0,z1)})
+ redraw
+ _ <- on xRange Gtk.entryActivate updateXRange
+ _ <- on yRange Gtk.entryActivate updateYRange
+
+ -- linear or log scaling on the x and y axis?
+ xScalingSelector <- Gtk.comboBoxNewText
+ yScalingSelector <- Gtk.comboBoxNewText
+ mapM_ (Gtk.comboBoxAppendText xScalingSelector . T.pack)
+ ["linear (auto)","linear (manual)","logarithmic (auto)"]
+ mapM_ (Gtk.comboBoxAppendText yScalingSelector . T.pack)
+ ["linear (auto)","linear (manual)","logarithmic (auto)"]
+ Gtk.comboBoxSetActive xScalingSelector 0
+ Gtk.comboBoxSetActive yScalingSelector 0
+ xScalingBox <- labeledWidget "x scaling:" xScalingSelector
+ yScalingBox <- labeledWidget "y scaling:" yScalingSelector
+ let updateXScaling = do
+ k <- Gtk.comboBoxGetActive xScalingSelector
+ _ <- case k of
+ 0 -> do
+ Gtk.set xRange [ Gtk.entryEditable := False
+ , Gtk.widgetSensitive := False
+ ]
+ CC.modifyMVar_ graphInfoMVar $
+ \gi -> return $ gi {giXScaling = LinearScaling, giXRange = Nothing}
+ 1 -> do
+ CC.modifyMVar_ graphInfoMVar $
+ \gi -> return $ gi {giXScaling = LinearScaling, giXRange = Nothing}
+ updateXRange
+ 2 -> do
+ Gtk.set xRange [ Gtk.entryEditable := False
+ , Gtk.widgetSensitive := False
+ ]
+ CC.modifyMVar_ graphInfoMVar $
+ \gi -> return $ gi {giXScaling = LogScaling, giXRange = Nothing}
+ _ -> error "the \"impossible\" happened: x scaling comboBox index should be < 3"
+ redraw
+ let updateYScaling = do
+ k <- Gtk.comboBoxGetActive yScalingSelector
+ _ <- case k of
+ 0 -> do
+ Gtk.set yRange [ Gtk.entryEditable := False
+ , Gtk.widgetSensitive := False
+ ]
+ CC.modifyMVar_ graphInfoMVar $
+ \gi -> return $ gi {giYScaling = LinearScaling, giYRange = Nothing}
+ 1 -> do
+ CC.modifyMVar_ graphInfoMVar $
+ \gi -> return $ gi {giYScaling = LinearScaling, giYRange = Nothing}
+ updateYRange
+ 2 -> do
+ Gtk.set yRange [ Gtk.entryEditable := False
+ , Gtk.widgetSensitive := False
+ ]
+ CC.modifyMVar_ graphInfoMVar $
+ \gi -> return $ gi {giYScaling = LogScaling, giYRange = Nothing}
+ _ -> error "the \"impossible\" happened: y scaling comboBox index should be < 3"
+ redraw
+ updateXScaling
+ updateYScaling
+ _ <- on xScalingSelector Gtk.changed updateXScaling
+ _ <- on yScalingSelector Gtk.changed updateYScaling
+
+ -- vbox to hold the little window on the left
+ vbox <- Gtk.vBoxNew False 4
+
+ Gtk.set vbox [ Gtk.containerChild := xScalingBox
+ , Gtk.boxChildPacking xScalingBox := Gtk.PackNatural
+ , Gtk.containerChild := xRangeBox
+ , Gtk.boxChildPacking xRangeBox := Gtk.PackNatural
+ , Gtk.containerChild := yScalingBox
+ , Gtk.boxChildPacking yScalingBox := Gtk.PackNatural
+ , Gtk.containerChild := yRangeBox
+ , Gtk.boxChildPacking yRangeBox := Gtk.PackNatural
+ ]
+
+ return vbox
+
+
+
+-- helper to make an hbox with a label
+labeledWidget :: Gtk.WidgetClass a => String -> a -> IO Gtk.HBox
+labeledWidget name widget = do
+ label <- Gtk.labelNew (Just name)
+ hbox <- Gtk.hBoxNew False 4
+ Gtk.set hbox [ Gtk.containerChild := label
+ , Gtk.containerChild := widget
+ , Gtk.boxChildPacking label := Gtk.PackNatural
+-- , Gtk.boxChildPacking widget := Gtk.PackNatural
+ ]
+ return hbox
diff --git a/src/Dyno/Server/PlotChart.hs b/src/Dyno/Server/PlotChart.hs
new file mode 100644
--- /dev/null
+++ b/src/Dyno/Server/PlotChart.hs
@@ -0,0 +1,75 @@
+{-# OPTIONS_GHC -Wall #-}
+
+module Dyno.Server.PlotChart
+ ( AxisScaling(..)
+ , displayChart
+ , chartGtkUpdateCanvas
+ ) where
+
+import Control.Lens ( (.~) )
+import Data.Default.Class ( def )
+--import qualified Data.Foldable as F
+--import qualified Data.Sequence as S
+import qualified Graphics.UI.Gtk as Gtk
+import qualified Graphics.Rendering.Chart as Chart
+import Graphics.Rendering.Chart.Backend.Cairo ( runBackend, defaultEnv )
+import Graphics.Rendering.Cairo hiding (width, height)
+ --( Render, Format(..)
+ --, renderWith, setSourceSurface, withImageSurface )
+
+import Dyno.Server.PlotTypes ( AxisScaling(..) )
+
+chartGtkUpdateCanvas :: Chart.Renderable () -> Gtk.DrawingArea -> IO ()
+chartGtkUpdateCanvas chart canvas = do
+ Gtk.threadsEnter
+ maybeWin <- Gtk.widgetGetWindow canvas
+ case maybeWin of
+ Nothing -> Gtk.threadsLeave >> return ()
+ Just win -> do
+ (width, height) <- Gtk.widgetGetSize canvas
+ regio <- Gtk.regionRectangle $ Gtk.Rectangle 0 0 width height
+ Gtk.threadsLeave
+ let sz = (fromIntegral width,fromIntegral height)
+ let render0 :: Render (Chart.PickFn ())
+ render0 = runBackend (defaultEnv Chart.bitmapAlignmentFns) (Chart.render chart sz)
+
+ withImageSurface FormatARGB32 width height $ \surface -> do
+ _ <- renderWith surface render0
+ let render1 = setSourceSurface surface 0 0 >> paint
+ Gtk.threadsEnter
+ Gtk.drawWindowBeginPaintRegion win regio
+ _ <- Gtk.renderWithDrawable win render1
+ Gtk.drawWindowEndPaint win
+ Gtk.threadsLeave
+
+displayChart :: (Chart.PlotValue a, Show a, RealFloat a) =>
+ (AxisScaling, AxisScaling) -> (Maybe (a,a),Maybe (a,a)) ->
+ [(String, [[(a,a)]])] -> Chart.Renderable ()
+displayChart (xScaling,yScaling) (xRange,yRange) namePcs = Chart.toRenderable layout
+ where
+ drawOne (name,pc) col
+ = Chart.plot_lines_values .~ pc
+ $ Chart.plot_lines_style . Chart.line_color .~ col
+-- $ Chart.plot_points_style ~. Chart.filledCircles 2 red
+ $ Chart.plot_lines_title .~ name
+ $ def
+ allLines = zipWith drawOne namePcs Chart.defaultColorSeq
+
+ xscaleFun = case xScaling of
+ LogScaling -> Chart.layout_x_axis . Chart.laxis_generate .~ Chart.autoScaledLogAxis def
+ LinearScaling -> case xRange of
+ Nothing -> id
+ Just range -> Chart.layout_x_axis . Chart.laxis_generate .~ Chart.scaledAxis def range
+
+ yscaleFun = case yScaling of
+ LogScaling -> Chart.layout_y_axis . Chart.laxis_generate .~ Chart.autoScaledLogAxis def
+ LinearScaling -> case yRange of
+ Nothing -> id
+ Just range -> Chart.layout_y_axis . Chart.laxis_generate .~ Chart.scaledAxis def range
+
+ layout = Chart.layout_plots .~ map Chart.toPlot allLines
+-- $ Chart.layout_title .~ "Wooo, Party Graph!"
+ $ Chart.layout_x_axis . Chart.laxis_title .~ "time [s]"
+ $ xscaleFun
+ $ yscaleFun
+ def
diff --git a/src/Dyno/Server/PlotTypes.hs b/src/Dyno/Server/PlotTypes.hs
new file mode 100644
--- /dev/null
+++ b/src/Dyno/Server/PlotTypes.hs
@@ -0,0 +1,47 @@
+{-# OPTIONS_GHC -Wall #-}
+--{-# Language ExistentialQuantification #-}
+--{-# Language GADTs #-}
+
+module Dyno.Server.PlotTypes
+ ( Channel(..)
+ , Message(..)
+ , GraphInfo(..)
+ , ListViewInfo(..)
+ , AxisScaling(..)
+ , MetaTree
+-- , XAxisType(..)
+ ) where
+
+import Data.Time ( NominalDiffTime )
+import qualified Graphics.UI.Gtk as Gtk
+
+import Dyno.DirectCollocation.Dynamic ( DynPlotPoints, CollTrajMeta, MetaTree )
+
+data ListViewInfo a = ListViewInfo { lviName :: String
+ , lviType :: String
+ , lviGetter :: Maybe (a -> [[(Double,Double)]])
+ , lviMarked :: Bool
+ }
+
+--data XAxisType a = XAxisTime
+-- | XAxisCounter
+-- | XAxisStaticCounter
+-- | XAxisFun (String, a -> Double)
+
+data AxisScaling = LogScaling
+ | LinearScaling
+
+-- what the graph should draw
+data GraphInfo =
+ GraphInfo { giXScaling :: AxisScaling
+ , giYScaling :: AxisScaling
+ , giXRange :: Maybe (Double,Double)
+ , giYRange :: Maybe (Double,Double)
+ , giGetters :: [(String, DynPlotPoints Double -> [[(Double,Double)]])]
+ }
+
+data Message = Message (DynPlotPoints Double) Int NominalDiffTime CollTrajMeta
+data Channel =
+ Channel { chanName :: String
+ , chanMsgStore :: Gtk.ListStore Message
+ }
diff --git a/src/Dyno/Server/Server.hs b/src/Dyno/Server/Server.hs
new file mode 100644
--- /dev/null
+++ b/src/Dyno/Server/Server.hs
@@ -0,0 +1,174 @@
+{-# OPTIONS_GHC -Wall #-}
+
+module Dyno.Server.Server
+ ( newChannel
+ , runPlotter
+ , Channel
+ ) where
+
+import Data.Vector ( Vector )
+import qualified Control.Concurrent as CC
+import qualified Data.IORef as IORef
+import Data.Time ( getCurrentTime, diffUTCTime )
+import Graphics.UI.Gtk ( AttrOp( (:=) ) )
+import qualified Graphics.UI.Gtk as Gtk
+import System.Glib.Signals ( on )
+--import System.IO ( withFile, IOMode ( WriteMode ) )
+--import qualified Data.ByteString.Lazy as BSL
+
+import qualified GHC.Stats
+
+import Dyno.Server.PlotTypes ( Channel(..), Message(..) )
+import Dyno.Server.GraphWidget ( newGraph )
+import Dyno.DirectCollocation.Dynamic ( DynCollTraj(..), CollTrajMeta(..)
+ , dynPlotPoints, catDynPlotPoints )
+
+newChannel ::
+ String -> IO (Channel, ([DynCollTraj (Vector Double)], CollTrajMeta) -> IO ())
+newChannel name = do
+ time0 <- getCurrentTime
+
+ msgStore <- Gtk.listStoreNew []
+ counter <- IORef.newIORef 0
+
+ let newMessage :: ([DynCollTraj (Vector Double)], CollTrajMeta) -> IO ()
+ newMessage (newTrajs, newMeta) = do
+ -- grab the time and counter
+ time <- getCurrentTime
+ k <- IORef.readIORef counter
+ IORef.writeIORef counter (k+1)
+ Gtk.postGUIAsync $ do
+ let pps = catDynPlotPoints $ map (flip dynPlotPoints newMeta) newTrajs
+ val = Message pps k (diffUTCTime time time0) newMeta
+ size <- Gtk.listStoreGetSize msgStore
+ if size == 0
+ then Gtk.listStorePrepend msgStore val
+ else Gtk.listStoreSetValue msgStore 0 val
+
+ let retChan = Channel { chanName = name
+ , chanMsgStore = msgStore
+ }
+
+ return (retChan, newMessage)
+
+runPlotter :: Channel -> [CC.ThreadId] -> IO ()
+runPlotter channel backgroundThreadsToKill = do
+ statsEnabled <- GHC.Stats.getGCStatsEnabled
+ if statsEnabled
+ then do putStrLn "stats enabled"
+ stats <- GHC.Stats.getGCStats
+ print stats
+ else putStrLn "stats not enabled"
+
+ _ <- Gtk.initGUI
+ _ <- Gtk.timeoutAddFull (CC.yield >> return True) Gtk.priorityDefault 50
+
+ -- start the main window
+ win <- Gtk.windowNew
+ _ <- Gtk.set win [ Gtk.containerBorderWidth := 8
+ , Gtk.windowTitle := "Plot-ho-matic"
+ ]
+
+ -- on close, kill all the windows and threads
+ graphWindowsToBeKilled <- CC.newMVar []
+ let killEverything = do
+ gws <- CC.readMVar graphWindowsToBeKilled
+ mapM_ Gtk.widgetDestroy gws
+ mapM_ CC.killThread backgroundThreadsToKill
+ Gtk.mainQuit
+ _ <- Gtk.onDestroy win killEverything
+
+ --------------- main widget -----------------
+ -- button to clear history
+ buttonClear <- Gtk.buttonNewWithLabel "clear history"
+ _ <- Gtk.onClicked buttonClear $ do
+ --let clearChan (Channel {chanSeq=cs}) = void (CC.swapMVar cs Seq.empty)
+ let clearChan _ = putStrLn "yeah, history clear doesn't really exist lol"
+ clearChan channel
+
+ -- list of channels
+ chanWidget <- newChannelWidget channel graphWindowsToBeKilled
+
+ -- vbox to hold buttons
+ vbox <- Gtk.vBoxNew False 4
+ Gtk.set vbox [ Gtk.containerChild := buttonClear
+ , Gtk.containerChild := chanWidget
+ ]
+
+ -- add widget to window and show
+ _ <- Gtk.set win [ Gtk.containerChild := vbox ]
+ Gtk.widgetShowAll win
+ Gtk.mainGUI
+
+
+-- the list of channels
+newChannelWidget :: Channel -> CC.MVar [Gtk.Window] -> IO Gtk.TreeView
+newChannelWidget channel graphWindowsToBeKilled = do
+ -- create a new tree model
+ model <- Gtk.listStoreNew [channel]
+ treeview <- Gtk.treeViewNewWithModel model
+ Gtk.treeViewSetHeadersVisible treeview True
+
+ -- add some columns
+ col0 <- Gtk.treeViewColumnNew
+ col1 <- Gtk.treeViewColumnNew
+ col2 <- Gtk.treeViewColumnNew
+ col3 <- Gtk.treeViewColumnNew
+
+ Gtk.treeViewColumnSetTitle col0 "channel"
+ Gtk.treeViewColumnSetTitle col1 "history"
+ Gtk.treeViewColumnSetTitle col2 "new"
+ Gtk.treeViewColumnSetTitle col3 "save"
+
+ renderer0 <- Gtk.cellRendererTextNew
+ renderer1 <- Gtk.cellRendererTextNew
+ renderer2 <- Gtk.cellRendererToggleNew
+ renderer3 <- Gtk.cellRendererToggleNew
+
+ Gtk.cellLayoutPackStart col0 renderer0 True
+ Gtk.cellLayoutPackStart col1 renderer1 True
+ Gtk.cellLayoutPackStart col2 renderer2 True
+ Gtk.cellLayoutPackStart col3 renderer3 True
+
+ Gtk.cellLayoutSetAttributes col0 renderer0 model $ \lv -> [ Gtk.cellText := chanName lv]
+ Gtk.cellLayoutSetAttributes col2 renderer2 model $ const [ Gtk.cellToggleActive := False]
+ Gtk.cellLayoutSetAttributes col3 renderer3 model $ const [ Gtk.cellToggleActive := False]
+
+
+ _ <- Gtk.treeViewAppendColumn treeview col0
+ _ <- Gtk.treeViewAppendColumn treeview col1
+ _ <- Gtk.treeViewAppendColumn treeview col2
+ _ <- Gtk.treeViewAppendColumn treeview col3
+
+ -- spawn a new graph when a checkbox is clicked
+ _ <- on renderer2 Gtk.cellToggled $ \pathStr -> do
+ let (i:_) = Gtk.stringToTreePath pathStr
+ lv <- Gtk.listStoreGetValue model i
+ graphWin <- newGraph (chanName lv) (chanMsgStore lv)
+
+ -- add this window to the list to be killed on exit
+ CC.modifyMVar_ graphWindowsToBeKilled (return . (graphWin:))
+
+
+-- -- save all channel data when this button is pressed
+-- _ <- on renderer3 Gtk.cellToggled $ \pathStr -> do
+-- let (i:_) = Gtk.stringToTreePath pathStr
+-- lv <- Gtk.listStoreGetValue model i
+-- let writerThread = do
+-- bct <- chanGetByteStrings (lvChan lv)
+-- let filename = chanName (lvChan lv) ++ "_log.dat"
+-- blah _ sizes [] = return (reverse sizes)
+-- blah handle sizes ((x,_,_):xs) = do
+-- BSL.hPut handle x
+-- blah handle (BSL.length x : sizes) xs
+-- putStrLn $ "trying to write file \"" ++ filename ++ "\"..."
+-- sizes <- withFile filename WriteMode $ \handle -> blah handle [] bct
+-- putStrLn $ "finished writing file, wrote " ++ show (length sizes) ++ " protos"
+--
+-- putStrLn "writing file with sizes..."
+-- writeFile (filename ++ ".sizes") (unlines $ map show sizes)
+-- putStrLn "done"
+-- _ <- CC.forkIO writerThread
+ return ()
+
+ return treeview
diff --git a/src/Dyno/Solvers.hs b/src/Dyno/Solvers.hs
new file mode 100644
--- /dev/null
+++ b/src/Dyno/Solvers.hs
@@ -0,0 +1,52 @@
+{-# OPTIONS_GHC -Wall #-}
+
+module Dyno.Solvers ( NlpSolverStuff(..), ipoptSolver, snoptSolver ) where
+
+--import qualified Data.Vector as V
+
+import Dyno.NlpSolver ( NlpSolverStuff(..), Opt(..) )
+
+snoptSolver :: NlpSolverStuff
+snoptSolver =
+ NlpSolverStuff
+ { solverName = "snopt"
+ , defaultOptions = [ -- ("_iprint", Opt (0::Int))
+-- , ("_isumm", Opt (6::Int))
+-- , ("_scale_option", Opt (0::Int))
+-- , ("_major_iteration_limit", Opt (3 :: Int))
+-- , ("_minor_iteration_limit", Opt (2000 :: Int))
+-- , ("_verify_level", Opt (2 :: Int))
+-- , ("_optimality_tolerance", Opt (1e-1 :: Double))
+-- , ("_feasibility_tolerance", Opt (1e-1 :: Double))
+-- , ("detect_linear", Opt False)
+-- , ("monitor", Opt (V.fromList ["setup_nlp"]) )
+-- , ("_start", Opt "Warm")
+ ]
+ , options = []
+ , solverInterruptCode = -2
+ , successCodes = ["1"]
+ , functionOptions = []
+ , functionCall = const (return ())
+ }
+
+ipoptSolver :: NlpSolverStuff
+ipoptSolver =
+ NlpSolverStuff
+ { solverName = "ipopt"
+ , defaultOptions = [ ("max_iter", Opt (3000 :: Int))
+ , ("tol", Opt (1e-9 :: Double))
+-- , ("hessian_approximation", Opt "limited-memory")
+-- , ("expand", Opt True)
+-- , ("linear_solver", Opt "ma27")
+-- , ("linear_solver", Opt "ma57")
+-- , ("linear_solver", Opt "ma86")
+-- , ("linear_solver", Opt "ma97")
+-- , ("fixed_variable_treatment", Opt "make_constraint") -- causes segfaults?
+-- , ("fixed_variable_treatment", Opt "make_parameter")
+ ]
+ , options = []
+ , solverInterruptCode = 1
+ , successCodes = ["Solve_Succeeded", "Solved_To_Acceptable_Level"]
+ , functionOptions = []
+ , functionCall = const (return ())
+ }
diff --git a/src/Dyno/TypeVecs.hs b/src/Dyno/TypeVecs.hs
new file mode 100644
--- /dev/null
+++ b/src/Dyno/TypeVecs.hs
@@ -0,0 +1,268 @@
+{-# OPTIONS_GHC -Wall #-}
+{-# LANGUAGE ScopedTypeVariables #-}
+{-# LANGUAGE RankNTypes #-}
+{-# LANGUAGE KindSignatures #-}
+{-# LANGUAGE FlexibleContexts #-}
+{-# LANGUAGE DeriveGeneric #-}
+{-# LANGUAGE DeriveTraversable #-}
+{-# LANGUAGE GeneralizedNewtypeDeriving #-}
+{-# LANGUAGE UndecidableInstances #-}
+
+module Dyno.TypeVecs
+ ( Vec
+ , Succ
+ , unSeq
+ , mkSeq
+ , mkUnit
+ , unVec
+ , mkVec
+ , mkVec'
+ , unsafeVec
+ , tvlength
+ , (|>)
+ , (<|)
+ , tvtranspose
+ , tvzip
+ , tvzip3
+ , tvzip4
+ , tvzipWith
+ , tvzipWith3
+ , tvzipWith4
+ , tvzipWith5
+ , tvzipWith6
+ , tvunzip
+ , tvunzip3
+ , tvunzip4
+ , tvunzip5
+ , tvhead
+ , tvtail
+ , tvlast
+ , tvshiftl
+ , tvshiftr
+ , tvlinspace
+ , reifyVector
+ , reifyDim
+ , Dim(..)
+ )
+ where
+
+import Control.Applicative
+import Data.Foldable ( Foldable )
+import Data.Traversable ( Traversable )
+import qualified Data.Traversable as T
+import qualified Data.Foldable as F
+import qualified Data.Sequence as S
+import qualified Data.Vector as V
+import Data.Serialize ( Serialize )
+import Linear.Vector
+import Linear.V ( Dim(..) )
+import Data.Proxy
+import Data.Reflection as R
+import GHC.Generics ( Generic )
+import Data.Distributive ( Distributive(..) )
+
+import Dyno.Vectorize
+
+-- length-indexed vectors using phantom types
+newtype Vec n a = MkVec {unSeq :: S.Seq a}
+ deriving (Eq, Ord, Functor, Traversable, Foldable, Generic, Generic1)
+instance Serialize a => Serialize (Vec n a)
+
+instance Dim n => Distributive (Vec n) where
+ distribute f = mkVec $ V.generate (reflectDim (Proxy :: Proxy n))
+ $ \i -> fmap (\v -> V.unsafeIndex (vectorize v) i) f
+ {-# INLINE distribute #-}
+
+data Succ n
+instance Dim n => Dim (Succ n) where
+ reflectDim _ = 1 + reflectDim (Proxy :: Proxy n)
+
+instance Dim n => Dim (Vec n a) where
+ reflectDim _ = reflectDim (Proxy :: Proxy n)
+
+instance Dim n => Applicative (Vec n) where
+ pure x = ret
+ where
+ ret = MkVec $ S.replicate (tvlength ret) x
+ MkVec xs <*> MkVec ys = MkVec $ S.zipWith id xs ys
+
+instance Dim n => Additive (Vec n) where
+ zero = pure 0
+ MkVec xs ^+^ MkVec ys = MkVec (S.zipWith (+) xs ys)
+ MkVec xs ^-^ MkVec ys = MkVec (S.zipWith (-) xs ys)
+
+instance Dim n => Vectorize (Vec n) where
+ vectorize = unVec
+ devectorize = mkVec
+ empty = pure ()
+
+tvtranspose :: Vec n (Vec m a) -> Vec m (Vec n a)
+tvtranspose vec = mkVec $ fmap mkVec $ T.sequence (unVec (fmap unVec vec))
+
+unVec :: Vec n a -> V.Vector a
+unVec = V.fromList . F.toList . unSeq
+
+infixr 5 <|
+infixl 5 |>
+(<|) :: a -> Vec n a -> Vec (Succ n) a
+(<|) x xs = MkVec $ x S.<| unSeq xs
+
+(|>) :: Vec n a -> a -> Vec (Succ n) a
+(|>) xs x = MkVec $ unSeq xs S.|> x
+
+-- create a Vec with a runtime check
+unsafeVec :: Dim n => V.Vector a -> Vec n a
+unsafeVec = unsafeSeq . S.fromList . V.toList
+
+unsafeSeq :: Dim n => S.Seq a -> Vec n a
+unsafeSeq xs = case MkVec xs of
+ ret -> let staticLen = tvlength ret
+ dynLen = S.length xs
+ in if staticLen == dynLen
+ then ret
+ else error $ "unsafeVec: static/dynamic length mismatch: " ++
+ "static: " ++ show staticLen ++ ", dynamic: " ++ show dynLen
+
+mkUnit :: Vec n a -> Vec () a
+mkUnit (MkVec v) = MkVec v
+
+mkVec :: V.Vector a -> Vec n a
+mkVec = MkVec . S.fromList . V.toList
+
+mkVec' :: Dim n => [a] -> Vec n a
+mkVec' = MkVec . S.fromList
+
+mkSeq :: S.Seq a -> Vec n a
+mkSeq = MkVec
+
+-- --mkVec :: (IntegerT n) => V.Vector a -> Vec n a
+-- --mkVec = unsafeVec -- lets just run the check every time for now
+--
+-- --mkSeq :: (IntegerT n) => S.Seq a -> Vec n a
+-- --mkSeq = unsafeSeq -- lets just run the check every time for now
+
+tvlength :: forall n a. Dim n => Vec n a -> Int
+tvlength _ = reflectDim (Proxy :: Proxy n)
+
+tvzip :: Vec n a -> Vec n b -> Vec n (a,b)
+tvzip x y = mkSeq (S.zip (unSeq x) (unSeq y))
+
+tvzip3 :: Vec n a -> Vec n b -> Vec n c -> Vec n (a,b,c)
+tvzip3 x y z = mkSeq (S.zip3 (unSeq x) (unSeq y) (unSeq z))
+
+tvzip4 :: Vec n a -> Vec n b -> Vec n c -> Vec n d -> Vec n (a,b,c,d)
+tvzip4 x y z w = mkSeq (S.zip4 (unSeq x) (unSeq y) (unSeq z) (unSeq w))
+
+tvzipWith :: (a -> b -> c) -> Vec n a -> Vec n b -> Vec n c
+tvzipWith f x y = mkSeq (S.zipWith f (unSeq x) (unSeq y))
+
+tvzipWith3 :: (a -> b -> c -> d) -> Vec n a -> Vec n b -> Vec n c -> Vec n d
+tvzipWith3 f x y z = mkSeq (S.zipWith3 f (unSeq x) (unSeq y) (unSeq z))
+
+tvzipWith4 :: (a -> b -> c -> d -> e) -> Vec n a -> Vec n b -> Vec n c -> Vec n d -> Vec n e
+tvzipWith4 f x y z u = mkSeq (S.zipWith4 f (unSeq x) (unSeq y) (unSeq z) (unSeq u))
+
+tvzipWith5 :: (a -> b -> c -> d -> e -> f)
+ -> Vec n a -> Vec n b -> Vec n c -> Vec n d -> Vec n e -> Vec n f
+tvzipWith5 f x0 x1 x2 x3 x4 =
+ mkSeq (szipWith5 f (unSeq x0) (unSeq x1) (unSeq x2) (unSeq x3) (unSeq x4))
+ where
+ szipWith5 :: (a -> b -> c -> d -> e -> f)
+ -> S.Seq a -> S.Seq b -> S.Seq c -> S.Seq d -> S.Seq e -> S.Seq f
+ szipWith5 f' s1 s2 s3 s4 s5 =
+ S.zipWith ($) (S.zipWith ($) (S.zipWith ($) (S.zipWith f' s1 s2) s3) s4) s5
+
+tvzipWith6 :: (a -> b -> c -> d -> e -> f -> g)
+ -> Vec n a -> Vec n b -> Vec n c -> Vec n d -> Vec n e -> Vec n f -> Vec n g
+tvzipWith6 f x0 x1 x2 x3 x4 x5 =
+ mkSeq (szipWith6 f (unSeq x0) (unSeq x1) (unSeq x2) (unSeq x3) (unSeq x4) (unSeq x5))
+ where
+ szipWith6 :: (a -> b -> c -> d -> e -> f -> g)
+ -> S.Seq a -> S.Seq b -> S.Seq c -> S.Seq d -> S.Seq e -> S.Seq f -> S.Seq g
+ szipWith6 f' s1 s2 s3 s4 s5 s6 =
+ S.zipWith ($) (S.zipWith ($) (S.zipWith ($) (S.zipWith ($) (S.zipWith f' s1 s2) s3) s4) s5) s6
+
+
+
+
+
+
+tvunzip :: Vec n (a,b) -> (Vec n a, Vec n b)
+tvunzip v = (mkVec v1, mkVec v2)
+ where
+ (v1,v2) = V.unzip (unVec v)
+
+tvunzip3 :: Vec n (a,b,c) -> (Vec n a, Vec n b, Vec n c)
+tvunzip3 v = (mkVec v1, mkVec v2, mkVec v3)
+ where
+ (v1,v2,v3) = V.unzip3 (unVec v)
+
+tvunzip4 :: Vec n (a,b,c,d) -> (Vec n a, Vec n b, Vec n c, Vec n d)
+tvunzip4 v = (mkVec v1, mkVec v2, mkVec v3, mkVec v4)
+ where
+ (v1,v2,v3,v4) = V.unzip4 (unVec v)
+
+tvunzip5 :: Vec n (a,b,c,d,e) -> (Vec n a, Vec n b, Vec n c, Vec n d, Vec n e)
+tvunzip5 v = (mkVec v1, mkVec v2, mkVec v3, mkVec v4, mkVec v5)
+ where
+ (v1,v2,v3,v4,v5) = V.unzip5 (unVec v)
+
+tvhead :: Vec n a -> a
+tvhead x = case S.viewl (unSeq x) of
+ y S.:< _ -> y
+ S.EmptyL -> error "vhead: empty"
+
+tvtail :: Dim n => Vec (Succ n) a -> Vec n a
+tvtail x = case S.viewl (unSeq x) of
+ _ S.:< ys -> mkSeq ys
+ S.EmptyL -> error "vtail: empty"
+
+tvlast :: Vec n a -> a
+tvlast x = case S.viewr (unSeq x) of
+ _ S.:> y -> y
+ S.EmptyR -> error "vlast: empty"
+
+tvshiftl :: Dim n => Vec n a -> a -> Vec n a
+tvshiftl xs x = case S.viewl (unSeq xs) of
+ _ S.:< ys -> mkSeq (ys S.|> x)
+ S.EmptyL -> error "tvshiftl: EmptyL"
+
+tvshiftr :: Dim n => a -> Vec n a -> Vec n a
+tvshiftr x xs = case S.viewr (unSeq xs) of
+ ys S.:> _ -> mkSeq (x S.<| ys)
+ S.EmptyR -> error "tvshiftr: EmptyR"
+
+instance Show a => Show (Vec n a) where
+ showsPrec _ = showV . F.toList . unSeq
+ where
+ showV [] = showString "<>"
+ showV (x:xs) = showChar '<' . shows x . showl xs
+ where
+ showl [] = showChar '>'
+ showl (y:ys) = showChar ',' . shows y . showl ys
+
+data ReifiedDim (s :: *)
+
+retagDim :: (Proxy s -> a) -> proxy (ReifiedDim s) -> a
+retagDim f _ = f Proxy
+{-# INLINE retagDim #-}
+
+instance Reifies s Int => Dim (ReifiedDim s) where
+ reflectDim = retagDim reflect
+ {-# INLINE reflectDim #-}
+
+reifyDim :: Int -> (forall (n :: *). Dim n => Proxy n -> r) -> r
+reifyDim i f = R.reify i (go f) where
+ go :: Reifies n Int => (Proxy (ReifiedDim n) -> a) -> proxy n -> a
+ go g _ = g Proxy
+{-# INLINE reifyDim #-}
+
+reifyVector :: forall a r. V.Vector a -> (forall (n :: *). Dim n => Vec n a -> r) -> r
+reifyVector v f = reifyDim (V.length v) $ \(Proxy :: Proxy n) -> f (MkVec (S.fromList (V.toList v)) :: Vec n a)
+{-# INLINE reifyVector #-}
+
+tvlinspace :: forall n a . (Dim n, Fractional a) => a -> a -> Vec n a
+tvlinspace x0 xf = mkVec' [x0 + h * fromIntegral k | k <- take n [(0::Int)..]]
+ where
+ n = reflectDim (Proxy :: Proxy n)
+ h = (xf - x0) / fromIntegral (n - 1)
diff --git a/src/Dyno/Vectorize.hs b/src/Dyno/Vectorize.hs
new file mode 100644
--- /dev/null
+++ b/src/Dyno/Vectorize.hs
@@ -0,0 +1,245 @@
+{-# OPTIONS_GHC -Wall #-}
+{-# LANGUAGE ScopedTypeVariables #-}
+{-# LANGUAGE KindSignatures #-}
+{-# LANGUAGE DefaultSignatures #-}
+{-# LANGUAGE TypeOperators #-}
+{-# LANGUAGE FlexibleContexts #-}
+{-# LANGUAGE DeriveGeneric #-}
+{-# LANGUAGE DeriveFunctor #-}
+{-# LANGUAGE DeriveFoldable #-}
+{-# LANGUAGE DeriveTraversable #-}
+
+module Dyno.Vectorize
+ ( Vectorize(..)
+ , None(..)
+ , Id(..)
+ , Tuple(..)
+ , Triple(..)
+ , vlength
+ , vzipWith
+ , vzipWith3
+ , vzipWith4
+ , fill
+ , GVectorize(..)
+ , Generic1
+ , Proxy(..)
+ ) where
+
+import Control.Applicative ( Applicative(..) )
+import GHC.Generics
+import qualified Data.Vector as V
+import Data.Foldable ( Foldable )
+import Data.Traversable ( Traversable )
+import Data.Proxy ( Proxy(..) )
+import qualified Linear
+
+import SpatialMath ( Euler )
+import SpatialMathT ( V3T, Rot )
+
+import Dyno.Server.Accessors
+
+-- | a length-0 vectorizable type
+data None a = None
+ deriving (Eq, Ord, Generic, Generic1, Functor, Foldable, Traversable, Show)
+instance Vectorize None
+instance Applicative None where
+ pure = const None
+ (<*>) = const (const None)
+instance Linear.Additive None where
+
+-- | a length-1 vectorizable type
+newtype Id a = Id a
+ deriving (Eq, Ord, Generic, Generic1, Functor, Foldable, Traversable, Show)
+instance Vectorize Id
+instance Applicative Id where
+ pure = Id
+ Id fx <*> Id x = Id (fx x)
+instance Linear.Additive Id where
+
+
+-- | a length-2 vectorizable type
+data Tuple f g a = Tuple (f a) (g a)
+ deriving (Eq, Ord, Generic, Generic1, Functor, Foldable, Traversable, Show)
+instance (Vectorize f, Vectorize g) => Vectorize (Tuple f g)
+instance (Applicative f, Applicative g) => Applicative (Tuple f g) where
+ pure x = Tuple (pure x) (pure x)
+ Tuple fx fy <*> Tuple x y = Tuple (fx <*> x) (fy <*> y)
+instance (Vectorize f, Vectorize g, Applicative f, Applicative g) => Linear.Additive (Tuple f g) where
+ zero = Tuple (fill 0) (fill 0)
+
+
+-- | a length-3 vectorizable type
+data Triple f g h a = Triple (f a) (g a) (h a)
+ deriving (Eq, Ord, Generic, Generic1, Functor, Foldable, Traversable, Show)
+instance (Vectorize f, Vectorize g, Vectorize h) => Vectorize (Triple f g h)
+instance (Applicative f, Applicative g, Applicative h) => Applicative (Triple f g h) where
+ pure x = Triple (pure x) (pure x) (pure x)
+ Triple fx fy fz <*> Triple x y z = Triple (fx <*> x) (fy <*> y) (fz <*> z)
+instance (Vectorize f, Vectorize g, Vectorize h,
+ Applicative f, Applicative g, Applicative h)
+ => Linear.Additive (Triple f g h) where
+ zero = Triple (fill 0) (fill 0) (fill 0)
+
+
+instance Lookup (None a)
+instance (Lookup a, Generic a) => Lookup (Id a)
+instance (Lookup (f a), Generic (f a),
+ Lookup (g a), Generic (g a)) => Lookup (Tuple f g a)
+instance (Lookup (f a), Generic (f a),
+ Lookup (g a), Generic (g a),
+ Lookup (h a), Generic (h a)) => Lookup (Triple f g h a)
+instance Vectorize Linear.V0
+instance Vectorize Linear.V1
+instance Vectorize Linear.V2
+instance Vectorize Linear.V3
+instance Vectorize Linear.V4
+instance Vectorize Linear.Quaternion
+instance Vectorize Euler
+instance Vectorize (V3T f)
+instance Vectorize (Rot f1 f2)
+
+fill :: Vectorize f => a -> f a
+fill x = fmap (const x) empty
+
+-- | fmap f == devectorize . (V.map f) . vectorize
+class Functor f => Vectorize (f :: * -> *) where
+ vectorize :: f a -> V.Vector a
+ devectorize :: V.Vector a -> f a
+ empty :: f ()
+
+ default vectorize :: (Generic1 f, GVectorize (Rep1 f)) => f a -> V.Vector a
+ vectorize f = gvectorize (from1 f)
+
+ default devectorize :: (Generic1 f, GVectorize (Rep1 f)) => V.Vector a -> f a
+ devectorize f = to1 (gdevectorize f)
+
+ default empty :: (Generic1 f, GVectorize (Rep1 f)) => f ()
+ empty = to1 gempty
+
+--vlength :: Vectorize f => Proxy f -> Int
+--vlength = const (gvlength (Proxy :: Proxy (Rep1 f)))
+
+vlength :: Vectorize f => Proxy f -> Int
+vlength = V.length . vectorize . (empty `asFunctorOf`)
+ where
+ asFunctorOf :: f a -> Proxy f -> f a
+ asFunctorOf x _ = x
+
+class GVectorize (f :: * -> *) where
+ gvectorize :: f a -> V.Vector a
+ gdevectorize :: V.Vector a -> f a
+ gempty :: f ()
+ gvlength :: Proxy f -> Int
+
+vzipWith :: Vectorize f => (a -> b -> c) -> f a -> f b -> f c
+vzipWith f x y = devectorize $ V.zipWith f (vectorize x) (vectorize y)
+
+vzipWith3 :: Vectorize f => (a -> b -> c -> d) -> f a -> f b -> f c -> f d
+vzipWith3 f x y z = devectorize $ V.zipWith3 f (vectorize x) (vectorize y) (vectorize z)
+
+vzipWith4 :: Vectorize f => (a -> b -> c -> d -> e) -> f a -> f b -> f c -> f d -> f e
+vzipWith4 f x y z w =
+ devectorize $ V.zipWith4 f (vectorize x) (vectorize y) (vectorize z) (vectorize w)
+
+-- product type (concatination)
+instance (GVectorize f, GVectorize g) => GVectorize (f :*: g) where
+ gvectorize (f :*: g) = gvectorize f V.++ gvectorize g
+ gdevectorize v0s
+ | V.length v0s < n0 =
+ error $ "gdevectorize (f :*: g): V.length v0s < vlength f0 (" ++
+ show (V.length v0s) ++ " < " ++ show n0 ++ ")"
+ | V.length v1 /= n1 =
+ error $ "gdevectorize (f :*: g): V.length v1 /= vlength f1 (" ++
+ show (V.length v1) ++ " /= " ++ show n1 ++ ")"
+ | otherwise = f0 :*: f1
+ where
+ f0 = gdevectorize v0
+ f1 = gdevectorize v1
+
+ n0 = gvlength (Proxy :: Proxy f)
+ n1 = gvlength (Proxy :: Proxy g)
+
+ (v0,v1) = V.splitAt n0 v0s
+
+ gempty = gempty :*: gempty
+ gvlength = const (nf + ng)
+ where
+ nf = gvlength (Proxy :: Proxy f)
+ ng = gvlength (Proxy :: Proxy g)
+
+-- Metadata (constructor name, etc)
+instance GVectorize f => GVectorize (M1 i c f) where
+ gvectorize = gvectorize . unM1
+ gdevectorize = M1 . gdevectorize
+ gempty = M1 gempty
+ gvlength = gvlength . proxy
+ where
+ proxy :: Proxy (M1 i c f) -> Proxy f
+ proxy = const Proxy
+
+-- singleton
+instance GVectorize Par1 where
+ gvectorize = V.singleton . unPar1
+ gdevectorize v = case V.toList v of
+ [] -> error "gdevectorize Par1: got empty list"
+ [x] -> Par1 x
+ xs -> error $ "gdevectorize Par1: got non-1 length: " ++ show (length xs)
+ gempty = Par1 ()
+ gvlength = const 1
+
+-- data with no fields
+instance GVectorize U1 where
+ gvectorize = const V.empty
+ gdevectorize v
+ | V.null v = U1
+ | otherwise = error $ "gdevectorize U1: got non-null vector, length: " ++ show (V.length v)
+ gempty = U1
+ gvlength = const 0
+
+-- Constants, additional parameters, and rank-1 recursion
+instance Vectorize f => GVectorize (Rec1 f) where
+ gvectorize = vectorize . unRec1
+ gdevectorize = Rec1 . devectorize
+ gempty = Rec1 empty
+ gvlength = vlength . proxy
+ where
+ proxy :: Proxy (Rec1 f) -> Proxy f
+ proxy = const Proxy
+
+-- composition
+instance (Vectorize f, GVectorize g) => GVectorize (f :.: g) where
+ gempty = Comp1 (devectorize (V.replicate k gempty))
+ where
+ k = vlength (Proxy :: Proxy f)
+ gvectorize = V.concatMap gvectorize . vectorize . unComp1
+ gdevectorize v = Comp1 (devectorize vs)
+ where
+ kf = vlength (Proxy :: Proxy f)
+ kg = gvlength (Proxy :: Proxy g)
+
+ -- vs :: V.Vector (g a)
+ vs = fmap gdevectorize (splitsAt kg kf v {-:: Vec nf (Vec ng a)-} )
+ gvlength = const (nf * ng)
+ where
+ nf = vlength (Proxy :: Proxy f)
+ ng = gvlength (Proxy :: Proxy g)
+
+-- break a vector jOuter vectors, each of length kInner
+splitsAt' :: Int -> Int -> V.Vector a -> [V.Vector a]
+splitsAt' 0 jOuter v
+ | V.null v = replicate jOuter V.empty
+ | otherwise = error $ "splitsAt 0 " ++ show jOuter ++ ": got non-zero vector"
+splitsAt' kInner 0 v
+ | V.null v = []
+ | otherwise = error $ "splitsAt " ++ show kInner ++ " 0: leftover vector of length: " ++ show (V.length v)
+splitsAt' kInner jOuter v
+ | kv0 < kInner =
+ error $ "splitsAt " ++ show kInner ++ " " ++ show jOuter ++ ": " ++ "ran out of vector input"
+ | otherwise = v0 : splitsAt' kInner (jOuter - 1) v1
+ where
+ kv0 = V.length v0
+ (v0,v1) = V.splitAt kInner v
+
+-- break a vector jOuter vectors, each of length kInner
+splitsAt :: Int -> Int -> V.Vector a -> V.Vector (V.Vector a)
+splitsAt k j = V.fromList . splitsAt' k j
diff --git a/src/Dyno/View.hs b/src/Dyno/View.hs
new file mode 100644
--- /dev/null
+++ b/src/Dyno/View.hs
@@ -0,0 +1,12 @@
+{-# OPTIONS_GHC -Wall #-}
+
+module Dyno.View ( module X
+ , Num()
+ ) where
+
+import Dyno.View.View as X
+import Dyno.View.JV as X
+import Dyno.View.Fun as X
+import Dyno.View.Viewable as X
+import Dyno.View.HList as X
+import Dyno.View.NumInstances()
diff --git a/src/Dyno/View/CasadiMat.hs b/src/Dyno/View/CasadiMat.hs
new file mode 100644
--- /dev/null
+++ b/src/Dyno/View/CasadiMat.hs
@@ -0,0 +1,153 @@
+{-# OPTIONS_GHC -Wall #-}
+
+module Dyno.View.CasadiMat
+ ( CasadiMat(..), MX.MX, SX.SX, DMatrix.DMatrix
+ , vertslice, horzslice
+ ) where
+
+import qualified Data.Vector as V
+
+import System.IO.Unsafe ( unsafePerformIO )
+import Casadi.Overloading ( Fmod, ArcTan2, SymOrd, Erf )
+import Casadi.Sparsity ( Sparsity )
+import qualified Casadi.SX as SX
+import qualified Casadi.MX as MX
+import qualified Casadi.DMatrix as DMatrix
+import Casadi.Slice ( Slice, slice )
+import Casadi.Core.Tools as C
+
+class (Eq a, Show a, Floating a, Fmod a, ArcTan2 a, SymOrd a, Erf a) => CasadiMat a where
+ vertsplit :: a -> V.Vector Int -> V.Vector a
+ vertcat :: V.Vector a -> a
+ horzsplit :: a -> V.Vector Int -> V.Vector a
+ horzcat :: V.Vector a -> a
+ veccat :: V.Vector a -> a
+ size1 :: a -> Int
+ size2 :: a -> Int
+ mm :: a -> a -> a
+ trans :: a -> a
+ diag :: a -> a
+ eye :: Int -> a
+ ones :: (Int,Int) -> a
+ zeros :: (Int,Int) -> a
+ zerosSp :: Sparsity -> a
+ fromDVector :: V.Vector Double -> a
+ solve :: a -> a -> a
+ indexed :: a -> Slice -> Slice -> a
+ sparsity :: a -> Sparsity
+ getNZ :: a -> Slice -> a
+ setNZ :: a -> a -> Slice -> IO ()
+ triu :: a -> a
+ tril :: a -> a
+ triu2symm :: a -> a
+ tril2symm :: a -> a
+ copy :: a -> IO a
+ dense :: a -> a
+
+instance CasadiMat SX.SX where
+ veccat = SX.sveccat
+-- vertsplit = vertslice
+ vertsplit = SX.svertsplit
+ vertcat = SX.svertcat
+-- horzsplit = horzslice
+ horzsplit = SX.shorzsplit
+ horzcat = SX.shorzcat
+ size1 = SX.ssize1
+ size2 = SX.ssize2
+ mm = SX.smm
+ trans = SX.strans
+ diag = SX.sdiag
+ eye = SX.seye
+ ones = SX.sones
+ zeros = SX.szeros
+ zerosSp = SX.szerosSp
+ fromDVector = SX.d2s . fromDVector
+ solve = SX.ssolve
+ indexed = SX.sindexed
+ sparsity = SX.scrs
+ getNZ = SX.sgetNZ
+ setNZ = SX.ssetNZ
+ triu = SX.striu
+ tril = SX.stril
+ triu2symm = SX.striu2symm
+ tril2symm = SX.stril2symm
+ copy = SX.scopy
+ dense = SX.sdense
+
+instance CasadiMat MX.MX where
+ veccat = MX.veccat
+-- vertsplit = vertslice
+ vertsplit = MX.vertsplit
+ vertcat = MX.vertcat
+-- horzsplit = horzslice
+ horzsplit = MX.horzsplit
+ horzcat = MX.horzcat
+ size1 = MX.size1
+ size2 = MX.size2
+ mm = MX.mm
+ trans = MX.trans
+ diag = MX.diag
+ eye = MX.eye
+ ones = MX.ones
+ zeros = MX.zeros
+ zerosSp = MX.zerosSp
+ fromDVector = MX.d2m . fromDVector
+ solve = MX.solve
+ indexed = MX.indexed
+ sparsity = MX.crs
+ getNZ = MX.getNZ
+ setNZ = MX.setNZ
+ triu = MX.triu
+ tril = MX.tril
+ triu2symm = MX.triu2symm
+ tril2symm = MX.tril2symm
+ copy = MX.copy
+ dense = MX.dense
+
+instance CasadiMat DMatrix.DMatrix where
+ veccat = DMatrix.dveccat
+-- vertsplit = vertslice
+ vertsplit = DMatrix.dvertsplit
+ vertcat = DMatrix.dvertcat
+-- horzsplit = horzslice
+ horzsplit = DMatrix.dhorzsplit
+ horzcat = DMatrix.dhorzcat
+ size1 = DMatrix.dsize1
+ size2 = DMatrix.dsize2
+ mm = DMatrix.dmm
+ trans = DMatrix.dtrans
+ diag = DMatrix.ddiag
+ eye = DMatrix.deye
+ ones = DMatrix.dones
+ zeros = DMatrix.dzeros
+ zerosSp = DMatrix.dzerosSp
+ fromDVector = DMatrix.dvector
+ solve x y = unsafePerformIO (C.solve__3 x y)
+ indexed = DMatrix.dindexed
+ sparsity = DMatrix.dcrs
+ getNZ = DMatrix.dgetNZ
+ setNZ = DMatrix.dsetNZ
+ triu = DMatrix.dtriu
+ tril = DMatrix.dtril
+ triu2symm = DMatrix.dtriu2symm
+ tril2symm = DMatrix.dtril2symm
+ copy = DMatrix.dcopy
+ dense = DMatrix.ddense
+
+vertslice :: CasadiMat a => a -> V.Vector Int -> V.Vector a
+vertslice x vs = V.fromList (f (V.toList vs))
+ where
+ cols = size2 x
+ hslice = slice 0 cols 1
+
+ f (v0:v1:others) = indexed x (slice v0 v1 1) hslice : f (v1:others)
+ f _ = []
+
+horzslice :: CasadiMat a => a -> V.Vector Int -> V.Vector a
+horzslice x vs = V.fromList (f (V.toList vs))
+ where
+ rows = size1 x
+ vslice = slice 0 rows 1
+
+ f (v0:v1:others) = indexed x vslice (slice v0 v1 1) : f (v1:others)
+ f _ = []
diff --git a/src/Dyno/View/CustomFunction.hs b/src/Dyno/View/CustomFunction.hs
new file mode 100644
--- /dev/null
+++ b/src/Dyno/View/CustomFunction.hs
@@ -0,0 +1,159 @@
+{-# OPTIONS_GHC -Wall #-}
+{-# LANGUAGE ScopedTypeVariables #-}
+{-# LANGUAGE RankNTypes #-}
+
+module Dyno.View.CustomFunction
+ ( CustomFun(..)
+ , DerivGen(..)
+ , toCustomFun
+ ) where
+
+import Control.Monad ( zipWithM )
+import Data.Proxy
+import qualified Data.Vector as V
+import Data.Vector ( Vector )
+import qualified Data.Foldable as F
+
+import Casadi.Sparsity ( Sparsity, dense )
+import Casadi.Option ( Opt(..), setOption )
+import Casadi.SharedObject ( soInit )
+
+import qualified Dyno.TypeVecs as TV
+import Dyno.TypeVecs ( Vec, Dim, reifyDim )
+import Dyno.View.CasadiMat ( DMatrix, sparsity )
+import Dyno.View.Scheme ( Scheme(..) )
+import Dyno.View.Fun ( Fun(..) )
+import Casadi.Callback ( makeCustomEvaluate, makeDerivativeGenerator )
+import qualified Casadi.Core.Classes.Function as C
+import qualified Casadi.Core.Classes.CustomFunction as C
+import qualified Casadi.Core.Classes.IOInterfaceFunction as C
+import Casadi.Core.Classes.DerivativeGenerator ( DerivativeGenerator )
+
+
+data CustomFun f g =
+ CustomFun
+ { cfFun :: f DMatrix -> IO (g DMatrix)
+ , cfSparsityIn :: Maybe (f Sparsity)
+ , cfSparsityOut :: Maybe (g Sparsity)
+ , cfDerivGenerator :: Maybe (DerivGen f g)
+ , cfOptions :: [(String, Opt)]
+ }
+
+data DerivGen f g =
+ DerivGen
+ { dgGetSeeds :: forall nfwd nadj
+ . (Dim nfwd, Dim nadj)
+ => f DMatrix -> Vec nfwd (f DMatrix) -> Vec nadj (g DMatrix)
+ -> IO (g DMatrix, Vec nfwd (g DMatrix), Vec nadj (f DMatrix))
+ , dgOptions :: [(String, Opt)]
+ , dgFwdSparsity :: Maybe (f Sparsity)
+ , dgAdjSparsity :: Maybe (g Sparsity)
+ }
+
+groupsOf :: Int -> [a] -> [[a]]
+groupsOf _ [] = []
+groupsOf k vs
+ | length vs0 == k = vs0 : groupsOf k vs1
+ | otherwise = error "groupsOf not divisible"
+ where
+ (vs0, vs1) = splitAt k vs
+
+toDerivGen :: forall f g . (Scheme f, Scheme g) => DerivGen f g -> IO DerivativeGenerator
+toDerivGen dg = makeDerivativeGenerator $ \originalFun nfwd nadj -> do
+ let f fun = do
+ numIn <- C.ioInterfaceFunction_getNumInputs fun
+ inputs <- mapM (C.ioInterfaceFunction_getInput__2 fun) (take numIn [0..])
+
+ let nf = numFields (Proxy :: Proxy f)
+ ng = numFields (Proxy :: Proxy g)
+
+ let f' :: forall nfwd nadj
+ . (Dim nfwd, Dim nadj)
+ => Proxy nfwd -> Proxy nadj -> IO (Vector DMatrix)
+ f' _ _ = do
+ let (inputs0', inputs12') = splitAt nf inputs
+ (inputs1', inputs2') = splitAt (nfwd*nf) inputs12'
+
+ inputs0 :: f DMatrix
+ inputs0 = fromVector (V.fromList inputs0')
+
+ inputs1 :: Vec nfwd (f DMatrix)
+ inputs1 = TV.mkVec' (map (fromVector . V.fromList) (groupsOf nf inputs1'))
+
+ inputs2 :: Vec nadj (g DMatrix)
+ inputs2 = TV.mkVec' (map (fromVector . V.fromList) (groupsOf ng inputs2'))
+ (out0, out1, out2) <- dgGetSeeds dg inputs0 inputs1 inputs2
+ let out0' = toVector out0
+ out1' = V.concat $ F.toList (fmap toVector out1)
+ out2' = V.concat $ F.toList (fmap toVector out2)
+ return (V.concat [out0', out1', out2'])
+
+ outs <- reifyDim nfwd $ \pnfwd ->
+ reifyDim nadj $ \pnadj -> f' pnfwd pnadj
+ _ <- zipWithM (C.ioInterfaceFunction_setOutput__2 fun) (V.toList outs) [0..]
+ return ()
+
+ ce <- makeCustomEvaluate f
+
+
+ numIn <- C.ioInterfaceFunction_getNumInputs originalFun
+ numOut <- C.ioInterfaceFunction_getNumOutputs originalFun
+ spIns0 <- mapM (fmap sparsity . (C.ioInterfaceFunction_getInput__2 originalFun)) (take numIn [0..])
+ spOuts0 <- mapM (fmap sparsity . (C.ioInterfaceFunction_getOutput__2 originalFun)) (take numOut [0..])
+
+ let spFwd = case dgFwdSparsity dg of
+-- Just sp -> toVector sp
+ _ -> V.fromList $ map (uncurry dense) $ sizeList (Proxy :: Proxy f)
+ spAdj = case dgAdjSparsity dg of
+-- Just sp -> toVector sp
+ _ -> V.fromList $ map (uncurry dense) $ sizeList (Proxy :: Proxy g)
+
+ -- TODO: this is only right when everything's dense because it depends on jac sparsity!!!
+ spIns = V.concat [ V.fromList spIns0
+ , V.concat (replicate nfwd spFwd)
+ , V.concat (replicate nadj spAdj)
+ ]
+ spOuts = V.concat [ V.fromList spOuts0
+ , V.concat (replicate nfwd spAdj)
+ , V.concat (replicate nadj spFwd)
+ ]
+ cf <- C.customFunction__1 ce spIns spOuts
+ mapM_ (\(n,Opt o) -> setOption cf n o) (dgOptions dg)
+
+ soInit cf
+
+ return (C.castFunction cf)
+
+toCustomFun ::
+ forall f g
+ . (Scheme f, Scheme g)
+ => CustomFun f g
+ -> IO (Fun f g)
+toCustomFun customFun = do
+ ce <- makeCustomEvaluate $ \fun -> do
+ numIn <- C.ioInterfaceFunction_getNumInputs fun
+ inputs <- mapM (C.ioInterfaceFunction_getInput__2 fun) (take numIn [0..])
+ outputs <- cfFun customFun $ fromVector (V.fromList inputs)
+ _ <- zipWithM (C.ioInterfaceFunction_setOutput__2 fun) (V.toList (toVector outputs)) [0..]
+ return ()
+
+ let spIn :: Vector Sparsity
+ spIn = case cfSparsityIn customFun of
+ Just spIn' -> toVector spIn'
+ Nothing -> V.fromList $ map (uncurry dense) $ sizeList (Proxy :: Proxy f)
+ spOut :: Vector Sparsity
+ spOut = case cfSparsityOut customFun of
+ Just spOut' -> toVector spOut'
+ Nothing -> V.fromList $ map (uncurry dense) $ sizeList (Proxy :: Proxy g)
+
+ cf <- C.customFunction__1 ce spIn spOut
+
+ mapM_ (\(n,Opt o) -> setOption cf n o) (cfOptions customFun)
+ case cfDerivGenerator customFun of
+ Nothing -> return ()
+ Just dg -> do
+ dgen <- toDerivGen dg
+ setOption cf "derivative_generator" dgen
+ soInit cf
+
+ return (Fun (C.castFunction cf))
diff --git a/src/Dyno/View/Fun.hs b/src/Dyno/View/Fun.hs
new file mode 100644
--- /dev/null
+++ b/src/Dyno/View/Fun.hs
@@ -0,0 +1,197 @@
+{-# OPTIONS_GHC -Wall #-}
+{-# LANGUAGE ScopedTypeVariables #-}
+{-# LANGUAGE KindSignatures #-}
+
+module Dyno.View.Fun
+ ( FunClass(..)
+ , MXFun
+ , SXFun
+ , Fun(..)
+ , toMXFun
+ , toSXFun
+ , eval
+ , call
+ , callSX
+ , expandMXFun
+ , toFunJac
+ ) where
+
+import Control.Monad ( zipWithM )
+import Data.Proxy
+import qualified Data.Vector as V
+import Data.Vector ( Vector )
+
+import Casadi.MX ( symM )
+import Casadi.SX ( ssymM )
+import qualified Casadi.Function as C
+import qualified Casadi.MXFunction as C
+import qualified Casadi.SXFunction as C
+import Casadi.Option
+import Casadi.SharedObject
+
+import qualified Casadi.Core.Classes.Function as F
+import qualified Casadi.Core.Classes.MXFunction as M
+import qualified Casadi.Core.Classes.SharedObject as C
+import qualified Casadi.Core.Classes.OptionsFunctionality as C
+
+import Dyno.View.CasadiMat
+import Dyno.View.Scheme
+import Dyno.View.FunJac
+
+newtype MXFun (f :: * -> *) (g :: * -> *) = MXFun C.MXFunction
+newtype SXFun (f :: * -> *) (g :: * -> *) = SXFun C.SXFunction
+newtype Fun (f :: * -> *) (g :: * -> *) = Fun C.Function
+
+instance Show (MXFun f g) where
+ showsPrec k (MXFun f) = showsPrec k f
+instance Show (SXFun f g) where
+ showsPrec k (SXFun f) = showsPrec k f
+instance Show (Fun f g) where
+ showsPrec k (Fun f) = showsPrec k f
+
+class FunClass fun where
+ fromFun :: Fun f g -> IO (fun f g)
+ toFun :: fun f g -> Fun f g
+
+instance FunClass Fun where
+ fromFun = return
+ toFun = id
+
+instance FunClass SXFun where
+ fromFun (Fun f) = do
+ sxf <- C.sxFunctionFromFunction f
+ return (SXFun sxf)
+ toFun (SXFun f) = Fun (F.castFunction f)
+
+
+instance FunClass MXFun where
+ fromFun (Fun f) = do
+ mxf <- C.mxFunctionFromFunction f
+ return (MXFun mxf)
+ toFun (MXFun f) = Fun (F.castFunction f)
+
+-- | call a Function on numeric inputs, getting numeric outputs
+eval :: (FunClass fun, Scheme f, Scheme g) => fun f g -> f DMatrix -> IO (g DMatrix)
+eval f' = fmap fromVector . C.evalDMatrix f . toVector
+ where
+ Fun f = toFun f'
+
+-- | call a function on MX inputs, yielding MX outputs
+call :: (FunClass fun, Scheme f, Scheme g) => fun f g -> f MX -> g MX
+call f' = fromVector . C.callMX f . toVector
+ where
+ Fun f = toFun f'
+
+-- | call an SXFunction on symbolic inputs, getting symbolic outputs
+callSX :: (Scheme f, Scheme g) => SXFun f g -> f SX -> g SX
+callSX (SXFun sxf) = fromVector . C.callSX sxf . toVector
+
+mkSym :: forall a f .
+ (Scheme f, CasadiMat a)
+ => (String -> Int -> Int -> IO a)
+ -> String -> Proxy f -> IO (f a)
+mkSym mk name _ = do
+ let sizes :: [(Int,Int)]
+ sizes = sizeList (Proxy :: Proxy f)
+ f :: (Int, Int) -> Int -> IO a
+ f (nrow,ncol) k = mk (name ++ show k) nrow ncol
+ ms <- zipWithM f sizes [(0::Int)..]
+ return $ fromVector (V.fromList ms)
+
+mkFun :: forall f g fun fun' a
+ . (Scheme f, Scheme g, C.SharedObjectClass fun, C.OptionsFunctionalityClass fun)
+ => (Vector a -> Vector a -> IO fun)
+ -> (String -> Proxy f -> IO (f a))
+ -> (fun -> fun' f g)
+ -> String
+ -> (f a -> g a)
+ -> IO (fun' f g)
+mkFun mkfun mksym con name userf = do
+ inputs <- mksym "x" (Proxy :: Proxy f)
+ fun <- mkfun (toVector inputs) (toVector (userf inputs))
+ setOption fun "name" name
+ soInit fun
+ return (con fun)
+
+-- | make an MXFunction
+toMXFun :: forall f g . (Scheme f, Scheme g) => String -> (f MX -> g MX) -> IO (MXFun f g)
+toMXFun name fun = mkFun C.mxFunction (mkSym symM) MXFun name fun
+
+-- | make an MXFunction
+toSXFun :: forall f g . (Scheme f, Scheme g) => String -> (f SX -> g SX) -> IO (SXFun f g)
+toSXFun name fun = mkFun C.sxFunction (mkSym ssymM) SXFun name fun
+
+-- | expand an MXFunction
+expandMXFun :: MXFun f g -> IO (SXFun f g)
+expandMXFun (MXFun mxf) = do
+ sxf <- M.mxFunction_expand__0 mxf
+ C.sharedObject_init__0 sxf
+ return (SXFun sxf)
+
+-- | make a function which also contains a jacobian
+toFunJac ::
+ FunClass fun =>
+ fun (JacIn xj x) (JacOut fj f) -> IO (fun (JacIn xj x) (Jac xj fj f))
+toFunJac fun0 = do
+ let Fun fun = toFun fun0
+ maybeName <- getOption fun "name"
+ let name = case maybeName of Nothing -> "no_name"
+ Just n -> n
+ let compact = False
+ symmetric = False
+ funJac <- C.jacobian fun 0 0 compact symmetric
+ setOption funJac "name" (name ++ "_dynobudJac")
+ soInit funJac
+
+ fromFun (Fun funJac)
+
+
+--toFunJac' ::
+-- forall x y f . (SymInputs x MX, SymInputs y MX, FunArgs f MX, FunArgs f (J x MX))
+-- => String -> ((x MX, y MX) -> f MX) -> IO ((x MX, y MX) -> Vector (Vector MX))
+--toFunJac' name f0 = do
+-- (diffInputs',_) <- sym' 0 (Proxy :: Proxy (x MX))
+-- let nsyms = F.sum $ fmap vsize1 (vectorize diffInputs')
+-- diffInputsCat <- symV "dx" nsyms
+-- let inputSizes = V.fromList ((0:) $ F.toList (sizeList 0 (Proxy :: Proxy (x MX))))
+-- diffInputs = vvertsplit diffInputsCat inputSizes
+--
+-- (inputs,_) <- sym' 0 (Proxy :: Proxy (y MX))
+-- let diffOutputs = f0 (devectorize diffInputs, inputs)
+-- diffOutputsCat = vveccat (vectorize diffOutputs)
+--
+-- allInputs = V.cons diffInputsCat (vectorize inputs)
+-- allOutputs = V.singleton diffOutputsCat
+--
+-- mxf <- mxFunction allInputs allOutputs
+-- setOption mxf "name" name
+-- soInit mxf
+-- let compact = False
+-- symmetric = False
+-- mxfJac <- jacobian mxf 0 0 compact symmetric
+-- soInit mxfJac
+--
+-- let callMe :: (x MX, y MX) -> Vector (Vector MX) -- , f MX)
+-- callMe (x',y')
+-- | 2 /= V.length vouts =
+-- error "toFunJac': bad number of outputs :("
+-- | otherwise = rows -- , devectorize fs)
+-- where
+-- --retJac :: f (J x MX)
+-- --retJac = devectorize retJac'
+-- --retJac' :: Vector (J x MX)
+-- --retJac' = fmap devectorize rows
+-- rows :: Vector (Vector MX)
+-- rows = fmap (`vhorzsplit` horzsizes) $ vvertsplit jac vertsizes
+-- vertsizes = V.fromList ((0:) $ F.toList (sizeList 0 (Proxy :: Proxy (f MX))))
+-- horzsizes = V.fromList ((0:) $ F.toList (sizeList 0 (Proxy :: Proxy (x MX))))
+--
+-- --fs = vvertsplit f vertsizes
+-- x = vveccat (vectorize x')
+--
+-- jac = vouts V.! 0
+-- --f = vouts V.! 1
+--
+-- vouts = callMX mxfJac $ V.cons x (vectorize y')
+--
+-- return callMe
diff --git a/src/Dyno/View/FunJac.hs b/src/Dyno/View/FunJac.hs
new file mode 100644
--- /dev/null
+++ b/src/Dyno/View/FunJac.hs
@@ -0,0 +1,82 @@
+{-# OPTIONS_GHC -Wall #-}
+
+module Dyno.View.FunJac
+ ( JacIn(..)
+ , JacOut(..)
+ , Jac(..)
+ ) where
+
+import Data.Proxy
+import qualified Data.Vector as V
+
+import Dyno.View.View
+import Dyno.View.Scheme
+import Dyno.View.M
+
+data JacIn xj x a = JacIn (J xj a) (x a)
+data JacOut fj f a = JacOut (J fj a) (f a)
+data Jac xj fj f a = Jac (M fj xj a) (J fj a) (f a)
+
+instance (View xj, Scheme x) => Scheme (JacIn xj x) where
+ numFields = (1+) . numFields . reproxy
+ where
+ reproxy :: Proxy (JacIn xj x) -> Proxy x
+ reproxy = const Proxy
+ fromVector v = JacIn j0 (fromVector (V.tail v))
+ where
+ j0 = case fromMat (V.head v) of
+ Left err -> error $ "JacIn fromVector error: " ++ err
+ Right j0' -> j0'
+
+ toVector (JacIn xj x) = V.cons (toFioMat xj) (toVector x)
+ sizeList p = matSizes (reproxy' p) : sizeList (reproxy p)
+ where
+ reproxy :: Proxy (JacIn xj x) -> Proxy x
+ reproxy = const Proxy
+ reproxy' :: Proxy (JacIn xj x) -> Proxy (J xj)
+ reproxy' = const Proxy
+
+instance (View fj, Scheme f) => Scheme (JacOut fj f) where
+ numFields = (1+) . numFields . reproxy
+ where
+ reproxy :: Proxy (JacOut fj f) -> Proxy f
+ reproxy = const Proxy
+ fromVector v = JacOut j0 (fromVector (V.tail v))
+ where
+ j0 = case fromMat (V.head v) of
+ Left err -> error $ "JacOut fromVector error: " ++ err
+ Right j0' -> j0'
+
+ toVector (JacOut fj f) = V.cons (toFioMat fj) (toVector f)
+ sizeList p = matSizes (reproxy' p) : sizeList (reproxy p)
+ where
+ reproxy :: Proxy (JacOut fj f) -> Proxy f
+ reproxy = const Proxy
+ reproxy' :: Proxy (JacOut fj f) -> Proxy (J fj)
+ reproxy' = const Proxy
+
+
+instance (View xj, View fj, Scheme f) => Scheme (Jac xj fj f) where
+ numFields = (2+) . numFields . reproxy
+ where
+ reproxy :: Proxy (Jac xj fj f) -> Proxy f
+ reproxy = const Proxy
+ fromVector v = Jac m fj (fromVector (V.drop 2 v))
+ where
+ m = case fromMat (v V.! 0) of
+ Left err -> error $ "Jac fromVector error: " ++ err
+ Right j0' -> j0'
+ fj = case fromMat (v V.! 1) of
+ Left err -> error $ "Jac fromVector error: " ++ err
+ Right j0' -> j0'
+ toVector (Jac m fj f) = V.fromList [toFioMat m, toFioMat fj] V.++ toVector f
+ sizeList p = matSizes (reproxy'' p) : matSizes (reproxy' p) : sizeList (reproxy p)
+ where
+ reproxy'' :: Proxy (Jac xj fj f) -> Proxy (M fj xj)
+ reproxy'' = const Proxy
+
+ reproxy' :: Proxy (Jac xj fj f) -> Proxy (J fj)
+ reproxy' = const Proxy
+
+ reproxy :: Proxy (Jac xj fj f) -> Proxy f
+ reproxy = const Proxy
diff --git a/src/Dyno/View/HList.hs b/src/Dyno/View/HList.hs
new file mode 100644
--- /dev/null
+++ b/src/Dyno/View/HList.hs
@@ -0,0 +1,112 @@
+{-# OPTIONS_GHC -Wall #-}
+{-# LANGUAGE TypeOperators #-}
+{-# LANGUAGE ScopedTypeVariables #-}
+--{-# LANGUAGE DeriveGeneric #-}
+{-# LANGUAGE PolyKinds #-}
+{-# LANGUAGE TypeOperators #-}
+{-# LANGUAGE ScopedTypeVariables #-}
+{-# LANGUAGE TypeFamilies #-}
+{-# LANGUAGE DefaultSignatures #-}
+{-# LANGUAGE TypeOperators #-}
+{-# LANGUAGE MultiParamTypeClasses #-}
+{-# LANGUAGE FlexibleContexts #-}
+{-# LANGUAGE FlexibleInstances #-}
+{-# LANGUAGE DeriveGeneric #-}
+{-# LANGUAGE DeriveFunctor #-}
+
+module Dyno.View.HList
+ ( (:*:)(..)
+ ) where
+
+--import qualified Data.Sequence as Seq
+import Data.Proxy
+import qualified Data.Vector as V
+
+import Dyno.View.Scheme
+--import Dyno.View.View
+--import Dyno.View.Viewable ( Viewable(..) )
+
+infixr 6 :*:
+data (:*:) f g a = (:*:) (f a) (g a)
+
+--class HFromVec f where
+--
+
+instance (Scheme f, Scheme g) => Scheme (f :*: g) where
+ numFields pxy = numFields px + numFields py
+ where
+ reproxy :: Proxy (x :*: y) -> (Proxy x, Proxy y)
+ reproxy = const (Proxy,Proxy)
+ (px, py) = reproxy pxy
+ toVector (x :*: y) = toVector x V.++ toVector y
+ fromVector xy = fromVector x :*: fromVector y
+ where
+ (x,y)
+ | V.length x' /= nx = error "splitting HList in casadi Fun got length mismatch"
+ | V.length y' /= ny = error "splitting HList in casadi Fun got length mismatch"
+ | otherwise = (x',y')
+ (x',y')= V.splitAt nx xy
+ nx = numFields (Proxy :: Proxy f)
+ ny = numFields (Proxy :: Proxy g)
+ sizeList pxy = xs ++ ys
+ where
+ xs = sizeList px
+ ys = sizeList py
+
+ reproxy :: Proxy (x :*: y) -> (Proxy x, Proxy y)
+ reproxy = const (Proxy,Proxy)
+ (px, py) = reproxy pxy
+
+--instance (View f, View g) => View (f :*: g) where
+-- cat (x :*: y) = mkJ (vveccat (V.fromList [x', y']))
+-- where
+-- UnsafeJ x' = cat x
+-- UnsafeJ y' = cat y
+-- size = const $ size (Proxy :: Proxy f) + size (Proxy :: Proxy g)
+-- sizes k0 = const $ xs Seq.>< ys
+-- where
+-- xs = sizes k0 (Proxy :: Proxy f)
+-- ys = sizes k1 (Proxy :: Proxy g)
+-- k1 = case Seq.viewr xs of
+-- Seq.EmptyR -> k0
+-- _ Seq.:> k1' -> k1'
+---- split :: forall a . Viewable a => J S a -> S a
+-- split = undefined -- S . unJ
+--
+--
+--
+--
+--
+--
+--
+--
+--data Tup f g -- only for proxies
+--
+--class HSplit f where
+---- hsplit :: M f g a -> HSplitT f g a
+-- hsizeList :: Proxy f -> [Int]
+-- hfromList :: Viewable a => [a] -> (HSplitT f a, [a])
+--
+--instance (HSplit f1, HSplit f2) => HSplit (f1 :*: f2) where
+---- hsplit = undefined
+-- hsizeList p = hsizeList p1 ++ hsizeList p2
+-- where
+-- reproxy :: Proxy (x :*: y) -> (Proxy x, Proxy y)
+-- reproxy = const (Proxy, Proxy)
+-- (p1,p2) = reproxy p
+--
+--instance View f => HSplit f where
+---- hsplit = undefined
+-- hsizeList p = [size p]
+-- where
+-- hfromList (x:xs) = (mkJ x, xs)
+--
+----hsplit :: HSplit f g => M f g a -> HSplitT f g a
+----hsplit m@(UnsafeM mat) = undefined
+---- where
+---- reproxy :: M f g a -> Proxy (Tup f g)
+---- splitargs = scanl (+) 0 $ hsizeList (reproxy m)
+--
+--type family HSplitT (f :: * -> *) a where
+-- HSplitT (f1 :*: f2) a = (HSplitT f1 :*: HSplitT f2) a
+-- HSplitT f a = J f a
diff --git a/src/Dyno/View/JV.hs b/src/Dyno/View/JV.hs
new file mode 100644
--- /dev/null
+++ b/src/Dyno/View/JV.hs
@@ -0,0 +1,73 @@
+{-# OPTIONS_GHC -Wall #-}
+{-# LANGUAGE ScopedTypeVariables #-}
+{-# LANGUAGE FlexibleInstances #-}
+{-# LANGUAGE FlexibleContexts #-}
+{-# LANGUAGE DeriveFunctor #-}
+{-# LANGUAGE DeriveGeneric #-}
+{-# LANGUAGE InstanceSigs #-}
+
+module Dyno.View.JV
+ ( JV(..)
+ , splitJV
+ , catJV
+ , splitJV'
+ , catJV'
+ , sxSplitJV
+ , sxCatJV
+ ) where
+
+import GHC.Generics hiding ( S )
+
+import qualified Data.Sequence as Seq
+import Data.Proxy ( Proxy(..) )
+import Data.Vector ( Vector )
+import qualified Data.Vector as V
+
+import Casadi.SX ( SX )
+
+import Dyno.SXElement
+import Dyno.View.Viewable ( Viewable(..) )
+import Dyno.View.View
+import Dyno.Vectorize ( Vectorize(..), Id, vlength )
+import Dyno.Server.Accessors ( Lookup(..) )
+
+-- | views into Vectorizable things
+newtype JV f a = JV { unJV :: f a } deriving (Functor, Generic)
+
+instance Vectorize f => View (JV f) where
+ cat :: forall a . Viewable a => JV f a -> J (JV f) a
+ cat = mkJ . vveccat . vectorize . unJV
+ size = const $ vlength (Proxy :: Proxy f)
+ sizes = const . Seq.singleton . (vlength (Proxy :: Proxy f) +)
+ split :: forall a . Viewable a => J (JV f) a -> JV f a
+ split = JV . devectorize . flip vvertsplit ks. unJ
+ where
+ ks = V.fromList (take (n+1) [0..])
+ n = size (Proxy :: Proxy (JV f))
+
+instance (Vectorize f, Lookup (f a)) => Lookup (J (JV f) (Vector a)) where
+ toAccessorTree x g = toAccessorTree (devectorize (unJ x) :: f a) (devectorize . unJ . g)
+
+splitJV :: Vectorize f => J (JV f) (Vector a) -> f a
+splitJV = devectorize . unJ
+
+catJV :: Vectorize f => f a -> J (JV f) (Vector a)
+catJV = mkJ . vectorize
+
+splitJV' :: (Vectorize f, Viewable a) => J (JV f) a -> f (J (JV Id) a)
+splitJV' = fmap mkJ . unJV . split
+
+catJV' :: (Vectorize f, Viewable a) => f (J (JV Id) a) -> J (JV f) a
+catJV' = cat . JV . fmap unJ
+
+sxSplitJV :: Vectorize f => J (JV f) SX -> f SXElement
+sxSplitJV v = fmap f (splitJV' v)
+ where
+ f :: J (JV Id) SX -> SXElement
+ f (UnsafeJ x) = sxToSXElement x
+
+sxCatJV :: Vectorize f => f SXElement -> J (JV f) SX
+sxCatJV v = catJV' (fmap f v)
+ where
+ f :: SXElement -> J (JV Id) SX
+ f x = mkJ (sxElementToSX x)
diff --git a/src/Dyno/View/M.hs b/src/Dyno/View/M.hs
new file mode 100644
--- /dev/null
+++ b/src/Dyno/View/M.hs
@@ -0,0 +1,284 @@
+{-# OPTIONS_GHC -Wall #-}
+{-# LANGUAGE ScopedTypeVariables #-}
+{-# LANGUAGE KindSignatures #-}
+{-# LANGUAGE DeriveFunctor #-}
+{-# LANGUAGE DeriveGeneric #-}
+
+module Dyno.View.M
+ ( M(..) -- TODO: hide the unsafe constructor
+ , mkM
+ , mkM'
+ , mm
+ , ms
+ , trans
+ , zeros
+ , eye
+ , ones
+ , countUp
+ , vsplit
+ , hsplit
+ , vcat
+ , hcat
+ , vsplit'
+ , hsplit'
+ , vcat'
+ , hcat'
+ , hsplitTup
+ , row
+ , col
+ , unrow
+ , uncol
+ , solve
+ , toHMat
+ , fromHMat
+ , fromHMat'
+ ) where
+
+import qualified Data.Packed.Matrix as Mat
+import Data.Proxy
+import qualified Data.Vector as V
+import GHC.Generics ( Generic )
+
+import Casadi.Overloading
+import Casadi.DMatrix ( ddata, ddense, dvector )
+
+import Dyno.Vectorize
+import Dyno.View.CasadiMat ( CasadiMat )
+import Dyno.View.JV
+import Dyno.TypeVecs ( Vec, Dim(..) )
+import Dyno.View.View
+import Dyno.View.Viewable
+import qualified Dyno.View.CasadiMat as CM
+
+newtype M (f :: * -> *) (g :: * -> *) (a :: *) =
+ UnsafeM { unM :: a } deriving (Eq, Functor, Generic)
+
+instance Show a => Show (M f g a) where
+ showsPrec p (UnsafeM x) = showsPrec p x
+
+over :: (View f, View g, CasadiMat a) => (a -> a) -> M f g a -> M f g a
+over f (UnsafeM x) = mkM (f x)
+
+over2 :: (View f, View g, CasadiMat a) => (a -> a -> a) -> M f g a -> M f g a -> M f g a
+over2 f (UnsafeM x) (UnsafeM y)= mkM (f x y)
+
+instance (View f, View g, CasadiMat a) => Num (M f g a) where
+ (+) = over2 (+)
+ (-) = over2 (-)
+ (*) = over2 (*)
+ negate = over negate
+ abs = over abs
+ signum = over signum
+ fromInteger k = mkM $ fromInteger k * CM.ones (nx,ny)
+ where
+ nx = size (Proxy :: Proxy f)
+ ny = size (Proxy :: Proxy f)
+instance (View f, View g, CasadiMat a) => Fractional (M f g a) where
+ (/) = over2 (/)
+ fromRational k = mkM $ fromRational k * CM.ones (nx,ny)
+ where
+ nx = size (Proxy :: Proxy f)
+ ny = size (Proxy :: Proxy f)
+instance (View f, View g, CasadiMat a) => Floating (M f g a) where
+ pi = mkM $ pi * CM.ones (nx,ny)
+ where
+ nx = size (Proxy :: Proxy f)
+ ny = size (Proxy :: Proxy f)
+ (**) = over2 (**)
+ exp = over exp
+ log = over log
+ sin = over sin
+ cos = over cos
+ tan = over tan
+ asin = over asin
+ atan = over atan
+ acos = over acos
+ sinh = over sinh
+ cosh = over cosh
+ tanh = over tanh
+ asinh = over asinh
+ atanh = over atanh
+ acosh = over acosh
+
+instance (View f, View g, CasadiMat a) => Fmod (M f g a) where
+ fmod = over2 fmod
+
+instance (View f, View g, CasadiMat a) => ArcTan2 (M f g a) where
+ arctan2 = over2 arctan2
+
+instance (View f, View g, CasadiMat a) => SymOrd (M f g a) where
+ leq = over2 leq
+ geq = over2 geq
+ eq = over2 eq
+
+mkM :: forall f g a . (View f, View g, CasadiMat a) => a -> M f g a
+mkM x = case mkM' x of
+ Right x' -> x'
+ Left msg -> error msg
+
+mkM' :: forall f g a . (View f, View g, CasadiMat a) => a -> Either String (M f g a)
+mkM' x
+ | nx == nx' && ny == ny' = Right (UnsafeM x)
+ | all (== 0) [nx,nx'] && ny' == 0 = Right zeros
+ | all (== 0) [ny,ny'] && nx' == 0 = Right zeros
+ | otherwise = Left $ "mkM length mismatch: typed size: " ++ show (nx,ny) ++
+ ", actual size: " ++ show (nx', ny')
+ where
+ nx = size (Proxy :: Proxy f)
+ ny = size (Proxy :: Proxy g)
+ nx' = CM.size1 x
+ ny' = CM.size2 x
+
+mm :: (View f, View h, CasadiMat a) => M f g a -> M g h a -> M f h a
+mm (UnsafeM m0) (UnsafeM m1) = mkM (CM.mm m0 m1)
+
+ms :: (View f, View h, CasadiMat a) => M f g a -> J S a -> M f h a
+ms (UnsafeM m0) (UnsafeJ m1) = mkM (m0 * m1)
+
+trans :: (View f, View g, CasadiMat a) => M f g a -> M g f a
+trans (UnsafeM m) = mkM (CM.trans m)
+
+vsplit ::
+ forall f g a .
+ (Vectorize f, View g, CasadiMat a)
+ => M (JV f) g a -> f (M (JV Id) g a)
+vsplit (UnsafeM x) = fmap mkM $ devectorize $ CM.vertsplit x nrs
+ where
+ nr = size (Proxy :: Proxy (JV f))
+ nrs = V.fromList [0,1..nr]
+
+vcat ::
+ forall f g a .
+ (Vectorize f, View g, CasadiMat a)
+ => f (M (JV Id) g a) -> M (JV f) g a
+vcat x = mkM $ CM.vertcat $ V.map unM (vectorize x)
+
+hsplit ::
+ forall f g a .
+ (View f, Vectorize g, CasadiMat a)
+ => M f (JV g) a -> g (M f (JV Id) a)
+hsplit (UnsafeM x) = fmap mkM $ devectorize $ CM.horzsplit x ncs
+ where
+ nc = size (Proxy :: Proxy (JV g))
+ ncs = V.fromList [0,1..nc]
+
+hsplitTup ::
+ forall f g h a .
+ (View f, View g, View h, CasadiMat a)
+ => M f (JTuple g h) a -> (M f g a, M f h a)
+hsplitTup (UnsafeM x) =
+ case V.toList (CM.horzsplit x ncs) of
+ [g,h] -> (mkM g, mkM h)
+ n -> error $ "hsplitTup made a bad split with length " ++ show (length n)
+ where
+ ng = size (Proxy :: Proxy g)
+ nh = size (Proxy :: Proxy h)
+ ncs = V.fromList [0,ng,ng+nh]
+
+hcat ::
+ forall f g a .
+ (View f, Vectorize g, CasadiMat a)
+ => g (M f (JV Id) a) -> M f (JV g) a
+hcat x = mkM $ CM.horzcat $ V.map unM (vectorize x)
+
+vcat' ::
+ forall f g n a .
+ (View f, View g, Dim n, CasadiMat a)
+ => Vec n (M f g a) -> M (JVec n f) g a
+vcat' x = mkM $ CM.vertcat $ V.map unM (vectorize x)
+
+vsplit' ::
+ forall f g n a .
+ (View f, View g, Dim n, CasadiMat a)
+ => M (JVec n f) g a -> Vec n (M f g a)
+vsplit' (UnsafeM x)
+ | n == 0 = fill zeros
+ | nr == 0 = fill zeros
+ | otherwise = fmap mkM $ devectorize $ CM.vertsplit x nrs
+ where
+ n = reflectDim (Proxy :: Proxy n)
+ nr = size (Proxy :: Proxy f)
+ nrs = V.fromList [0,nr..n*nr]
+
+hcat' ::
+ forall f g n a .
+ (View f, View g, Dim n, CasadiMat a)
+ => Vec n (M f g a) -> M f (JVec n g) a
+hcat' x = mkM $ CM.horzcat $ V.map unM (vectorize x)
+
+hsplit' ::
+ forall f g n a .
+ (View f, View g, Dim n, CasadiMat a)
+ => M f (JVec n g) a -> Vec n (M f g a)
+hsplit' (UnsafeM x)
+ | n == 0 = fill zeros
+ | nc == 0 = fill zeros
+ | otherwise = fmap mkM $ devectorize $ CM.horzsplit x ncs
+ where
+ n = reflectDim (Proxy :: Proxy n)
+ nc = size (Proxy :: Proxy g)
+ ncs = V.fromList [0,nc..n*nc]
+
+zeros :: forall f g a . (View f, View g, CasadiMat a) => M f g a
+zeros = mkM z
+ where
+ z = CM.zeros (rows, cols)
+ rows = size (Proxy :: Proxy f)
+ cols = size (Proxy :: Proxy g)
+
+eye :: forall f a . (View f, CasadiMat a) => M f f a
+eye = mkM z
+ where
+ z = CM.eye n
+ n = size (Proxy :: Proxy f)
+
+ones :: forall f g a . (View f, View g, CasadiMat a) => M f g a
+ones = mkM z
+ where
+ z = CM.ones (rows, cols)
+ rows = size (Proxy :: Proxy f)
+ cols = size (Proxy :: Proxy g)
+
+-- this is mainly for unit tests
+countUp :: forall f g a . (View f, View g, CasadiMat a) => M f g a
+countUp = mkM z
+ where
+ z = CM.vertcat (V.fromList [CM.horzcat (V.fromList [ fromIntegral (c + cols*r)
+ | c <- [0..(cols-1)]
+ ])
+ | r <- [0..(rows-1)]
+ ])
+ rows = size (Proxy :: Proxy f)
+ cols = size (Proxy :: Proxy g)
+
+row :: (CasadiMat a, View f) => J f a -> M (JV Id) f a
+row (UnsafeJ x) = mkM (CM.trans x)
+
+col :: (CasadiMat a, View f) => J f a -> M f (JV Id) a
+col (UnsafeJ x) = mkM x
+
+unrow :: (Viewable a, CasadiMat a, View f) => M (JV Id) f a -> J f a
+unrow (UnsafeM x) = mkJ (CM.trans x)
+
+uncol :: (Viewable a, CasadiMat a, View f) => M f (JV Id) a -> J f a
+uncol (UnsafeM x) = mkJ x
+
+solve :: (View g, View h, CasadiMat a) => M f g a -> M f h a -> M g h a
+solve (UnsafeM x) (UnsafeM y) = mkM (CM.solve x y)
+
+toHMat :: forall n m
+ . (View n, View m)
+ => M n m DMatrix -> Mat.Matrix Double
+toHMat (UnsafeM d) = Mat.trans $ (m Mat.>< n) (V.toList v)
+ where
+ v = ddata (ddense d)
+ n = size (Proxy :: Proxy n)
+ m = size (Proxy :: Proxy m)
+
+fromHMat :: (View g, View f) => Mat.Matrix Double -> M f g DMatrix
+fromHMat x = case fromHMat' x of
+ Right x' -> x'
+ Left msg -> error msg
+
+fromHMat' :: (View g, View f) => Mat.Matrix Double -> Either String (M f g DMatrix)
+fromHMat' = mkM' . CM.vertcat . V.fromList . fmap (CM.trans . dvector . V.fromList) . Mat.toLists
diff --git a/src/Dyno/View/NumInstances.hs b/src/Dyno/View/NumInstances.hs
new file mode 100644
--- /dev/null
+++ b/src/Dyno/View/NumInstances.hs
@@ -0,0 +1,166 @@
+{-# OPTIONS_GHC -Wall -fno-warn-orphans #-}
+{-# LANGUAGE FlexibleInstances #-}
+{-# LANGUAGE ScopedTypeVariables #-}
+
+module Dyno.View.NumInstances
+ (
+ ) where
+
+import Data.Proxy ( Proxy(..) )
+import Data.Vector ( Vector )
+import qualified Data.Vector as V
+
+import Dyno.View.CasadiMat ( CasadiMat(..) )
+import Casadi.MX ( MX )
+import Casadi.SX ( SX )
+import Casadi.DMatrix ( DMatrix )
+
+import Dyno.View.View
+
+--------------------------- SX ---------------------------
+instance (View f) => Num (J f SX) where
+ (UnsafeJ x) + (UnsafeJ y) = mkJ (x + y)
+ (UnsafeJ x) - (UnsafeJ y) = mkJ (x - y)
+ (UnsafeJ x) * (UnsafeJ y) = mkJ (x * y)
+ abs = fmap abs
+ signum = fmap signum
+ fromInteger k = mkJ (fromInteger k * ones (n, 1))
+ where
+ n = size (Proxy :: Proxy f)
+
+instance (View f) => Fractional (J f SX) where
+ (UnsafeJ x) / (UnsafeJ y) = mkJ (x / y)
+ fromRational x = mkJ (fromRational x * ones (n, 1))
+ where
+ n = size (Proxy :: Proxy f)
+
+instance (View f) => Floating (J f SX) where
+ pi = mkJ (pi * ones (n, 1))
+ where
+ n = size (Proxy :: Proxy f)
+ (**) (UnsafeJ x) (UnsafeJ y) = mkJ (x ** y)
+ exp = fmap exp
+ log = fmap log
+ sin = fmap sin
+ cos = fmap cos
+ tan = fmap tan
+ asin = fmap asin
+ atan = fmap atan
+ acos = fmap acos
+ sinh = fmap sinh
+ cosh = fmap cosh
+ tanh = fmap tanh
+ asinh = fmap asinh
+ atanh = fmap atanh
+ acosh = fmap acosh
+
+--------------------------- MX ---------------------------
+instance (View f) => Num (J f MX) where
+ (UnsafeJ x) + (UnsafeJ y) = mkJ (x + y)
+ (UnsafeJ x) - (UnsafeJ y) = mkJ (x - y)
+ (UnsafeJ x) * (UnsafeJ y) = mkJ (x * y)
+ abs = fmap abs
+ signum = fmap signum
+ fromInteger k = mkJ (fromInteger k * ones (n, 1))
+ where
+ n = size (Proxy :: Proxy f)
+
+instance (View f) => Fractional (J f MX) where
+ (UnsafeJ x) / (UnsafeJ y) = mkJ (x / y)
+ fromRational x = mkJ (fromRational x * ones (n, 1))
+ where
+ n = size (Proxy :: Proxy f)
+
+instance (View f) => Floating (J f MX) where
+ pi = mkJ (pi * ones (n, 1))
+ where
+ n = size (Proxy :: Proxy f)
+ (**) (UnsafeJ x) (UnsafeJ y) = mkJ (x ** y)
+ exp = fmap exp
+ log = fmap log
+ sin = fmap sin
+ cos = fmap cos
+ tan = fmap tan
+ asin = fmap asin
+ atan = fmap atan
+ acos = fmap acos
+ sinh = fmap sinh
+ cosh = fmap cosh
+ tanh = fmap tanh
+ asinh = fmap asinh
+ atanh = fmap atanh
+ acosh = fmap acosh
+
+---------------------------- DMatrix ----------------------------------
+instance (View f) => Num (J f DMatrix) where
+ (UnsafeJ x) + (UnsafeJ y) = mkJ (x + y)
+ (UnsafeJ x) - (UnsafeJ y) = mkJ (x - y)
+ (UnsafeJ x) * (UnsafeJ y) = mkJ (x * y)
+ abs = fmap abs
+ signum = fmap signum
+ fromInteger k = mkJ (fromInteger k * ones (n, 1))
+ where
+ n = size (Proxy :: Proxy f)
+
+instance (View f) => Fractional (J f DMatrix) where
+ (UnsafeJ x) / (UnsafeJ y) = mkJ (x / y)
+ fromRational x = mkJ (fromRational x * ones (n, 1))
+ where
+ n = size (Proxy :: Proxy f)
+
+instance (View f) => Floating (J f DMatrix) where
+ pi = mkJ (pi * ones (n, 1))
+ where
+ n = size (Proxy :: Proxy f)
+ (**) (UnsafeJ x) (UnsafeJ y) = mkJ (x ** y)
+ exp = fmap exp
+ log = fmap log
+ sin = fmap sin
+ cos = fmap cos
+ tan = fmap tan
+ asin = fmap asin
+ atan = fmap atan
+ acos = fmap acos
+ sinh = fmap sinh
+ cosh = fmap cosh
+ tanh = fmap tanh
+ asinh = fmap asinh
+ atanh = fmap atanh
+ acosh = fmap acosh
+
+---------------------- Vector a ------------------------
+instance (View f, Num a) => Num (J f (Vector a)) where
+ (UnsafeJ x) + (UnsafeJ y) = mkJ $ V.zipWith (+) x y
+ (UnsafeJ x) - (UnsafeJ y) = mkJ $ V.zipWith (-) x y
+ (UnsafeJ x) * (UnsafeJ y) = mkJ $ V.zipWith (*) x y
+ abs = fmap (fmap abs)
+ signum = fmap (fmap signum)
+ fromInteger k = mkJ (V.replicate n (fromInteger k))
+ where
+ n = size (Proxy :: Proxy f)
+
+instance (View f, Fractional a) => Fractional (J f (Vector a)) where
+ (UnsafeJ x) / (UnsafeJ y) = mkJ $ V.zipWith (/) x y
+ fromRational x = mkJ (V.replicate n (fromRational x))
+ where
+ n = size (Proxy :: Proxy f)
+
+instance (View f, Floating a) => Floating (J f (Vector a)) where
+ pi = mkJ (V.replicate n pi)
+ where
+ n = size (Proxy :: Proxy f)
+ (**) (UnsafeJ x) (UnsafeJ y) = mkJ $ V.zipWith (**) x y
+ exp = fmap (fmap exp)
+ log = fmap (fmap log)
+ sin = fmap (fmap sin)
+ cos = fmap (fmap cos)
+ tan = fmap (fmap tan)
+ asin = fmap (fmap asin)
+ atan = fmap (fmap atan)
+ acos = fmap (fmap acos)
+ sinh = fmap (fmap sinh)
+ cosh = fmap (fmap cosh)
+ tanh = fmap (fmap tanh)
+ asinh = fmap (fmap asinh)
+ atanh = fmap (fmap atanh)
+ acosh = fmap (fmap acosh)
diff --git a/src/Dyno/View/Scheme.hs b/src/Dyno/View/Scheme.hs
new file mode 100644
--- /dev/null
+++ b/src/Dyno/View/Scheme.hs
@@ -0,0 +1,231 @@
+{-# OPTIONS_GHC -Wall #-}
+{-# LANGUAGE ScopedTypeVariables #-}
+{-# LANGUAGE TypeFamilies #-}
+{-# LANGUAGE DefaultSignatures #-}
+{-# LANGUAGE TypeOperators #-}
+{-# LANGUAGE MultiParamTypeClasses #-}
+{-# LANGUAGE FlexibleContexts #-}
+{-# LANGUAGE FlexibleInstances #-}
+{-# LANGUAGE DeriveGeneric #-}
+{-# LANGUAGE DeriveFunctor #-}
+
+module Dyno.View.Scheme
+ ( Scheme(..)
+ , FunctionIO(..)
+ , blockSplit
+ ) where
+
+import Data.Proxy
+import qualified Data.Foldable as F
+import qualified Data.Sequence as Seq
+import qualified Data.Vector as V
+import Data.Vector ( Vector )
+import GHC.Generics hiding ( S )
+
+import Dyno.Nats
+import Dyno.View.View
+import Dyno.View.CasadiMat
+import Dyno.View.M ( M(..) )
+
+data MyScheme a = MyScheme (J (JVec D3 S) a) (J (JVec D2 S) a) deriving (Generic, Generic1, Show)
+instance Scheme MyScheme
+
+--go :: MyScheme MX
+--go = fromVector (V.fromList [400,500])
+--
+--og :: V.Vector MX
+--og = toVector go
+
+blockSplit :: forall f g a . (View f, View g, CasadiMat a) => M f g a -> Vector (Vector a)
+blockSplit (UnsafeM m) = fmap (flip horzsplit hsizes) ms
+ where
+ vsizes = V.fromList $ 0 : (F.toList (sizes 0 (Proxy :: Proxy f)))
+ hsizes = V.fromList $ 0 : (F.toList (sizes 0 (Proxy :: Proxy g)))
+ ms = vertsplit m vsizes
+
+class FunctionIO (f :: * -> *) where
+ fromMat :: CasadiMat a => a -> Either String (f a)
+ toFioMat :: f a -> a
+ matSizes :: Proxy f -> (Int,Int)
+
+instance View x => Scheme (J x) where
+ numFields = const 1
+ fromVector v = case V.toList v of
+ [m] -> case fromMat m of Left err -> error $ "Scheme fromVector J error: " ++ err
+ Right m' -> m'
+ _ -> error $ "Scheme fromVector (J x) length mismatch, should be 1 but got: "
+ ++ show (V.length v)
+ toVector = V.singleton . toFioMat
+ sizeList p = [matSizes p]
+
+instance (View f, View g) => Scheme (M f g) where
+ numFields = const 1
+ fromVector v = case V.toList v of
+ [m] -> case fromMat m of Left err -> error $ "Scheme fromVector M error: " ++ err
+ Right m' -> m'
+ _ -> error $ "Scheme fromVector (M f g) length mismatch, should be 1 but got: "
+ ++ show (V.length v)
+ toVector = V.singleton . toFioMat
+ sizeList p = [matSizes p]
+
+instance View f => FunctionIO (J f) where
+ toFioMat = unsafeUnJ
+ fromMat x
+ | n1 /= n1' = mismatch
+ | n1 /= 0 && n2 /= n2' = mismatch
+ | n1 == 0 && not (n2 `elem` [0,1]) = mismatch
+ | otherwise = Right (UnsafeJ x)
+ where
+ mismatch = Left $ "length mismatch: typed size: " ++ show (n1',n2') ++
+ ", actual size: " ++ show (n1,n2)
+ n1' = size (Proxy :: Proxy f)
+ n2' = 1
+ n1 = size1 x
+ n2 = size2 x
+ matSizes = const (size (Proxy :: Proxy f), 1)
+
+instance (View f, View g) => FunctionIO (M f g) where
+ toFioMat = unM
+ fromMat x
+ | n2 /= n2' = mismatch
+ | n1 /= n1' = mismatch
+ | otherwise = Right (UnsafeM x)
+ where
+ mismatch = Left $ "length mismatch: typed size: " ++ show (n1',n2') ++
+ ", actual size: " ++ show (n1,n2)
+ n1' = size (Proxy :: Proxy f)
+ n2' = size (Proxy :: Proxy g)
+ n1 = size1 x
+ n2 = size2 x
+ matSizes = const (size (Proxy :: Proxy f), size (Proxy :: Proxy g))
+
+class Scheme (f :: * -> *) where
+ numFields :: Proxy f -> Int
+ fromVector :: CasadiMat a => V.Vector a -> f a
+ toVector :: f a -> V.Vector a
+ sizeList :: Proxy f -> [(Int,Int)]
+
+ default numFields :: (GNumFields (Rep (f ())), Generic (f ())) => Proxy f -> Int
+ numFields = gnumFields . reproxy
+ where
+ reproxy :: Proxy g -> Proxy ((Rep (g ())) p)
+ reproxy = const Proxy
+
+ default sizeList :: (GSizeList (Rep (f ())), Generic (f ())) => Proxy f -> [(Int,Int)]
+ sizeList = F.toList . gsizeList . reproxy
+ where
+ reproxy :: Proxy g -> Proxy ((Rep (g ())) p)
+ reproxy = const Proxy
+
+ default fromVector :: ( Rep (f a) aa ~ M1 t d ff aa, GFromVector (Rep (f a)) a
+ , Generic (f a), Datatype d, CasadiMat a )
+ => Vector a -> f a
+ fromVector vs = out'
+ where
+ out' = to out
+ name = datatypeName (from out')
+ out = gfromVector name vs proxy
+
+ reproxy :: g b -> Proxy (g b)
+ reproxy = const Proxy
+
+ proxy = reproxy out
+
+ default toVector :: (GToVector (Rep (f a)) a, Generic (f a)) => f a -> Vector a
+ toVector = V.fromList . F.toList . gtoVector . from
+
+----------------------------------------------------------
+class GNumFields f where
+ gnumFields :: Proxy (f p) -> Int
+class GSizeList f where
+ gsizeList :: Proxy (f p) -> Seq.Seq (Int,Int)
+class GFromVector f a where
+ gfromVector :: CasadiMat a => String -> Vector a -> Proxy (f a) -> f a
+class GToVector f a where
+ gtoVector :: f a -> Seq.Seq a
+
+------------------------------------ GNumFields ------------------------------
+instance (GNumFields f, GNumFields g) => GNumFields (f :*: g) where
+ gnumFields pxy = gnumFields px + gnumFields py
+ where
+ reproxy :: Proxy ((x :*: y) p) -> (Proxy (x p), Proxy (y p))
+ reproxy = const (Proxy,Proxy)
+ (px, py) = reproxy pxy
+instance GNumFields f => GNumFields (M1 i d f) where
+ gnumFields = gnumFields . reproxy
+ where
+ reproxy :: Proxy (M1 i d f p) -> Proxy (f p)
+ reproxy _ = Proxy
+instance GNumFields (Rec0 f) where
+ gnumFields = const 1
+--instance GNumFields U1 where
+-- gnumFields = const 0
+
+------------------------------------ GSizeList ------------------------------
+instance (GSizeList f, GSizeList g) => GSizeList (f :*: g) where
+ gsizeList pxy = gsizeList px Seq.>< gsizeList py
+ where
+ reproxy :: Proxy ((x :*: y) p) -> (Proxy (x p), Proxy (y p))
+ reproxy = const (Proxy,Proxy)
+ (px, py) = reproxy pxy
+instance GSizeList f => GSizeList (M1 i d f) where
+ gsizeList = gsizeList . reproxy
+ where
+ reproxy :: Proxy (M1 i d f p) -> Proxy (f p)
+ reproxy = const Proxy
+instance FunctionIO f => GSizeList (Rec0 (f p)) where
+ gsizeList = Seq.singleton . matSizes . reproxy
+ where
+ reproxy :: Proxy (Rec0 (f p) q) -> Proxy f
+ reproxy = const Proxy
+
+--------------------- GFromVector ----------------------------
+instance (GFromVector f a, GFromVector g a, GNumFields f, GNumFields g) => GFromVector (f :*: g) a where
+ gfromVector name vs pxy
+ | V.length vs == nx + ny = gfromVector name vx px :*: gfromVector name vy py
+ | otherwise = error $ "\"" ++ name ++ "\" GFromVector (:*:) length error, need " ++
+ show (nx,ny) ++ " but got " ++ show (V.length vs)
+ where
+ nx = gnumFields px
+ ny = gnumFields py
+ (vx,vy) = V.splitAt nx vs
+
+ reproxy :: Proxy ((x :*: y) p) -> (Proxy (x p), Proxy (y p))
+ reproxy = const (Proxy,Proxy)
+ (px, py) = reproxy pxy
+
+instance GFromVector f a => GFromVector (M1 i d f) a where
+ gfromVector name vs = M1 . gfromVector name vs . reproxy
+ where
+ reproxy :: Proxy (M1 i d f p) -> Proxy (f p)
+ reproxy = const Proxy
+
+instance FunctionIO f => GFromVector (Rec0 (f a)) a where
+ gfromVector name ms = const (K1 j)
+ where
+ j = case fromMat m of
+ Right j' -> j'
+ Left err -> error $ "\"" ++ name ++ "\" GFromVector fromMat error: " ++ err
+ m = case V.toList ms of
+ [m'] -> m'
+ _ -> error $ "\"" ++ name ++ "\" GFromVector Rec0 length error, " ++
+ "need exactly 1 value but got " ++ show (V.length ms)
+--instance GFromVector U1 a where
+-- gfromVector = const $ const $ const U1
+
+
+--------------------- GToVector ----------------------------
+instance (GToVector f a, GToVector g a, GNumFields f, GNumFields g) => GToVector (f :*: g) a where
+ gtoVector (x :*: y) = gtoVector x Seq.>< gtoVector y
+
+instance GToVector f a => GToVector (M1 i d f) a where
+ gtoVector = gtoVector . unM1
+
+instance View f => GToVector (Rec0 (J f a)) a where
+ gtoVector = Seq.singleton . unsafeUnJ . unK1
+
+instance (View f, View g) => GToVector (Rec0 (M f g a)) a where
+ gtoVector = Seq.singleton . unM . unK1
+
+--instance GToVector U1 a where
+-- gtoVector = const Seq.empty
diff --git a/src/Dyno/View/Symbolic.hs b/src/Dyno/View/Symbolic.hs
new file mode 100644
--- /dev/null
+++ b/src/Dyno/View/Symbolic.hs
@@ -0,0 +1,74 @@
+{-# OPTIONS_GHC -Wall #-}
+{-# LANGUAGE ScopedTypeVariables #-}
+
+module Dyno.View.Symbolic
+ ( Symbolic(..)
+ , Matrix(..)
+ , MX
+ , SX
+ , DMatrix.DMatrix
+ ) where
+
+import Data.Proxy ( Proxy(..) )
+import Data.Vector ( Vector )
+
+import Casadi.Core.Classes.SharedObject
+import Casadi.Core.Classes.Function ( Function, castFunction )
+import Casadi.Core.Classes.SXFunction
+import Casadi.Core.Classes.MXFunction
+import Casadi.Core.Enums ( InputOutputScheme(..) )
+
+import Casadi.SX ( SX, ssymV )
+import Casadi.Option ( setOption )
+import Casadi.MX ( MX, symV )
+import qualified Casadi.SX as SX
+import qualified Casadi.MX as MX
+import qualified Casadi.DMatrix as DMatrix
+import Casadi.IOSchemes
+
+
+import Dyno.View.View ( View(..), J, mkJ )
+import Dyno.View.Viewable ( Viewable(..) )
+import Dyno.View.CasadiMat ( CasadiMat )
+
+
+class (Viewable a, CasadiMat a) => Symbolic a where
+ -- | creating symbolics
+ sym :: View f => String -> IO (J f a)
+ mkScheme :: InputOutputScheme -> [(String,a)] -> IO (Vector a)
+ mkFunction :: String -> Vector a -> Vector a -> IO Function
+instance Symbolic SX where
+ sym = mkSym ssymV
+ mkScheme = mkSchemeSX
+ mkFunction name x y = do
+ f <- sxFunction__0 x y
+ setOption f "name" name
+ sharedObject_init__0 f
+ return (castFunction f)
+
+instance Symbolic MX where
+ sym = mkSym symV
+ mkScheme = mkSchemeMX
+ mkFunction name x y = do
+ f <- mxFunction__0 x y
+ setOption f "name" name
+ sharedObject_init__0 f
+ return (castFunction f)
+
+
+class Matrix a where
+ diag :: a -> a
+instance Matrix DMatrix.DMatrix where
+ diag = DMatrix.ddiag
+instance Matrix SX where
+ diag = SX.sdiag
+instance Matrix MX where
+ diag = MX.diag
+
+
+mkSym :: forall f a . (View f, Viewable a) => (String -> Int -> IO a) -> String -> IO (J f a)
+mkSym vsym name = ret
+ where
+ ret :: IO (J f a)
+ ret = fmap mkJ (vsym name n)
+ n = size (Proxy :: Proxy f)
diff --git a/src/Dyno/View/View.hs b/src/Dyno/View/View.hs
new file mode 100644
--- /dev/null
+++ b/src/Dyno/View/View.hs
@@ -0,0 +1,302 @@
+{-# OPTIONS_GHC -Wall #-}
+{-# LANGUAGE ScopedTypeVariables #-}
+{-# LANGUAGE KindSignatures #-}
+{-# LANGUAGE DefaultSignatures #-}
+{-# LANGUAGE TypeOperators #-}
+{-# LANGUAGE MultiParamTypeClasses #-}
+{-# LANGUAGE FlexibleContexts #-}
+{-# LANGUAGE FlexibleInstances #-}
+{-# LANGUAGE DeriveGeneric #-}
+{-# LANGUAGE DeriveFunctor #-}
+{-# LANGUAGE DeriveFoldable #-}
+{-# LANGUAGE DeriveTraversable #-}
+{-# LANGUAGE GeneralizedNewtypeDeriving #-}
+{-# LANGUAGE RankNTypes #-}
+{-# LANGUAGE InstanceSigs #-}
+
+module Dyno.View.View
+ ( J(..), mkJ, mkJ', unJ, unJ', View(..), JVec(..), JNone(..), S(..)
+ , JTuple(..)
+ , JTriple(..)
+ , jreplicate, jreplicate'
+ , reifyJVec, jfill
+ ) where
+
+import GHC.Generics hiding ( S )
+
+import Data.Foldable ( Foldable )
+import qualified Data.Foldable as F
+import qualified Data.Sequence as Seq
+import Data.Traversable ( Traversable )
+import Data.Proxy ( Proxy(..) )
+import Linear.V ( Dim(..) )
+import Data.Vector ( Vector )
+import qualified Data.Vector as V
+import Data.Serialize ( Serialize(..) )
+
+import Dyno.TypeVecs ( Vec(..), unVec, mkVec, mkVec', reifyVector )
+import Dyno.View.Viewable ( Viewable(..) )
+import Dyno.Vectorize ( Vectorize(..) )
+import Dyno.Server.Accessors ( Lookup(..), AccessorTree )
+
+data JTuple f g a = JTuple (J f a) (J g a) deriving ( Generic, Show )
+instance (View f, View g) => View (JTuple f g)
+data JTriple f g h a = JTriple (J f a) (J g a) (J h a) deriving ( Generic, Show )
+instance (View f, View g, View h) => View (JTriple f g h)
+--instance View Id
+--instance View Xy
+--instance View Xyz
+--instance View f => View (Fctr f)
+
+newtype J (f :: * -> *) (a :: *) = UnsafeJ { unsafeUnJ :: a } deriving (Eq, Functor, Generic)
+
+
+mkJ :: forall f a . (View f, Viewable a) => a -> J f a
+mkJ x = case mkJ' x of
+ Right x' -> x'
+ Left msg -> error msg
+
+mkJ' :: forall f a . (View f, Viewable a) => a -> Either String (J f a)
+mkJ' x
+ | ny' == 1 && nx == nx' = Right (UnsafeJ x)
+ | ny' == 0 && nx == nx' = Right (UnsafeJ (vrecoverDimension x 0))
+ | otherwise = Left $ "mkJ length mismatch: typed size: " ++ show nx ++
+ ", actual size: " ++ show nx'
+ where
+ nx = size (Proxy :: Proxy f)
+ nx' = vsize1 x
+ ny' = vsize2 x
+
+unJ :: forall f a . (View f, Viewable a) => J f a -> a
+unJ (UnsafeJ x)
+ | nx == nx' = x
+ | otherwise = error $ "unJ length mismatch: typed size: " ++ show nx ++
+ ", actual size: " ++ show nx'
+ where
+ nx = size (Proxy :: Proxy f)
+ nx' = vsize1 x
+
+unJ' :: forall f a . (View f, Viewable a) => String -> J f a -> a
+unJ' msg (UnsafeJ x)
+ | nx == nx' = x
+ | otherwise = error $ "unJ length mismatch in \"" ++ msg ++ "\": typed size: " ++ show nx ++
+ ", actual size: " ++ show nx'
+ where
+ nx = size (Proxy :: Proxy f)
+ nx' = vsize1 x
+
+instance Serialize a => Serialize (J f a)
+instance Show a => Show (J f a) where
+ showsPrec p (UnsafeJ x) = showsPrec p x
+instance Lookup a => Lookup (J S (Vector a)) where
+ toAccessorTree :: J S (Vector a) -> (b -> J S (Vector a)) -> AccessorTree b
+ toAccessorTree (UnsafeJ x) f =
+ toAccessorTree (V.head x) (V.head . unJ . f)
+
+-- | vectors in View
+newtype JVec n f a = JVec { unJVec :: Vec n (J f a) } deriving ( Show, Eq )
+instance (Dim n, View f) => View (JVec n f) where
+ cat = mkJ . vveccat . fmap unJ . unVec . unJVec
+ split = JVec . fmap mkJ . mkVec . flip vvertsplit ks . unJ
+ where
+ ks = V.fromList (take (n+1) [0,m..])
+ n = reflectDim (Proxy :: Proxy n)
+ m = size (Proxy :: Proxy f)
+ size = const (n * m)
+ where
+ n = reflectDim (Proxy :: Proxy n)
+ m = size (Proxy :: Proxy f)
+ sizes = const . Seq.iterateN n (+m) . (+ m)
+ where
+ n = reflectDim (Proxy :: Proxy n)
+ m = size (Proxy :: Proxy f)
+instance (Dim n, Serialize (J f a)) => Serialize (JVec n f a) where
+ get = fmap (JVec . mkVec') get
+ put = put . F.toList . unJVec
+
+jreplicate' :: forall a n f . (Dim n, View f) => J f a -> JVec n f a
+jreplicate' el = ret
+ where
+ ret = JVec (mkVec (V.replicate nvec el))
+ nvec = reflectDim (Proxy :: Proxy n)
+
+jreplicate :: forall a n f . (Dim n, View f, Viewable a) => J f a -> J (JVec n f) a
+jreplicate = cat . jreplicate'
+
+jfill :: forall a f . View f => a -> J f (Vector a)
+jfill x = mkJ (V.replicate n x)
+ where
+ n = size (Proxy :: Proxy f)
+
+reifyJVec :: forall a f r . Vector (J f a) -> (forall (n :: *). Dim n => JVec n f a -> r) -> r
+reifyJVec v f = reifyVector v $ \(v' :: Vec n (J f a)) -> f (JVec v' :: JVec n f a)
+{-# INLINE reifyJVec #-}
+
+-- | view into a None, for convenience
+data JNone a = JNone deriving ( Eq, Generic, Generic1, Show, Functor, Foldable, Traversable )
+instance Vectorize JNone where
+instance View JNone where
+
+-- | view into a scalar, for convenience
+newtype S a = S { unS :: a } deriving ( Eq, Num, Fractional, Floating, Generic, Generic1, Show, Functor, Foldable, Traversable )
+instance View S where
+ cat :: forall a . Viewable a => S a -> J S a
+ cat (S x) = mkJ x
+ size = const 1
+ sizes = const . Seq.singleton . (1 +)
+ split :: forall a . Viewable a => J S a -> S a
+ split = S . unJ
+
+-- | Type-save "views" into vectors, which can access subvectors
+-- without splitting then concatenating everything.
+class View f where
+ cat :: Viewable a => f a -> J f a
+ default cat :: (GCat (Rep (f a)) a, Generic (f a), Viewable a) => f a -> J f a
+ cat = mkJ . vveccat . V.fromList . F.toList . gcat . from
+
+ size :: Proxy f -> Int
+ default size :: (GSize (Rep (f ())), Generic (f ())) => Proxy f -> Int
+ size = gsize . reproxy
+ where
+ reproxy :: Proxy g -> Proxy ((Rep (g ())) p)
+ reproxy = const Proxy
+
+ sizes :: Int -> Proxy f -> Seq.Seq Int
+ default sizes :: (GSize (Rep (f ())), Generic (f ())) => Int -> Proxy f -> Seq.Seq Int
+ sizes k0 = gsizes k0 . reproxy
+ where
+ reproxy :: Proxy g -> Proxy ((Rep (g ())) p)
+ reproxy = const Proxy
+
+ split :: Viewable a => J f a -> f a
+ default split :: (GBuild (Rep (f a)) a, Generic (f a), Viewable a) => J f a -> f a
+ split x'
+ | null leftovers = to ret
+ | otherwise = error $ unlines
+ [ "split got " ++ show (length leftovers) ++ " leftover fields"
+ , "ns: " ++ show ns ++ "\n" ++ show (map vsize1 leftovers)
+ --, "x: " ++ show x'
+ , "size1(x): " ++ show (vsize1 (unJ x'))
+ --, "leftovers: " ++ show leftovers
+ , "errors: " ++ show (reverse errors)
+ ]
+ where
+ x = unJ x'
+ (ret,leftovers,errors) = gbuild [] xs
+ xs = V.toList $ vvertsplit x (V.fromList ns)
+ ns :: [Int]
+ ns = (0 :) $ F.toList $ sizes 0 (Proxy :: Proxy f)
+
+------------------------------------ SIZE ------------------------------
+class GSize f where
+ gsize :: Proxy (f p) -> Int
+ gsizes :: Int -> Proxy (f p) -> Seq.Seq Int
+
+instance (GSize f, GSize g) => GSize (f :*: g) where
+ gsize pxy = gsize px + gsize py
+ where
+ reproxy :: Proxy ((x :*: y) p) -> (Proxy (x p), Proxy (y p))
+ reproxy = const (Proxy,Proxy)
+ (px, py) = reproxy pxy
+ gsizes k0 pxy = xs Seq.>< ys
+ where
+ xs = gsizes k0 px
+ ys = gsizes k1 py
+ k1 = case Seq.viewr xs of
+ Seq.EmptyR -> k0
+ _ Seq.:> k1' -> k1'
+
+ reproxy :: Proxy ((x :*: y) p) -> (Proxy (x p), Proxy (y p))
+ reproxy = const (Proxy,Proxy)
+ (px, py) = reproxy pxy
+instance GSize f => GSize (M1 i d f) where
+ gsize = gsize . reproxy
+ where
+ reproxy :: Proxy (M1 i d f p) -> Proxy (f p)
+ reproxy _ = Proxy
+ gsizes k0 = gsizes k0 . reproxy
+ where
+ reproxy :: Proxy (M1 i d f p) -> Proxy (f p)
+ reproxy _ = Proxy
+
+instance View f => GSize (Rec0 (J f a)) where
+ gsize = size . reproxy
+ where
+ reproxy :: Proxy (Rec0 (J f a) p) -> Proxy f
+ reproxy _ = Proxy
+ gsizes k0 = Seq.singleton . (k0 +) . size . reproxy
+ where
+ reproxy :: Proxy (Rec0 (J f a) p) -> Proxy f
+ reproxy _ = Proxy
+
+instance GSize U1 where
+ gsize = const 0
+ gsizes = const . Seq.singleton
+
+----------------------------- CAT -------------------------------
+class GCat f a where
+ gcat :: f p -> Seq.Seq a
+
+-- concatenate fields recursively
+instance (GCat f a, GCat g a) => GCat (f :*: g) a where
+ gcat (x :*: y) = x' Seq.>< y'
+ where
+ x' = gcat x
+ y' = gcat y
+-- discard the metadata
+instance GCat f a => GCat (M1 i d f) a where
+ gcat = gcat . unM1
+
+-- any field should just hold a view, no recursion here
+instance (View f, Viewable a) => GCat (Rec0 (J f a)) a where
+ gcat (K1 x) = Seq.singleton (unJ x)
+
+instance GCat U1 a where
+ gcat U1 = Seq.empty
+
+-------------------------
+class GBuild f a where
+ gbuild :: [String] -> [a] -> (f p, [a], [String])
+
+-- split fields recursively
+instance (GBuild f a, GBuild g a, GSize f, GSize g) => GBuild (f :*: g) a where
+ gbuild errs0 xs0 = (x :*: y, xs2, errs2)
+ where
+ (x,xs1,errs1) = gbuild errs0 xs0
+ (y,xs2,errs2) = gbuild errs1 xs1
+
+instance (GBuild f a, Datatype d) => GBuild (D1 d f) a where
+ gbuild :: forall p . [String] -> [a] -> (D1 d f p, [a], [String])
+ gbuild errs0 xs0 = (ret, xs1, errs1)
+ where
+ err = moduleName ret ++ "." ++ datatypeName ret :: String
+ ret = M1 x :: D1 d f p
+ (x,xs1,errs1) = gbuild (err:errs0) xs0
+
+instance (GBuild f a, Constructor c) => GBuild (C1 c f) a where
+ gbuild :: forall p . [String] -> [a] -> (C1 c f p, [a], [String])
+ gbuild errs0 xs0 = (ret, xs1, errs1)
+ where
+ err = conName ret :: String
+ ret = M1 x :: C1 c f p
+ (x,xs1,errs1) = gbuild (err:errs0) xs0
+
+instance (GBuild f a, Selector s) => GBuild (S1 s f) a where
+ gbuild :: forall p . [String] -> [a] -> (S1 s f p, [a], [String])
+ gbuild errs0 xs0 = (ret, xs1, errs1)
+ where
+ err = selName ret :: String
+ ret = M1 x :: S1 s f p
+ (x,xs1,errs1) = gbuild (err:errs0) xs0
+
+-- any field should just hold a view, no recursion here
+instance (View f, Viewable a) => GBuild (Rec0 (J f a)) a where
+ gbuild errs (x:xs) = (K1 (mkJ x), xs, errs)
+ gbuild errs [] = error $ "GBuild (Rec0 (J f a)) a: empty list" ++ show (reverse errs)
+
+instance Viewable a => GBuild U1 a where
+ gbuild errs (x:xs)
+ | vsize1 x /= 0 = error $ "GBuild U1: got non-empty element: " ++
+ show (vsize1 x) ++ "\n" ++ show (reverse errs)
+ | otherwise = (U1, xs, errs)
+ gbuild errs [] = error $ "GBuild U1: got empty" ++ show (reverse errs)
diff --git a/src/Dyno/View/Viewable.hs b/src/Dyno/View/Viewable.hs
new file mode 100644
--- /dev/null
+++ b/src/Dyno/View/Viewable.hs
@@ -0,0 +1,73 @@
+{-# OPTIONS_GHC -Wall #-}
+{-# Language TypeFamilies #-}
+
+module Dyno.View.Viewable
+ ( Viewable(..), MX.MX, SX.SX, DMatrix.DMatrix
+ ) where
+
+import qualified Data.Vector as V
+
+import qualified Casadi.SX as SX
+import qualified Casadi.MX as MX
+import qualified Casadi.DMatrix as DMatrix
+import qualified Dyno.View.CasadiMat as CM
+
+class Viewable a where
+ vvertsplit :: a -> V.Vector Int -> V.Vector a
+ vhorzsplit :: a -> V.Vector Int -> V.Vector a
+ vveccat :: V.Vector a -> a
+ vsize1 :: a -> Int
+ vsize2 :: a -> Int
+ vrecoverDimension :: a -> Int -> a
+
+instance Viewable SX.SX where
+ vveccat = SX.sveccat
+ vvertsplit = CM.vertsplit
+ vhorzsplit = CM.horzsplit
+ vsize1 = CM.size1
+ vsize2 = CM.size2
+ vrecoverDimension _ k = CM.zeros (k,1)
+
+instance Viewable MX.MX where
+ vveccat = MX.veccat
+ vvertsplit = CM.vertsplit
+ vhorzsplit = CM.horzsplit
+ vsize1 = CM.size1
+ vsize2 = CM.size2
+ vrecoverDimension _ k = CM.zeros (k,1)
+
+instance Viewable DMatrix.DMatrix where
+ vveccat = DMatrix.dveccat
+ vvertsplit = CM.vertsplit
+ vhorzsplit = CM.horzsplit
+ vsize1 = CM.size1
+ vsize2 = CM.size2
+ vrecoverDimension _ k = CM.zeros (k,1)
+
+--instance CM.CasadiMat a => Viewable a where
+-- vveccat = CM.veccat
+-- vvertsplit = CM.vertsplit
+-- vhorzsplit = CM.horzsplit
+-- vsize1 x
+-- | CM.size2 x == 1 = CM.size1 x
+-- | otherwise = error "Dyno.View.Viewable(vsize1): not a column!!"
+
+instance Viewable (V.Vector a) where
+ vsize1 = V.length
+ vsize2 = const 1
+ vveccat = V.concat . V.toList
+ vvertsplit x ks = V.fromList (split x (V.toList ks))
+ vhorzsplit _ _ = error "vhorzsplit not defined for Vector"
+ vrecoverDimension x _ = x
+
+split :: V.Vector a -> [Int] -> [V.Vector a]
+split v xs@(0:_) = split' v xs
+split _ _ = error "split: first index must be 0"
+
+split' :: V.Vector a -> [Int] -> [V.Vector a]
+split' _ [] = error "can't split with no input"
+split' x [kf]
+ | V.length x == kf = []
+ | otherwise = error "split: last index must be length of vector"
+split' x (k0:k1:ks) = V.slice k0 (k1 - k0) x : split' x (k1:ks)
+
diff --git a/tests/NewUnitTests.hs b/tests/NewUnitTests.hs
new file mode 100644
--- /dev/null
+++ b/tests/NewUnitTests.hs
@@ -0,0 +1,26 @@
+{-# OPTIONS_GHC -Wall #-}
+
+module Main ( main ) where
+
+import Data.Monoid ( mempty )
+import Test.Framework ( ColorMode(..), RunnerOptions'(..), TestOptions'(..), defaultMainWithOpts )
+
+import VectorizeTests ( vectorizeTests )
+import ViewTests ( viewTests )
+
+main :: IO ()
+main = do
+ defaultMainWithOpts
+ [ vectorizeTests
+ , viewTests
+ ]
+ opts
+
+opts :: RunnerOptions' Maybe
+opts = mempty { ropt_color_mode = Just ColorAlways
+ , ropt_threads = Just 1
+ , ropt_test_options = Just my_test_opts
+ }
+
+my_test_opts :: TestOptions' Maybe
+my_test_opts = mempty { topt_timeout = Just (Just 2000000) }
diff --git a/tests/Utils.hs b/tests/Utils.hs
new file mode 100644
--- /dev/null
+++ b/tests/Utils.hs
@@ -0,0 +1,20 @@
+{-# OPTIONS_GHC -Wall #-}
+{-# Language PolyKinds #-}
+
+module Utils
+ ( reproxy
+ , reproxy2
+ , reproxy3
+ ) where
+
+import Data.Proxy
+
+reproxy :: Proxy f -> Proxy g -> Proxy (f g)
+reproxy _ _ = Proxy
+
+reproxy2 :: Proxy f -> Proxy g -> Proxy h -> Proxy (f g h)
+reproxy2 _ _ _ = Proxy
+
+reproxy3 :: Proxy f -> Proxy g -> Proxy h -> Proxy j -> Proxy (f g h j)
+reproxy3 _ _ _ _ = Proxy
+
diff --git a/tests/VectorizeTests.hs b/tests/VectorizeTests.hs
new file mode 100644
--- /dev/null
+++ b/tests/VectorizeTests.hs
@@ -0,0 +1,149 @@
+{-# OPTIONS_GHC -Wall #-}
+{-# Language ScopedTypeVariables #-}
+{-# Language GADTs #-}
+{-# Language DeriveFunctor #-}
+{-# Language DeriveGeneric #-}
+
+module VectorizeTests
+ ( Vectorizes(..)
+ , Dims(..)
+ , vectorizeTests
+ ) where
+
+import GHC.Generics ( Generic )
+import qualified Data.Vector as V
+import Linear
+import Linear.V
+
+import Test.QuickCheck
+import Test.Framework ( Test, testGroup )
+import Test.Framework.Providers.QuickCheck2 ( testProperty )
+
+import Dyno.Vectorize
+import Dyno.Nats
+import qualified Dyno.TypeVecs as TV
+
+import Utils
+
+
+data X0 a = X0 a (V3 a) a (V2 a) deriving (Show, Eq, Functor, Generic, Generic1)
+data X1 f g h a = X1 (f a) (V3 (g a)) a (V2 a) a (h a) deriving (Show, Eq, Functor, Generic, Generic1)
+
+instance Vectorize X0
+instance (Vectorize f, Vectorize g, Vectorize h) => Vectorize (X1 f g h)
+
+data Vectorizes where
+ Vectorizes ::
+ (Show (f Int), Eq (f Int), Vectorize f)
+ => { vShrinks :: [Vectorizes]
+ , vName :: String
+ , vProxy :: Proxy f } -> Vectorizes
+
+
+data Dims where
+ Dims :: Dim n =>
+ { dShrinks :: [Dims]
+ , dProxy :: Proxy n
+ } -> Dims
+instance Show Dims where
+ show (Dims _ p) = "D" ++ show (reflectDim p)
+
+instance Arbitrary Dims where
+ arbitrary = elements [ d0, d1, d2, d3, d4, d10, d100 ]
+ where
+ d0 = Dims [] (Proxy :: Proxy D0)
+ d1 = Dims [d0] (Proxy :: Proxy D1)
+ d2 = Dims [d0,d1] (Proxy :: Proxy D2)
+ d3 = Dims [d0,d1,d2] (Proxy :: Proxy D3)
+ d4 = Dims [d0,d1,d2,d3] (Proxy :: Proxy D4)
+ d10 = Dims [d0,d1,d2,d3,d4] (Proxy :: Proxy D10)
+ d100 = Dims [d0,d1,d2,d3,d4,d10] (Proxy :: Proxy D100)
+ shrink = dShrinks
+
+instance Show Vectorizes where
+ show = vName
+
+maxVSize :: Int
+maxVSize = 1000
+
+instance Arbitrary Vectorizes where
+ arbitrary = do
+ x <- oneof [primitives, compounds primitives, compounds (compounds primitives)]
+ if vecSize x <= maxVSize then return x else arbitrary
+ shrink = filter ((<= maxVSize) . vecSize) . shrink' True
+ where
+ shrink' True v = vShrinks v ++ concatMap (shrink' False) (vShrinks v)
+ shrink' False v = vShrinks v
+
+vecSize :: Vectorizes -> Int
+vecSize (Vectorizes _ _ p) = vlength p
+
+primitives :: Gen Vectorizes
+primitives = do
+ d <- arbitrary
+ elements
+ [ Vectorizes [] "None" (Proxy :: Proxy None)
+ , Vectorizes [] "Id" (Proxy :: Proxy Id)
+ , Vectorizes [] "V0" (Proxy :: Proxy V0)
+ , Vectorizes [] "V1" (Proxy :: Proxy V1)
+ , Vectorizes [] "V2" (Proxy :: Proxy V2)
+ , Vectorizes [] "V3" (Proxy :: Proxy V3)
+ , Vectorizes [] "V4" (Proxy :: Proxy V4)
+ , Vectorizes [] "X0" (Proxy :: Proxy X0)
+ , mkTypeVec True d
+ ]
+
+compounds :: Gen Vectorizes -> Gen Vectorizes
+compounds genIt = do
+ v1@(Vectorizes _ m1 p1) <- genIt
+ v2@(Vectorizes _ m2 p2) <- genIt
+ v3@(Vectorizes _ m3 p3) <- genIt
+ elements
+ [ Vectorizes
+ { vShrinks = [v1, v2]
+ , vName = "Tuple (" ++ m1 ++ ") (" ++ m2 ++ ")"
+ , vProxy = reproxy2 (Proxy :: Proxy Tuple) p1 p2
+ }
+ , Vectorizes
+ { vShrinks = [v1, v2, v3]
+ , vName = "Triple (" ++ m1 ++ ") (" ++ m2 ++ ") (" ++ m3 ++ ")"
+ , vProxy = reproxy3 (Proxy :: Proxy Triple) p1 p2 p3
+ }
+ , Vectorizes
+ { vShrinks = [v1, v2, v3]
+ , vName = "X1 (" ++ m1 ++ ") (" ++ m2 ++ ") " ++ m3 ++ ")"
+ , vProxy = reproxy3 (Proxy :: Proxy X1) p1 p2 p3
+ }
+ ]
+
+mkTypeVec :: Bool -> Dims -> Vectorizes
+mkTypeVec shrinkThis d@(Dims _ pd) =
+ Vectorizes
+ { vShrinks = if shrinkThis then map (mkTypeVec False) (shrink d) else []
+ , vName = "Vec " ++ show d
+ , vProxy = reproxy (Proxy :: Proxy TV.Vec) pd
+ }
+
+fillInc :: forall x . Vectorize x => x Int
+fillInc = devectorize $ V.fromList $ take (vlength (Proxy :: Proxy x)) [0..]
+
+vectorizeThenDevectorize ::
+ forall x
+ . (Show (x Int), Eq (x Int), Vectorize x)
+ => Proxy x -> Bool
+vectorizeThenDevectorize _ = x0 == x1
+ where
+ x0 :: x Int
+ x0 = fillInc
+
+ x1 :: x Int
+ x1 = devectorize (vectorize x0)
+
+prop_vecThenDevec :: Vectorizes -> Bool
+prop_vecThenDevec (Vectorizes _ _ p) = vectorizeThenDevectorize p
+
+vectorizeTests :: Test
+vectorizeTests =
+ testGroup "vectorize tests"
+ [ testProperty "vec . devec" prop_vecThenDevec
+ ]
diff --git a/tests/ViewTests.hs b/tests/ViewTests.hs
new file mode 100644
--- /dev/null
+++ b/tests/ViewTests.hs
@@ -0,0 +1,300 @@
+{-# OPTIONS_GHC -Wall #-}
+{-# Language ScopedTypeVariables #-}
+{-# Language GADTs #-}
+{-# Language DeriveGeneric #-}
+{-# Language FlexibleInstances #-}
+
+module ViewTests
+ ( Views(..)
+ , CasadiMats(..)
+ , viewTests
+ ) where
+
+import qualified Data.Packed.Matrix as Mat
+import qualified Numeric.LinearAlgebra ( ) -- for Eq Matrix
+import qualified Data.Vector as V
+import GHC.Generics ( Generic )
+import System.IO.Unsafe ( unsafePerformIO )
+import Test.QuickCheck
+import Test.Framework ( Test, testGroup )
+import Test.Framework.Providers.QuickCheck2 ( testProperty )
+
+import Casadi.Function ( evalDMatrix )
+import Casadi.MXFunction ( mxFunction )
+import Casadi.SharedObject ( soInit )
+
+import Dyno.TypeVecs ( Vec, Dim )
+import Dyno.Vectorize
+import Dyno.View
+import Dyno.View.M
+import Dyno.View.CasadiMat ( CasadiMat )
+import Dyno.Cov
+
+import Utils
+import VectorizeTests ( Vectorizes(..), Dims(..) )
+
+data Views where
+ Views :: View f =>
+ { vwShrinks :: [Views]
+ , vwName :: String
+ , vwProxy :: Proxy f
+ } -> Views
+instance Show Views where
+ show = vwName
+
+data CasadiMats where
+ CasadiMats :: (Viewable f, CasadiMat f, MyEq f) =>
+ { cmName :: String
+ , cmProxy :: Proxy f
+ } -> CasadiMats
+instance Show CasadiMats where
+ show = cmName
+
+-- MX is less frequent because evalMX takes a while
+instance Arbitrary CasadiMats where
+ arbitrary = frequency [ (1, return (CasadiMats "MX" (Proxy :: Proxy MX)))
+ , (5, return (CasadiMats "SX" (Proxy :: Proxy SX)))
+ , (5, return (CasadiMats "DMatrix" (Proxy :: Proxy DMatrix)))
+ ]
+
+evalMX :: MX -> DMatrix
+evalMX x = unsafePerformIO $ do
+ f <- mxFunction V.empty (V.singleton x)
+ soInit f
+ ret <- evalDMatrix f V.empty
+ return (V.head ret)
+
+data JX0 f a = JX0 (J (JV f) a) (J (JV f) a) deriving (Show, Generic, Generic1)
+instance Vectorize f => View (JX0 f)
+--instance Scheme JX
+
+data JX1 f g a = JX1 (J (JV f) a) (J g a) deriving (Show, Generic, Generic1)
+instance (Vectorize f, View g) => View (JX1 f g)
+--instance Scheme JX
+
+data JX2 f g h a = JX2 (J f a) (J (JTuple g (JV h)) a) (J g a) (J (JV h) a) (J f a)
+ deriving (Show, Generic, Generic1)
+instance (View f, View g, Vectorize h) => View (JX2 f g h)
+----instance Scheme JX2
+
+maxViewSize :: Int
+maxViewSize = 200
+
+class MyEq a where
+ myEq :: a -> a -> Bool
+
+instance MyEq a => MyEq (J f a) where
+ myEq (UnsafeJ x) (UnsafeJ y) = myEq x y
+instance MyEq a => MyEq (M f g a) where
+ myEq (UnsafeM x) (UnsafeM y) = myEq x y
+instance MyEq SX where
+ myEq = (==)
+instance MyEq DMatrix where
+ myEq = (==)
+instance MyEq MX where
+ myEq x y = myEq (evalMX x) (evalMX y)
+instance (Dim n, MyEq a) => MyEq (Vec n a) where
+ myEq f g = V.and $ V.zipWith myEq (vectorize f) (vectorize g)
+instance MyEq (Mat.Matrix Double) where
+ myEq x y
+ | and [rowx == 0, rowy == 0, colx == coly] = True
+ | and [colx == 0, coly == 0, rowx == rowy] = True
+ | otherwise = x == y
+ where
+ rowx = Mat.rows x
+ colx = Mat.cols x
+ rowy = Mat.rows y
+ coly = Mat.cols y
+
+instance Arbitrary Views where
+ arbitrary = do
+ x <- oneof [primitives, compound primitives, compound (compound primitives)]
+ if viewSize x <= maxViewSize then return x else arbitrary
+ shrink = filter ((<= maxViewSize) . viewSize) . vwShrinks
+
+compound :: Gen Views -> Gen Views
+compound genIt = do
+ vc'@(Vectorizes _ mz pz) <- arbitrary
+ let vc = mkJV vc'
+ vw0@(Views _ mv0 pv0) <- genIt
+ vw1@(Views _ mv1 pv1) <- genIt
+ elements
+ [ Views [vc] ("JX0 (" ++ mz ++ ")") (reproxy (Proxy :: Proxy JX0) pz)
+ , Views [vc,vw0] ("JX1 (" ++ mz ++ ") (" ++ mv0 ++ ")") (reproxy2 (Proxy :: Proxy JX1) pz pv0)
+ , Views [vc, vw0, vw1] ("JX2 (" ++ mv0 ++ ") (" ++ mv1 ++ ") (" ++ mz ++ ")")
+ (reproxy3 (Proxy :: Proxy JX2) pv0 pv1 pz)
+ , Views [vw0] ("Cov (" ++ mv0 ++ ")") (reproxy (Proxy :: Proxy Cov) pv0)
+ ]
+
+viewSize :: Views -> Int
+viewSize (Views _ _ p) = size p
+
+mkJV :: Vectorizes -> Views
+mkJV = mkJV' True
+ where
+ mkJV' :: Bool -> Vectorizes -> Views
+ mkJV' sh v@(Vectorizes _ m p) = Views shrinks ("JV (" ++ m ++ ")") (reproxyJV p)
+ where
+ shrinks :: [Views]
+ shrinks = if sh then map (mkJV' False) (shrink v) else []
+
+ reproxyJV :: Proxy f -> Proxy (JV f)
+ reproxyJV = const Proxy
+
+primitives :: Gen Views
+primitives = do
+ v <- arbitrary
+ elements
+ [ Views [] "JNone" (Proxy :: Proxy JNone)
+ , Views [] "S" (Proxy :: Proxy S)
+ , mkJV v
+ ]
+
+--data M1 a = M1 (M JX JX2 a) deriving (Show, Generic, Generic1)
+--data M2 a = M2 (M JNone JNone a) deriving (Show, Generic, Generic1)
+--data M3 a = M3 (M JX2 JNone a) deriving (Show, Generic, Generic1)
+--data M4 a = M4 (M JNone JX2 a) deriving (Show, Generic, Generic1)
+
+--instance Scheme M1
+--instance Scheme M2
+--instance Scheme M3
+--instance Scheme M4
+
+beEqual :: (MyEq a, Show a) => a -> a -> Property
+beEqual x y = counterexample (sx ++ " =/= " ++ sy) (myEq x y)
+ where
+ sx = show x
+ sy = show y
+
+prop_VSplitVCat :: Test
+prop_VSplitVCat =
+ testProperty "vcat . vsplit" $
+ \(Vectorizes _ _ p1) (Views _ _ p2) (CasadiMats {cmProxy = pm}) -> test p1 p2 pm
+ where
+ test :: forall f g a
+ . (Vectorize f, View g, CasadiMat a, MyEq a)
+ => Proxy f -> Proxy g -> Proxy a -> Property
+ test _ _ _ = beEqual x0 x1
+ where
+ x0 :: M (JV f) g a
+ x0 = countUp
+
+ x1 :: M (JV f) g a
+ x1 = vcat (vsplit x0)
+
+prop_HSplitHCat :: Test
+prop_HSplitHCat =
+ testProperty "hcat . hsplit" $
+ \(Views _ _ p1) (Vectorizes _ _ p2) (CasadiMats {cmProxy = pm}) -> test p1 p2 pm
+ where
+ test :: forall f g a
+ . (View f, Vectorize g, CasadiMat a, MyEq a)
+ => Proxy f -> Proxy g -> Proxy a -> Property
+ test _ _ _ = beEqual x0 x1
+ where
+ x0 :: M f (JV g) a
+ x0 = countUp
+
+ x1 :: M f (JV g) a
+ x1 = hcat (hsplit x0)
+
+prop_VSplitVCat' :: Test
+prop_VSplitVCat' =
+ testProperty "vsplit' . vcat'" $
+ \(Dims _ pd) (Views _ _ p1) (Views _ _ p2) (CasadiMats {cmProxy = pm}) -> test pd p1 p2 pm
+ where
+ test :: forall f g n a
+ . (View f, View g, Dim n, CasadiMat a, MyEq a)
+ => Proxy n -> Proxy f -> Proxy g -> Proxy a -> Property
+ test _ _ _ _ = beEqual x0 x1
+ where
+ x0 :: Vec n (M f g a)
+ x0 = fill countUp
+
+ x1 :: Vec n (M f g a)
+ x1 = vsplit' (vcat' x0)
+
+
+prop_HSplitHCat' :: Test
+prop_HSplitHCat' =
+ testProperty "hsplit' . hcat'" $
+ \(Dims _ pd) (Views _ _ p1) (Views _ _ p2) (CasadiMats {cmProxy = pm}) -> test pd p1 p2 pm
+ where
+ test :: forall f g n a
+ . (View f, View g, Dim n, CasadiMat a, MyEq a)
+ => Proxy n -> Proxy f -> Proxy g -> Proxy a -> Property
+ test _ _ _ _ = beEqual x0 x1
+ where
+ x0 :: Vec n (M f g a)
+ x0 = fill countUp
+
+ x1 :: Vec n (M f g a)
+ x1 = hsplit' (hcat' x0)
+
+prop_testSplitJ :: Test
+prop_testSplitJ =
+ testProperty "split . cat J" $
+ \(Vectorizes _ _ p) (CasadiMats {cmProxy = pm}) -> test p pm
+ where
+ test :: forall f a
+ . (Vectorize f, CasadiMat a, Viewable a, MyEq a)
+ => Proxy f -> Proxy a -> Property
+ test _ _ = beEqual xj0 xj2
+ where
+ UnsafeM xm0 = countUp :: M (JV f) (JV Id) a
+
+ xj0 :: J (JV f) a
+ xj0 = mkJ xm0
+
+ xj1 :: JV f a
+ xj1 = split xj0
+
+ xj2 :: J (JV f) a
+ xj2 = cat xj1
+
+prop_toFromHMat :: Test
+prop_toFromHMat =
+ testProperty "fromHMat . toHMat" $
+ \(Views _ _ p1) (Views _ _ p2) -> test p1 p2
+ where
+ test :: forall f g
+ . (View f, View g)
+ => Proxy f -> Proxy g -> Property
+ test _ _ = beEqual m0 m2
+ where
+ m0 = countUp :: M f g DMatrix
+
+ m1 = toHMat m0 :: Mat.Matrix Double
+
+ m2 = fromHMat m1 :: M f g DMatrix
+
+prop_fromToHMat :: Test
+prop_fromToHMat =
+ testProperty "toHMat . fromHMat" $
+ \(Views _ _ p1) (Views _ _ p2) -> test p1 p2
+ where
+ test :: forall f g
+ . (View f, View g)
+ => Proxy f -> Proxy g -> Property
+ test _ _ = beEqual m1 m3
+ where
+ m0 = countUp :: M f g DMatrix
+
+ m1 = toHMat m0 :: Mat.Matrix Double
+
+ m2 = fromHMat m1 :: M f g DMatrix
+
+ m3 = toHMat m2 :: Mat.Matrix Double
+
+
+viewTests :: Test
+viewTests =
+ testGroup "view tests"
+ [ prop_VSplitVCat
+ , prop_HSplitHCat
+ , prop_VSplitVCat'
+ , prop_HSplitHCat'
+ , prop_testSplitJ
+ , prop_toFromHMat
+ , prop_fromToHMat
+ ]