packages feed

dynobud (empty) → 1.0.0.0

raw patch · 70 files changed

+12155/−0 lines, 70 filesdep +Chartdep +Chart-cairodep +Chart-gtksetup-changed

Dependencies added: Chart, Chart-cairo, Chart-gtk, HUnit, QuickCheck, base, bytestring, cairo, casadi-bindings, casadi-bindings-core, casadi-bindings-internal, cereal, cmdargs, colour, containers, data-default, data-default-class, distributive, dynobud, ghc-prim, glib, gtk, hmatrix, jacobi-roots, lens, linear, mtl, not-gloss, primitive, process, reflection, semigroups, spatial-math, stm, tagged, test-framework, test-framework-hunit, test-framework-quickcheck2, text, time, transformers, unordered-containers, vector, zeromq4-haskell

Files

+ LICENSE view
@@ -0,0 +1,165 @@+                  GNU LESSER GENERAL PUBLIC LICENSE+                       Version 3, 29 June 2007++ Copyright (C) 2007 Free Software Foundation, Inc. <http://fsf.org/>+ 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.
+ README.md view
@@ -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
+ Setup.hs view
@@ -0,0 +1,2 @@+import Distribution.Simple+main = defaultMain
+ dynobud.cabal view
@@ -0,0 +1,396 @@+name:                dynobud+version:             1.0.0.0+synopsis:            your dynamic optimization buddy+description:         See readme at <http://www.github.com/ghorn/dynobud http://www.github.com/ghorn/dynobud>+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
+ examples/Basic.hs view
@@ -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
+ examples/BasicJ.hs view
@@ -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
+ examples/DaeColl.hs view
@@ -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 ()
+ examples/Glider.hs view
@@ -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 ()
+ examples/Homotopy.hs view
@@ -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
+ examples/MultipleShooting.hs view
@@ -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
+ examples/OcpM.hs view
@@ -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)
+ examples/PlotSofa.hs view
@@ -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
+ examples/Plotter.hs view
@@ -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"
+ examples/Rocket.hs view
@@ -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)
+ examples/Sailboat.hs view
@@ -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 ()
+ examples/Sofa.hs view
@@ -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 ()+
+ examples/StaticExample.hs view
@@ -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
+ examples/Vec.hs view
@@ -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)
+ src/Dyno/Cov.hs view
@@ -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'
+ src/Dyno/Dae.hs view
@@ -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 (+)
+ src/Dyno/DirectCollocation.hs view
@@ -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
+ src/Dyno/DirectCollocation/Dynamic.hs view
@@ -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')
+ src/Dyno/DirectCollocation/Export.hs view
@@ -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
+ src/Dyno/DirectCollocation/Formulate.hs view
@@ -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)
+ src/Dyno/DirectCollocation/Integrate.hs view
@@ -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
+ src/Dyno/DirectCollocation/Profile.hs view
@@ -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
+ src/Dyno/DirectCollocation/Quadratures.hs view
@@ -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
+ src/Dyno/DirectCollocation/Reify.hs view
@@ -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
+ src/Dyno/DirectCollocation/Robust.hs view
@@ -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))
+ src/Dyno/DirectCollocation/Types.hs view
@@ -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
+ src/Dyno/Interface/LogsAndErrors.hs view
@@ -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))
+ src/Dyno/Interface/Types.hs view
@@ -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 #-}
+ src/Dyno/LagrangePolynomials.lhs view
@@ -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}
+ src/Dyno/Models/AeroCoeffs.hs view
@@ -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 )
+ src/Dyno/Models/Aircraft.hs view
@@ -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'+          }
+ src/Dyno/Models/Betty.hs view
@@ -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
+ src/Dyno/MultipleShooting.hs view
@@ -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
+ src/Dyno/Nats.hs view
@@ -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))))))))))))+
+ src/Dyno/Nlp.hs view
@@ -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))+  }
+ src/Dyno/NlpMonad.hs view
@@ -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)
+ src/Dyno/NlpScaling.hs view
@@ -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)
+ src/Dyno/NlpSolver.hs view
@@ -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
+ src/Dyno/Ocp.hs view
@@ -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))+  }
+ src/Dyno/OcpMonad.hs view
@@ -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
+ src/Dyno/SXElement.hs view
@@ -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)
+ src/Dyno/Server/Accessors.hs view
@@ -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
+ src/Dyno/Server/GraphWidget.hs view
@@ -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
+ src/Dyno/Server/PlotChart.hs view
@@ -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
+ src/Dyno/Server/PlotTypes.hs view
@@ -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+          }
+ src/Dyno/Server/Server.hs view
@@ -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
+ src/Dyno/Solvers.hs view
@@ -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 ())+  }
+ src/Dyno/TypeVecs.hs view
@@ -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)
+ src/Dyno/Vectorize.hs view
@@ -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
+ src/Dyno/View.hs view
@@ -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()
+ src/Dyno/View/CasadiMat.hs view
@@ -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 _ = []
+ src/Dyno/View/CustomFunction.hs view
@@ -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))
+ src/Dyno/View/Fun.hs view
@@ -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
+ src/Dyno/View/FunJac.hs view
@@ -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
+ src/Dyno/View/HList.hs view
@@ -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
+ src/Dyno/View/JV.hs view
@@ -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)
+ src/Dyno/View/M.hs view
@@ -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
+ src/Dyno/View/NumInstances.hs view
@@ -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)
+ src/Dyno/View/Scheme.hs view
@@ -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
+ src/Dyno/View/Symbolic.hs view
@@ -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)
+ src/Dyno/View/View.hs view
@@ -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)
+ src/Dyno/View/Viewable.hs view
@@ -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)+
+ tests/NewUnitTests.hs view
@@ -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) }
+ tests/Utils.hs view
@@ -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+
+ tests/VectorizeTests.hs view
@@ -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+  ]
+ tests/ViewTests.hs view
@@ -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+  ]