dynobud 1.1.0.0 → 1.3.0.0
raw patch · 53 files changed
+2331/−1888 lines, 53 filesdep +Plot-ho-maticdep +binarydep +generic-accessorsdep −Chart-cairodep −cairodep −cerealdep ~casadi-bindingsdep ~not-glossnew-component:exe:basic-nlpnew-component:exe:easy-nlpnew-component:exe:nlp-solver
Dependencies added: Plot-ho-matic, binary, generic-accessors, hmatrix-gsl, vector-binary-instances
Dependencies removed: Chart-cairo, cairo, cereal, data-default, glib, gtk, text, time
Dependency ranges changed: casadi-bindings, not-gloss
Files
- README.md +5/−15
- dynobud.cabal +44/−34
- examples/Basic.hs +0/−59
- examples/BasicJ.hs +0/−68
- examples/BasicNlp.hs +68/−0
- examples/DaeColl.hs +15/−13
- examples/Dynoplot.hs +9/−10
- examples/EasyNlp.hs +46/−0
- examples/ExampleDsl/NlpMonad.hs +23/−22
- examples/ExampleDsl/OcpMonad.hs +39/−28
- examples/Glider.hs +11/−9
- examples/Glider/AeroCoeffs.hs +2/−1
- examples/Glider/Aircraft.hs +3/−2
- examples/Homotopy.hs +32/−24
- examples/MultipleShooting.hs +4/−4
- examples/NlpSolverEx.hs +146/−0
- examples/OcpDslRocket.hs +2/−1
- examples/OcpDslSpring.hs +2/−1
- examples/Sailboat.hs +22/−17
- examples/Sofa/Common.hs +3/−3
- examples/SofaExpando.hs +57/−71
- examples/SofaVisualizer.hs +5/−4
- src/Dyno/AutoScaling.hs +329/−0
- src/Dyno/DirectCollocation.hs +12/−10
- src/Dyno/DirectCollocation/Dynamic.hs +109/−113
- src/Dyno/DirectCollocation/Export.hs +4/−3
- src/Dyno/DirectCollocation/Formulate.hs +231/−141
- src/Dyno/DirectCollocation/Integrate.hs +6/−7
- src/Dyno/DirectCollocation/Profile.hs +20/−17
- src/Dyno/DirectCollocation/Quadratures.hs +5/−4
- src/Dyno/DirectCollocation/Types.hs +37/−5
- src/Dyno/MultipleShooting.hs +13/−13
- src/Dyno/Nlp.hs +36/−50
- src/Dyno/NlpScaling.hs +46/−3
- src/Dyno/NlpSolver.hs +218/−218
- src/Dyno/NlpUtils.hs +273/−0
- src/Dyno/Ocp.hs +19/−43
- src/Dyno/Server/Accessors.hs +0/−178
- src/Dyno/Server/GraphWidget.hs +0/−359
- src/Dyno/Server/PlotChart.hs +0/−75
- src/Dyno/Server/PlotTypes.hs +0/−47
- src/Dyno/Server/Server.hs +0/−171
- src/Dyno/Solvers.hs +37/−7
- src/Dyno/TypeVecs.hs +5/−4
- src/Dyno/Vectorize.hs +1/−1
- src/Dyno/View/JV.hs +0/−6
- src/Dyno/View/JVec.hs +1/−7
- src/Dyno/View/M.hs +88/−8
- src/Dyno/View/Unsafe/M.hs +7/−0
- src/Dyno/View/Unsafe/View.hs +4/−5
- tests/IntegrationTests.hs +196/−0
- tests/NewUnitTests.hs +11/−7
- tests/ViewTests.hs +85/−0
README.md view
@@ -1,9 +1,11 @@ ## dynobud - your dynamic optimization buddy +[](http://travis-ci.org/ghorn/dynobud)+ 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)-* OCP modeling/solving (examles/Glider.hs)+* NLP modeling/solving (examples/EasyNlp.hs, examples/BasicNlp.hs, examples/SofaExpando.hs)+* OCP modeling/solving (examles/Glider.hs, examples/DaeColl.hs) * proof of concept monadic NLP/OCP modeling DSL (examples/{NlpDsl.hs,OcpDslRocket.hs}) * live plotter for OCP solving (examples/Dynoplot.hs) @@ -30,19 +32,7 @@ >> 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+ >> sudo apt-get install coinor-libipopt-dev liblapack-dev libblas-dev libglpk-dev libgl1-mesa-dev libglu1-mesa-dev freeglut3-dev libzmq3-dev libglib2.0-dev libcairo2-dev libpango1.0-dev libgtk2.0-dev libgsl0-dev To build dynobud from source
dynobud.cabal view
@@ -1,5 +1,5 @@ name: dynobud-version: 1.1.0.0+version: 1.3.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@@ -18,7 +18,8 @@ location: git://github.com/ghorn/dynobud.git library- exposed-modules: Dyno.LagrangePolynomials+ exposed-modules: Dyno.AutoScaling+ Dyno.LagrangePolynomials Dyno.TypeVecs Dyno.MultipleShooting Dyno.Ocp@@ -50,42 +51,31 @@ Dyno.Nlp Dyno.NlpScaling Dyno.NlpSolver+ Dyno.NlpUtils 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, casadi-bindings-core >= 2.2.0.2,- casadi-bindings >= 2.2.0.4,+ casadi-bindings >= 2.2.0.8, -- casadi-bindings-internal, jacobi-roots >=0.2 && <0.3, spatial-math >= 0.2.1.0, vector >=0.10,- data-default,+ vector-binary-instances, mtl >=2.2.1, containers >=0.5, hmatrix, linear >= 1.3.1.1, reflection >= 1.3.2,- lens,- cereal,- time,- gtk >= 0.13,- glib,- Chart-cairo >= 1.3.3,- cairo,- Chart >= 1.3.3,- data-default-class,+ binary, distributive,- text,- process+ process,+ Plot-ho-matic >= 0.5.0.2,+ generic-accessors >= 0.1.0.1 -- cplex hs-source-dirs: src default-language: Haskell2010@@ -97,6 +87,19 @@ description: build the examples default: False +executable nlp-solver+ if flag(examples)+ Buildable: True+ else+ Buildable: False+ hs-source-dirs: examples+ main-is: NlpSolverEx.hs+ default-language: Haskell2010+ build-depends: dynobud+ , base >=4.6 && < 5+ , casadi-bindings+ ghc-options: -O2+ executable multiple_shooting if flag(examples) Buildable: True@@ -129,10 +132,10 @@ default-language: Haskell2010 build-depends: dynobud, base >=4.6 && < 5,- not-gloss >= 0.7.0.0,+ not-gloss >= 0.7.0.1, stm, containers,- cereal,+ binary, linear, bytestring, zeromq4-haskell,@@ -153,7 +156,7 @@ casadi-bindings-core, zeromq4-haskell, bytestring,- cereal,+ binary, linear, base >= 4.6 && < 5 ghc-options: -threaded -O2@@ -206,7 +209,7 @@ , lens , bytestring , zeromq4-haskell- , cereal+ , binary , mtl ghc-options: -threaded -O2 @@ -230,7 +233,7 @@ , lens , bytestring , zeromq4-haskell- , cereal+ , binary , mtl ghc-options: -threaded -O2@@ -257,26 +260,26 @@ , mtl ghc-options: -threaded -O2 -executable basic+executable easy-nlp if flag(examples) Buildable: True else Buildable: False hs-source-dirs: examples- main-is: Basic.hs+ main-is: EasyNlp.hs default-language: Haskell2010 build-depends: dynobud, vector >=0.10, base >=4.6 && < 5 ghc-options: -threaded -O2 -executable basicJ+executable basic-nlp if flag(examples) Buildable: True else Buildable: False hs-source-dirs: examples- main-is: BasicJ.hs+ main-is: BasicNlp.hs default-language: Haskell2010 build-depends: dynobud, vector >=0.10,@@ -294,6 +297,7 @@ default-language: Haskell2010 build-depends: dynobud, base >=4.6 && < 5,+ generic-accessors >= 0.1.0.0, vector ghc-options: -threaded -O2 @@ -313,8 +317,9 @@ containers, linear, bytestring,- cereal,+ binary, vector,+ generic-accessors >= 0.1.0.0, zeromq4-haskell ghc-options: -threaded -O2 @@ -331,9 +336,10 @@ containers, linear, bytestring,- cereal,+ binary, vector, semigroups,+ generic-accessors >= 0.1.0.0, zeromq4-haskell ghc-options: -threaded -O2 @@ -349,11 +355,12 @@ base >=4.6 && < 5, containers, vector,- cereal,+ binary, bytestring, zeromq4-haskell,+ Plot-ho-matic >= 0.5.0.0, cmdargs- ghc-options: -O2+ ghc-options: -O2 -with-rtsopts=-T --test-suite lp_tests -- type: exitcode-stdio-1.0@@ -395,7 +402,8 @@ type: exitcode-stdio-1.0 hs-source-dirs: tests main-is: NewUnitTests.hs- other-modules: VectorizeTests+ other-modules: IntegrationTests+ VectorizeTests ViewTests Utils default-language: Haskell2010@@ -407,7 +415,9 @@ test-framework-quickcheck2, vector, linear,+ binary, casadi-bindings, hmatrix,+ hmatrix-gsl, base >=4.6 && < 5 ghc-options: -O2
− examples/Basic.hs
@@ -1,59 +0,0 @@--- | Minimize the Rosenbrock function (plus a trivial constraint) using--- the basic NLP interface.--{-# OPTIONS_GHC -Wall #-}-{-# Language DeriveFunctor #-}-{-# Language DeriveGeneric #-}--module Main where--import GHC.Generics ( Generic1 )--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
@@ -1,68 +0,0 @@--- | Minimize the Rosenbrock function (plus a trivial constraint) using--- the more complicated NLP' interface.--- Unfortunately, at the moment there only types here are (JV ) compound types--- so the use of Views aren't fully illustrated.--- todo: comment up the multiple shooting code as an example--{-# OPTIONS_GHC -Wall #-}-{-# Language DeriveFunctor #-}-{-# Language DeriveGeneric #-}--module Main where--import GHC.Generics ( Generic, Generic1 )--import Data.Vector ( Vector )-import qualified Data.Vector as V--import Casadi.MX ( MX )-import Dyno.View.View-import Dyno.View.JV ( JV, catJV, catJV', splitJV' )-import Dyno.Vectorize-import Dyno.Nlp-import Dyno.NlpSolver-import Dyno.Solvers---data X a = X a a deriving (Functor, Generic, Generic1, Show)-data G a = G a deriving (Functor, Generic, Generic1, Show)--instance Vectorize X-instance Vectorize G--myNlp :: Nlp' (JV X) JNone (JV 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 (JV X) (V.Vector Double)- x0 = catJV $ X (-8) (-8)-- bx :: J (JV X) (Vector Bounds)- bx = catJV $- X (Just (-21), Just 0.5)- (Just (-2), Just 2)-- bg :: J (JV G) (Vector Bounds)- bg = catJV $ G (Just (-10), Just 10)-- fg :: J (JV X) MX -> J JNone MX -> (J (JV Id) MX, J (JV G) MX)- fg xy _ = (f, catJV' g)- where- f = (1-x)**2 + 100*(y - x**2)**2- g = G x-- X x y = splitJV' xy--main :: IO ()-main = do- opt <- solveNlp' ipoptSolver myNlp Nothing- print opt
+ examples/BasicNlp.hs view
@@ -0,0 +1,68 @@+-- | Minimize the Rosenbrock function (plus a trivial constraint) using+-- the View-based NLP interface.+-- Unfortunately, at the moment there only types here are (JV ) compound types+-- so the use of Views aren't fully illustrated.+-- todo: comment up the multiple shooting code as an example++{-# OPTIONS_GHC -Wall #-}+{-# Language DeriveFunctor #-}+{-# Language DeriveGeneric #-}++module Main where++import GHC.Generics ( Generic, Generic1 )++import Data.Vector ( Vector )+import qualified Data.Vector as V++import Casadi.MX ( MX )+import Dyno.View.View+import Dyno.View.JV ( JV, catJV, catJV', splitJV' )+import Dyno.Vectorize+import Dyno.Nlp+import Dyno.NlpUtils+import Dyno.Solvers+++data X a = X a a deriving (Functor, Generic, Generic1, Show)+data G a = G a deriving (Functor, Generic, Generic1, Show)++instance Vectorize X+instance Vectorize G++myNlp :: Nlp (JV X) JNone (JV 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 (JV X) (V.Vector Double)+ x0 = catJV $ X (-8) (-8)++ bx :: J (JV X) (Vector Bounds)+ bx = catJV $+ X (Just (-21), Just 0.5)+ (Just (-2), Just 2)++ bg :: J (JV G) (Vector Bounds)+ bg = catJV $ G (Just (-10), Just 10)++ fg :: J (JV X) MX -> J JNone MX -> (J (JV Id) MX, J (JV G) MX)+ fg xy _ = (f, catJV' g)+ where+ f = (1-x)**2 + 100*(y - x**2)**2+ g = G x++ X x y = splitJV' xy++main :: IO ()+main = do+ opt <- solveNlp ipoptSolver myNlp Nothing+ print opt
examples/DaeColl.hs view
@@ -11,6 +11,8 @@ import Data.Vector ( Vector ) +import Accessors+ import Dyno.Vectorize import Dyno.View.View ( J, jfill ) import Dyno.TypeVecs@@ -18,11 +20,10 @@ --import Dyno.Sqp.Sqp --import Dyno.Sqp.LineSearch import Dyno.Nlp-import Dyno.NlpSolver-import Dyno.Server.Accessors-+import Dyno.NlpUtils import Dyno.Ocp import Dyno.DirectCollocation+import Dyno.DirectCollocation.Quadratures ( QuadratureRoots(..) ) data PendX a = PendX { pX :: a , pY :: a@@ -46,8 +47,8 @@ instance Lookup (PendZ ()) instance Lookup (PendU ()) -mayer :: Num a => t -> PendX a -> PendX a -> a-mayer _ _ _ = 0+mayer :: Num a => t -> PendX a -> PendX a -> None a -> PendP 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@@ -70,9 +71,10 @@ fx = torque*y fy = -torque*x + m*9.8 -pendOcp :: OcpPhase PendX PendZ PendU PendP PendR PendO (Vec 8) None+pendOcp :: OcpPhase PendX PendZ PendU PendP PendR PendO (Vec 8) None None pendOcp = OcpPhase { ocpMayer = mayer , ocpLagrange = lagrange+ , ocpQuadratures = \_ _ _ _ _ _ _ -> None , ocpDae = pendDae , ocpBc = bc , ocpPathC = pathc@@ -107,8 +109,8 @@ ubnd :: PendU Bounds ubnd = PendU (Just (-40), Just 40) -bc :: Floating a => PendX a -> PendX a -> Vec 8 a-bc (PendX x0 y0 vx0 vy0) (PendX xf yf vxf vyf) =+bc :: Floating a => PendX a -> PendX a -> None a -> PendP a -> a -> Vec 8 a+bc (PendX x0 y0 vx0 vy0) (PendX xf yf vxf vyf) _ _ _ = mkVec' [ x0 , y0 + r@@ -126,17 +128,17 @@ guess :: J (CollTraj PendX PendZ PendU PendP NCollStages CollDeg) (Vector Double) guess = jfill 1 -solver :: NlpSolverStuff+solver :: Solver solver = ipoptSolver -solver2 :: NlpSolverStuff+solver2 :: Solver solver2 = ipoptSolver { options = [("expand", Opt True)] } main :: IO () main = do- cp <- makeCollProblem pendOcp+ cp <- makeCollProblem Legendre pendOcp let nlp = cpNlp cp- _ <- solveNlp' solver (nlp { nlpX0' = guess }) Nothing--- _ <- solveNlp solver2 (nlp { nlpX0' = guess }) Nothing+ _ <- solveNlp solver (nlp { nlpX0 = guess }) Nothing+-- _ <- solveNlp solver2 (nlp { nlpX0 = guess }) Nothing return ()
examples/Dynoplot.hs view
@@ -3,17 +3,18 @@ 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.ByteString.Lazy ( fromStrict )+import Data.Binary ( decodeOrFail ) 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 ( DynPlotPoints, CollTrajMeta )+import PlotHo ( runPlotter ) +import Dyno.DirectCollocation.Dynamic ( DynPlotPoints, CollTrajMeta, addCollocationChannel )+ import Dynoplot.Channel ( dynoplotUrl, dynoplotChannelName ) sub :: String -> ((DynPlotPoints Double, CollTrajMeta) -> IO ()) -> String -> IO ()@@ -27,9 +28,9 @@ when mre $ do msg <- ZMQ.receive subscriber let decoded :: (DynPlotPoints Double, CollTrajMeta)- decoded = case decode msg of- Left err -> error err- Right t -> t+ decoded = case decodeOrFail (fromStrict msg) of+ Left (_,_,err) -> error $ "decode failure: " ++ err+ Right (_,_,t) -> t writeChan decoded main :: IO ()@@ -40,10 +41,8 @@ putStrLn $ "using ip \""++ip'++"\"" putStrLn $ "using channel \""++channel'++"\"" - (c0, writeMe) <- newChannel channel'+ runPlotter $ addCollocationChannel channel' (\w -> sub ip' w channel') - listenerTid0 <- CC.forkIO (sub ip' writeMe channel')- runPlotter c0 [listenerTid0] data VisArgs = VisArgs { ip :: String , channel :: String
+ examples/EasyNlp.hs view
@@ -0,0 +1,46 @@+-- | Minimize the Rosenbrock function (plus a trivial constraint) using+-- the most basic NLP interface.++{-# OPTIONS_GHC -Wall #-}+{-# Language DeriveFunctor #-}+{-# Language DeriveGeneric #-}++module Main where++import GHC.Generics ( Generic1 )++import Dyno.Vectorize ( Vectorize )+import Dyno.Nlp ( Bounds )+import Dyno.NlpUtils ( solveNlpV )+import Dyno.Solvers ( Solver, ipoptSolver )++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++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 :: Floating a => X a -> (a, G a)+fg (X x y) = (f, g)+ where+ f = (1-x)**2 + 100*(y - x**2)**2+ g = G x++solver :: Solver+solver = ipoptSolver+--solver = snoptSolver++main :: IO ()+main = do+ opt <- solveNlpV solver fg bx bg x0 Nothing+ print opt
examples/ExampleDsl/NlpMonad.hs view
@@ -46,8 +46,9 @@ import Dyno.View.JV ( JV ) import Dyno.View.JVec ( JVec ) import qualified Dyno.TypeVecs as TV-import Dyno.NlpSolver ( NlpSolverStuff, solveNlp' )-import Dyno.Nlp ( Nlp'(..), NlpOut'(..), Bounds)+import Dyno.Solvers ( Solver )+import Dyno.NlpUtils ( solveNlp )+import Dyno.Nlp ( Nlp(..), NlpOut(..), Bounds) import ExampleDsl.LogsAndErrors import ExampleDsl.Types@@ -149,7 +150,7 @@ toG nlpConstraints' = TV.mkVec $ V.fromList $ F.toList $ fmap constr nlpConstraints' buildNlp :: forall nx ng .- (Dim nx, Dim ng) => NlpMonadState -> IO (Nlp' (JVec nx (JV Id)) JNone (JVec ng (JV Id)) MX)+ (Dim nx, Dim ng) => NlpMonadState -> IO (Nlp (JVec nx (JV Id)) JNone (JVec ng (JV Id)) MX) buildNlp state = do obj <- case nlpObj state of Objective obj' -> return obj'@@ -174,24 +175,24 @@ 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- }+ 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)+ => 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@@ -208,13 +209,13 @@ 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 (JV Id)) JNone (JVec ng (JV Id)) MX)- let nlp = nlp0 { nlpX0' = mkJ x0 }+ nlp0 <- buildNlp state :: IO (Nlp (JVec nx (JV Id)) JNone (JVec ng (JV Id)) MX)+ let nlp = nlp0 { nlpX0 = mkJ x0 } f nlp (fmap (. unJ) cb) state solveStaticNlp ::- NlpSolverStuff+ Solver -> 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@@ -226,11 +227,11 @@ foo :: (View x, View p, View g) =>- Nlp' x p g MX -> Maybe (J x (Vector Double) -> IO Bool) -> NlpMonadState ->+ 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]+ (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)
examples/ExampleDsl/OcpMonad.hs view
@@ -6,6 +6,7 @@ {-# Language GeneralizedNewtypeDeriving #-} {-# Language FlexibleContexts #-} {-# Language RankNTypes #-}+{-# Language DataKinds #-} module ExampleDsl.OcpMonad ( OcpMonad@@ -55,7 +56,7 @@ import Dyno.Vectorize ( Vectorize(..), fill ) import Dyno.TypeVecs ( Vec ) import qualified Dyno.TypeVecs as TV-import Dyno.NlpSolver ( NlpSolverStuff )+import Dyno.Solvers ( Solver ) import Dyno.DirectCollocation.Quadratures ( QuadratureRoots(..) ) import Dyno.DirectCollocation.Dynamic ( DynPlotPoints, CollTrajMeta(..), NameTree(..) ) import Dyno.DirectCollocation ( solveOcp )@@ -242,12 +243,13 @@ -> ((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)+ ( 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 (Vec 0) -> CollTrajMeta -> IO ret) -> IO ret-reifyOcpPhase daeMonad mayerMonad bcMonad ocpMonad tbnds n deg f = do+reifyOcpPhase daeMonad mayerMonad bcMonad ocpMonad tbnds f = do time <- sxElementSym "_t" endT <- sxElementSym "T" let time' = sxElementToSX time@@ -355,7 +357,12 @@ (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)])+ mayerFunSX <- sxFunction (V.fromList [ svector (V.singleton endT)+ , svector (V.fromList x0s)+ , svector (V.fromList xFs)+ , svector V.empty+ , svector V.empty+ ]) (V.singleton (svector (V.singleton mayerObj))) setOption mayerFunSX "name" "mayer" soInit mayerFunSX@@ -385,7 +392,12 @@ ((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)])+ bcFunSX <- sxFunction (V.fromList [ svector (V.fromList x0s)+ , svector (V.fromList xFs)+ , svector V.empty+ , svector V.empty+ , svector (V.singleton endT)+ ]) (V.singleton (svector bcs)) setOption bcFunSX "name" "boundaryConditions" soInit bcFunSX@@ -396,20 +408,17 @@ , 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+ , ctmQ = NameTreeNode ("", "") [] }- 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) ->+ ctmNx = V.length xnames+ ctmNz = V.length znames+ ctmNu = V.length unames+ ctmNp = V.length pnames++ TV.reifyDim (ctmNx) $ \(Proxy :: Proxy nx) ->+ TV.reifyDim (ctmNz) $ \(Proxy :: Proxy nz) ->+ TV.reifyDim (ctmNu) $ \(Proxy :: Proxy nu) ->+ TV.reifyDim (ctmNp) $ \(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) ->@@ -442,16 +451,17 @@ 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+ mayerFun :: SXElement -> Vec nx SXElement -> Vec nx SXElement -> Vec 0 SXElement -> Vec np SXElement -> SXElement- mayerFun endT'' x0 xF = sxToSXElement $ V.head $ callSX mayerFunSX (V.fromList [sxElementToSX endT'', vec x0, vec xF])+ mayerFun endT'' x0 xF qF p = sxToSXElement $ V.head $ callSX mayerFunSX (V.fromList [sxElementToSX endT'', vec x0, vec xF, vec qF, vec p]) - bcFun :: Vec nx SXElement -> Vec nx SXElement -> Vec nc SXElement- bcFun x0 xF = devec $ V.head $ callSX bcFunSX (V.fromList [vec x0, vec xF])+ bcFun :: Vec nx SXElement -> Vec nx SXElement -> Vec 0 SXElement -> Vec np SXElement -> SXElement -> Vec nc SXElement+ bcFun x0 xF qF p t = devec $ V.head $ callSX bcFunSX (V.fromList [vec x0, vec xF, vec qF, vec p, sxElementToSX t]) ocpPhase = OcpPhase { ocpMayer = mayerFun , ocpLagrange = lagrangeFun+ , ocpQuadratures = \_ _ _ _ _ _ _ -> fill 0 , ocpDae = daeFun , ocpBc = bcFun , ocpBcBnds = devectorize bcbnds@@ -481,7 +491,8 @@ devec = sxSplitJV . mkJ solveStaticOcp ::- NlpSolverStuff+ QuadratureRoots+ -> Solver -> (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 ())@@ -490,7 +501,7 @@ -> Int -> Int -> Maybe (CollTrajMeta -> DynPlotPoints 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+solveStaticOcp roots solverStuff dae mayer bc ocp tbnds n deg cb =+ reifyOcpPhase dae mayer bc ocp tbnds woo where- woo ocpphase meta = solveOcp solverStuff n deg (cb <*> pure meta) ocpphase+ woo ocpphase meta = solveOcp roots solverStuff n deg (cb <*> pure meta) ocpphase
examples/Glider.hs view
@@ -13,11 +13,12 @@ --import Dyno.Sqp.Sqp --import Dyno.Sqp.LineSearch import Dyno.Nlp-import Dyno.NlpSolver+import Dyno.NlpUtils import Dyno.Ocp import Dyno.DirectCollocation import Dyno.DirectCollocation.Dynamic ( toMeta )+import Dyno.DirectCollocation.Quadratures ( QuadratureRoots(..) ) import Glider.Aircraft import Glider.AeroCoeffs@@ -28,8 +29,8 @@ type NCollStages = 100 type CollDeg = 2 -mayer :: Floating a => a -> AcX a -> AcX a -> a-mayer _ _ _ = 0+mayer :: Floating a => a -> AcX a -> AcX a -> None a -> None 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') _ _ _ _ =@@ -55,9 +56,10 @@ mcs = bettyMc refs = bettyRefs -ocp :: OcpPhase AcX None AcU None AcX None AcX None+ocp :: OcpPhase AcX None AcU None AcX None AcX None None ocp = OcpPhase { ocpMayer = mayer , ocpLagrange = lagrange+ , ocpQuadratures = \_ _ _ _ _ _ _ -> None , ocpDae = dae , ocpBc = bc , ocpPathC = pathc@@ -103,12 +105,12 @@ , 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+bc :: Floating a => AcX a -> AcX a -> None a -> None a -> 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- cp <- makeCollProblem ocp+ cp <- makeCollProblem Legendre ocp let nlp = cpNlp cp withCallback $ \cb -> do let guess = jfill 1@@ -118,9 +120,9 @@ plotPoints <- cpPlotPoints cp traj let proxy :: Proxy (CollTraj AcX None AcU None NCollStages CollDeg) proxy = Proxy- cb (plotPoints, toMeta (cpRoots cp) (Proxy :: Proxy None) proxy)+ cb (plotPoints, toMeta (Proxy :: Proxy None) (Proxy :: Proxy None) proxy) - (msg,_) <- solveNlp' ipoptSolver (nlp { nlpX0' = guess }) (Just cb')+ (msg,_) <- solveNlp ipoptSolver (nlp { nlpX0 = guess }) (Just cb') case msg of Left msg' -> putStrLn $ "optimization failed, message: " ++ msg' Right _ -> putStrLn "optimization succeeded" -- let xopt = xOpt opt
examples/Glider/AeroCoeffs.hs view
@@ -10,7 +10,8 @@ import Data.Foldable ( Foldable ) import Linear -import Dyno.Server.Accessors ( Lookup(..) )+import Accessors ( Lookup )+ import Dyno.Vectorize atan2' :: Floating a => a -> a -> a
examples/Glider/Aircraft.hs view
@@ -1,4 +1,4 @@-{-# OPTIONS_GHC -Wall -fno-warn-orphans #-}+{-# OPTIONS_GHC -Wall #-} {-# Language ScopedTypeVariables #-} {-# Language DeriveFunctor #-} {-# Language DeriveGeneric #-}@@ -9,8 +9,9 @@ import Linear +import Accessors ( Lookup )+ import Dyno.Vectorize-import Dyno.Server.Accessors ( Lookup(..) ) import Glider.AeroCoeffs
examples/Homotopy.hs view
@@ -15,10 +15,17 @@ import Dyno.View.View ( J ) import Dyno.View.JV ( JV, catJV, catJV', splitJV, splitJV' ) import Dyno.Vectorize ( Vectorize, Id )-import Dyno.Nlp ( Nlp'(..), Bounds )-import Dyno.NlpSolver ( Opt(..), solveNlpHomotopy' )+import Dyno.Nlp ( Nlp(..), Bounds )+import Dyno.NlpUtils ( HomotopyParams(..), solveNlpHomotopy ) import Dyno.Solvers +hp :: HomotopyParams+hp = HomotopyParams+ { reduction = 0.6+ , increase = 2+ , iterIncrease = 10+ , iterDecrease = 20+ } data P a = P a a deriving (Functor, Generic, Generic1, Show) data X a = X a a deriving (Functor, Generic, Generic1, Show)@@ -29,18 +36,18 @@ instance Vectorize G instance Vectorize P -myNlp :: Nlp' (JV X) (JV P) (JV G) MX-myNlp = Nlp' { nlpFG' = fg- , nlpBX' = bx- , nlpBG' = bg- , nlpX0' = x0- , nlpP' = catJV $ P (-2) 0- , nlpLamX0' = Nothing- , nlpLamG0' = Nothing- , nlpScaleF' = Nothing- , nlpScaleX' = Nothing- , nlpScaleG' = Nothing- }+myNlp :: Nlp (JV X) (JV P) (JV G) MX+myNlp = Nlp { nlpFG = fg+ , nlpBX = bx+ , nlpBG = bg+ , nlpX0 = x0+ , nlpP = catJV $ P (-2) 0+ , nlpLamX0 = Nothing+ , nlpLamG0 = Nothing+ , nlpScaleF = Nothing+ , nlpScaleX = Nothing+ , nlpScaleG = Nothing+ } where x0 :: J (JV X) (V.Vector Double) x0 = catJV $ X (-8) (-8)@@ -61,16 +68,16 @@ g = G (x - px) -solver :: NlpSolverStuff---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")+solver :: Solver+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 (JV X) (Vector Double) -> J (JV P) (Vector Double) -> Double -> IO ()@@ -79,5 +86,6 @@ P px py = splitJV pxy printf "X: (%.3f,%.3f), P: (%.3f, %.3f), a: %.4f\n" x y px py alpha return ()- opt <- solveNlpHomotopy' 1e-3 (0.6, 2, 10, 20) solver myNlp (catJV (P 2 0)) Nothing (Just cbp)+ pfs = [catJV (P 2 0), catJV (P 3 0)]+ opt <- solveNlpHomotopy 1e-3 hp solver myNlp pfs Nothing (Just cbp) print opt
examples/MultipleShooting.hs view
@@ -29,7 +29,7 @@ import Dyno.View.JV import Dyno.View.JVec import Dyno.Nlp-import Dyno.NlpSolver+import Dyno.NlpUtils import Dyno.Solvers import Dyno.Vectorize import Dyno.MultipleShooting@@ -77,12 +77,12 @@ -- run the thing main :: IO () main = do- myNlp <- makeMsNlp ocp :: IO (Nlp' (MsDvs X U P 40) JNone (MsConstraints X 40) MX)- (msg,opt') <- solveNlp' ipoptSolver myNlp Nothing+ myNlp <- makeMsNlp ocp :: IO (Nlp (MsDvs X U P 40) JNone (MsConstraints X 40) MX)+ (msg,opt') <- solveNlp ipoptSolver myNlp Nothing opt <- case msg of Left err -> error err Right _ -> return opt'- let xopt = split $ xOpt' opt+ let xopt = split $ xOpt opt splitXU xu = (splitJV x, splitJV u) where JTuple x u = split xu
+ examples/NlpSolverEx.hs view
@@ -0,0 +1,146 @@+-- | Example of NlpSolver monad and autoscaling++{-# OPTIONS_GHC -Wall #-}+{-# Language DeriveFunctor #-}+{-# Language DeriveGeneric #-}++module Main where++import GHC.Generics ( Generic, Generic1 )++import Text.Printf ( printf )++import Casadi.MX ( MX )++import Dyno.Vectorize ( Vectorize, Id(..), None(..), fill )+import Dyno.View.View+import Dyno.View.Viewable+import Dyno.View.JV -- ( JV )+import Dyno.Nlp+import Dyno.NlpSolver+import Dyno.NlpUtils+import Dyno.Solvers+import Dyno.AutoScaling++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 (JV X) (JV None) (JV G) MX+myNlp = Nlp { nlpFG = fg+ , nlpBX = catJV bx+ , nlpBG = catJV bg+ , nlpX0 = catJV x0+ , nlpP = catJV None+ , nlpLamX0 = Nothing+ , nlpLamG0 = Nothing+ , nlpScaleF = Just 9.86+ , nlpScaleX = Just $ catJV $ (X (4.7e-3) (4.7e4))+ , nlpScaleG = Just $ catJV $ (G 4.7)+-- , nlpScaleF = Just 1+-- , nlpScaleX = Just $ catJV (X 1 1)+-- , nlpScaleG = Just $ catJV (G 1) -- 1)+ }+ where+ x0 :: X Double+ x0 = X 0 0++ bx :: X Bounds+ bx = fill (Nothing, Nothing)++ bg :: G Bounds+ bg = G (Just 2, Nothing)++ fg :: J (JV X) MX -> J (JV None) MX -> (J (JV Id) MX, J (JV G) MX)+ fg xy _ = (f, catJV' g)+ where+ X x y = splitJV' xy+ x' = 1e3*x+ y' = 1e-4*y+ f = x'**2 + y'**2 + 0.1*x' * y'+ g = G (x' + y')++solver :: Solver+solver = ipoptSolver { options = [ ("print_time", Opt False)+ , ("linear_solver", Opt "ma86")+ --, ("print_level", Opt (0 :: Int))+ ] }++quietSolver :: Solver+quietSolver = ipoptSolver { options = [ ("print_time", Opt False)+ , ("print_level", Opt (0 :: Int))+ , ("linear_solver", Opt "ma86")+ ] }++computeKKTs :: NlpSolver (JV X) (JV None) (JV G)+ (KKT (JV X) (JV G), KKT (JV X) (JV G))+computeKKTs = do+ kktU <- evalKKT+ kktS <- evalScaledKKT+ return (kktU, kktS)+++runMe :: NlpSolver (JV X) (JV None) (JV G) ((Double, X Double, G Double), (KKT (JV X) (JV G), KKT (JV X) (JV G)))+runMe = do+ (msg,opt') <- solve'+ let opt = case msg of+ Left m -> error m+ Right _ -> opt'+ f = fOpt opt+ x = xOpt opt+ g = gOpt opt+ getX >>= setX0+ getLamX >>= setLamX0+ getLamG >>= setLamG0+ kkts <- computeKKTs+ return ((unId (splitJV f), splitJV x, splitJV g), kkts)++data Sdv a = Sdv (J (JV Id) a) (J (JV X) a) (J (JV G) a) deriving (Generic)+instance View Sdv++expand :: Viewable a => J Sdv a -> (J (JV Id) a, J (JV X) a, J (JV G) a)+expand sdv = (f, x, g)+ where+ Sdv f x g = split sdv++main :: IO ()+main = do+ (opt, (kktU, kktS)) <- runNlp solver myNlp Nothing runMe+ putStrLn "***********************************************************"+ putStrLn "unscaled kkt:"+ putStrLn $ kktScalingInfo kktU+ putStrLn "\nscaled kkt:"+ putStrLn $ kktScalingInfo kktS+ putStrLn "***********************************************************"+ putStrLn $ "unscaled gradF: " ++ show (kktGradF kktU)+ putStrLn $ "scaled gradF: " ++ show (kktGradF kktS)+ putStrLn ""+ putStrLn $ "unscaled jacG: " ++ show (kktJacG kktU)+ putStrLn $ "scaled jacG: " ++ show (kktJacG kktS)+ putStrLn ""+ putStrLn $ "unscaled hessLag: " ++ show (kktHessLag kktU)+ putStrLn $ "scaled hessLag: " ++ show (kktHessLag kktS)++ let snlp = scalingNlp kktU expand+ (msg,opt') <- solveNlp quietSolver snlp Nothing+ let xopt = case msg of+ Left m -> error m+ Right _ -> xOpt opt'+ Sdv obj' x' g' = split (fmapJ exp xopt)+ Id obj = splitJV obj'+ x = splitJV x'+ g = splitJV g'+ putStrLn "***********************************************************"+ putStrLn "solution:"+ print opt+ putStrLn "***********************************************************"+ putStrLn "scaling:"+ putStrLn $ "f: " ++ (printf "%.2e" obj)+ putStrLn $ "x: " ++ show (fmap (printf "%.2e" :: Double -> String) x)+ putStrLn $ "g: " ++ show (fmap (printf "%.2e" :: Double -> String) g)+ putStrLn "***********************************************************"+ putStrLn "before and after"+ putStrLn $ beforeAndAfter kktU expand xopt+ return ()
examples/OcpDslRocket.hs view
@@ -6,6 +6,7 @@ --import Control.Concurrent ( threadDelay ) import Dyno.Solvers+import Dyno.DirectCollocation.Quadratures ( QuadratureRoots(..) ) import Dynoplot.Callback @@ -85,7 +86,7 @@ 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')+ go cb = solveStaticOcp Radau 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/OcpDslSpring.hs view
@@ -6,6 +6,7 @@ import Dyno.Solvers +import Dyno.DirectCollocation.Quadratures ( QuadratureRoots(..) ) import Dynoplot.Callback import ExampleDsl.OcpMonad @@ -66,6 +67,6 @@ n = 100 deg = 3 tbnds = (Just 4, Just 4)- go cb = solveStaticOcp ipoptSolver myDae mayer boundaryConditions myOcp tbnds n deg (Just cb')+ go cb = solveStaticOcp Legendre ipoptSolver myDae mayer boundaryConditions myOcp tbnds n deg (Just cb') where cb' meta x = cb (x, meta)
examples/Sailboat.hs view
@@ -21,17 +21,18 @@ 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.Lazy as BSL import qualified Data.ByteString.Char8 as BS8-import qualified Data.Serialize as Ser+import qualified Data.Binary as B import Text.Printf ( printf ) +import Accessors ( Lookup )+ import Dyno.Vectorize import Dyno.View.View ( View(..), J ) import Dyno.View.JV ( splitJV ) import Dyno.Solvers-import Dyno.NlpSolver-import Dyno.Server.Accessors+import Dyno.NlpUtils import Dyno.Nlp import Dyno.Ocp import Dyno.DirectCollocation@@ -75,10 +76,10 @@ instance Lookup (SbO ()) ------------------------------ zmq helpers --------------------------------------newtype Packed = Packed { unPacked :: BS.ByteString }+newtype Packed = Packed { unPacked :: BSL.ByteString } -encodeSerial :: Ser.Serialize a => a -> Packed-encodeSerial = Packed . Ser.encode+encodeSerial :: B.Binary a => a -> Packed+encodeSerial = Packed . B.encode -------------------------------------------------------------------------- norm2sqr :: Num a => V2 a -> a@@ -156,10 +157,13 @@ , bcP0 :: V2 a } deriving (Functor, Generic, Generic1, Show)-bc :: Num a => SbX a -> SbX a -> SbBc a+bc :: Num a => SbX a -> SbX a -> None a -> SbP a -> a -> SbBc a bc (SbX gamma0 p0@(V2 _ pz0) (V2 vx0 vz0)) (SbX gammaF (V2 _ pzF) (V2 vxF vzF))+ _+ _+ _ = SbBc { bcPeriodicGamma = gamma0 + gammaF , bcPeriodicPz = pz0 - pzF@@ -168,8 +172,8 @@ , bcP0 = p0 } -mayer :: Floating a => a -> SbX a -> SbX a -> a-mayer tf _ (SbX _ (V2 pxF _) _) = - pxF / tf+mayer :: Floating a => a -> SbX a -> SbX a -> None a -> SbP 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@@ -192,9 +196,10 @@ pathc :: t -> t1 -> t2 -> t3 -> t4 -> t5 -> None a pathc _ _ _ _ _ _ = None -ocp :: OcpPhase SbX SbZ SbU SbP SbR SbO SbBc None+ocp :: OcpPhase SbX SbZ SbU SbP SbR SbO SbBc None None ocp = OcpPhase { ocpMayer = mayer , ocpLagrange = lagrange+ , ocpQuadratures = \_ _ _ _ _ _ _ -> None , ocpDae = sbDae , ocpBc = bc , ocpPathC = pathc@@ -232,7 +237,7 @@ let send :: String -> Packed -> IO () send channel msg = ZMQ.sendMulti publisher (NE.fromList [ BS8.pack channel- , unPacked msg+ , BSL.toStrict (unPacked msg) ]) f send @@ -252,13 +257,13 @@ type NCollStages = 200 type CollDeg = 2 -solver :: NlpSolverStuff+solver :: Solver solver = ipoptSolver --solver = snoptSolver { options = [("detect_linear", Opt False)] } main :: IO () main = do- cp <- makeCollProblem ocp+ cp <- makeCollProblem Legendre ocp let nlp = cpNlp cp ZMQ.withContext $ \context -> withPublisher context urlDynoPlot $ \sendDynoPlotMsg -> do@@ -266,7 +271,7 @@ let guess = cat initialGuess proxy :: Proxy (CollTraj SbX SbZ SbU SbP NCollStages CollDeg) proxy = Proxy- meta = toMeta (cpRoots cp) (Proxy :: Proxy SbO) proxy+ meta = toMeta (Proxy :: Proxy None) (Proxy :: Proxy SbO) proxy callback :: J (CollTraj SbX SbZ SbU SbP NCollStages CollDeg) (Vector Double) -> IO Bool@@ -312,10 +317,10 @@ -- sendOptTelemMsg "opt_telem" (encodeProto optTelemMsg) return True - (msg0,opt0') <- solveNlp' solver (nlp { nlpX0' = guess }) (Just callback)+ (msg0,opt0') <- solveNlp solver (nlp { nlpX0 = guess }) (Just callback) opt0 <- case msg0 of Left msg' -> error msg' Right _ -> return opt0'- let CollTraj endTime' _ _ xf = split (xOpt' opt0)+ let CollTraj endTime' _ _ xf = split (xOpt opt0) endTime = unId $ splitJV endTime' V2 pxF _ = xP $ splitJV xf printf "optimal velocity: %.2f m/s\n" (pxF / endTime)
examples/Sofa/Common.hs view
@@ -17,7 +17,7 @@ import GHC.Generics ( Generic, Generic1 ) import qualified Data.Foldable as F-import Data.Serialize+import Data.Binary import Dyno.TypeVecs ( Vec, Dim ) import qualified Dyno.TypeVecs as TV@@ -52,8 +52,8 @@ , smMeanThetas :: [(Point Double, Double)] } deriving Generic -instance Serialize SofaMessage-instance Serialize a => Serialize (Point a)+instance Binary SofaMessage+instance Binary a => Binary (Point a) url :: String url = "tcp://127.0.0.1:5563"
examples/SofaExpando.hs view
@@ -13,13 +13,14 @@ import Data.Proxy ( Proxy(..) ) import Data.IORef ( newIORef, readIORef, writeIORef ) import qualified Data.Foldable as F-import Data.Serialize+import Data.Binary import qualified System.ZMQ4 as ZMQ import Data.ByteString.Char8 ( pack )+import Data.ByteString.Lazy ( toStrict ) import Dyno.Vectorize import Dyno.Nlp-import Dyno.NlpSolver+import Dyno.NlpUtils import Dyno.TypeVecs ( Vec ) import qualified Dyno.TypeVecs as TV import Dyno.Solvers@@ -103,15 +104,15 @@ ----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@@ -139,75 +140,61 @@ 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- }++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- - 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))- }+ 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)- }+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)+fg :: forall a . Floating a => X a -> (a, G a)+fg (X r points stages) = (f, g) where ds :: Vec NPoints (Point a) ds = zipWithNext (\x0 x1 -> x1 - x0) points@@ -237,21 +224,21 @@ 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 :: Solver 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 :: Binary 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+ ZMQ.send publisher [] (toStrict bs) main :: IO () main =@@ -261,7 +248,7 @@ putStrLn $ "# design vars: " ++ show (vlength (Proxy :: Proxy X)) putStrLn $ "# constraints: " ++ show (vlength (Proxy :: Proxy G)) iters <- newIORef 0- _ <- solveNlp solver myNlp $ Just $ \x -> do+ _ <- solveNlpV solver fg bx bg guess $ Just $ \x -> do k <- readIORef iters writeIORef iters (k + 1) let msg = SofaMessage@@ -274,4 +261,3 @@ send publisher sofaChannel msg return True return ()-
examples/SofaVisualizer.hs view
@@ -8,7 +8,8 @@ import Linear.Quaternion ( Quaternion(..) ) import Control.Monad ( when, forever ) import Data.ByteString.Char8 ( pack )-import Data.Serialize+import Data.ByteString.Lazy ( fromStrict )+import Data.Binary import qualified System.ZMQ4 as ZMQ import qualified Control.Concurrent.STM as STM import qualified Control.Concurrent as CC@@ -42,9 +43,9 @@ when mre $ do msg <- ZMQ.receive subscriber let decoded :: SofaMessage- decoded = case decode msg of- Left err -> error err- Right t -> t+ decoded = case decodeOrFail (fromStrict msg) of+ Left (_,_,err) -> error $ "decode failure: " ++ err+ Right (_,_,t) -> t writeChan decoded main :: IO ()
+ src/Dyno/AutoScaling.hs view
@@ -0,0 +1,329 @@+{-# OPTIONS_GHC -Wall #-}+{-# Language ScopedTypeVariables #-}+{-# Language DeriveFunctor #-}+{-# Language DeriveGeneric #-}++module Dyno.AutoScaling+ ( scalingNlp+ , kktScalingInfo+ , beforeAndAfter+ ) where++import Data.List ( minimumBy, maximumBy )+import Data.Proxy ( Proxy(..) )+import qualified Data.Vector as V+--import qualified Numeric.LinearAlgebra.Data as HMat+--import qualified Numeric.LinearAlgebra.HMatrix as HMat+import Text.Printf ( printf )++import Casadi.Sparsity ( getRow, getCol )+import Casadi.SX ( SX )+import Casadi.DMatrix ( DMatrix, ddata )+import qualified Casadi.CMatrix as CM++import Dyno.View.Unsafe.View ( mkJ, unJ )+import Dyno.View.Unsafe.M ( unM )++import Dyno.Vectorize ( Id(..) )+import Dyno.Nlp ( KKT(..), Nlp(..) )+import Dyno.View.View ( View(..), J, JNone(..), v2d, d2v, jfill)+import Dyno.View.Viewable ( Viewable )+import qualified Dyno.View.M as M+import Dyno.View.M ( M )+import Dyno.View.JV ( JV, splitJV )+++toSparse :: (View f, View g) => String -> M f g DMatrix -> [(Int,Int,Double)]+toSparse name mat0+ | V.length row /= V.length col = error $ name ++ " row/column index mismatch"+ | V.length row /= V.length dat = error $ name ++ " sparsity patter size doesn't match data size"+ | otherwise = V.toList $ V.zip3 row col dat+ where+ mat = unM $ M.sparse mat0++ sp = CM.sparsity mat+ dat = ddata mat+ row = getRow sp+ col = getCol sp++kktScalingInfo :: (View f, View g) => KKT f g -> String+kktScalingInfo kkt =+ init $ unlines+ [ showOne "hessLag " (kktHessLag kkt)+ , showOne "hessF " (kktHessF kkt)+ , showOne "hessLamG " (kktHessLambdaG kkt)+ , showOne "jacG " (kktJacG kkt)+ , showOne "gradF " (M.col (kktGradF kkt))+ ]+ where+ showOne name m =+ printf "%s size (%5d, %5d), nonzeros %7d/%10d (%6.2f %%), min: %s, max: %s, ratio: %s"+ name r c nz (r*c)+ (100 * fromIntegral nz / fromIntegral (r*c) :: Double)+ min' max' ratio+ where+ byAbs x y = compare (abs x) (abs y)+ min' = case d of+ [] -> " N/A"+ ds -> printf "% 8.2e" (minimumBy byAbs ds)+ max' = case d of+ [] -> " N/A"+ ds -> printf "% 8.2e" (maximumBy byAbs ds)+ ratio = case d of+ [] -> " N/A"+ ds -> printf "% 8.2e" (minimumBy byAbs ds / maximumBy byAbs ds)++ nz = length d+ (_,_,d) = unzip3 (toSparse name m)+ r = CM.size1 (unM m)+ c = CM.size2 (unM m)++-- log |aij| + sj + si (+ sf)+data LogScaling a =+ LogScaling+ { lsHessF :: [a]+ , lsHessLambdaG :: [a]+ , lsHessLag :: [a]+ , lsJacG :: [a]+ , lsGradF :: [a]+ } deriving Functor+++toObjective :: Floating a => LogScaling a -> a+toObjective (LogScaling hf hlg hl jg gf) = sum (map sqr hf) + sum (map sqr hlg) + 0*sum (map sqr hl) + 2*sum (map sqr jg) + sum (map sqr gf)+ where+ sqr x = x*x++toMatrixCoeffs :: Floating a => LogScaling a -> LogScaling a+toMatrixCoeffs (LogScaling hf hlg hl jg gf) = LogScaling (f hf) (f hlg) (f hl) (f jg) (f gf)+ where+ f = map exp++toLogScaling ::+ forall x g sdv a+ . (View x, View g, View sdv, Viewable a, CM.CMatrix a)+ => KKT x g -> (J sdv a -> (J (JV Id) a, J x a, J g a)) -> J sdv a -> LogScaling (J (JV Id) a)+toLogScaling kkt expand sdvs =+ LogScaling+ { lsJacG = jacGObjValues+ , lsHessF = hessFObjValues+ , lsHessLambdaG = hessLambdaGObjValues+ , lsHessLag = hessLagObjValues+ , lsGradF = gradFObjValues+ }+ where+ jacGMatValues = toSparse "jacG" (kktJacG kkt)+ hessFMatValues = toSparse "hessF" (kktHessF kkt)+ hessLambdaGMatValues = toSparse "hessLamG" (kktHessLambdaG kkt)+ hessLagMatValues = toSparse "hessLag" (kktHessLag kkt)+ gradFMatValues = toSparse "gradF" (M.col (kktGradF kkt))++ objScale' :: J (JV Id) a+ x :: J x a+ g' :: J g a+ (objScale', x, g') = expand sdvs+ -- constraints and objective are inverted+ objScale = negate objScale'+ g = negate g'++ reproxy :: J f a -> Proxy f+ reproxy = const Proxy++ nx = size (reproxy x)+ ng = size (reproxy g)+ xs,gs :: V.Vector (J (JV Id) a)+ xs = fmap mkJ $ CM.vertsplit (unJ x) (V.fromList [0..nx])+ gs = fmap mkJ $ CM.vertsplit (unJ g) (V.fromList [0..ng])++ gradFObjValues :: [J (JV Id) a]+ gradFObjValues = map (toSum xs (V.singleton objScale)) gradFMatValues++ jacGObjValues :: [J (JV Id) a]+ jacGObjValues = map (toSum gs xs) jacGMatValues++ hessFObjValues :: [J (JV Id) a]+ hessFObjValues = map ((+ objScale) . toSum xs xs) hessFMatValues++ hessLambdaGObjValues :: [J (JV Id) a]+ hessLambdaGObjValues = map ((+ objScale) . toSum xs xs) hessLambdaGMatValues++ hessLagObjValues :: [J (JV Id) a]+ hessLagObjValues = map ((+ objScale) . toSum xs xs) hessLagMatValues+++toSum :: forall a .+ (Fractional a) =>+ V.Vector a -> V.Vector a -> (Int, Int, Double) -> a+toSum rowVec colVec (rowi,colj,value)+ | absValue == 0 = error "toSum: log(0)"+ | logAbsValue' < -1000 = error "really really small value"+ | logAbsValue' > 1000 = error "really really big value"+ | otherwise = logAbsValue + si + sj+ where+ absValue = abs value+ logAbsValue = realToFrac (log absValue)+ logAbsValue' = log absValue++ si,sj :: a+ si = rowVec V.! rowi+ sj = colVec V.! colj++++scalingNlp ::+ forall x g sdv+ . (View x, View g, View sdv)+ => KKT x g -> (J sdv SX -> (J (JV Id) SX, J x SX, J g SX)) -> Nlp sdv JNone JNone SX+scalingNlp kkt expand =+ Nlp+ { nlpBX = jfill (Nothing, Nothing)+ , nlpBG = cat JNone+ , nlpX0 = jfill 0 -- unit scaling, initially+ , nlpP = cat JNone+ , nlpLamX0 = Nothing+ , nlpLamG0 = Nothing+ , nlpScaleF = Nothing+ , nlpScaleX = Nothing+ , nlpScaleG = Nothing+ , nlpFG = fg+ }+ where+ fg :: J sdv SX -> J JNone SX -> (J (JV Id) SX, J JNone SX)+ fg sdvs _ = (obj, cat JNone)+ where+ obj = toObjective $ toLogScaling kkt expand sdvs+++beforeAndAfter+ :: (View x, View g, View sdv)+ => KKT x g+ -> (J sdv DMatrix -> (J (JV Id) DMatrix, J x DMatrix, J g DMatrix))+ -> J sdv (V.Vector Double)+ -> String+beforeAndAfter kkts expand scalingSol =+ init $ unlines+ [ minMax "hessF0" hessF0+ , minMax "hessF " hessF+ , ""+ , minMax "hessLamG0" hessLamG0+ , minMax "hessLamG " hessLamG+ , ""+ , minMax "hessLag0" hessLag0+ , minMax "hessLag " hessLag+ , ""+ , minMax "jacG0" jacG0+ , minMax "jacG " jacG+ , ""+ , minMax "gradF0" gradF0+ , minMax "gradF " gradF+ ]+ where+ ls0 = fmap (unId . splitJV . d2v) $ toLogScaling kkts expand (v2d (jfill 0))+ LogScaling hessF0 hessLamG0 hessLag0 jacG0 gradF0 = toMatrixCoeffs ls0 :: LogScaling Double++ ls :: LogScaling Double+ ls = fmap (unId . splitJV . d2v) $ toLogScaling kkts expand (v2d scalingSol)+ LogScaling hessF hessLamG hessLag jacG gradF = toMatrixCoeffs ls :: LogScaling Double+ minMax name xs = printf "%s min: %s, max: %s, ratio: %s" name min' max' ratio+ where+ -- protect against empty list+ min' = case xs of+ [] -> "N/A"+ xs' -> printf "% 8.2e" (minimum xs')+ max' = case xs of+ [] -> "N/A"+ xs' -> printf "% 8.2e" (maximum xs')+ ratio = case xs of+ [] -> "N/A"+ xs' -> printf "% 8.2e" (minimum xs' / maximum xs')++++--analyzeSol :: Nlp'+-- (CollTraj AcX None AcU AcP NCollStages CollDeg)+-- JNone+-- (CollOcpConstraints NCollStages CollDeg AcX AcX Bc PathC)+-- MX ->+-- Save+-- (CollTraj AcX None AcU AcP NCollStages CollDeg)+-- (CollOcpConstraints NCollStages CollDeg AcX AcX Bc PathC) ->+-- IO ()+--analyzeSol nlp save = do+-- let sol = savedNlpOut save+-- putStrLn "creating jacobian..."+-- --nj <- nlpJac nlp+-- nj' <- nlpJac' nlp+-- putStrLn "evaluating jacobian..."+-- --(jacFG', fg) <- nj (v2d (xOpt' sol)) (cat JNone)+-- (dgdx, _) <- nj' (v2d (xOpt' sol))+-- putStrLn "finished! analyzing..."+-- let --JTuple f0' g0' = split fg+-- --Id _f0 = splitJV (d2v f0')+-- --_g0 = unJ $ d2v g0'+-- --+-- --dfgdx :: M+-- -- (JTuple (JV Id) (CollOcpConstraints NCollStages CollDeg AcX AcX Bc PathC))+-- -- (CollTraj AcX None AcU AcP NCollStages CollDeg)+-- -- DMatrix+-- --(dfgdx,_) = M.hsplitTup jacFG'+-- --_dfdx :: M (JV Id) (CollTraj AcX None AcU AcP NCollStages CollDeg) DMatrix+-- --dgdx :: M+-- -- (CollOcpConstraints NCollStages CollDeg AcX AcX Bc PathC)+-- -- (CollTraj AcX None AcU AcP NCollStages CollDeg)+-- -- DMatrix+-- --(_dfdx, dgdx) = M.vsplitTup dfgdx+--+--+-- -- todo: this only works for worhp heh+-- isActive :: Double -> Bool+-- isActive lambda = (abs lambda) > 1e-15+---- isActive :: (Double,Double) -> Double -> Double -> Bool+---- isActive (lb, ub) val lambda+---- | val <= lb = True+---- | ub <= val = True+---- | (abs lambda) > 1e-15 = True+---- | otherwise = False+--+-- activeX = V.map isActive (unJ (lambdaXOpt' sol))+-- activeG = V.map isActive (unJ (lambdaGOpt' sol))+-- activeAll = activeX V.++ activeG+--+-- activeXIndices = map fst $ filter snd $ zip [(0::Int)..] (V.toList activeX)+--+-- nx = size (Proxy :: Proxy (CollTraj AcX None AcU AcP NCollStages CollDeg))+-- ng = size (Proxy :: Proxy (CollOcpConstraints NCollStages CollDeg AcX AcX Bc PathC))+-- fullJac = (HMat.ident nx) HMat.=== dgdx'+-- dgdx' = M.toHMat dgdx+--+-- delRows [] [] = []+-- delRows (False:act) (_:gs) = delRows act gs+-- delRows (True :act) (g:gs) = g : delRows act gs+-- delRows _ _ = error "delRows got length mismatch"+--+-- activeFullJac :: HMat.Matrix Double+-- activeFullJac = HMat.fromRows $ delRows (V.toList activeAll) (HMat.toRows fullJac)+--+-- activeGJac :: HMat.Matrix Double+-- activeGJac = HMat.fromRows $ delRows (V.toList activeG) (HMat.toRows dgdx')+--+-- printf "num x: %5d, active x: %5d\n" nx (V.length (V.filter id activeX))+-- printf "num g: %5d, active g: %5d\n" ng (V.length (V.filter id activeG))+---- putStrLn $ take 100 $ show dgdx+---- putStrLn "===================="+---- putStrLn $ take 100 $ show dgdx'+-- printf "dgdx': (%d, %d)\n" (HMat.rows dgdx') (HMat.cols dgdx')+-- printf "active full jac size: (%d,%d)\n" (HMat.rows activeFullJac) (HMat.cols activeFullJac)+-- printf "active g jac size: (%d,%d)\n" (HMat.rows activeGJac) (HMat.cols activeGJac)+-- putStrLn $ "active design vars: " ++ take 100 (show activeXIndices)+-- writeFile "/home/ghorn/takeIt.txt" (saveMat (HMat.toLists activeFullJac))+-- printf " g jac rank: %d\n" (HMat.rank activeGJac)+-- printf " g jac rcond: %.3e\n" (HMat.rcond activeGJac)+-- printf "full jac rank: %d\n" (HMat.rank activeFullJac)+-- printf "full jac rcond: %.3e\n" (HMat.rcond activeFullJac)+--+-- let CollTraj tf' p' _stages _xf = split $ lambdaXOpt' sol+-- p = splitJV p'+-- Id tf = splitJV tf'+-- print tf+-- print p+-- return ()
src/Dyno/DirectCollocation.hs view
@@ -14,32 +14,34 @@ import Dyno.View.View ( J, jfill ) import Dyno.Vectorize ( Vectorize ) import Dyno.Ocp ( OcpPhase )-import Dyno.NlpSolver ( NlpSolverStuff, solveNlp' )-import Dyno.Nlp ( Nlp'(..) )+import Dyno.NlpUtils ( solveNlp )+import Dyno.Solvers ( Solver )+import Dyno.Nlp ( Nlp(..) ) import Dyno.DirectCollocation.Formulate ( CollProblem(..), makeCollProblem ) import Dyno.DirectCollocation.Types ( CollTraj(..) ) import Dyno.DirectCollocation.Dynamic ( DynPlotPoints )+import Dyno.DirectCollocation.Quadratures ( QuadratureRoots ) import qualified Dyno.TypeVecs as TV solveOcp ::- forall x z u p r o c h .+ forall x z u p r o c h q . (Vectorize x, Vectorize z, Vectorize u, Vectorize p,- Vectorize r, Vectorize o, Vectorize c, Vectorize h)- => NlpSolverStuff -> Int -> Int -> Maybe (DynPlotPoints Double -> IO Bool)- -> OcpPhase x z u p r o c h+ Vectorize r, Vectorize o, Vectorize c, Vectorize h, Vectorize q)+ => QuadratureRoots -> Solver -> Int -> Int -> Maybe (DynPlotPoints Double -> IO Bool)+ -> OcpPhase x z u p r o c h q -> IO (Either String String)-solveOcp solverStuff n deg cb0 ocp =+solveOcp roots 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+ cp <- makeCollProblem roots ocp let nlp = cpNlp cp toPlotPoints = cpPlotPoints cp- --_ <- solveNlp' solverStuff (nlp {nlpX0' = guess}) (fmap (. ctToDynamic) cb)+ --_ <- solveNlp solverStuff (nlp {nlpX0 = guess}) (fmap (. ctToDynamic) cb) let cb = case cb0 of Nothing -> Nothing Just cb' -> Just $ \x -> toPlotPoints x >>= cb' - (res, _) <- solveNlp' solverStuff (nlp {nlpX0' = guess}) cb+ (res, _) <- solveNlp solverStuff (nlp {nlpX0 = guess}) cb return res
src/Dyno/DirectCollocation/Dynamic.hs view
@@ -1,4 +1,4 @@-{-# OPTIONS_GHC -Wall -fno-warn-orphans #-}+{-# OPTIONS_GHC -Wall #-} {-# Language ScopedTypeVariables #-} {-# Language DeriveGeneric #-} {-# Language PolyKinds #-}@@ -6,63 +6,86 @@ module Dyno.DirectCollocation.Dynamic ( DynPlotPoints , CollTrajMeta(..)- , MetaTree- , forestFromMeta+ , addCollocationChannel , toMeta , toMetaCov , dynPlotPoints , catDynPlotPoints--- , toPlotTree , NameTree(..) ) where import GHC.Generics ( Generic ) import Data.Proxy ( Proxy(..) )-import Data.List ( mapAccumL, unzip5 )+import Data.List ( mapAccumL ) import Data.Tree ( Tree(..) ) 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.Tree as Tree-import Data.Serialize ( Serialize(..) )+import Data.Binary ( Binary ) import Linear.V +import Accessors ( AccessorTree(..), Lookup(..), accessors )+import PlotHo ( Plotter, addChannel )+ import Dyno.View.Unsafe.View ( unJ, unJ' ) -import Dyno.Server.Accessors ( AccessorTree(..), Lookup(..), accessors )-import Dyno.Vectorize ( Vectorize, Id(..) )-import Dyno.View.JV-import Dyno.View.View+import Dyno.Vectorize ( Vectorize, Id(..), fill )+import Dyno.View.JV ( JV, splitJV )+import Dyno.View.View ( View(..), J ) import Dyno.View.JVec ( JVec(..) ) import qualified Dyno.TypeVecs as TV import Dyno.TypeVecs ( Vec )- import Dyno.DirectCollocation.Types import Dyno.DirectCollocation.Quadratures ( QuadratureRoots, mkTaus ) +addCollocationChannel ::+ String -> (((DynPlotPoints Double, CollTrajMeta) -> IO ()) -> IO ()) -> Plotter ()+addCollocationChannel name action = addChannel name sameMeta toSignalTree action+ where+ toSignalTree ::+ (DynPlotPoints Double, CollTrajMeta)+ -> [Tree ( String+ , String+ , Maybe ((DynPlotPoints Double, CollTrajMeta) -> [[(Double, Double)]])+ )]+ toSignalTree = forestFromMeta . snd++sameMeta :: (DynPlotPoints Double, CollTrajMeta)+ -> (DynPlotPoints Double, CollTrajMeta)+ -> Bool+sameMeta (_,ctm0) (_,ctm1) =+ and [ ctmX ctm0 == ctmX ctm1+ , ctmZ ctm0 == ctmZ ctm1+ , ctmU ctm0 == ctmU ctm1+ , ctmP ctm0 == ctmP ctm1+ , ctmO ctm0 == ctmO ctm1+ , ctmQ ctm0 == ctmQ ctm1+ ]+ data DynPlotPoints a = DynPlotPoints- [[(a, Vector a)]]- [[(a, Vector a)]]- [[(a, Vector a)]]- [[(a, Vector a)]]- [[(a, Vector a)]]- deriving (Show, Generic)-instance Serialize a => Serialize (DynPlotPoints a)-instance Serialize a => Serialize (Vector a) where- get = fmap V.fromList get- put = put . V.toList+ (Vector (Vector (a, Vector a)))+ (Vector (Vector (a, Vector a)))+ (Vector (Vector (a, Vector a)))+ (Vector (Vector (a, Vector a)))+ (Vector (Vector (a, Vector a)))+ deriving Generic -catDynPlotPoints :: [DynPlotPoints a] -> DynPlotPoints a+instance Binary a => Binary (DynPlotPoints a)++catDynPlotPoints :: V.Vector (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)+ (V.concatMap (\(DynPlotPoints x _ _ _ _) -> x) pps)+ (V.concatMap (\(DynPlotPoints _ x _ _ _) -> x) pps)+ (V.concatMap (\(DynPlotPoints _ _ x _ _) -> x) pps)+ (V.concatMap (\(DynPlotPoints _ _ _ x _) -> x) pps)+ (V.concatMap (\(DynPlotPoints _ _ _ _ x) -> x) pps) + dynPlotPoints :: forall x z u p o n deg a . (Dim n, Dim deg, Real a, Fractional a, Show a,@@ -72,7 +95,7 @@ -> Vec n (Vec deg (J (JV o) (Vector a), J (JV x) (Vector a)), J (JV x) (Vector a)) -> DynPlotPoints a dynPlotPoints quadratureRoots (CollTraj tf' _ stages' xf) outputs =- DynPlotPoints (xss++[[(tf,unJ xf)]]) zss uss oss xdss+ DynPlotPoints xss' zss uss oss xdss where nStages = size (Proxy :: Proxy (JVec n (JV Id))) tf,h :: a@@ -84,94 +107,75 @@ 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) + xss' = xss `V.snoc` (V.singleton (tf, unJ xf)) - -- todo: check this final time against expected tf+ xss,zss,uss,oss,xdss :: Vector (Vector (a, Vector a))+ (xss,zss,uss,oss,xdss) = V.unzip5 xzuoxds++ -- todo: check this final time tf'' against expected tf+ (_tf'', xzuoxds) = T.mapAccumL f 0 $ V.zip (TV.unVec stages) (TV.unVec outputs)++ f :: a- -> [( CollStage (JV x) (JV z) (JV u) deg (Vector a)- , (Vec deg (J (JV o) (Vector a), J (JV x) (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, xnext)) : css) = (xs,zs,us,os,xds) : f tnext css+ -> ( CollStage (JV x) (JV z) (JV u) deg (Vector a)+ , (Vec deg (J (JV o) (Vector a), J (JV x) (Vector a)), J (JV x) (Vector a))+ )+ -> ( a+ , ( V.Vector (a, V.Vector a)+ , V.Vector (a, V.Vector a)+ , V.Vector (a, V.Vector a)+ , V.Vector (a, V.Vector a)+ , V.Vector (a, V.Vector a)+ )+ )+ f t0 (CollStage x0 xzus', (xdos, xnext)) = (tnext, (xs,zs,us,os,xds)) where tnext = t0 + h xzus0 = fmap split (unJVec (split xzus')) :: Vec deg (CollPoint (JV x) (JV z) (JV u) (Vector a)) - xs :: [(a,Vector a)]- xs = (t0,unJ x0):xs'++[(tnext,unJ xnext)]+ xs :: V.Vector (a, V.Vector a)+ xs = (t0, unJ x0) `V.cons` xs' `V.snoc` (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+ xs',zs,us,os,xds :: Vector (a, Vector a)+ (xs',zs,us,os,xds) = V.unzip5 $ TV.unVec $ 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') )+ 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+instance Binary 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+ , ctmQ :: NameTree } deriving (Eq, Generic, Show)-instance Serialize CollTrajMeta+instance Binary 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' k (nm, ATGetter _) = (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)]]))+type MetaTree a = Tree.Forest (String, String, Maybe ((DynPlotPoints a, CollTrajMeta) -> [[(a,a)]])) forestFromMeta :: CollTrajMeta -> MetaTree Double forestFromMeta meta = [xTree,zTree,uTree,oTree,xdTree]@@ -182,43 +186,35 @@ 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 :: forall f c t+ . (Functor f, F.Foldable f)+ => (c -> f (f (t, Vector t))) -> String -> NameTree+ -> Tree (String, String, Maybe ((c,CollTrajMeta) -> [[(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)) []+ blah f myname (NameTreeLeaf k) = Tree.Node (myname,"",Just (woo . f . fst)) [] where- woo = map (map (\(t,x) -> (t, x V.! k)))+ woo :: f (f (t, Vector t)) -> [[(t, t)]]+ woo = F.toList . fmap (F.toList . fmap (\(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,+toMeta :: forall x z u p o q n deg .+ (Lookup (x ()), Lookup (z ()), Lookup (u ()), Lookup (p ()), Lookup (o ()), Lookup (q ()),+ Vectorize x, Vectorize z, Vectorize u, Vectorize p, Vectorize o, Vectorize q, 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+ => Proxy o -> Proxy q -> Proxy (CollTraj x z u p n deg) -> CollTrajMeta+toMeta _ _ _ =+ CollTrajMeta { ctmX = namesFromAccTree $ accessors (fill () :: x ())+ , ctmZ = namesFromAccTree $ accessors (fill () :: z ())+ , ctmU = namesFromAccTree $ accessors (fill () :: u ())+ , ctmP = namesFromAccTree $ accessors (fill () :: p ())+ , ctmO = namesFromAccTree $ accessors (fill () :: o ())+ , ctmQ = namesFromAccTree $ accessors (fill () :: q ()) } -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,+toMetaCov :: forall sx x z u p o q n deg .+ (Lookup (x ()), Lookup (z ()), Lookup (u ()), Lookup (p ()), Lookup (o ()), Lookup (q ()),+ Vectorize x, Vectorize z, Vectorize u, Vectorize p, Vectorize o, Vectorize q, 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))+ => Proxy o -> Proxy q -> Proxy (CollTrajCov sx x z u p n deg) -> CollTrajMeta+toMetaCov po pq _ = toMeta po pq (Proxy :: Proxy (CollTraj x z u p n deg))
src/Dyno/DirectCollocation/Export.hs view
@@ -12,9 +12,10 @@ import qualified Data.Vector as V import qualified Data.Foldable as F +import Accessors ( Lookup, flatten, accessors )+ import Dyno.View.Unsafe.View ( unJ ) -import Dyno.Server.Accessors ( Lookup, flatten, accessors ) import Dyno.TypeVecs ( Vec ) import Dyno.Vectorize ( Vectorize, fill ) import Dyno.View.View ( View(..) )@@ -25,7 +26,7 @@ import Dyno.DirectCollocation.Quadratures ( timesFromTaus ) toMatlab ::- forall x z u p r c h o n deg+ forall x z u p r c h o q n deg . ( Lookup (x Double), Vectorize x , Lookup (z Double), Vectorize z , Lookup (u Double), Vectorize u@@ -34,7 +35,7 @@ , Dim deg , Dim n )- => CollProblem x z u p r c h o n deg+ => CollProblem x z u p r c h o q n deg -> CollTraj x z u p n deg (Vector Double) -> IO String toMatlab cp ct@(CollTraj tf' p' stages' xf) = do
src/Dyno/DirectCollocation/Formulate.hs view
@@ -47,7 +47,7 @@ import Dyno.TypeVecs ( Vec ) import qualified Dyno.TypeVecs as TV import Dyno.LagrangePolynomials ( lagrangeDerivCoeffs )-import Dyno.Nlp ( Nlp'(..), Bounds )+import Dyno.Nlp ( Nlp(..), Bounds ) import Dyno.Ocp ( OcpPhase(..), OcpPhaseWithCov(..) ) import Dyno.DirectCollocation.Types@@ -55,28 +55,34 @@ import Dyno.DirectCollocation.Quadratures ( QuadratureRoots(..), mkTaus, interpolate, timesFromTaus ) import Dyno.DirectCollocation.Robust -data CollProblem x z u p r c h o n deg =+data CollProblem x z u p r c h o q 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+ { 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 q , cpPlotPoints :: J (CollTraj x z u p n deg) (Vector Double) -> IO (DynPlotPoints Double)+ , cpHellaOutputs :: J (CollTraj x z u p n deg) (Vector Double)+ -> IO ( DynPlotPoints Double+ , Vec n ( Vec deg (J (JV o) (Vector Double), J (JV x) (Vector Double))+ , J (JV x) (Vector Double)+ )+ ) , cpOutputs :: J (CollTraj x z u p n deg) (Vector Double) -> IO (Vec n (Vec deg (o Double, x Double), x Double)) , cpTaus :: Vec deg Double , cpRoots :: QuadratureRoots+ , cpEvalQuadratures :: Vec n (Vec deg Double) -> Double -> IO Double } 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+ forall x z u p r o c h q deg n .+ ( Dim deg, Dim n+ , Vectorize x, Vectorize p, Vectorize u, Vectorize z+ , Vectorize r, Vectorize o, Vectorize h, Vectorize c, Vectorize q+ )+ => QuadratureRoots -> OcpPhase x z u p r o c h q+ -> IO (CollProblem x z u p r c h o q n deg)+makeCollProblem roots ocp = do let -- the collocation points- roots :: QuadratureRoots- roots = Legendre- taus :: Vec deg Double taus = mkTaus roots @@ -87,11 +93,29 @@ cijs = lagrangeDerivCoeffs (0 TV.<| taus) interpolate' :: (J (JV x) :*: J (JVec deg (JV x))) MX -> J (JV x) MX- interpolate' (x0 :*: xs) = interpolate taus x0 (unJVec (split xs))+ interpolate' (x0 :*: xs) = case roots of+ Legendre -> interpolate taus x0 (unJVec (split xs))+ Radau -> TV.tvlast $ unJVec $ split xs++ interpolateq' :: (J (JV q) :*: J (JVec deg (JV q))) MX -> J (JV q) MX+ interpolateq' (q0 :*: qs) = case roots of+ Legendre -> interpolate taus q0 (unJVec (split qs))+ Radau -> TV.tvlast $ unJVec $ split qs+ interpolateScalar' :: (J (JV Id) :*: J (JVec deg (JV Id))) MX -> J (JV Id) MX- interpolateScalar' (x0 :*: xs) = interpolate taus x0 (unJVec (split xs))+ interpolateScalar' (x0 :*: xs) = case roots of+ Legendre -> interpolate taus x0 (unJVec (split xs))+ Radau -> TV.tvlast $ unJVec $ split xs + dynamicsFunction (t :*: parm :*: x' :*: collPoint) = (sxCatJV r) :*: (sxCatJV o)+ where+ CollPoint x z u = split collPoint+ (r,o) = ocpDae ocp+ (sxSplitJV x') (sxSplitJV x) (sxSplitJV z) (sxSplitJV u)+ (sxSplitJV parm) (unId (sxSplitJV t))+ interpolateFun <- toMXFun "interpolate (JV x)" interpolate' >>= expandMXFun+ interpolateQFun <- toMXFun "interpolate (JV q)" interpolateq' >>= expandMXFun interpolateScalarFun <- toMXFun "interpolate (JV Id)" interpolateScalar' >>= expandMXFun let callInterpolateScalar :: J (JV Id) MX -> Vec deg (J (JV Id) MX) -> J (JV Id) MX callInterpolateScalar x0 xs = call interpolateScalarFun (x0 :*: cat (JVec xs))@@ -99,20 +123,27 @@ callInterpolate :: J (JV x) MX -> Vec deg (J (JV x) MX) -> J (JV x) MX callInterpolate x0 xs = call interpolateFun (x0 :*: cat (JVec xs)) - bcFun <- toSXFun "bc" $ \(x0:*:x1) -> sxCatJV $ ocpBc ocp (sxSplitJV x0) (sxSplitJV x1)- mayerFun <- toSXFun "mayer" $ \(x0:*:x1:*:x2) ->- sxCatJV $ Id $ ocpMayer ocp (unId (sxSplitJV x0)) (sxSplitJV x1) (sxSplitJV x2)+ callInterpolateQ :: J (JV q) MX -> Vec deg (J (JV q) MX) -> J (JV q) MX+ callInterpolateQ q0 qs = call interpolateQFun (q0 :*: cat (JVec qs))++ bcFun <- toSXFun "bc" $ \(x0:*:x1:*:x2:*:x3:*:x4) -> sxCatJV $ ocpBc ocp (sxSplitJV x0) (sxSplitJV x1) (sxSplitJV x2) (sxSplitJV x3) (unId (sxSplitJV x4))+ mayerFun <- toSXFun "mayer" $ \(x0:*:x1:*:x2:*:x3:*:x4) ->+ sxCatJV $ Id $ ocpMayer ocp (unId (sxSplitJV x0)) (sxSplitJV x1) (sxSplitJV x2) (sxSplitJV x3) (sxSplitJV x4)+ lagrangeFun <- toSXFun "lagrange" $ \(x0:*:x1:*:x2:*:x3:*:x4:*:x5:*:x6) -> sxCatJV $ Id $ ocpLagrange ocp (sxSplitJV x0) (sxSplitJV x1) (sxSplitJV x2) (sxSplitJV x3) (sxSplitJV x4) (unId (sxSplitJV x5)) (unId (sxSplitJV x6))- quadFun <- toMXFun "quadratures" $ evaluateQuadraturesFunction lagrangeFun callInterpolateScalar cijs n--- let callQuadFun = call quadFun- callQuadFun <- fmap call (expandMXFun quadFun)+ lagQuadFun <- toMXFun "lagrange quadratures" $ evaluateQuadraturesFunction lagrangeFun callInterpolateScalar cijs n+ callLagQuadFun <- fmap call (expandMXFun lagQuadFun) -- necessary to discard unused outputs - 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) (unId (sxSplitJV x5))- in (sxCatJV r, sxCatJV o)+ quadratureDotFun <- toSXFun "quadrature derivative" $ \(x0:*:x1:*:x2:*:x3:*:x4:*:x5:*:x6) ->+ sxCatJV $ ocpQuadratures ocp (sxSplitJV x0) (sxSplitJV x1) (sxSplitJV x2) (sxSplitJV x3) (sxSplitJV x4) (unId (sxSplitJV x5)) (unId (sxSplitJV x6))+ quadFun <- toMXFun "quadratures" $ evaluateQuadraturesFunction quadratureDotFun callInterpolateQ cijs n+ callQuadFun <- fmap call (expandMXFun quadFun) -- necessary to discard unused outputs + genericQuadraturesFun <- toMXFun "generic quadratures" $ genericQuadraturesFunction callInterpolateScalar cijs n++ dynFun <- toSXFun "dynamics" dynamicsFunction+ pathConFun <- toSXFun "pathConstraints" $ pathConFunction $ \x0 x1 x2 x3 x4 x5 -> sxCatJV $ ocpPathC ocp (sxSplitJV x0) (sxSplitJV x1) (sxSplitJV x2) (sxSplitJV x3) (sxSplitJV x4) (unId (sxSplitJV x5)) pathStageConFun <- toMXFun "pathStageCon" (pathStageConstraints pathConFun)@@ -126,9 +157,7 @@ outputFun <- toMXFun "stageOutputs" $ outputFunction callInterpolate 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+ let f :: J (JV o) DMatrix -> J (JV x) DMatrix -> (J (JV o) (Vector Double), J (JV x) (Vector Double)) f o' x' = (d2v o', d2v x') @@ -159,11 +188,21 @@ T.sequence $ TV.tvzipWith (callOutputFun p h) vstages ks - getPlotPoints :: J (CollTraj x z u p n deg) (Vector Double) -> IO (DynPlotPoints Double)- getPlotPoints traj = do+ getHellaOutputs ::+ J (CollTraj x z u p n deg) (Vector Double)+ -> IO ( DynPlotPoints Double+ , Vec n ( Vec deg (J (JV o) (Vector Double), J (JV x) (Vector Double))+ , J (JV x) (Vector Double)+ )+ )+ getHellaOutputs traj = do outputs <- mapOutputFun traj- return (dynPlotPoints roots (split traj) outputs)+ return (dynPlotPoints roots (split traj) outputs, outputs) + getPlotPoints :: J (CollTraj x z u p n deg) (Vector Double)+ -> IO (DynPlotPoints Double)+ getPlotPoints traj = fmap fst $ getHellaOutputs traj+ getOutputs :: J (CollTraj x z u p n deg) (Vector Double) -> IO (Vec n (Vec deg (o Double, x Double), x Double)) getOutputs traj = do@@ -173,52 +212,71 @@ devec = fmap (\(x,y) -> (splitJV x, splitJV y)) return $ fmap (\(x,y) -> (devec x, splitJV y)) outputs - let nlp = Nlp' {- nlpFG' =+ let nlp = Nlp {+ nlpFG = getFg taus- (bcFun :: SXFun (J (JV x) :*: J (JV x)) (J (JV c)))- (mayerFun :: SXFun (J (JV Id) :*: (J (JV x) :*: (J (JV x)))) (J (JV Id)))- (callQuadFun :: (J (JV p) :*: J (JVec deg (CollPoint (JV x) (JV z) (JV u))) :*: J (JVec deg (JV o)) :*: J (JV Id) :*: J (JVec deg (JV Id))) MX+ (bcFun :: SXFun (J (JV x) :*: J (JV x) :*: J (JV q) :*: J (JV p) :*: J (JV Id)) (J (JV c)))+ (mayerFun :: SXFun (J (JV Id) :*: (J (JV x) :*: (J (JV x)) :*: (J (JV q)) :*: (J (JV p)))) (J (JV Id)))+ (callLagQuadFun :: (J (JV p) :*: J (JVec deg (CollPoint (JV x) (JV z) (JV u))) :*: J (JVec deg (JV o)) :*: J (JV Id) :*: J (JVec deg (JV Id))) MX -> J (JV Id) MX)+ (callQuadFun :: (J (JV p) :*: J (JVec deg (CollPoint (JV x) (JV z) (JV u))) :*: J (JVec deg (JV o)) :*: J (JV Id) :*: J (JVec deg (JV Id))) MX+ -> J (JV q) MX) (callStageFun :: (J (JV Id) :*: J (JV p) :*: J (JVec deg (JV Id)) :*: 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))+ , nlpBX = cat $ fillCollTraj'+ (fill (Nothing, Nothing))+ (ocpXbnd ocp)+ (ocpZbnd ocp)+ (ocpUbnd ocp)+ (ocpPbnd ocp)+ (ocpTbnd ocp)+ , nlpBG = cat (getBg ocp)+ , nlpX0 = jfill 0 :: J (CollTraj x z u p n deg) (Vector Double) -- todo: don't do that+ , 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))+ , nlpScaleG = Just $ cat $ fillCollConstraints+ (fromMaybe (fill 1) (ocpXScale ocp))+ (fromMaybe (fill 1) (ocpResidualScale ocp))+ (fromMaybe (fill 1) (ocpBcScale ocp))+ (fromMaybe (fill 1) (ocpPathCScale ocp)) }+ evalQuadratures :: Vec n (Vec deg Double) -> Double -> IO Double+ evalQuadratures qs' tf' = do+ let d2d :: Double -> J (JV Id) DMatrix+ d2d = realToFrac+ qs :: Vec n (J (JVec deg (JV Id)) DMatrix)+ qs = fmap (cat . JVec . fmap d2d) qs'+ tf :: J (JV Id) DMatrix+ tf = realToFrac tf'+ evalq :: J (JVec deg (JV Id)) DMatrix -> IO (J (JV Id) DMatrix)+ evalq q = eval genericQuadraturesFun (q :*: tf)+ stageIntegrals' <- T.mapM evalq qs :: IO (Vec n (J (JV Id) DMatrix))+ let stageIntegrals = fmap (unId . splitJV . d2v) stageIntegrals' :: Vec n Double+ return (F.sum stageIntegrals)+ return $ CollProblem { cpNlp = nlp , cpOcp = ocp , cpPlotPoints = getPlotPoints+ , cpHellaOutputs = getHellaOutputs , cpOutputs = getOutputs , cpTaus = taus , cpRoots = roots+ , cpEvalQuadratures = evalQuadratures } data CollCovProblem x z u p r o c h n deg sx sw sh shr sc = CollCovProblem- { ccpNlp :: Nlp'+ { ccpNlp :: Nlp (CollTrajCov sx x z u p n deg) JNone (CollOcpCovConstraints n deg x r c h sh shr sc) MX@@ -238,18 +296,18 @@ } 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+ forall x z u p r o c h q 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, Vectorize q+ , View sh, Vectorize shr, View sc+ )+ => QuadratureRoots+ -> OcpPhase x z u p r o c h q+ -> OcpPhaseWithCov (OcpPhase x z u p r o c h q) 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+makeCollCovProblem roots ocp ocpCov = do let -- the collocation points- roots = Legendre- taus :: Vec deg Double taus = mkTaus roots @@ -264,7 +322,7 @@ lagrangeFun <- toSXFun "cov lagrange" $ \(x0:*:x1:*:x2:*:x3) -> sxCatJV $ Id $ ocpCovLagrange ocpCov (unId (sxSplitJV x0)) (sxSplitJV x1) x2 (unId (sxSplitJV x3)) - cp0 <- makeCollProblem ocp+ cp0 <- makeCollProblem roots ocp robustify <- mkRobustifyFunction (ocpCovProjection ocpCov) (ocpCovRobustifyPathC ocpCov) @@ -292,7 +350,7 @@ (shFun :: SXFun (J (JV x) :*: J (Cov (JV sx))) (J sh)) (lagrangeFun :: SXFun (J (JV Id) :*: J (JV x) :*: J (Cov (JV sx)) :*: J (JV Id)) (J (JV Id))) (mayerFun :: SXFun (J (JV Id) :*: (J (JV x) :*: (J (JV x) :*: (J (Cov (JV sx)) :*: J (Cov (JV sx)))))) (J (JV Id)))- (nlpFG' nlp0)+ (nlpFG nlp0) computeCovariancesFun' <- toMXFun "compute covariances" computeCovariances -- callbacks@@ -316,41 +374,41 @@ return (outputs, fmap d2v covs, d2v 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))+ 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)- }+ , 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@@ -362,20 +420,22 @@ } getFg ::- forall z x u p r o c h n deg .+ forall z x u p r o c h q n deg . (Dim deg, Dim n, Vectorize x, Vectorize z, Vectorize u, Vectorize p,- Vectorize r, Vectorize o, Vectorize c, Vectorize h)+ Vectorize r, Vectorize o, Vectorize c, Vectorize h, Vectorize q) => Vec deg Double- -> SXFun (J (JV x) :*: J (JV x)) (J (JV c))+ -> SXFun (J (JV x) :*: J (JV x) :*: J (JV q) :*: J (JV p) :*: J (JV Id)) (J (JV c)) -> SXFun- (J (JV Id) :*: J (JV x) :*: J (JV x)) (J (JV Id))+ (J (JV Id) :*: J (JV x) :*: J (JV x) :*: J (JV q) :*: J (JV p)) (J (JV Id)) -> ((J (JV p) :*: J (JVec deg (CollPoint (JV x) (JV z) (JV u))) :*: J (JVec deg (JV o)) :*: J (JV Id) :*: J (JVec deg (JV Id))) MX -> (J (JV Id)) MX)+ -> ((J (JV p) :*: J (JVec deg (CollPoint (JV x) (JV z) (JV u))) :*: J (JVec deg (JV o)) :*: J (JV Id) :*: J (JVec deg (JV Id))) MX ->+ (J (JV q)) MX) -> ((J (JV Id) :*: J (JV p) :*: J (JVec deg (JV Id)) :*: 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 (JV Id) MX, J (CollOcpConstraints n deg x r c h) MX)-getFg taus bcFun mayerFun quadFun stageFun collTraj _ = (obj, cat g)+getFg taus bcFun mayerFun lagQuadFun quadFun stageFun collTraj _ = (obj, cat g) where -- split up the design vars CollTraj tf parm stages' xf = split collTraj@@ -387,15 +447,16 @@ obj = objLagrange + objMayer - objMayer = call mayerFun (tf :*: x0 :*: xf)+ objMayer = call mayerFun (tf :*: x0 :*: xf :*: finalQuadratures :*: parm) objLagrange :: J (JV Id) 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 (JV Id)) MX- -> J (JV Id) MX- oneStage stagePoints stageOutputs stageTimes =- quadFun (parm :*: stagePoints :*: stageOutputs :*: dt :*: stageTimes)+ objLagrange = F.sum $ TV.tvzipWith3 (oneStage lagQuadFun) spstagesPoints outputs times' + finalQuadratures :: J (JV q) MX+ finalQuadratures = F.sum $ TV.tvzipWith3 (oneStage quadFun) spstagesPoints outputs times'+ oneStage qfun stagePoints stageOutputs stageTimes =+ qfun (parm :*: stagePoints :*: stageOutputs :*: dt :*: stageTimes)+ -- timestep dt = tf / fromIntegral n n = reflectDim (Proxy :: Proxy n)@@ -420,7 +481,7 @@ { coCollPoints = cat $ JVec dcs , coContinuity = cat $ JVec integratorMatchingConstraints , coPathC = cat $ JVec hs- , coBc = call bcFun (x0 :*: xf)+ , coBc = call bcFun (x0 :*: xf :*: finalQuadratures :*: parm :*: tf) } integratorMatchingConstraints :: Vec n (J (JV x) MX) -- THIS SHOULD BE A NONLINEAR FUNCTION@@ -536,9 +597,9 @@ -getBg :: forall x z u p r o c h deg n .+getBg :: forall x z u p r o c h q deg n . (Dim n, Dim deg, Vectorize x, Vectorize r, Vectorize c, Vectorize h)- => OcpPhase x z u p r o c h+ => OcpPhase x z u p r o c h q -> CollOcpConstraints n deg x r c h (Vector Bounds) getBg ocp = CollOcpConstraints@@ -551,16 +612,16 @@ hbnds = catJV (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 (JV Id) :*: J (JV Id)) (J (JV Id))- -> (J (JV Id) MX -> Vec deg (J (JV Id) MX) -> J (JV Id) MX)+ forall x z u p o q deg .+ (Dim deg, View x, View z, View u, View o, View p, View q)+ => SXFun (J x :*: J z :*: J u :*: J p :*: J o :*: J (JV Id) :*: J (JV Id)) (J q)+ -> (J q MX -> Vec deg (J q MX) -> J q MX) -> Vec (TV.Succ deg) (Vec (TV.Succ deg) Double) -> Int -> (J p :*: J (JVec deg (CollPoint x z u)) :*: J (JVec deg o) :*: J (JV Id) :*: J (JVec deg (JV Id))) MX- -> J (JV Id) MX+ -> J q MX evaluateQuadraturesFunction f interpolate' cijs' n (p :*: stage' :*: outputs' :*: dt :*: stageTimes') =- dt * qnext+ M.uncol $ M.ms (M.col qnext) dt where tf = dt * fromIntegral n @@ -573,12 +634,52 @@ stageTimes :: Vec deg (J (JV Id) MX) stageTimes = unJVec (split stageTimes') - qnext :: J (JV Id) MX- qnext = interpolate' 0 qs+ qdots :: Vec deg (J q MX)+ qdots = TV.tvzipWith3 (\(CollPoint x z u) o t -> call f (x:*:z:*:u:*:p:*:o:*:t:*:tf)) stage outputs stageTimes + qnext :: J q MX+ qnext = interpolate' (0 :: J q MX) qs++ qs :: Vec deg (J q MX)+ 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 q MX))+ cijInvFr = fmap (fmap realToFrac) cijInv+++-- todo: merging this with evaluateQuadraturesFunction would reduce duplication,+-- but could be inefficient+genericQuadraturesFunction ::+ forall deg+ . Dim deg+ => (J (JV Id) MX -> Vec deg (J (JV Id) MX) -> J (JV Id) MX)+ -> Vec (TV.Succ deg) (Vec (TV.Succ deg) Double)+ -> Int+ -> (J (JVec deg (JV Id)) :*: J (JV Id)) MX+ -> J (JV Id) MX+genericQuadraturesFunction interpolate' cijs' n (qdots' :*: tf) =+ dt * qnext+ where+ dt = tf / fromIntegral n+ qdots :: Vec deg (J (JV Id) MX)- qdots = TV.tvzipWith3 (\(CollPoint x z u) o t -> call f (x:*:z:*:u:*:p:*:o:*:t:*:tf)) stage outputs stageTimes+ qdots = unJVec $ split qdots' + qnext :: J (JV Id) MX+ qnext = interpolate' 0 qs+ qs = cijInvFr !* qdots cijs :: Vec deg (Vec deg Double)@@ -596,6 +697,7 @@ cijInvFr :: Vec deg (Vec deg (J (JV Id) MX)) cijInvFr = fmap (fmap realToFrac) cijInv + -- todo: code duplication dot :: forall x deg a b. (Fractional (J x a), Real b, Dim deg) => Vec deg b -> Vec deg (J x a) -> J x a dot cks xs = F.sum $ TV.unVec elemwise@@ -618,18 +720,6 @@ -> 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 (JV Id) a -> (J r a, J o a))- -> (J (JV Id) :*: 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 ::
src/Dyno/DirectCollocation/Integrate.hs view
@@ -31,10 +31,11 @@ import Dyno.TypeVecs ( Vec ) import qualified Dyno.TypeVecs as TV import Dyno.LagrangePolynomials ( lagrangeDerivCoeffs )-import Dyno.NlpSolver ( NlpSolverStuff, runNlpSolver, liftIO, solve+import Dyno.Solvers ( Solver )+import Dyno.NlpSolver ( runNlpSolver, liftIO, solve , setX0, setLbg, setUbg, setP, setLbx, setUbx, getX ) import Dyno.DirectCollocation.Types ( CollStage(..), CollPoint(..) )-import Dyno.DirectCollocation.Quadratures ( QuadratureRoots(..), mkTaus, interpolate, timesFromTaus )+import Dyno.DirectCollocation.Quadratures ( QuadratureRoots, mkTaus, interpolate, timesFromTaus ) @@ -141,16 +142,14 @@ 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)+ -> QuadratureRoots -> x Double -> (x Sxe -> x Sxe -> z Sxe -> u Sxe -> p Sxe -> Sxe -> r Sxe)- -> NlpSolverStuff+ -> Solver -> ((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+withIntegrator _ roots initialX dae solver userFun = do let -- the collocation points- roots :: QuadratureRoots- roots = Legendre- taus :: Vec deg Double taus = mkTaus roots
src/Dyno/DirectCollocation/Profile.hs view
@@ -14,12 +14,13 @@ import Dyno.View.View ( J ) import Dyno.Vectorize ( Vectorize ) import Dyno.Ocp ( OcpPhase )-import Dyno.Solvers ( NlpSolverStuff )+import Dyno.Solvers ( Solver ) import Dyno.DirectCollocation.Types ( CollTraj, CollOcpConstraints ) import Dyno.DirectCollocation.Formulate ( CollProblem(..), makeCollProblem )+import Dyno.DirectCollocation.Quadratures ( QuadratureRoots ) import qualified Dyno.TypeVecs as TV-import Dyno.NlpSolver ( solveNlp' )-import Dyno.Nlp ( Nlp'(..), NlpOut'(..) )+import Dyno.NlpUtils ( solveNlp )+import Dyno.Nlp ( Nlp(..), NlpOut(..) ) data ProfileReport = ProfileReport@@ -28,37 +29,39 @@ toProfileReport :: Either String String- -> NlpOut' (CollTraj x z u p n deg) (CollOcpConstraints n deg x r c h) (Vector Double)+ -> 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 .+profile :: forall x z u p r o c h q . (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+ Vectorize r, Vectorize o, Vectorize c, Vectorize h, Vectorize q)+ => QuadratureRoots+ -> OcpPhase x z u p r o c h q -> (forall deg n . (Dim deg, Dim n) => J (CollTraj x z u p n deg) (Vector Double))- -> NlpSolverStuff+ -> Solver -> [(Int,Int)] -> IO [ProfileReport]-profile ocp guess solver range = do+profile roots 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+ profileOne roots 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 .+ forall x z u p r o c h q n deg . (Vectorize x, Vectorize z, Vectorize u, Vectorize p,- Vectorize r, Vectorize o, Vectorize c, Vectorize h,+ Vectorize r, Vectorize o, Vectorize c, Vectorize h, Vectorize q, Dim n, Dim deg)- => OcpPhase x z u p r o c h+ => QuadratureRoots+ -> OcpPhase x z u p r o c h q -> J (CollTraj x z u p n deg) (Vector Double)- -> NlpSolverStuff+ -> Solver -> IO ProfileReport-profileOne ocp guess solver = do- cp <- makeCollProblem ocp+profileOne roots ocp guess solver = do+ cp <- makeCollProblem roots ocp let nlp = cpNlp cp- x <- solveNlp' solver (nlp { nlpX0' = guess }) Nothing+ x <- solveNlp solver (nlp { nlpX0 = guess }) Nothing uncurry toProfileReport x
src/Dyno/DirectCollocation/Quadratures.hs view
@@ -17,10 +17,10 @@ import Data.Proxy ( Proxy(..) ) import qualified Data.Vector as V import qualified Data.Foldable as F-import Data.Serialize ( Serialize(..) )+import Data.Binary ( Binary ) import Linear.V -import JacobiRoots ( shiftedLegendreRoots ) --, shiftedRadauRoots )+import JacobiRoots ( shiftedLegendreRoots, shiftedRadauRoots ) import Dyno.View.View ( View, J ) import Dyno.TypeVecs ( Vec )@@ -28,7 +28,7 @@ import Dyno.LagrangePolynomials ( lagrangeXis ) data QuadratureRoots = Legendre | Radau deriving (Show, Eq, Ord, Enum, Generic)-instance Serialize QuadratureRoots+instance Binary QuadratureRoots mkTaus :: forall deg a@@ -39,9 +39,10 @@ Nothing -> error "makeTaus: too high degree" where deg = reflectDim (Proxy :: Proxy deg)+ taus :: Maybe (V.Vector Double) taus = case quadratureRoots of Legendre -> shiftedLegendreRoots deg- Radau -> error "radau not yet supported" -- shiftedRadauRoots (deg-1) ++ [1.0]+ Radau -> fmap (`V.snoc` 1.0) (shiftedRadauRoots (deg-1)) -- todo: code duplication
src/Dyno/DirectCollocation/Types.hs view
@@ -12,7 +12,9 @@ , CollTrajCov(..) , CollOcpCovConstraints(..) , fillCollTraj+ , fillCollTraj' , fmapCollTraj+ , fmapCollTraj' , fmapStage , fmapCollPoint , fillCollConstraints@@ -120,8 +122,17 @@ (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+fillCollTraj x = fillCollTraj' x x++-- | first argument maps over the non-collocation points+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 -> x a -> z a -> u a -> p a -> a -> CollTraj x z u p n deg (Vector a)+fillCollTraj' x' x z u p t =+ fmapCollTraj'+ (const x') (const x) (const z) (const u)@@ -144,11 +155,31 @@ -> (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)+fmapCollTraj fx = fmapCollTraj' fx fx++-- | first argument maps over the non-collocation points+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)+ -> (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' fx fz fu fp ft (CollTraj tf1 p stages1 xf) =+ CollTraj tf2 (fj fp p) stages2 (fj fx' xf) where tf2 :: J (JV Id) (Vector b) tf2 = catJV $ fmap ft (splitJV tf1)- stages2 = cat $ fmapJVec (fmapStage fx fz fu) (split stages1)+ stages2 = cat $ fmapJVec (fmapStage fx' fx fz fu) (split stages1) fj :: (Vectorize f1, Vectorize f2) => (f1 a -> f2 b)@@ -166,11 +197,12 @@ , Dim deg , Show a, Show b ) => (x1 a -> x2 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+fmapStage fx' fx fz fu (CollStage x0 points0) = CollStage (fj fx' x0) points1 where points1 = cat $ fmapJVec (fmapCollPoint fx fz fu) (split points0)
src/Dyno/MultipleShooting.hs view
@@ -29,7 +29,7 @@ import Dyno.View.Fun ( MXFun, toMXFun, call ) import Dyno.View.Scheme ( Scheme ) import Dyno.Vectorize ( Vectorize, Id )-import Dyno.Nlp ( Bounds, Nlp'(..) )+import Dyno.Nlp ( Bounds, Nlp(..) ) data IntegratorIn x u p a = IntegratorIn (J (JV x) a) (J (JV u) a) (J (JV p) a)@@ -92,7 +92,7 @@ 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)+ => 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))@@ -108,17 +108,17 @@ 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+ 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)
src/Dyno/Nlp.hs view
@@ -6,21 +6,24 @@ module Dyno.Nlp ( Bounds , Nlp(..), NlpOut(..)- , Nlp'(..), NlpOut'(..)+ , KKT(..) ) where -import GHC.Generics ( Generic, Generic1 )+import GHC.Generics ( Generic ) +import Casadi.DMatrix ( DMatrix ) import qualified Data.Vector as V-import Data.Serialize ( Serialize(..) )+import Data.Binary ( Binary ) -import Dyno.Vectorize ( Vectorize(..), Id )+import Dyno.Vectorize ( Id ) import Dyno.View.View ( View(..), J )+import Dyno.View.Viewable ( Viewable ) import Dyno.View.JV ( JV )+import Dyno.View.M ( M ) type Bounds = (Maybe Double, Maybe Double) --- | user-friendly NLP+-- | nonlinear program (NLP) -- -- > minimize f(x,p) -- > x@@ -32,56 +35,39 @@ -- 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)+ { nlpFG :: J x a -> J p a -> (J (JV Id) 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 (x Double)- , nlpScaleG :: Maybe (g Double)+ , nlpScaleX :: Maybe (J x (V.Vector Double))+ , nlpScaleG :: Maybe (J g (V.Vector Double)) } +-- | NLP output 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 (JV Id) a- , xOpt' :: J x a- , gOpt' :: J g a- , lambdaXOpt' :: J x a- , lambdaGOpt' :: J g a+ { fOpt :: J (JV Id) 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 . cat- get = fmap split get+instance (View x, View g, Binary a, Viewable a) => Binary (NlpOut x g a) -data Nlp' x p g a =- Nlp'- { nlpFG' :: J x a -> J p a -> (J (JV Id) 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))- }+-- | Karush–Kuhn–Tucker (KKT) matrix+data KKT x g =+ KKT+ { kktHessLag :: M x x DMatrix -- ^ unscaled version only valid at solution+ , kktHessF :: M x x DMatrix+ , kktHessLambdaG :: M x x DMatrix -- ^ unscaled version only valid at solution+ , kktJacG :: M g x DMatrix+ , kktG :: J g DMatrix+ , kktGradF :: J x DMatrix+ , kktF :: J (JV Id) DMatrix+ } deriving (Generic, Eq, Show)+instance (View x, View g) => Binary (KKT x g)
src/Dyno/NlpScaling.hs view
@@ -14,9 +14,11 @@ import Dyno.View.Unsafe.View ( unJ, mkJ ) -import Dyno.Vectorize ( Id )-import Dyno.View.View ( View, J )-import Dyno.View.JV ( JV )+import Dyno.View.M ( M )+import qualified Dyno.View.M as M+import Dyno.Vectorize ( Id(..) )+import Dyno.View.View ( View, J, v2d, fromDMatrix )+import Dyno.View.JV ( JV, catJV' ) import Dyno.View.Viewable ( Viewable ) data ScaleFuns x g a =@@ -31,6 +33,11 @@ , lamXBarToLamX :: J x a -> J x a , lamGToLamGBar :: J g a -> J g a , lamGBarToLamG :: J g a -> J g a+ , gradFBarToGradF :: J x a -> J x a+ , jacGBarToJacG :: M g x a -> M g x a+ , hessFBarToHessF :: M x x a -> M x x a+ , hessLamGBarToHessLamG :: M x x a -> M x x a+ , hessLagBarToHessLag :: M x x a -> M x x a } scaledFG ::@@ -48,6 +55,9 @@ allPositive :: Maybe (V.Vector Double) -> Bool allPositive = all (> 0) . fromMaybe [] . fmap V.toList +-- todo:+-- Could make this return casadi Functions for better performance.+-- Doesn't seem to be a bottleneck mkScaleFuns :: forall x g a . (View x, View g, CMatrix a, Viewable a)@@ -72,8 +82,41 @@ , lamXBarToLamX = lamXBarToLamX' , lamGToLamGBar = lamGToLamGBar' , lamGBarToLamG = lamGBarToLamG'+ , gradFBarToGradF = gradFBarToGradF'+ , jacGBarToJacG = jacGBarToJacG'+ , hessFBarToHessF = hessFBarToHessF'+ , hessLamGBarToHessLamG = hessFBarToHessF' -- only valid at the solution+ , hessLagBarToHessLag = hessFBarToHessF' -- only valid at the solution } where+ xdiaginv :: Maybe (M x x a)+ xdiaginv = fmap (\scl -> M.diag (fromDMatrix (1.0 / (v2d scl)))) mx++ gdiag :: Maybe (M g g a)+ gdiag = fmap (\scl -> M.diag (fromDMatrix (v2d scl))) mg++ jacGBarToJacG' :: M g x a -> M g x a+ jacGBarToJacG' g0 = gg0x+ where+ gg0x = case gdiag of+ Nothing -> g0x+ Just gd -> gd `M.mm` g0x+ g0x = case xdiaginv of+ Nothing -> g0+ Just xdi -> g0 `M.mm` xdi++ gradFBarToGradF' :: J x a -> J x a+ gradFBarToGradF' = lamXBarToLamX'++ hessFBarToHessF' :: M x x a -> M x x a+ hessFBarToHessF' h0 = case mf of+ Nothing -> h1+ Just fscl -> h1 `M.ms` (catJV' (Id (realToFrac fscl)))+ where+ h1 = case xdiaginv of+ Nothing -> h0+ Just xdi -> xdi `M.mm` h0 `M.mm` xdi+ (lamXToLamXBar', lamXBarToLamX') = case mf of Nothing -> (mulByXScale, divByXScale) Just fscl -> ( \lamx -> mkJ ((unJ (mulByXScale lamx)) / fs)
src/Dyno/NlpSolver.hs view
@@ -3,17 +3,12 @@ {-# Language PackageImports #-} {-# Language KindSignatures #-} {-# Language GeneralizedNewtypeDeriving #-}-{-# Language MultiWayIf #-} module Dyno.NlpSolver ( NlpSolver , SXElement , runNlpSolver- , runNlp -- * solve- , solveNlp- , solveNlp'- , solveNlpHomotopy' , solve , solve' -- * inputs@@ -39,16 +34,31 @@ , getG , getLamX , getLamG- , NlpSolverStuff(..)+ , getStat+ , getNlpOut+ -- * kkt conditions, evalKKT is in user units, evalScaledKKT is the internal one+ , evalGradF+ , evalJacG+ , evalHessF+ , evalHessLambdaG+ , evalKKT+ , evalScaledGradF+ , evalScaledJacG+ , evalScaledHessLag+ , evalScaledHessF+ , evalScaledHessLambdaG+ , evalScaledKKT -- * options , Op.Opt(..) , setOption , reinit+ -- * other , MonadIO , liftIO , generateAndCompile ) where +import Data.Proxy ( Proxy(..) ) import System.Process ( callProcess, showCommandForUser ) import Control.Exception ( AsyncException( UserInterrupt ), try ) import Control.Concurrent ( forkIO, newEmptyMVar, takeMVar, putMVar )@@ -59,8 +69,6 @@ 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@@ -70,7 +78,6 @@ import Casadi.Callback ( makeCallback ) import Casadi.DMatrix ( DMatrix, ddata )-import Casadi.SX ( SX ) import Casadi.Function ( Function, externalFunction ) import qualified Casadi.Option as Op import qualified Casadi.GenericC as Gen@@ -79,31 +86,23 @@ import qualified Casadi.CMatrix as CM import Dyno.View.Unsafe.View ( unJ, mkJ )+import Dyno.View.Unsafe.M ( mkM ) -import Dyno.SXElement ( SXElement, sxSplitJV, sxCatJV )-import Dyno.Vectorize ( Vectorize(..), Id(..) )+import Dyno.SXElement ( SXElement )+import Dyno.Vectorize ( Id(..) ) import Dyno.View.JV ( JV )-import Dyno.View.View ( View(..), J, JNone(..), JTuple(..), jfill, unzipJ, fmapJ )+import Dyno.View.View ( View(..), J, fmapJ, d2v, v2d, jfill )+import Dyno.View.M ( M )+import qualified Dyno.View.M as M import Dyno.View.Symbolic ( Symbolic, sym, mkScheme, mkFunction ) import Dyno.View.Viewable ( Viewable )-import Dyno.Nlp ( Nlp(..), NlpOut(..), Nlp'(..), NlpOut'(..), Bounds )+import Dyno.Nlp ( NlpOut(..), KKT(..) ) import Dyno.NlpScaling ( ScaleFuns(..), scaledFG, mkScaleFuns )-import Data.Proxy+import Dyno.Solvers ( Solver(..) ) 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@@ -220,6 +219,184 @@ getLamG = getOutput lamGBarToLamG "lam_g" +evalScaledGradF :: forall x p g . (View x, View g, View p)+ => NlpSolver x p g (J x DMatrix, J (JV Id) DMatrix)+evalScaledGradF = do+ x0bar <- getInput (const id) "x0" :: NlpSolver x p g (J x (Vector Double))+ pbar <- getInput (const id) "p" :: NlpSolver x p g (J p (Vector Double))++ nlpState <- ask+ let solver = isSolver nlpState :: C.NlpSolver+ liftIO $ do+ gradF <- C.nlpSolver_gradF solver+ C.ioInterfaceFunction_setInput__0 gradF (unJ (v2d x0bar)) "x"+ C.ioInterfaceFunction_setInput__0 gradF (unJ (v2d pbar)) "p"+ C.function_evaluate gradF+ gradF' <- C.ioInterfaceFunction_output__0 gradF "grad"+ f' <- C.ioInterfaceFunction_output__0 gradF "f"+ return (mkJ gradF', mkJ f')++evalGradF :: forall x p g . (View x, View g, View p)+ => NlpSolver x p g (J x DMatrix, J (JV Id) DMatrix)+evalGradF = do+ nlpState <- ask+ let scale = isScale nlpState+ (gradF, f) <- evalScaledGradF+ return (gradFBarToGradF scale gradF, fbarToF scale f)++evalScaledJacG :: forall x p g . (View x, View g, View p)+ => NlpSolver x p g (M g x DMatrix, J g DMatrix)+evalScaledJacG = do+ x0bar <- getInput (const id) "x0" :: NlpSolver x p g (J x (Vector Double))+ pbar <- getInput (const id) "p" :: NlpSolver x p g (J p (Vector Double))++ nlpState <- ask+ let solver = isSolver nlpState :: C.NlpSolver+ -- todo: remove this workaround when casadi fixes https://github.com/casadi/casadi/issues/1345+ if size (Proxy :: Proxy g) == 0+ then return (M.zeros, M.uncol M.zeros)+ else liftIO $ do+ jacG <- C.nlpSolver_jacG solver+ C.ioInterfaceFunction_setInput__0 jacG (unJ (v2d x0bar)) "x"+ C.ioInterfaceFunction_setInput__0 jacG (unJ (v2d pbar)) "p"+ C.function_evaluate jacG+ jacG' <- C.ioInterfaceFunction_output__0 jacG "jac"+ g' <- C.ioInterfaceFunction_output__0 jacG "g"+ return (mkM jacG', mkJ g')++evalJacG :: forall x p g . (View x, View g, View p)+ => NlpSolver x p g (M g x DMatrix, J g DMatrix)+evalJacG = do+ (jacG, g) <- evalScaledJacG++ nlpState <- ask+ let scale = isScale nlpState+ return (jacGBarToJacG scale jacG, gbarToG scale g)++evalScaledHessLag :: forall x p g . (View x, View g, View p)+ => NlpSolver x p g (M x x DMatrix)+evalScaledHessLag = do+ x0bar <- getInput (const id) "x0" :: NlpSolver x p g (J x (Vector Double))+ pbar <- getInput (const id) "p" :: NlpSolver x p g (J p (Vector Double))+ lamGbar <- getInput (const id) "lam_g0" :: NlpSolver x p g (J g (Vector Double))++ nlpState <- ask+ let solver = isSolver nlpState :: C.NlpSolver+ liftIO $ do+ hessLag <- C.nlpSolver_hessLag solver+ C.ioInterfaceFunction_setInput__0 hessLag (unJ (v2d x0bar)) "x"+ C.ioInterfaceFunction_setInput__0 hessLag (unJ (v2d pbar)) "p"+ C.ioInterfaceFunction_setInput__0 hessLag (unJ (v2d lamGbar)) "lam_g"+ C.ioInterfaceFunction_setInput__0 hessLag 1.0 "lam_f"+ C.function_evaluate hessLag+ hess' <- C.ioInterfaceFunction_output__0 hessLag "hess"+ return (mkM hess')++-- | only valid at the solution+evalHessLag :: forall x p g . (View x, View g, View p)+ => NlpSolver x p g (M x x DMatrix)+evalHessLag = do+ hess <- evalScaledHessLambdaG+ nlpState <- ask+ let scale = isScale nlpState+ return (hessLagBarToHessLag scale hess)+++evalScaledHessF :: forall x p g . (View x, View g, View p)+ => NlpSolver x p g (M x x DMatrix)+evalScaledHessF = do+ x0bar <- getInput (const id) "x0" :: NlpSolver x p g (J x (Vector Double))+ pbar <- getInput (const id) "p" :: NlpSolver x p g (J p (Vector Double))+ let lamGbar = jfill 0 :: J g (Vector Double)+ nlpState <- ask+ let solver = isSolver nlpState :: C.NlpSolver+ liftIO $ do+ hessLag <- C.nlpSolver_hessLag solver+ C.ioInterfaceFunction_setInput__0 hessLag (unJ (v2d x0bar)) "x"+ C.ioInterfaceFunction_setInput__0 hessLag (unJ (v2d pbar)) "p"+ C.ioInterfaceFunction_setInput__0 hessLag (unJ (v2d lamGbar)) "lam_g"+ C.ioInterfaceFunction_setInput__0 hessLag 1.0 "lam_f"+ C.function_evaluate hessLag+ hess' <- C.ioInterfaceFunction_output__0 hessLag "hess"+ return (mkM hess')++evalHessF :: forall x p g . (View x, View g, View p)+ => NlpSolver x p g (M x x DMatrix)+evalHessF = do+ hess <- evalScaledHessLag+ nlpState <- ask+ let scale = isScale nlpState+ return (hessFBarToHessF scale hess)+++evalScaledHessLambdaG :: forall x p g . (View x, View g, View p)+ => NlpSolver x p g (M x x DMatrix)+evalScaledHessLambdaG = do+ x0bar <- getInput (const id) "x0" :: NlpSolver x p g (J x (Vector Double))+ pbar <- getInput (const id) "p" :: NlpSolver x p g (J p (Vector Double))+ lamGbar <- getInput (const id) "lam_g0" :: NlpSolver x p g (J g (Vector Double))+ nlpState <- ask+ let solver = isSolver nlpState :: C.NlpSolver+ liftIO $ do+ hessLag <- C.nlpSolver_hessLag solver+ C.ioInterfaceFunction_setInput__0 hessLag (unJ (v2d x0bar)) "x"+ C.ioInterfaceFunction_setInput__0 hessLag (unJ (v2d pbar)) "p"+ C.ioInterfaceFunction_setInput__0 hessLag (unJ (v2d lamGbar)) "lam_g"+ C.ioInterfaceFunction_setInput__0 hessLag 0.0 "lam_f"+ C.function_evaluate hessLag+ hess' <- C.ioInterfaceFunction_output__0 hessLag "hess"+ return (mkM hess')+++-- | only valid at solution+evalHessLambdaG :: forall x p g . (View x, View g, View p)+ => NlpSolver x p g (M x x DMatrix)+evalHessLambdaG = do+ hess <- evalScaledHessLambdaG+ nlpState <- ask+ let scale = isScale nlpState+ return (hessLamGBarToHessLamG scale hess)++++evalKKT :: (View x, View p, View g) => NlpSolver x p g (KKT x g)+evalKKT = do+ (gradF,f) <- evalGradF+ (jacG, g) <- evalJacG+ hessF <- evalHessF+ hessLambdaG <- evalHessLambdaG+ hessLag <- evalHessLag+ return $+ KKT+ { kktF = f+ , kktJacG = jacG+ , kktG = g+ , kktGradF = gradF+ , kktHessLag = hessLag+ , kktHessF = hessF+ , kktHessLambdaG = hessLambdaG+ }+++evalScaledKKT :: (View x, View p, View g) => NlpSolver x p g (KKT x g)+evalScaledKKT = do+ (gradF,f) <- evalScaledGradF+ (jacG, g) <- evalScaledJacG+ hessL <- evalScaledHessLag+ hessF <- evalScaledHessF+ hessLambdaG <- evalScaledHessLambdaG+ return $+ KKT+ { kktF = f+ , kktJacG = jacG+ , kktG = g+ , kktGradF = gradF+ , kktHessLag = hessL+ , kktHessF = hessF+ , kktHessLambdaG = hessLambdaG+ }++ setOption :: Gen.GenericC a => String -> a -> NlpSolver x p g () setOption name val = do nlpState <- ask@@ -266,25 +443,25 @@ 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' :: (View x, View g) => NlpSolver x p g (Either String String, NlpOut x g (Vector Double)) solve' = do solveStatus <- solve- nlpOut <- getNlpOut'+ nlpOut <- getNlpOut return (solveStatus, nlpOut) -getNlpOut' :: (View x, View g) => NlpSolver x p g (NlpOut' x g (Vector Double))-getNlpOut' = do+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- }+ let nlpOut = NlpOut { fOpt = fopt+ , xOpt = xopt+ , gOpt = gopt+ , lambdaXOpt = lamXOpt+ , lambdaGOpt = lamGOpt+ } return nlpOut @@ -321,7 +498,7 @@ runNlpSolver :: forall x p g a s . (View x, View p, View g, Symbolic s)- => NlpSolverStuff+ => Solver -> (J x s -> J p s -> (J (JV Id) s, J g s)) -> Maybe (J x (Vector Double)) -> Maybe (J g (Vector Double))@@ -376,7 +553,7 @@ callbackRet <- case callback' of Nothing -> return True Just callback -> do- xval <- fmap (mkJ . ddata . unJ . xbarToX scale . mkJ . CM.dense) $+ xval <- fmap (d2v . xbarToX scale . mkJ . CM.dense) $ C.ioInterfaceFunction_output__2 function' 0 callback xval interrupt <- readIORef intref@@ -408,7 +585,10 @@ mapM_ (\(l,Op.Opt o) -> Op.setOption solver l o) (defaultOptions solverStuff ++ options solverStuff) soInit solver - let nlpState = NlpState { isNx = size (proxy inputsX)+ let proxy :: J f b -> Proxy f+ proxy = const Proxy++ nlpState = NlpState { isNx = size (proxy inputsX) , isNp = size (proxy inputsP) , isNg = size (proxy g) , isSolver = solver@@ -417,183 +597,3 @@ , 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' = sxCatJV (Id obj) :: J (JV Id) 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)----- | 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 =- runNlp solverStuff nlp callback solve'----- | set all inputs, handle scaling, and let the user run a NlpMonad-runNlp ::- (View x, View p, View g, Symbolic a)- => NlpSolverStuff- -> Nlp' x p g a -> Maybe (J x (Vector Double) -> IO Bool)- -> NlpSolver x p g b- -> IO b-runNlp solverStuff nlp callback runMe =- runNlpSolver solverStuff (nlpFG' nlp) (nlpScaleX' nlp) (nlpScaleG' nlp) (nlpScaleF' nlp) callback $ do- let (lbx,ubx) = unzipJ (nlpBX' nlp)- (lbg,ubg) = unzipJ (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 ()- runMe---- | 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 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 (JV Id) 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) = unzipJ (nlpBX' nlp)- (lbg,ubg) = unzipJ (nlpBG' nlp)- p0 = unJ $ nlpP' nlp-- setAlpha :: Double -> NlpSolver (JTuple x p) JNone g ()- setAlpha alpha = do- let p = mkJ $ V.zipWith (+) p0 (V.map (alpha*) (V.zipWith (-) (unJ 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/NlpUtils.hs view
@@ -0,0 +1,273 @@+{-# OPTIONS_GHC -Wall #-}+{-# Language ScopedTypeVariables #-}+{-# Language RankNTypes #-}++module Dyno.NlpUtils+ ( HomotopyParams(..)+ , solveNlpHomotopy+ , solveNlp+ , solveNlpV+ , setNlpInputs+ , runNlp+ ) where++import Control.Applicative ( Applicative(..) )+import qualified Data.Traversable as T+import Control.Monad ( when, void )+import Data.Vector ( Vector )+import qualified Data.Vector as V+import System.IO ( hFlush, stdout )+import Text.Printf ( printf )++import Casadi.SX ( SX )+import qualified Casadi.GenericC as Gen++import Dyno.View.Unsafe.View ( unJ, mkJ )++import Dyno.Vectorize ( Vectorize(..), Id(..) )+import Dyno.View.JV ( JV, catJV, catJV', splitJV, splitJV' )+import Dyno.View.View ( View(..), J, JNone(..), JTuple(..), jfill, unzipJ, fmapJ )+import Dyno.View.Symbolic ( Symbolic )+import Dyno.Nlp ( Nlp(..), NlpOut(..), Bounds )+import Dyno.Solvers ( Solver )+import Dyno.NlpSolver++-- for mapAccumL'+newtype StateL m s a = StateL { runStateL :: s -> m (s, a) }+instance Monad m => Functor (StateL m s) where+ fmap f (StateL k) = StateL $ \ s -> do+ (s', v) <- k s+ return (s', f v)+instance Monad m => Applicative (StateL m s) where+ pure x = StateL (\s -> return (s, x))+ StateL kf <*> StateL kv = StateL $ \ s -> do+ (s', f) <- kf s+ (s'', v) <- kv s'+ return (s'', f v)++-- mapAccumL with monads+mapAccumL' :: (T.Traversable t, Monad m) => (a -> b -> m (a, c)) -> a -> t b -> m (a, t c)+mapAccumL' f s t = runStateL (T.traverse (StateL . flip f) t) s++data HomotopyParams =+ HomotopyParams+ { reduction :: Double+ , increase :: Double+ , iterIncrease :: Int+ , iterDecrease :: Int+ }++-- | solve a homotopy nlp+solveNlpHomotopy ::+ forall x p g t a .+ (View x, View p, View g, T.Traversable t, Symbolic a)+ => Double -> HomotopyParams+ -> Solver+ -> Nlp x p g a -> t (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 (t (NlpOut (JTuple x p) g (Vector Double)))+solveNlpHomotopy userStep hp+ solverStuff nlp pFs callback callbackP = do+ when ((reduction hp) >= 1) $ error $ "homotopy reduction factor " ++ show (reduction hp) ++ " >= 1"+ when ((increase hp) <= 1) $ error $ "homotopy increase factor " ++ show (increase hp) ++ " <= 1"+ let fg :: J (JTuple x p) a -> J JNone a -> (J (JV Id) 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) = unzipJ (nlpBX nlp)+ (lbg,ubg) = unzipJ (nlpBG nlp)+ p0 = nlpP nlp++ setBnds p' = do+ 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+ setBnds p0+ 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)++ let solveOneStage ::+ (Int, Double, J p (Vector Double))+ -> J p (Vector Double)+ -> NlpSolver (JTuple x p) JNone g+ ((Int, Double, J p (Vector Double)), NlpOut (JTuple x p) g (Vector Double))+ solveOneStage (stage, step0, p0') pF' = do+ ((msg, ret'), stepF) <- tryStep 0 0 step0+ ret <- case msg of+ Left x -> error x+ Right _ -> return ret'+ return ((stage + 1, stepF, pF'), ret)+ where+ setAlpha :: Double -> NlpSolver (JTuple x p) JNone g ()+ setAlpha alpha = do+ let p0'' = unJ p0'+ let p = mkJ $ V.zipWith (+) p0'' (V.map (alpha*) (V.zipWith (-) (unJ pF') p0''))+ setBnds p++ tryStep :: Int -> Double -> Double+ -> NlpSolver (JTuple x p) JNone g+ ((Either String String, NlpOut (JTuple x p) g (Vector Double)), Double)+ tryStep majorIter alpha0 step+ | step < 1e-12 = do _no <- getNlpOut+ error "step size too small"+-- return (Left "step size too small", no)+ | otherwise = do+ liftIO $ printf "%3d %4d, alpha: %.2e, step: %.2e " stage 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 hp)*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+ getX >>= setX0+ getLamX >>= setLamX0+ getLamG >>= setLamG0++ if alphaIsOne+ then return (ret, step)+ else do let nextStep+ | iters < (iterIncrease hp) = step*(increase hp)+ | iters < (iterDecrease hp) = step+ | otherwise = step*(reduction hp)+ tryStep (majorIter + 1) alphaTrial nextStep++ (_, ret) <- mapAccumL' solveOneStage (0, userStep, p0) pFs+ liftIO $ putStrLn "homotopy successful"+ return ret+++-- | convenience function to solve a simple Nlp+-- .+-- For better performance and more options, use the View-based interfaces instead+solveNlpV :: forall x g .+ (Vectorize x, Vectorize g)+ => Solver+ -> (forall a . Floating a => x a -> (a, g a))+ -> x Bounds+ -> g Bounds+ -> x Double+ -> Maybe (x Double -> IO Bool)+ -> IO (Either String (Double, x Double))+solveNlpV solverStuff fg bx bg x0 cb = do+ let nlp :: Nlp (JV x) JNone (JV g) SX+ nlp = Nlp { nlpFG = \x' _ -> let _ = x' :: J (JV x) SX+ x = splitJV' x' :: x (J (JV Id) SX)+ (obj,g) = fg x :: (J (JV Id) SX, g (J (JV Id) SX))+ --obj' = sxCatJV (Id obj) :: J (JV Id) SX+ --g' = sxCatJV g :: J (JV g) SX+ in (obj, catJV' g)+ , nlpBX = catJV bx -- mkJ $ vectorize (nlpBX nlp) :: J (JV x) (V.Vector Bounds)+ , nlpBG = catJV bg -- mkJ $ vectorize (nlpBG nlp) :: J (JV g) (V.Vector Bounds)+ , nlpX0 = catJV x0 -- mkJ $ vectorize (nlpX0 nlp) :: J (JV x) (V.Vector Double)+ , nlpP = cat JNone -- mkJ $ vectorize (nlpP nlp) :: J (JV p) (V.Vector Double)+ , nlpLamX0 = Nothing --fmap (mkJ . vectorize) (nlpLamX0 nlp)+ -- :: Maybe (J (JV x) (V.Vector Double))+ , nlpLamG0 = Nothing -- fmap (mkJ . vectorize) (nlpLamG0 nlp)+ -- :: Maybe (J (JV g) (V.Vector Double))+ , nlpScaleF = Nothing -- nlpScaleF nlp+ , nlpScaleX = Nothing -- fmap (mkJ . vectorize) (nlpScaleX nlp)+ -- :: Maybe (J (JV x) (V.Vector Double))+ , nlpScaleG = Nothing -- fmap (mkJ . vectorize) (nlpScaleG nlp)+ -- :: Maybe (J (JV g) (V.Vector Double))+ }++ callback :: Maybe (J (JV x) (Vector Double) -> IO Bool)+ callback = fmap (. splitJV) cb++ (r0, r1) <- solveNlp solverStuff nlp callback+ return $ case r0 of+ Left m -> Left m+ Right _ -> Right $ (unId (splitJV (fOpt r1)), splitJV (xOpt r1))++-- 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)+++-- | convenience function to solve a pure Nlp+solveNlp ::+ (View x, View p, View g, Symbolic a)+ => Solver+ -> 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 =+ runNlp solverStuff nlp callback solve'+++-- | set all inputs+setNlpInputs :: (View x, View p, View g, Symbolic a) => Nlp x p g a -> NlpSolver x p g ()+setNlpInputs nlp = do+ let (lbx,ubx) = unzipJ (nlpBX nlp)+ (lbg,ubg) = unzipJ (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 ()+++-- | set all inputs, handle scaling, and let the user run a NlpMonad+runNlp ::+ (View x, View p, View g, Symbolic a)+ => Solver+ -> Nlp x p g a -> Maybe (J x (Vector Double) -> IO Bool)+ -> NlpSolver x p g b+ -> IO b+runNlp solverStuff nlp callback runMe =+ runNlpSolver solverStuff (nlpFG nlp) (nlpScaleX nlp) (nlpScaleG nlp) (nlpScaleF nlp) callback $ do+ setNlpInputs nlp+ runMe
src/Dyno/Ocp.hs view
@@ -8,10 +8,8 @@ , OcpPhaseClass(..) ) where -import Data.Default ( Default(..) ) import Data.Vector ( Vector ) -import Dyno.Vectorize ( Vectorize, None(..), fill ) import Dyno.View.JV ( JV ) import Dyno.View.View ( J ) import Dyno.View.Cov ( Cov )@@ -33,16 +31,18 @@ type O a :: * -> * type C a :: * -> * type H a :: * -> *+ type Q 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+instance OcpPhaseClass (OcpPhase x z u p r o c h q) where+ type X (OcpPhase x z u p r o c h q) = x+ type Z (OcpPhase x z u p r o c h q) = z+ type U (OcpPhase x z u p r o c h q) = u+ type P (OcpPhase x z u p r o c h q) = p+ type R (OcpPhase x z u p r o c h q) = r+ type O (OcpPhase x z u p r o c h q) = o+ type C (OcpPhase x z u p r o c h q) = c+ type H (OcpPhase x z u p r o c h q) = h+ type Q (OcpPhase x z u p r o c h q) = q -- | One stage of an optimal control problem, solvable as a stand-alone optimal control problem. --@@ -68,23 +68,25 @@ -- -- boundary conditions: ----- > c(x(0), x(T)) == 0+-- > c(x(0), x(T), q(T), p) == 0 -- -- perhaps this should be: -- -- > c(x(0), 0, x(T), T) == 0-data OcpPhase x z u p r o c h =+data OcpPhase x z u p r o c h q = OcpPhase- { -- | the Mayer term @Jm(T, x(0), x(T))@- ocpMayer :: Sxe -> x Sxe -> x Sxe -> Sxe+ { -- | the Mayer term @Jm(T, x(0), x(T), q(T), p)@+ ocpMayer :: Sxe -> x Sxe -> x Sxe -> q Sxe -> p 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+ -- | derivative of quadrature state @q(x(t),z(t),u(t),p,o,t,T)@+ , ocpQuadratures :: x Sxe -> z Sxe -> u Sxe -> p Sxe -> o Sxe -> Sxe -> Sxe -> q 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 boundary conditions @clb <= c(x(0), x(T), q(T), T) <= cub@+ , ocpBc :: x Sxe -> x Sxe -> q Sxe -> p Sxe -> 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@@@ -112,32 +114,6 @@ , 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
− src/Dyno/Server/Accessors.hs
@@ -1,178 +0,0 @@-{-# 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- ( Lookup(..)- , AccessorTree(..)- , accessors- , flatten- ) where--import GHC.Generics--import Data.List ( intercalate )-import qualified Linear--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
@@ -1,359 +0,0 @@-{-# 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
@@ -1,75 +0,0 @@-{-# 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
@@ -1,47 +0,0 @@-{-# 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
@@ -1,171 +0,0 @@-{-# OPTIONS_GHC -Wall #-}--module Dyno.Server.Server- ( newChannel- , runPlotter- , Channel- ) where--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 ( CollTrajMeta(..), DynPlotPoints )--newChannel :: String -> IO (Channel, (DynPlotPoints Double, CollTrajMeta) -> IO ())-newChannel name = do- time0 <- getCurrentTime-- msgStore <- Gtk.listStoreNew []- counter <- IORef.newIORef 0-- let newMessage :: (DynPlotPoints 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 = 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
@@ -1,14 +1,27 @@ {-# OPTIONS_GHC -Wall #-} -module Dyno.Solvers ( NlpSolverStuff(..), ipoptSolver, snoptSolver ) where+module Dyno.Solvers ( Solver(..)+ , Opt(..)+ , ipoptSolver, snoptSolver, worhpSolver+ ) where ---import qualified Data.Vector as V+import Casadi.Core.Classes.Function ( Function )+import Casadi.Option ( Opt(..) ) -import Dyno.NlpSolver ( NlpSolverStuff(..), Opt(..) )+data Solver =+ Solver+ { solverName :: String+ , defaultOptions :: [(String,Opt)]+ , options :: [(String,Opt)]+ , solverInterruptCode :: Int+ , successCodes :: [String]+ , functionOptions :: [(String, Opt)]+ , functionCall :: Function -> IO ()+ } -snoptSolver :: NlpSolverStuff+snoptSolver :: Solver snoptSolver =- NlpSolverStuff+ Solver { solverName = "snopt" , defaultOptions = [ -- ("_iprint", Opt (0::Int)) -- , ("_isumm", Opt (6::Int))@@ -29,9 +42,9 @@ , functionCall = const (return ()) } -ipoptSolver :: NlpSolverStuff+ipoptSolver :: Solver ipoptSolver =- NlpSolverStuff+ Solver { solverName = "ipopt" , defaultOptions = [ ("max_iter", Opt (3000 :: Int)) , ("tol", Opt (1e-9 :: Double))@@ -50,3 +63,20 @@ , functionOptions = [] , functionCall = const (return ()) }++worhpSolver :: Solver+worhpSolver =+ Solver+ { solverName = "worhp"+ , defaultOptions = []+ , options = []+ , solverInterruptCode = 1+ , successCodes = [ "OptimalSolution"+ , "LowPassFilterOptimal"+ ]+ , functionOptions = []+ , functionCall = const (return ())+ }+++
src/Dyno/TypeVecs.hs view
@@ -50,7 +50,8 @@ import Data.Traversable ( Traversable ) import qualified Data.Traversable as T import qualified Data.Vector as V-import Data.Serialize ( Serialize(..) )+import Data.Vector.Binary () -- instances+import Data.Binary ( Binary(..) ) import Linear.Vector import Linear.V ( Dim(..) ) import Data.Proxy@@ -62,9 +63,9 @@ -- length-indexed vectors using phantom types newtype Vec (n :: k) a = MkVec (V.Vector a) deriving (Eq, Ord, Functor, Traversable, Foldable, Generic, Generic1)-instance (Dim n, Serialize a) => Serialize (Vec n a) where- put = put . V.toList . unVec- get = fmap (mkVec . V.fromList) get+instance (Dim n, Binary a) => Binary (Vec n a) where+ put = put . unVec+ get = fmap mkVec get instance Dim n => Distributive (Vec n) where distribute f = mkVec $ V.generate (reflectDim (Proxy :: Proxy n))
src/Dyno/Vectorize.hs view
@@ -35,7 +35,7 @@ import SpatialMath ( Euler ) import SpatialMathT ( V3T, Rot ) -import Dyno.Server.Accessors+import Accessors ( Lookup ) -- | a length-0 vectorizable type data None a = None
src/Dyno/View/JV.hs view
@@ -1,7 +1,5 @@ {-# OPTIONS_GHC -Wall #-} {-# LANGUAGE ScopedTypeVariables #-}-{-# LANGUAGE FlexibleInstances #-}-{-# LANGUAGE FlexibleContexts #-} {-# LANGUAGE DeriveFunctor #-} {-# LANGUAGE DeriveGeneric #-} {-# LANGUAGE InstanceSigs #-}@@ -26,7 +24,6 @@ import Dyno.View.View ( View(..), J ) import Dyno.View.Viewable ( Viewable(..) ) 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, Generic1)@@ -41,9 +38,6 @@ 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
src/Dyno/View/JVec.hs view
@@ -11,21 +11,18 @@ , reifyJVec ) where -import qualified Data.Foldable as F import qualified Data.Sequence as Seq import Data.Proxy ( Proxy(..) ) import Linear.V ( Dim(..) ) import Data.Vector ( Vector ) import qualified Data.Vector as V-import Data.Serialize ( Serialize(..) ) import Dyno.View.Unsafe.View ( mkJ, unJ ) -import Dyno.TypeVecs ( Vec, unVec, mkVec, mkVec', reifyVector )+import Dyno.TypeVecs ( Vec, unVec, mkVec, reifyVector ) import Dyno.View.Viewable ( Viewable(..) ) import Dyno.View.View ( View(..), J ) - -- | vectors in View newtype JVec (n :: k) f a = JVec { unJVec :: Vec n (J f a) } deriving ( Show, Eq ) instance (Dim n, View f) => View (JVec n f) where@@ -43,9 +40,6 @@ 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
src/Dyno/View/M.hs view
@@ -7,12 +7,14 @@ module Dyno.View.M ( M+ , sparse, dense , mm , ms , vs , trans , zeros , eye+ , diag , ones , countUp , vsplit@@ -25,6 +27,12 @@ , hcat' , hsplitTup , hsplitTrip+ , hcatTup+ , hcatTrip+ , vsplitTup+ , vsplitTrip+ , vcatTup+ , vcatTrip , row , col , unrow@@ -33,14 +41,18 @@ , toHMat , fromHMat , fromHMat'+ -- * hmatrix wrappers+ , rcond+ , rank ) where import qualified Data.Vector as V import Data.Proxy ( Proxy(..) ) import Casadi.CMatrix ( CMatrix )-import Casadi.DMatrix ( DMatrix, ddata )+import Casadi.DMatrix ( DMatrix, ddata, dsparsify ) import qualified Casadi.CMatrix as CM-import qualified Data.Packed.Matrix as Mat+import qualified Data.Packed.Matrix as HMat+import qualified Numeric.LinearAlgebra.HMatrix as HMat import Dyno.View.Unsafe.View ( unJ, mkJ ) import Dyno.View.Unsafe.M ( M(UnsafeM), mkM, mkM', unM )@@ -53,6 +65,13 @@ import Dyno.View.Viewable ( Viewable ) +-- todo: generalize once casadi 2.3 is ready+sparse :: (View f, View g) => M f g DMatrix -> M f g DMatrix+sparse (UnsafeM m) = mkM (dsparsify m)++dense :: (View f, View g, CMatrix a) => M f g a -> M f g a+dense (UnsafeM m) = mkM (CM.dense m)+ mm :: (View f, View h, CMatrix a) => M f g a -> M g h a -> M f h a mm (UnsafeM m0) (UnsafeM m1) = mkM (CM.mm m0 m1) @@ -102,6 +121,12 @@ nh = size (Proxy :: Proxy h) ncs = V.fromList [0,ng,ng+nh] +hcatTup ::+ forall f g h a .+ (View f, View g, View h, CMatrix a)+ => M f g a -> M f h a -> M f (JTuple g h) a+hcatTup (UnsafeM x) (UnsafeM y) = mkM (CM.horzcat (V.fromList [x,y]))+ hsplitTrip :: forall f g h j a . (View f, View g, View h, View j, CMatrix a)@@ -109,13 +134,19 @@ hsplitTrip (UnsafeM x) = case V.toList (CM.horzsplit x ncs) of [g,h,j] -> (mkM g, mkM h, mkM j)- n -> error $ "hsplitTup made a bad split with length " ++ show (length n)+ n -> error $ "hsplitTrip made a bad split with length " ++ show (length n) where ng = size (Proxy :: Proxy g) nh = size (Proxy :: Proxy h) nj = size (Proxy :: Proxy j) ncs = V.fromList [0,ng,ng+nh,ng+nh+nj] +hcatTrip ::+ forall f g1 g2 g3 a .+ (View f, View g1, View g2, View g3, CMatrix a)+ => M f g1 a -> M f g2 a -> M f g3 a -> M f (JTriple g1 g2 g3) a+hcatTrip (UnsafeM x) (UnsafeM y) (UnsafeM z) = mkM (CM.horzcat (V.fromList [x,y,z]))+ hcat :: forall f g a . (View f, Vectorize g, CMatrix a)@@ -141,6 +172,45 @@ nr = size (Proxy :: Proxy f) nrs = V.fromList [0,nr..n*nr] +vsplitTup ::+ forall f g h a .+ (View f, View g, View h, CMatrix a)+ => M (JTuple f g) h a -> (M f h a, M g h a)+vsplitTup (UnsafeM x) =+ case V.toList (CM.vertsplit x ncs) of+ [f,g] -> (mkM f, mkM g)+ n -> error $ "vsplitTup made a bad split with length " ++ show (length n)+ where+ nf = size (Proxy :: Proxy f)+ ng = size (Proxy :: Proxy g)+ ncs = V.fromList [0,nf,nf+ng]++vcatTup ::+ forall f g h a .+ (View f, View g, View h, CMatrix a)+ => M f h a -> M g h a -> M (JTuple f g) h a+vcatTup (UnsafeM x) (UnsafeM y) = mkM (CM.vertcat (V.fromList [x,y]))++vsplitTrip ::+ forall f g h j a .+ (View f, View g, View h, View j, CMatrix a)+ => M (JTriple f g h) j a -> (M f j a, M g j a, M h j a)+vsplitTrip (UnsafeM x) =+ case V.toList (CM.vertsplit x ncs) of+ [f,g,h] -> (mkM f, mkM g, mkM h)+ n -> error $ "vsplitTrip made a bad split with length " ++ show (length n)+ where+ nf = size (Proxy :: Proxy f)+ ng = size (Proxy :: Proxy g)+ nh = size (Proxy :: Proxy h)+ ncs = V.fromList [0,nf,nf+ng,nf+ng+nh]++vcatTrip ::+ forall f1 f2 f3 h a .+ (View f1, View f2, View f3, View h, CMatrix a)+ => M f1 h a -> M f2 h a -> M f3 h a -> M (JTriple f1 f2 f3) h a+vcatTrip (UnsafeM x) (UnsafeM y) (UnsafeM z) = mkM (CM.vertcat (V.fromList [x,y,z]))+ hcat' :: forall f g n a . (View f, View g, Dim n, CMatrix a)@@ -173,6 +243,11 @@ z = CM.eye n n = size (Proxy :: Proxy f) +diag :: forall f a . (View f, Viewable a, CMatrix a) => J f a -> M f f a+diag x = mkM z+ where+ z = CM.diag (unJ x)+ ones :: forall f g a . (View f, View g, CMatrix a) => M f g a ones = mkM z where@@ -209,18 +284,23 @@ 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)+ => M n m DMatrix -> HMat.Matrix Double+toHMat (UnsafeM d) = HMat.trans $ (m HMat.>< n) (V.toList v) where v = ddata (CM.dense d) n = size (Proxy :: Proxy n) m = size (Proxy :: Proxy m) -fromHMat :: (View g, View f) => Mat.Matrix Double -> M f g DMatrix+fromHMat :: (View f, View g) => HMat.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 . CM.fromDVector . V.fromList) . Mat.toLists+fromHMat' :: (View f, View g) => HMat.Matrix Double -> Either String (M f g DMatrix)+fromHMat' = mkM' . CM.vertcat . V.fromList . fmap (CM.trans . CM.fromDVector . V.fromList) . HMat.toLists +rcond :: (View f, View g) => M f g DMatrix -> Double+rcond = HMat.rcond . toHMat++rank :: (View f, View g) => M f g DMatrix -> Int+rank = HMat.rank . toHMat
src/Dyno/View/Unsafe/M.hs view
@@ -1,6 +1,7 @@ {-# OPTIONS_GHC -Wall #-} {-# LANGUAGE ScopedTypeVariables #-} {-# LANGUAGE KindSignatures #-}+{-# LANGUAGE FlexibleInstances #-} {-# LANGUAGE DeriveFunctor #-} {-# LANGUAGE DeriveGeneric #-} @@ -14,11 +15,13 @@ import GHC.Generics ( Generic ) import Data.Proxy+import Data.Binary ( Binary(..) ) import qualified Data.Foldable as F import qualified Data.Vector as V import Data.Vector ( Vector ) import Casadi.Overloading ( Fmod(..), ArcTan2(..), SymOrd(..) )+import Casadi.DMatrix ( DMatrix ) import Casadi.CMatrix ( CMatrix ) import qualified Casadi.CMatrix as CM @@ -26,6 +29,10 @@ newtype M (f :: * -> *) (g :: * -> *) (a :: *) = UnsafeM { unM :: a } deriving (Eq, Functor, Generic)++instance (View f, View g) => Binary (M f g DMatrix) where+ put = put . unM+ get = fmap mkM get instance Show a => Show (M f g a) where showsPrec p (UnsafeM x) = showsPrec p x
src/Dyno/View/Unsafe/View.hs view
@@ -20,9 +20,8 @@ import qualified Data.Foldable as F import qualified Data.Sequence as Seq import Data.Proxy ( Proxy(..) )-import Data.Vector ( Vector ) import qualified Data.Vector as V-import Data.Serialize ( Serialize(..) )+import Data.Binary ( Binary(..) ) import qualified Casadi.CMatrix as CM @@ -30,9 +29,9 @@ newtype J (f :: * -> *) (a :: *) = UnsafeJ { unsafeUnJ :: a } deriving (Eq, Generic) -instance (Serialize a, View f) => Serialize (J f (Vector a)) where- put = put . V.toList . unJ- get = fmap (mkJ . V.fromList) get+instance (View f, Binary a, Viewable a) => Binary (J f a) where+ put = put . unJ+ get = fmap mkJ get instance Show a => Show (J f a) where showsPrec p (UnsafeJ x) = showsPrec p x
+ tests/IntegrationTests.hs view
@@ -0,0 +1,196 @@+{-# OPTIONS_GHC -Wall #-}+{-# Language ScopedTypeVariables #-}+{-# Language RankNTypes #-}+{-# Language FlexibleInstances #-}+{-# Language DeriveFunctor #-}+{-# Language DeriveGeneric #-}+{-# Language DataKinds #-}+{-# Language PolyKinds #-}+{-# Language GADTs #-}+{-# Language DeriveGeneric #-}+{-# Language FlexibleInstances #-}+{-# Language PolyKinds #-}++module IntegrationTests+ ( integrationTests+ ) where++import GHC.Generics ( Generic, Generic1 )++import Data.Proxy ( Proxy(..) )+import Data.Vector ( Vector )+import qualified Data.Vector as V+import qualified Data.Vector.Storable as SV+import qualified Numeric.GSL.ODE as ODE+import qualified Numeric.LinearAlgebra.Data as D+import qualified Test.HUnit.Base as HUnit+import Test.Framework ( Test, testGroup )+import Test.Framework.Providers.HUnit ( testCase )++import Dyno.Vectorize ( Vectorize(..), None(..), fill )+import Dyno.View.View ( View(..), J )+import Dyno.View.JV ( splitJV )+import Dyno.TypeVecs ( Dim )+import Dyno.Solvers+import Dyno.Nlp ( Nlp(..), NlpOut(..) )+import Dyno.NlpUtils++import Dyno.Ocp+import Dyno.DirectCollocation.Formulate+import Dyno.DirectCollocation.Types ( CollTraj(..) )+import Dyno.DirectCollocation.Quadratures ( QuadratureRoots(..) )+++data PendX a = PendX a a deriving (Functor, Generic, Generic1, Show)+data PendP a = PendP a deriving (Functor, Generic, Generic1, Show)++instance Vectorize PendX+instance Vectorize PendP++over :: Vectorize f => (a -> a -> a) -> f a -> f a -> f a+over f x y = devectorize $ V.zipWith f (vectorize x) (vectorize y)++minus :: (Vectorize f, Num a) => f a -> f a -> f a+minus = over (-)++--divv :: (Vectorize f, Fractional a) => f a -> f a -> f a+--divv = over (/)++toOcpPhase ::+ (Vectorize x, Vectorize p)+ => (forall a . Floating a => x a -> p a -> a -> x a)+ -> x Double -> p Double -> Double+ -> OcpPhase x None None p x None x None None+toOcpPhase ode x0 p tf =+ OcpPhase+ { ocpMayer = \_ _ _ _ _ -> 0+ , ocpLagrange = \_ _ _ _ _ _ _ -> 0+ , ocpDae = \x' x _ _ pp t -> ((ode x pp t) `minus` x', None)+ , ocpQuadratures = \_ _ _ _ _ _ _ -> None+ , ocpBc = \x0' _ _ _ _ -> x0'+ , ocpPathC = \_ _ _ _ _ _ -> None+ , ocpPathCBnds = None+ , ocpBcBnds = fmap (\x -> (Just x, Just x)) x0+ , ocpXbnd = fill (Nothing, Nothing)+ , ocpUbnd = None+ , ocpZbnd = None+ , ocpPbnd = fmap (\x -> (Just x, Just x)) p+ , ocpTbnd = (Just tf, Just tf)+ , ocpObjScale = Nothing+ , ocpTScale = Nothing+ , ocpXScale = Nothing+ , ocpZScale = Nothing+ , ocpUScale = Nothing+ , ocpPScale = Nothing+ , ocpResidualScale = Nothing+ , ocpBcScale = Nothing+ , ocpPathCScale = Nothing+ }+++++pendOde :: Floating a => PendX a -> PendP a -> a -> PendX a+pendOde (PendX theta omega) (PendP mass) t = PendX omega ((9.8 * sin theta + force) / mass)+ where+ force = 0.3 * sin t++solver :: Solver+solver = ipoptSolver { options = [ ("expand", Opt True)+ --, ("linear_solver", Opt "ma86")+ --, ("ma86_order", Opt "metis")+ , ("tol", Opt (1e-11 :: Double))+ ] }++pendX0 :: PendX Double+pendX0 = PendX 0 0.2++pendP :: PendP Double+pendP = PendP 2.3+++rk45 :: (Vectorize x, Vectorize p)+ => (x Double -> p Double -> Double -> x Double)+ -> Double -> p Double -> x Double -> x Double+rk45 f h p x0 = devectorize $ sv $ last sol+ where+ vs :: V.Vector Double -> SV.Vector Double+ vs = SV.fromList . V.toList+ sv :: SV.Vector Double -> V.Vector Double+ sv = V.fromList . SV.toList++ sol = D.toRows $+ ODE.odeSolveV+ ODE.RKf45+ h 1e-10 1e-8 f'+ (vs (vectorize x0))+ (SV.fromList [0.0, h])+ f' :: Double -> SV.Vector Double -> SV.Vector Double+ f' t x = vs $ vectorize $ f (devectorize (sv x)) p t++toXf :: (Vectorize x, Vectorize z, Vectorize u, Vectorize p, Dim n, Dim deg)+ => J (CollTraj x z u p n deg) (Vector Double)-> x Double+toXf traj = splitJV xf+ where+ CollTraj _ _ _ xf = split traj+++integrationTests :: Test+integrationTests =+ testGroup "integration tests"+ [ testCase "pendulum" $ compareIntegration (Proxy :: Proxy 80) (Proxy :: Proxy 3) pendOde pendX0 pendP tf+ ]+ where+ tf = 3.0+++compareIntegration ::+ forall x p n deg+ . (Vectorize x, Vectorize p, Dim n, Dim deg)+ => Proxy n -> Proxy deg+ -> (forall a . Floating a => x a -> p a -> a -> x a)+ -> x Double -> p Double -> Double -> HUnit.Assertion+compareIntegration pn pdeg ode x0 p tf = HUnit.assert $ do+ xL' <- runIntegration pn pdeg Legendre ode x0 p tf+ xR' <- runIntegration pn pdeg Radau ode x0 p tf+ let xGsl = rk45 ode tf p x0+ worstErr :: x Double -> x Double -> Double+ worstErr x y = V.maximum $ V.map abs $ vectorize $ x `minus` y++ ret :: HUnit.Assertion+ ret = case (xL', xR') of+ (Left ml, Left mr) -> HUnit.assertString $ "legendre and radau solve failed with: "+ ++ show ml ++ ", " ++ show mr+ (Left ml, _) -> HUnit.assertString $ "legendre solve failed with: " ++ show ml+ (_, Left mr) -> HUnit.assertString $ "legendre solve failed with: " ++ show mr+ (Right xL, Right xR) ->+ case ( 1e-6 >= worstErr xL xGsl+ , 1e-6 >= worstErr xR xGsl+ ) of+ ( True, True) -> HUnit.assert True+ (False, False) -> HUnit.assertString $ "legendre and radau have insufficient accuracy: "+ ++ show (worstErr xL xGsl, worstErr xR xGsl)+ (False, True) -> HUnit.assertString $ "legendre has insufficient accuracy: "+ ++ show (worstErr xL xGsl)+ ( True, False) -> HUnit.assertString $ "radau has insufficient accuracy failed: "+ ++ show (worstErr xR xGsl)+ return ret :: IO HUnit.Assertion+++runIntegration ::+ forall x p n deg+ . (Vectorize x, Vectorize p, Dim n, Dim deg)+ => Proxy n+ -> Proxy deg+ -> QuadratureRoots+ -> (forall a . Floating a => x a -> p a -> a -> x a)+ -> x Double -> p Double -> Double -> IO (Either String (x Double))+runIntegration _ _ roots ode x0 p tf = do+ cp <- makeCollProblem roots (toOcpPhase ode x0 p tf)+ let guess :: CollTraj x None None p n deg (Vector Double)+ guess = makeGuessSim roots tf x0 (\x _ -> ode x p 0) (\_ _ -> None) p+ nlp = (cpNlp cp) { nlpX0 = cat guess }+ (msg, opt') <- solveNlp solver nlp Nothing+ return $ case msg of+ Left m -> Left m+ Right _ -> Right (toXf (xOpt opt'))
tests/NewUnitTests.hs view
@@ -3,18 +3,22 @@ module Main ( main ) where import Data.Monoid ( mempty )-import Test.Framework ( ColorMode(..), RunnerOptions'(..), TestOptions'(..), defaultMainWithOpts )+import Test.Framework ( Test, ColorMode(..), RunnerOptions'(..), TestOptions'(..)+ , defaultMainWithOpts ) import VectorizeTests ( vectorizeTests ) import ViewTests ( viewTests )+import IntegrationTests ( integrationTests ) main :: IO ()-main = do- defaultMainWithOpts- [ vectorizeTests- , viewTests- ]- opts+main = defaultMainWithOpts tests opts++tests :: [Test]+tests =+ [ integrationTests+ , vectorizeTests+ , viewTests+ ] opts :: RunnerOptions' Maybe opts = mempty { ropt_color_mode = Just ColorAlways
tests/ViewTests.hs view
@@ -14,6 +14,7 @@ import GHC.Generics ( Generic1 ) import Data.Proxy ( Proxy(..) )+import Data.Binary ( encode, decodeOrFail ) import qualified Data.Traversable as T import qualified Data.Packed.Matrix as Mat import qualified Numeric.LinearAlgebra ( ) -- for Eq Matrix@@ -360,6 +361,85 @@ m2 = toMat m1 :: M f f DMatrix return $ beEqual m0 m2 +prop_serializeDeserialize :: Test+prop_serializeDeserialize =+ testProperty "(M f g DMatrix): deserialize . serialize" $+ \(Views {vwProxy = p1}) (Views {vwProxy = p2}) -> test p1 p2+ where+ test :: forall f g . (View f, View g) => Proxy f -> Proxy g -> Gen Property+ test _ _ = do+ m0 <- arbitrary :: Gen (M f g DMatrix)+ let m1 = encode m0+ return $+ case decodeOrFail m1 of+ Left (_,_,msg) -> counterexample ("deserialization failure " ++ show msg) False+ Right (_,_,m2) -> beEqual m0 m2++prop_vsplitTup :: Test+prop_vsplitTup =+ testProperty "vcatTup . vsplitTup" $+ \(Views {vwProxy = p1}) (Views {vwProxy = p2}) (Views {vwProxy = p3}) (CMatrices {cmProxy = p4})+ -> test p1 p2 p3 p4+ where+ test :: forall f g h a+ . (View f, View g, View h, CMatrix a, MyEq a)+ => Proxy f -> Proxy g -> Proxy h -> Proxy a+ -> Gen Property+ test _ _ _ _ = do+ m0 <- arbitrary :: Gen (M (JTuple f g) h a)+ let (mx,my) = vsplitTup m0+ m1 = vcatTup mx my+ return (beEqual m0 m1)++prop_hsplitTup :: Test+prop_hsplitTup =+ testProperty "hcatTup . hsplitTup" $+ \(Views {vwProxy = p1}) (Views {vwProxy = p2}) (Views {vwProxy = p3}) (CMatrices {cmProxy = p4})+ -> test p1 p2 p3 p4+ where+ test :: forall f g h a+ . (View f, View g, View h, CMatrix a, MyEq a)+ => Proxy f -> Proxy g -> Proxy h -> Proxy a+ -> Gen Property+ test _ _ _ _ = do+ m0 <- arbitrary :: Gen (M f (JTuple g h) a)+ let (mx,my) = hsplitTup m0+ m1 = hcatTup mx my+ return (beEqual m0 m1)++prop_vsplitTrip :: Test+prop_vsplitTrip =+ testProperty "vcatTrip . vsplitTrip" $+ \(Views {vwProxy = p1}) (Views {vwProxy = p2}) (Views {vwProxy = p3}) (Views {vwProxy = p4}) (CMatrices {cmProxy = p5})+ -> test p1 p2 p3 p4 p5+ where+ test :: forall f1 f2 f3 g a+ . (View f1, View f2, View f3, View g, CMatrix a, MyEq a)+ => Proxy f1 -> Proxy f2 -> Proxy f3 -> Proxy g -> Proxy a+ -> Gen Property+ test _ _ _ _ _ = do+ m0 <- arbitrary :: Gen (M (JTriple f1 f2 f3) g a)+ let (mx,my,mz) = vsplitTrip m0+ m1 = vcatTrip mx my mz+ return (beEqual m0 m1)++prop_hsplitTrip :: Test+prop_hsplitTrip =+ testProperty "hcatTrip . hsplitTrip" $+ \(Views {vwProxy = p1}) (Views {vwProxy = p2}) (Views {vwProxy = p3}) (Views {vwProxy = p4}) (CMatrices {cmProxy = p5})+ -> test p1 p2 p3 p4 p5+ where+ test :: forall f g1 g2 g3 a+ . (View f, View g1, View g2, View g3, CMatrix a, MyEq a)+ => Proxy f -> Proxy g1 -> Proxy g2 -> Proxy g3 -> Proxy a+ -> Gen Property+ test _ _ _ _ _ = do+ m0 <- arbitrary :: Gen (M f (JTriple g1 g2 g3) a)+ let (mx,my,mz) = hsplitTrip m0+ m1 = hcatTrip mx my mz+ return (beEqual m0 m1)++ viewTests :: Test viewTests = testGroup "view tests"@@ -372,4 +452,9 @@ , prop_fromToHMat , prop_covFromToMat , prop_covToFromMat+ , prop_serializeDeserialize+ , prop_vsplitTup+ , prop_hsplitTup+ , prop_vsplitTrip+ , prop_hsplitTrip ]