dynobud 1.3.0.0 → 1.4.0.0
raw patch · 47 files changed
+1743/−1493 lines, 47 filesdep +cerealdep ~casadi-bindingsdep ~casadi-bindings-coredep ~containersnew-component:exe:beginner-qpnew-component:exe:dae-pendulumnew-component:exe:rocketnew-component:exe:spring
Dependencies added: cereal
Dependency ranges changed: casadi-bindings, casadi-bindings-core, containers, generic-accessors, mtl
Files
- dynobud.cabal +46/−33
- examples/DaeColl.hs +0/−144
- examples/DaePendulum.hs +197/−0
- examples/Dynoplot.hs +9/−5
- examples/ExampleDsl/OcpMonad.hs +0/−507
- examples/Glider.hs +30/−13
- examples/Homotopy.hs +1/−1
- examples/OcpDslRocket.hs +0/−92
- examples/OcpDslSpring.hs +0/−72
- examples/Rocket.hs +169/−0
- examples/Sailboat.hs +30/−20
- examples/Sofa/Common.hs +3/−3
- examples/SofaExpando.hs +5/−4
- examples/SofaVisualizer.hs +9/−5
- examples/Spring.hs +157/−0
- examples/beginner/SimpleQp.hs +48/−0
- src/Dyno/AutoScaling.hs +2/−2
- src/Dyno/DirectCollocation.hs +0/−47
- src/Dyno/DirectCollocation/Dynamic.hs +78/−51
- src/Dyno/DirectCollocation/Export.hs +20/−14
- src/Dyno/DirectCollocation/Formulate.hs +148/−66
- src/Dyno/DirectCollocation/Interpolate.hs +209/−0
- src/Dyno/DirectCollocation/Profile.hs +12/−10
- src/Dyno/DirectCollocation/Quadratures.hs +3/−1
- src/Dyno/DirectCollocation/Robust.hs +16/−7
- src/Dyno/DirectCollocation/Types.hs +114/−69
- src/Dyno/LagrangePolynomials.lhs +18/−6
- src/Dyno/Nlp.hs +3/−0
- src/Dyno/NlpSolver.hs +7/−7
- src/Dyno/NlpUtils.hs +9/−2
- src/Dyno/Ocp.hs +32/−22
- src/Dyno/OcpHomotopy.hs +184/−0
- src/Dyno/TypeVecs.hs +21/−4
- src/Dyno/Vectorize.hs +28/−1
- src/Dyno/View/Cov.hs +1/−1
- src/Dyno/View/CustomFunction.hs +0/−160
- src/Dyno/View/HList.hs +0/−1
- src/Dyno/View/M.hs +3/−3
- src/Dyno/View/Scheme.hs +26/−41
- src/Dyno/View/Unsafe/M.hs +9/−4
- src/Dyno/View/Unsafe/View.hs +10/−6
- src/Dyno/View/View.hs +1/−1
- src/Dyno/View/Viewable.hs +0/−1
- tests/IntegrationTests.hs +60/−58
- tests/NewUnitTests.hs +1/−1
- tests/VectorizeTests.hs +1/−1
- tests/ViewTests.hs +23/−7
dynobud.cabal view
@@ -1,5 +1,5 @@ name: dynobud-version: 1.3.0.0+version: 1.4.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@@ -23,18 +23,19 @@ Dyno.TypeVecs Dyno.MultipleShooting Dyno.Ocp- Dyno.DirectCollocation+ Dyno.OcpHomotopy Dyno.DirectCollocation.Dynamic Dyno.DirectCollocation.Export Dyno.DirectCollocation.Formulate Dyno.DirectCollocation.Integrate+ Dyno.DirectCollocation.Interpolate Dyno.DirectCollocation.Profile Dyno.DirectCollocation.Quadratures Dyno.DirectCollocation.Robust Dyno.DirectCollocation.Types Dyno.SXElement Dyno.View.Cov- Dyno.View.CustomFunction+-- Dyno.View.CustomFunction Dyno.View.Fun Dyno.View.FunJac Dyno.View.HList@@ -59,8 +60,8 @@ other-modules: build-depends: base >=4.6 && < 5,- casadi-bindings-core >= 2.2.0.2,- casadi-bindings >= 2.2.0.8,+ casadi-bindings-core >= 2.3.0.0,+ casadi-bindings >= 2.3.0.0, -- casadi-bindings-internal, jacobi-roots >=0.2 && <0.3, spatial-math >= 0.2.1.0,@@ -72,6 +73,7 @@ linear >= 1.3.1.1, reflection >= 1.3.2, binary,+ cereal, distributive, process, Plot-ho-matic >= 0.5.0.2,@@ -135,7 +137,8 @@ not-gloss >= 0.7.0.1, stm, containers,- binary,+-- binary,+ cereal, linear, bytestring, zeromq4-haskell,@@ -156,7 +159,8 @@ casadi-bindings-core, zeromq4-haskell, bytestring,- binary,+-- binary,+ cereal, linear, base >= 4.6 && < 5 ghc-options: -threaded -O2@@ -189,52 +193,42 @@ ghc-options: -threaded -O2 -executable ocpDslSpring+executable spring if flag(examples) Buildable: True else Buildable: False hs-source-dirs: examples- main-is: OcpDslSpring.hs- other-modules: ExampleDsl.OcpMonad- ExampleDsl.LogsAndErrors- ExampleDsl.Types+ main-is: Spring.hs default-language: Haskell2010 build-depends: base >=4.6 && < 5 , dynobud , casadi-bindings , vector- , containers- , unordered-containers- , lens+ , generic-accessors , bytestring , zeromq4-haskell- , binary- , mtl+-- , binary+ , cereal ghc-options: -threaded -O2 -executable ocpDslRocket+executable rocket if flag(examples) Buildable: True else Buildable: False hs-source-dirs: examples- main-is: OcpDslRocket.hs- other-modules: ExampleDsl.OcpMonad- ExampleDsl.LogsAndErrors- ExampleDsl.Types+ main-is: Rocket.hs default-language: Haskell2010 build-depends: base >=4.6 && < 5 , dynobud , casadi-bindings , vector- , containers- , unordered-containers- , lens+ , generic-accessors , bytestring , zeromq4-haskell- , binary- , mtl+-- , binary+ , cereal ghc-options: -threaded -O2 @@ -283,21 +277,37 @@ default-language: Haskell2010 build-depends: dynobud, vector >=0.10,- casadi-bindings >=0.10,+ casadi-bindings, base >=4.6 && < 5 ghc-options: -threaded -O2 -executable daeColl+executable beginner-qp if flag(examples) Buildable: True else Buildable: False+ hs-source-dirs: examples/beginner+ main-is: SimpleQp.hs+ default-language: Haskell2010+ build-depends: dynobud,+ base >=4.6 && < 5+ ghc-options: -O2++executable dae-pendulum+ if flag(examples)+ Buildable: True+ else+ Buildable: False hs-source-dirs: examples- main-is: DaeColl.hs+ main-is: DaePendulum.hs default-language: Haskell2010 build-depends: dynobud, base >=4.6 && < 5, generic-accessors >= 0.1.0.0,+ bytestring,+ zeromq4-haskell,+-- binary,+ cereal, vector ghc-options: -threaded -O2 @@ -317,7 +327,8 @@ containers, linear, bytestring,- binary,+-- binary,+ cereal, vector, generic-accessors >= 0.1.0.0, zeromq4-haskell@@ -336,7 +347,8 @@ containers, linear, bytestring,- binary,+-- binary,+ cereal, vector, semigroups, generic-accessors >= 0.1.0.0,@@ -355,7 +367,7 @@ base >=4.6 && < 5, containers, vector,- binary,+ cereal, bytestring, zeromq4-haskell, Plot-ho-matic >= 0.5.0.0,@@ -416,6 +428,7 @@ vector, linear, binary,+ cereal, casadi-bindings, hmatrix, hmatrix-gsl,
− examples/DaeColl.hs
@@ -1,144 +0,0 @@-{-# OPTIONS_GHC -Wall #-}-{-# Language FlexibleInstances #-}-{-# Language DeriveFunctor #-}-{-# Language DeriveGeneric #-}-{-# Language DataKinds #-}-{-# Language PolyKinds #-}--module Main where--import GHC.Generics ( Generic, Generic1 )--import Data.Vector ( Vector )--import Accessors--import Dyno.Vectorize-import Dyno.View.View ( J, jfill )-import Dyno.TypeVecs-import Dyno.Solvers---import Dyno.Sqp.Sqp---import Dyno.Sqp.LineSearch-import Dyno.Nlp-import Dyno.NlpUtils-import Dyno.Ocp-import Dyno.DirectCollocation-import Dyno.DirectCollocation.Quadratures ( QuadratureRoots(..) )--data PendX a = PendX { pX :: a- , pY :: a- , pVx :: a- , pVy :: a- } deriving (Functor, Generic, Generic1, Show)-data PendZ a = PendZ { pTau :: a} deriving (Functor, Generic, Generic1, Show)-data PendU a = PendU { pTorque :: a } deriving (Functor, Generic, Generic1, Show)-data PendP a = PendP { pMass :: a } deriving (Functor, Generic, Generic1, Show)-data PendR a = PendR a a a a a deriving (Functor, Generic, Generic1, Show)-data PendO a = PendO deriving (Functor, Generic, Generic1, Show)--instance Vectorize PendX-instance Vectorize PendZ-instance Vectorize PendU-instance Vectorize PendP-instance Vectorize PendR-instance Vectorize PendO--instance Lookup (PendX ())-instance Lookup (PendZ ())-instance Lookup (PendU ())--mayer :: Num a => t -> PendX a -> PendX a -> 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- where- PendX _ _ vx vy = x- PendU torque = u--r :: Floating a => a-r = 0.3--pendDae :: Floating a => PendX a -> PendX a -> PendZ a -> PendU a -> PendP a -> a -> (PendR a, PendO a)-pendDae (PendX x' y' vx' vy') (PendX x y vx vy) (PendZ tau) (PendU torque) (PendP m) _ =- (PendR (x' - vx) (y' - vy)- (m*vx' + x*tau - fx)- (m*vy' + y*tau - fy)- (x*vx' + y*vy' + (vx*vx + vy*vy))- , PendO- )- where- fx = torque*y- fy = -torque*x + m*9.8--pendOcp :: OcpPhase PendX PendZ PendU PendP PendR PendO (Vec 8) None None-pendOcp = OcpPhase { ocpMayer = mayer- , ocpLagrange = lagrange- , ocpQuadratures = \_ _ _ _ _ _ _ -> None- , ocpDae = pendDae- , ocpBc = bc- , ocpPathC = pathc- , ocpPathCBnds = None- , ocpBcBnds = fill (Just 0, Just 0)- , ocpXbnd = xbnd- , ocpUbnd = ubnd- , ocpZbnd = fill (Nothing, Nothing)- , ocpPbnd = PendP (Just 0.3, Just 0.3)- , ocpTbnd = (Just 4, Just 10)- , ocpObjScale = Nothing- , ocpTScale = Nothing- , ocpXScale = Nothing- , ocpZScale = Nothing- , ocpUScale = Nothing- , ocpPScale = Nothing- , ocpResidualScale = Nothing- , ocpBcScale = Nothing- , ocpPathCScale = Nothing- }--pathc :: Floating a => PendX a -> PendZ a -> PendU a -> PendP a -> PendO a -> a -> None a-pathc _ _ _ _ _ _ = None--xbnd :: PendX Bounds-xbnd = PendX { pX = (Just (-10), Just 10)- , pY = (Just (-10), Just 10)- , pVx = (Just (-10), Just 10)- , pVy = (Just (-10), Just 10)- }--ubnd :: PendU Bounds-ubnd = PendU (Just (-40), Just 40)--bc :: Floating a => PendX a -> PendX a -> None a -> PendP a -> a -> Vec 8 a-bc (PendX x0 y0 vx0 vy0) (PendX xf yf vxf vyf) _ _ _ =- mkVec'- [ x0- , y0 + r- , vx0- , vy0- , xf- , yf - r- , vxf- , vyf- ]--type NCollStages = 80-type CollDeg = 3--guess :: J (CollTraj PendX PendZ PendU PendP NCollStages CollDeg) (Vector Double)-guess = jfill 1--solver :: Solver-solver = ipoptSolver--solver2 :: Solver-solver2 = ipoptSolver { options = [("expand", Opt True)] }---main :: IO ()-main = do- cp <- makeCollProblem Legendre pendOcp- let nlp = cpNlp cp- _ <- solveNlp solver (nlp { nlpX0 = guess }) Nothing--- _ <- solveNlp solver2 (nlp { nlpX0 = guess }) Nothing- return ()
+ examples/DaePendulum.hs view
@@ -0,0 +1,197 @@+{-# OPTIONS_GHC -Wall #-}+{-# Language TypeFamilies #-}+{-# Language FlexibleInstances #-}+{-# Language DeriveFunctor #-}+{-# Language DeriveGeneric #-}+{-# Language DataKinds #-}+{-# Language PolyKinds #-}++module Main where++import GHC.Generics ( Generic, Generic1 )++import Data.Vector ( Vector )++import Accessors++import Dyno.Vectorize+import Dyno.View.View ( View(..), J )+import Dyno.Solvers+import Dyno.Nlp+import Dyno.NlpUtils+import Dyno.Ocp+import Dyno.DirectCollocation.Formulate ( CollProblem(..), makeCollProblem, makeGuess )+import Dyno.DirectCollocation.Types ( CollTraj' )+import Dyno.DirectCollocation.Dynamic ( toMeta )+import Dyno.DirectCollocation.Quadratures ( QuadratureRoots(..) )++import Dynoplot.Callback ( withCallback )++data PendOcp+type instance X PendOcp = PendX+type instance Z PendOcp = PendZ+type instance U PendOcp = PendU+type instance P PendOcp = PendP+type instance R PendOcp = PendR+type instance O PendOcp = PendO+type instance C PendOcp = PendBc+type instance H PendOcp = None+type instance Q PendOcp = None++data PendX a = PendX { pX :: a+ , pY :: a+ , pVx :: a+ , pVy :: a+ , pTorque :: a+ } deriving (Functor, Generic, Generic1, Show)+data PendZ a = PendZ { pTau :: a} deriving (Functor, Generic, Generic1, Show)+data PendU a = PendU { pTorqueDot :: a } deriving (Functor, Generic, Generic1, Show)+data PendP a = PendP { pMass :: a } deriving (Functor, Generic, Generic1, Show)+data PendR a = PendR a a a a a a deriving (Functor, Generic, Generic1, Show)+data PendO a = PendO deriving (Functor, Generic, Generic1, Show)+data PendBc a = PendBc (PendX a) (PendX a) deriving (Functor, Generic, Generic1, Show)++instance Vectorize PendX+instance Vectorize PendZ+instance Vectorize PendU+instance Vectorize PendP+instance Vectorize PendR+instance Vectorize PendO+instance Vectorize PendBc++instance Lookup (PendX ())+instance Lookup (PendZ ())+instance Lookup (PendU ())+instance Lookup (PendO ())+instance Lookup (PendP ())++mayer :: a -> PendX a -> PendX a -> None a -> PendP a -> a+mayer tf _ _ _ _ = tf++lagrange :: Floating a => PendX a -> PendZ a -> PendU a -> PendP a -> PendO a -> a -> a -> a+lagrange _ _ u _ _ _ tf = 1e-3*torque'**2 / tf+ where+ PendU torque' = u++r :: Floating a => a+r = 0.3++pendDae :: Floating a => PendX a -> PendX a -> PendZ a -> PendU a -> PendP a -> a -> (PendR a, PendO a)+pendDae (PendX x' y' vx' vy' torque') (PendX x y vx vy torque)+ (PendZ tau) (PendU uTorque') (PendP m) _ = (residual, outputs)+ where+ residual =+ PendR (x' - vx) (y' - vy)+ (m*vx' + x*tau - fx)+ (m*vy' + y*tau - fy)+ (x*vx' + y*vy' + (vx*vx + vy*vy))+ (torque' - uTorque')+ outputs = PendO++ fx = torque*y+ fy = -torque*x + m*9.8++pendOcp :: OcpPhase' PendOcp+pendOcp = OcpPhase { ocpMayer = mayer+ , ocpLagrange = lagrange+ , ocpQuadratures = \_ _ _ _ _ _ _ -> None+ , ocpDae = pendDae+ , ocpBc = bc+ , ocpPathC = pathc+ , ocpPathCBnds = None+ , ocpBcBnds = bcBnds+ , ocpXbnd = xbnd+ , ocpUbnd = ubnd+ , ocpZbnd = PendZ (Just (-200), Just 200)+ , ocpPbnd = PendP (Just 0.3, Just 0.3)+ , ocpTbnd = (Just 0.1, Just 5)+ , ocpObjScale = Nothing+ , ocpTScale = Nothing+ , ocpXScale = Just pendXScale+ , ocpZScale = Just (PendZ 10)+ , ocpUScale = Just (PendU 50)+ , ocpPScale = Just (PendP 0.3)+ , ocpResidualScale = Nothing+ , ocpBcScale = Just $ PendBc pendXScale pendXScale+ , ocpPathCScale = Just None+ }+pendXScale :: PendX Double+pendXScale = PendX 0.3 0.3 1 1 10++pathc :: Floating a => PendX a -> PendZ a -> PendU a -> PendP a -> PendO a -> a -> None a+pathc _ _ _ _ _ _ = None++xbnd :: PendX Bounds+xbnd = PendX { pX = (Nothing, Nothing)+ , pY = (Nothing, Nothing)+ , pVx = (Nothing, Nothing)+ , pVy = (Nothing, Nothing)+ , pTorque = (Just (-30), Just 30)+ }++ubnd :: PendU Bounds+ubnd = PendU (Just (-100), Just 100)++bc :: Floating a => PendX a -> PendX a -> None a -> PendP a -> a -> PendBc a+bc x0 xf _ _ _ = PendBc x0 xf++bcBnds :: PendBc Bounds+bcBnds =+ PendBc+ (PendX+ { pX = (Just 0, Just 0)+ , pY = (Just (-r), Just (-r))+ , pVx = (Just 0, Just 0)+ , pVy = (Just 0, Just 0)+ , pTorque = (Nothing, Nothing)+ })+ (PendX+ { pX = (Nothing, Nothing) -- LICQ+ , pY = (Just r, Just r)+ , pVx = (Just 0, Just 0)+ , pVy = (Nothing, Nothing) -- LICQ+ , pTorque = (Nothing, Nothing)+ })++type NCollStages = 120+type CollDeg = 3++guess :: J (CollTraj' PendOcp NCollStages CollDeg) (Vector Double)+guess = cat $ makeGuess Radau tf guessX guessZ guessU parm+ where+ tf = 1+ guessX t = PendX { pX = r * sin q+ , pY = - r * cos q+ , pVx = r*w*cos q+ , pVy = r*w*sin q+ , pTorque = 0+ }+ where+ q = pi*t/tf+ w = pi/tf+ guessZ _ = PendZ {pTau = 0}+ guessU _ = PendU {pTorqueDot = 0}+ parm = PendP 0.3++solver :: Solver+solver = ipoptSolver { options = [ ("expand", Opt True)+ , ("linear_solver", Opt "ma86")+ , ("ma86_order", Opt "metis")+ ]}++solver2 :: Solver+solver2 = ipoptSolver { options = [("expand", Opt True)] }+++main :: IO ()+main = do+ cp <- makeCollProblem Legendre pendOcp guess+ withCallback $ \send -> do+ let nlp = cpNlp cp+ meta = toMeta (cpMetaProxy cp)+ cb' traj = do+ plotPoints <- cpPlotPoints cp traj+ send (plotPoints, meta)+ _ <- solveNlp solver nlp (Just cb')+-- _ <- solveNlp solver2 nlp Nothing+ return ()
examples/Dynoplot.hs view
@@ -5,8 +5,9 @@ import Control.Monad ( when, forever ) import Data.ByteString.Char8 ( pack )-import Data.ByteString.Lazy ( fromStrict )-import Data.Binary ( decodeOrFail )+--import Data.ByteString.Lazy ( fromStrict )+--import Data.Binary ( decodeOrFail )+import Data.Serialize ( decode ) import qualified System.ZMQ4 as ZMQ import System.Console.CmdArgs ( (&=), Data, Typeable ) import qualified System.Console.CmdArgs as CA@@ -28,9 +29,12 @@ when mre $ do msg <- ZMQ.receive subscriber let decoded :: (DynPlotPoints Double, CollTrajMeta)- decoded = case decodeOrFail (fromStrict msg) of- Left (_,_,err) -> error $ "decode failure: " ++ err- Right (_,_,t) -> t+ decoded = case decode msg of+ Left err -> error $ "decode failure: " ++ err+ Right t -> t+-- decoded = case decodeOrFail (fromStrict msg) of+-- Left (_,_,err) -> error $ "decode failure: " ++ err+-- Right (_,_,t) -> t writeChan decoded main :: IO ()
− examples/ExampleDsl/OcpMonad.hs
@@ -1,507 +0,0 @@-{-# OPTIONS_GHC -Wall #-}-{-# Language ScopedTypeVariables #-}-{-# Language PackageImports #-}-{-# Language MultiParamTypeClasses #-}-{-# Language FunctionalDependencies #-}-{-# Language GeneralizedNewtypeDeriving #-}-{-# Language FlexibleContexts #-}-{-# Language RankNTypes #-}-{-# Language DataKinds #-}--module ExampleDsl.OcpMonad- ( OcpMonad- , EqMonad(..)- , LeqMonad(..)- , DaeMonad- , BCMonad- , SXElement- , diffState- , algVar- , control- , parameter- , output- , lagrangeTerm- , solveStaticOcp- ) where--import Control.Applicative ( Applicative(..) )-import Control.Lens ( Lens', over )-import Control.Monad ( when )-import qualified "mtl" Control.Monad.State as State-import "mtl" Control.Monad.Reader ( MonadIO(..) )-import "mtl" Control.Monad.Writer ( WriterT, Writer, MonadWriter, runWriterT, runWriter )-import "mtl" Control.Monad.State ( StateT, MonadState, runStateT )-import "mtl" Control.Monad.Except ( ExceptT, MonadError, runExceptT )-import qualified Data.Foldable as F-import qualified Data.HashSet as HS-import qualified Data.Sequence as S-import qualified Data.Map as M-import Data.Sequence ( (|>) )-import Data.Vector ( Vector )-import qualified Data.Vector as V-import Data.Proxy ( Proxy(..) )--import Casadi.Option ( setOption )-import Casadi.SXFunction ( sxFunction )-import Casadi.SX ( SX )-import Casadi.Function ( callSX )-import Casadi.SharedObject ( soInit )-import Casadi.CMatrix as CM--import Dyno.View.Unsafe.View ( mkJ )--import Dyno.SXElement ( SXElement, sxElementSym, sxElementToSX, sxToSXElement, sxSplitJV )-import Dyno.Ocp ( OcpPhase(..) )-import Dyno.Nlp ( Bounds )-import Dyno.Vectorize ( Vectorize(..), fill )-import Dyno.TypeVecs ( Vec )-import qualified Dyno.TypeVecs as TV-import Dyno.Solvers ( Solver )-import Dyno.DirectCollocation.Quadratures ( QuadratureRoots(..) )-import Dyno.DirectCollocation.Dynamic ( DynPlotPoints, CollTrajMeta(..), NameTree(..) )-import Dyno.DirectCollocation ( solveOcp )--import ExampleDsl.LogsAndErrors-import ExampleDsl.Types----withEllipse :: Int -> String -> String---withEllipse n blah--- | length blah <= n = blah--- | otherwise = take n blah ++ "..."--newtype OcpMonad a =- OcpMonad- { runOcp :: ExceptT ErrorMessage (WriterT [LogMessage] (StateT OcpState IO)) a- } deriving ( Functor- , Applicative- , Monad- , MonadError ErrorMessage- , MonadState OcpState- , MonadWriter [LogMessage]- , MonadIO- )--newtype BCMonad a =- BCMonad- { runBc :: ExceptT ErrorMessage (WriterT [LogMessage] (StateT (S.Seq (Constraint SXElement)) IO)) a- } deriving ( Functor- , Applicative- , Monad- , MonadError ErrorMessage- , MonadState (S.Seq (Constraint SXElement))- , MonadWriter [LogMessage]- , MonadIO- )--newtype DaeMonad a =- DaeMonad- { runDae :: ExceptT ErrorMessage (WriterT [LogMessage] (StateT DaeState IO)) a- } deriving ( Functor- , Applicative- , Monad- , MonadError ErrorMessage- , MonadState DaeState- , MonadWriter [LogMessage]- , MonadIO- )--emptySymbolicDae :: DaeState-emptySymbolicDae = DaeState S.empty S.empty S.empty S.empty S.empty M.empty HS.empty S.empty--buildDae :: DaeMonad a -> IO (Either ErrorMessage a, [LogMessage], DaeState)-buildDae = buildDae' emptySymbolicDae- where- buildDae' :: DaeState -> DaeMonad a -> IO (Either ErrorMessage a, [LogMessage], DaeState)- buildDae' nlp0 builder = do- ((result,logs),state) <- flip runStateT nlp0 . runWriterT . runExceptT . runDae $ builder- return (result, logs, state)--newDaeVariable ::- (MonadState DaeState m, MonadError ErrorMessage m, MonadWriter [LogMessage] m, MonadIO m)- => String -> Lens' DaeState (S.Seq (String, SXElement)) -> String -> m SXElement-newDaeVariable description lens name = do- debug $ "adding " ++ description ++ " \""++name++"\""- case name of [] -> err "name cannot be empty"- ('_':_) -> err $ "name \"" ++ name ++- "\" cannot have leading underscore (this is reserved for internal use)"- _ -> return ()- state0 <- State.get- let map0 = daeNameSet state0- sym <- liftIO (sxElementSym name)- when (HS.member name map0) $ err $ name ++ " already in name set"- let state1 = state0 { daeNameSet = HS.insert name map0 }- state2 = over lens (|> (name, sym)) state1- State.put state2- return sym--svector :: Vector SXElement -> SX-svector = CM.vertcat . fmap sxElementToSX--diffState :: String -> DaeMonad (SXElement, SXElement)-diffState name = do- x <- newDaeVariable "differential state" daeX name- xdot <- newDaeVariable "differential state derivative" daeXDot ("ddt( " ++ name ++ " )")- return (x, xdot)--algVar :: String -> DaeMonad SXElement-algVar = newDaeVariable "algebraic variable" daeZ--control :: String -> DaeMonad SXElement-control = newDaeVariable "control" daeU--parameter :: String -> DaeMonad SXElement-parameter = newDaeVariable "parameter" daeP--output :: String -> SXElement -> DaeMonad ()-output name expr = do- debug $ "adding output \""++name++"\""--- debug $ "adding output \""++name++"\": " ++ withEllipse 30 (show expr)- state0 <- State.get- let nameSet0 = daeNameSet state0- outputs0 = _daeO state0- when (HS.member name nameSet0) $ err $ name ++ " already in name set"- when (M.member name outputs0) $ impossible $ name ++ " already in output map"- let state1 = state0 { daeNameSet = HS.insert name nameSet0- , _daeO = M.insert name expr outputs0- }- State.put state1--infix 4 ===-class EqMonad m a | m -> a where- (===) :: a -> a -> m ()--instance EqMonad DaeMonad SXElement where- (===) lhs rhs = do- debug $ "adding equality constraint: "--- ++ withEllipse 30 (show lhs) ++ " == " ++ withEllipse 30 (show rhs)- state0 <- State.get- State.put $ state0 { daeConstraints = daeConstraints state0 |> (lhs, rhs) }--instance EqMonad OcpMonad SXElement where- (===) lhs rhs = do- debug $ "adding equality constraint: "--- ++ withEllipse 30 (show lhs) ++ " == " ++ withEllipse 30 (show rhs)- state0 <- State.get- State.put $ state0 { ocpPathConstraints = ocpPathConstraints state0 |> Eq2 lhs rhs }---infix 4 <==-class LeqMonad m where- (<==) :: SXElement -> SXElement -> m ()--instance LeqMonad OcpMonad where- (<==) lhs rhs = do- debug $ "adding inequality constraint: "--- ++ withEllipse 30 (show lhs) ++ " <= " ++ withEllipse 30 (show rhs)- state0 <- State.get- State.put $ state0 { ocpPathConstraints = ocpPathConstraints state0 |> Ineq2 lhs rhs }--instance EqMonad BCMonad SXElement where- (===) lhs rhs = do- debug $ "adding inequality constraint: "- -- ++ withEllipse 30 (show lhs) ++ " == " ++ withEllipse 30 (show rhs)- state0 <- State.get- State.put $ state0 |> Eq2 lhs rhs--instance LeqMonad BCMonad where- (<==) lhs rhs = do- debug $ "adding inequality constraint: "--- ++ withEllipse 30 (show lhs) ++ " <= " ++ withEllipse 30 (show rhs)- state0 <- State.get- State.put $ state0 |> Ineq2 lhs rhs---constr :: Constraint SXElement -> (SXElement, Bounds)-constr (Eq2 lhs rhs) = (lhs - rhs, (Just 0, Just 0))-constr (Ineq2 lhs rhs) = (lhs - rhs, (Nothing, Just 0))-constr (Ineq3 x (lhs,rhs)) = (x, (Just lhs, Just rhs))----lagrangeTerm :: SXElement -> OcpMonad ()-lagrangeTerm obj = do- debug "setting lagrange term"- --debug $ "setting lagrange term: " ++ withEllipse 30 (show obj)- state0 <- State.get- case ocpLagrangeObj state0 of- Objective _x -> err $ init $ unlines- [ "you set the lagrange objective function twice"--- , " old val: " ++ withEllipse 30 (show x)--- , " new val: " ++ withEllipse 30 (show obj)- ]- ObjectiveUnset -> State.put $ state0 { ocpLagrangeObj = Objective obj }----emptySymbolicOcp :: OcpState-emptySymbolicOcp = OcpState S.empty ObjectiveUnset HomotopyParamUnset--reifyOcpPhase ::- forall ret .- (SXElement -> DaeMonad ())- -> (forall a m . (Floating a, Monad m) => a -> (String -> m a) -> (String -> m a) -> m a)- -> ((String -> BCMonad SXElement) -> (String -> BCMonad SXElement) -> BCMonad ())- -> (SXElement -> (String -> OcpMonad SXElement) -> OcpMonad ())- -> (Maybe Double, Maybe Double)- -> (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 (Vec 0) -> CollTrajMeta -> IO ret)- -> IO ret-reifyOcpPhase daeMonad mayerMonad bcMonad ocpMonad tbnds f = do- time <- sxElementSym "_t"- endT <- sxElementSym "T"- let time' = sxElementToSX time- endT' = sxElementToSX endT- dae' <- buildDae (daeMonad time)- let dae :: DaeState- dae = case dae' of- (Left errmsg, _, _) -> error $ "buildOcpPhase: buildDae failure: " ++ show errmsg- (_, _, daeState) -> daeState-- xdotnames, xnames, znames, unames, pnames :: Vector String- xdots, xs, zs, us, ps :: Vector SXElement- (xdotnames,xdots) = V.unzip $ V.fromList $ F.toList $ _daeXDot dae- (xnames,xs) = V.unzip $ V.fromList $ F.toList $ _daeX dae- (znames,zs) = V.unzip $ V.fromList $ F.toList $ _daeZ dae- (unames,us) = V.unzip $ V.fromList $ F.toList $ _daeU dae- (pnames,ps) = V.unzip $ V.fromList $ F.toList $ _daeP dae-- xdots' = svector xdots- xs' = svector xs- zs' = svector zs- us' = svector us- ps' = svector ps-- daeResidual :: Vector SXElement- daeResidual = V.map (uncurry (-)) $ V.fromList $ F.toList $ daeConstraints dae-- onames :: Vector String- osOut :: Vector SXElement- (onames, osOut) = V.unzip $ V.fromList $ M.toList $ _daeO dae- os <- V.mapM sxElementSym onames :: IO (Vector SXElement)- let os' = svector os-- lookupThingy :: String -> OcpMonad SXElement- lookupThingy name = do- debug $ "ocp monad: looking up \"" ++ name ++ "\""- case M.lookup name varmap of- Nothing -> err $ "ocp monad: nothing named \"" ++ name ++ "\""- Just expr -> do- debug $ "ocp monad: found \"" ++ name ++ "\""- --debug $ "ocp monad: found \"" ++ name ++ "\": " ++ show expr- return expr- where- varmap :: M.Map String SXElement- varmap = M.fromList $ F.toList $ V.concat- [ V.zip xdotnames xdots- , V.zip xnames xs- , V.zip znames zs- , V.zip unames us- , V.zip pnames ps- , V.zip onames os- ]-- ocp' <- flip runStateT emptySymbolicOcp $ runWriterT $ runExceptT (runOcp (ocpMonad time lookupThingy))- let ocp :: OcpState- ocp = case ocp' of- ((Left errmsg, logs),_) ->- error $ unlines $ ("" : map show logs) ++ ["","ocp monad failure: " ++ show errmsg]- ((Right _, _), ocpState) -> ocpState-- obj = case ocpLagrangeObj ocp of- ObjectiveUnset -> 0- Objective obj' -> obj'-- lagFunSX <- sxFunction (V.fromList [xs',zs',us',ps',os',time',endT']) (V.fromList [svector (V.singleton obj)])- setOption lagFunSX "name" "lagrange"- soInit lagFunSX-- let pathConstraints :: [SXElement]- pathConstraintBnds :: [(Maybe Double, Maybe Double)]- (pathConstraints, pathConstraintBnds) = unzip $ map constr (F.toList (ocpPathConstraints ocp))-- pathcFunSX <- sxFunction (V.fromList [xs',zs',us',ps',os',time'])- (V.singleton (svector (V.fromList pathConstraints)))- setOption pathcFunSX "name" "pathConstraints"- soInit pathcFunSX--- daeFunSX <- sxFunction (V.fromList [xdots', xs', zs', us', ps', time'])- (V.fromList [svector daeResidual, svector osOut])- setOption pathcFunSX "name" "daeResidualAndOutputs"- soInit daeFunSX-- -- run the mayer function- x0s <- mapM (sxElementSym . (++ "_0")) (F.toList xnames)- xFs <- mapM (sxElementSym . (++ "_F")) (F.toList xnames)- let lookupState :: M.Map String SXElement -> String- -> ExceptT ErrorMessage (Writer [LogMessage]) SXElement- lookupState xmap name = do- debug $ "mayer monad: looking up \"" ++ name ++ "\""- case M.lookup name xmap of- Nothing -> err $ "mayer monad: no state named \"" ++ name ++ "\""- Just expr -> do- debug $ "mayer monad: found \"" ++ name ++ "\""- return expr-- xmap0 :: M.Map String SXElement- xmap0 = M.fromList $ zip (F.toList xnames) x0s-- xmapF :: M.Map String SXElement- xmapF = M.fromList $ zip (F.toList xnames) xFs-- mayerObj :: SXElement- mayerObj = case runWriter (runExceptT (mayerMonad endT (lookupState xmap0) (lookupState xmapF))) of- (Left errmsg, logs) ->- error $ unlines $ ("" : map show logs) ++ ["","mayer monad failure: " ++ show errmsg]- (Right ret, _) -> ret- mayerFunSX <- sxFunction (V.fromList [ svector (V.singleton endT)- , svector (V.fromList x0s)- , svector (V.fromList xFs)- , svector V.empty- , svector V.empty- ])- (V.singleton (svector (V.singleton mayerObj)))- setOption mayerFunSX "name" "mayer"- soInit mayerFunSX--- let lookupState0 :: String -> BCMonad SXElement- lookupState0 name = do- debug $ "boundary condition monad: looking up initial \"" ++ name ++ "\""- case M.lookup name xmap0 of- Nothing -> err $ "boundary condition monad: no state named \"" ++ name ++ "\""- Just expr -> do- debug $ "boundary condition monad: found \"" ++ name ++ "\""- return expr-- lookupStateF :: String -> BCMonad SXElement- lookupStateF name = do- debug $ "boundary condition monad: looking up final \"" ++ name ++ "\""- case M.lookup name xmapF of- Nothing -> err $ "boundary condition monad: no state named \"" ++ name ++ "\""- Just expr -> do- debug $ "boundary condition monad: found \"" ++ name ++ "\""- return expr- bcs' <- flip runStateT S.empty $ runWriterT (runExceptT (runBc $ bcMonad lookupState0 lookupStateF))- let bcs :: Vector SXElement- bcbnds :: Vector Bounds- (bcs,bcbnds) = case bcs' of- ((Left errmsg, logs),_) ->- error $ unlines $ ("" : map show logs) ++ ["","boundary condition monad failure: " ++ show errmsg]- ((Right _,_), ret) -> V.unzip $ V.fromList $ map constr $ F.toList ret- bcFunSX <- sxFunction (V.fromList [ svector (V.fromList x0s)- , svector (V.fromList xFs)- , svector V.empty- , svector V.empty- , svector (V.singleton endT)- ])- (V.singleton (svector bcs))- setOption bcFunSX "name" "boundaryConditions"- soInit bcFunSX-- let meta = CollTrajMeta- { ctmX = NameTreeNode ("", "") (zip (F.toList xnames) (map NameTreeLeaf [0..]))- , ctmZ = NameTreeNode ("", "") (zip (F.toList znames) (map NameTreeLeaf [0..]))- , ctmU = NameTreeNode ("", "") (zip (F.toList unames) (map NameTreeLeaf [0..]))- , ctmP = NameTreeNode ("", "") (zip (F.toList pnames) (map NameTreeLeaf [0..]))- , ctmO = NameTreeNode ("", "") (zip (F.toList onames) (map NameTreeLeaf [0..]))- , ctmQ = NameTreeNode ("", "") []- }- 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) ->- TV.reifyDim (length pathConstraints) $ \(Proxy :: Proxy nh) -> do- -- TV.reifyDim ncov $ \(Proxy :: Proxy ncov) -> do- -- TV.reifyDim nsh $ \(Proxy :: Proxy nsh) -> do- -- TV.reifyDim nsc $ \(Proxy :: Proxy nsc) -> do-- let daeFun :: Vec nx SXElement -> Vec nx SXElement -> Vec nz SXElement -> Vec nu SXElement- -> Vec np SXElement -> SXElement- -> (Vec nr SXElement, Vec no SXElement)- daeFun x' x z u p t = (devec (rets V.! 0), devec (rets V.! 1))- where- rets = callSX daeFunSX (V.fromList [vec x', vec x, vec z, vec u, vec p, sxElementToSX t])-- lagrangeFun :: Vec nx SXElement -> Vec nz SXElement -> Vec nu SXElement -> Vec np SXElement -> Vec no SXElement -> SXElement -> SXElement -> SXElement- lagrangeFun x z u p o t tf =- sxToSXElement $ V.head $ callSX lagFunSX $- (V.fromList [vec x, vec z, vec u, vec p, vec o, sxElementToSX t, sxElementToSX tf])- --Left errmsg -> error $ "toOcpPhase: lagrangeFun: " ++ errmsg ++- -- "\ninputs: " ++ show (xnames ++ znames ++ unames ++ pnames) ++ show onames ++- -- "\nnumeric inputs x: " ++ show (V.length x) ++- -- "\nnumeric inputs z: " ++ show (V.length z) ++- -- "\nnumeric inputs u: " ++ show (V.length u) ++- -- "\nnumeric inputs p: " ++ show (V.length p) ++- -- "\nnumeric inputs o: " ++ show (V.length o)-- pathConstraintFun :: Vec nx SXElement -> Vec nz SXElement -> Vec nu SXElement- -> Vec np SXElement -> Vec no SXElement -> SXElement -> Vec nh SXElement- pathConstraintFun x z u p o t =- devec $ V.head $ callSX pathcFunSX (V.fromList [vec x, vec z, vec u, vec p, vec o, sxElementToSX t])-- mayerFun :: SXElement -> Vec nx SXElement -> Vec nx SXElement -> Vec 0 SXElement -> Vec np SXElement- -> SXElement- 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 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- , ocpPathC = pathConstraintFun- , ocpPathCBnds = devectorize (V.fromList pathConstraintBnds)- , ocpXbnd = fill (Nothing, Nothing)- , ocpZbnd = fill (Nothing, Nothing)- , ocpUbnd = fill (Nothing, Nothing)- , ocpPbnd = fill (Nothing, Nothing)- , ocpTbnd = tbnds- , ocpObjScale = Nothing- , ocpTScale = Nothing- , ocpXScale = Nothing- , ocpZScale = Nothing- , ocpUScale = Nothing- , ocpPScale = Nothing- , ocpResidualScale = Nothing- , ocpBcScale = Nothing- , ocpPathCScale = Nothing- }- f ocpPhase meta--vec :: Vectorize f => f SXElement -> SX-vec = svector . vectorize--devec :: Vectorize f => SX -> f SXElement-devec = sxSplitJV . mkJ--solveStaticOcp ::- 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 ())- -> (SXElement -> (String -> OcpMonad SXElement) -> OcpMonad ())- -> (Maybe Double, Maybe Double)- -> Int -> Int- -> Maybe (CollTrajMeta -> DynPlotPoints Double -> IO Bool)- -> IO (Either String String)-solveStaticOcp roots solverStuff dae mayer bc ocp tbnds n deg cb =- reifyOcpPhase dae mayer bc ocp tbnds woo- where- woo ocpphase meta = solveOcp roots solverStuff n deg (cb <*> pure meta) ocpphase
examples/Glider.hs view
@@ -1,9 +1,9 @@ {-# OPTIONS_GHC -Wall #-}+{-# Language TypeFamilies #-} {-# Language DataKinds #-} module Main ( main ) where -import Data.Proxy ( Proxy(..) ) import Linear import Data.Vector ( Vector ) @@ -12,11 +12,11 @@ import Dyno.Solvers --import Dyno.Sqp.Sqp --import Dyno.Sqp.LineSearch-import Dyno.Nlp import Dyno.NlpUtils import Dyno.Ocp-import Dyno.DirectCollocation+import Dyno.DirectCollocation.Formulate+import Dyno.DirectCollocation.Types import Dyno.DirectCollocation.Dynamic ( toMeta ) import Dyno.DirectCollocation.Quadratures ( QuadratureRoots(..) ) @@ -29,6 +29,17 @@ type NCollStages = 100 type CollDeg = 2 +data GliderOcp+type instance X GliderOcp = AcX+type instance Z GliderOcp = None+type instance U GliderOcp = AcU+type instance P GliderOcp = None+type instance R GliderOcp = AcX+type instance O GliderOcp = None+type instance C GliderOcp = AcX+type instance H GliderOcp = None+type instance Q GliderOcp = None+ mayer :: Floating a => a -> AcX a -> AcX a -> None a -> None a -> a mayer _ _ _ _ _ = 0 @@ -56,7 +67,7 @@ mcs = bettyMc refs = bettyRefs -ocp :: OcpPhase AcX None AcU None AcX None AcX None None+ocp :: OcpPhase' GliderOcp ocp = OcpPhase { ocpMayer = mayer , ocpLagrange = lagrange , ocpQuadratures = \_ _ _ _ _ _ _ -> None@@ -106,23 +117,29 @@ } 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+bc (AcX x0 v0 dcm0 w0 cs) _ _ _ _ = AcX x0 (v0 - V3 30 0 0) (dcm0 - eye3') w0 cs +eye3' :: Num a => M33 a+eye3' =+ V3+ (V3 1 0 0)+ (V3 0 1 0)+ (V3 0 0 1)++ main :: IO () main = do- cp <- makeCollProblem Legendre ocp+ let guess = jfill 1 :: J (CollTraj' GliderOcp NCollStages CollDeg) (Vector Double)+ cp <- makeCollProblem Legendre ocp guess let nlp = cpNlp cp- withCallback $ \cb -> do- let guess = jfill 1+ withCallback $ \send -> do+ let meta = toMeta (cpMetaProxy cp) - cb' :: J (CollTraj AcX None AcU None NCollStages CollDeg) (Vector Double) -> IO Bool cb' traj = do plotPoints <- cpPlotPoints cp traj- let proxy :: Proxy (CollTraj AcX None AcU None NCollStages CollDeg)- proxy = Proxy- cb (plotPoints, toMeta (Proxy :: Proxy None) (Proxy :: Proxy None) proxy)+ send (plotPoints, meta) - (msg,_) <- solveNlp ipoptSolver (nlp { nlpX0 = guess }) (Just cb')+ (msg,_) <- solveNlp ipoptSolver nlp (Just cb') case msg of Left msg' -> putStrLn $ "optimization failed, message: " ++ msg' Right _ -> putStrLn "optimization succeeded" -- let xopt = xOpt opt
examples/Homotopy.hs view
@@ -87,5 +87,5 @@ printf "X: (%.3f,%.3f), P: (%.3f, %.3f), a: %.4f\n" x y px py alpha return () pfs = [catJV (P 2 0), catJV (P 3 0)]- opt <- solveNlpHomotopy 1e-3 hp solver myNlp pfs Nothing (Just cbp)+ opt <- solveNlpHomotopy 1e-3 hp solver Nothing myNlp pfs Nothing (Just cbp) print opt
− examples/OcpDslRocket.hs
@@ -1,92 +0,0 @@-{-# OPTIONS_GHC -Wall #-}--module Main ( main ) where--import Control.Monad ( void )---import Control.Concurrent ( threadDelay )--import Dyno.Solvers-import Dyno.DirectCollocation.Quadratures ( QuadratureRoots(..) )--import Dynoplot.Callback--import ExampleDsl.OcpMonad--myDae :: SXElement -> DaeMonad ()-myDae _time = do- (_,p') <- diffState "p"- (v,v') <- diffState "v"- (m,m') <- diffState "m"- (u,u') <- diffState "u"- u'' <- control "u'"-- let g = 9.8- force = u - m*g-- output "force" force-- p' === v- v' === force/m- m' === -1e-2*u**2- u'' === u'--boundaryConditions :: (String -> BCMonad SXElement) -> (String -> BCMonad SXElement) -> BCMonad ()-boundaryConditions get0 getF = do- -- initial- p0 <- get0 "p"- v0 <- get0 "v"- m0 <- get0 "m"-- p0 === 1- v0 === 0- m0 === 10-- -- final- pF <- getF "p"- vF <- getF "v"-- pF === 0- vF === 0--mayer :: (Floating a, Monad m) => a -> (String -> m a) -> (String -> m a) -> m a-mayer _endTime _get0 getF = do- m <- getF "m"-- return (-m) -- endTime -- (p**2 + v**2)--myOcp :: SXElement -> (String -> OcpMonad SXElement) -> OcpMonad ()-myOcp _time get = do- p <- get "p"- v <- get "v"- m <- get "m"- u <- get "u"- u' <- get "u'"-- -200 <== u- u <== 200-- -100 <== u'- u' <== 100-- 0.01 <== m-- 0 <== p-- -10 <== v- v <== 0.0-- lagrangeTerm (1e-4*u'*u')- --lagrangeTerm (1e-8*u*u + 1e-9*p*p + 1e-9*v*v + 1e-9*m*m)- --lagrangeTerm (1e-6*u*u + 1e-6*p*p + 1e-6*v*v + 1e-6*m*m)--main :: IO ()-main = void $ withCallback go- where- n = 100- deg = 3- tbnds = (Just 0.2, Just 6)- --tbnds = (Just 1.5, Just 1.5)- 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
@@ -1,72 +0,0 @@-{-# OPTIONS_GHC -Wall #-}--module Main ( main ) where--import Control.Monad ( void )--import Dyno.Solvers--import Dyno.DirectCollocation.Quadratures ( QuadratureRoots(..) )-import Dynoplot.Callback-import ExampleDsl.OcpMonad--myDae :: SXElement -> DaeMonad ()-myDae time = do- (p,p') <- diffState "p"- (v,v') <- diffState "v"- u <- control "u"-- let k = 4- b = 0.3-- force = u - k * p - b * v- obj = p**2 + v**2 + u**2- output "force" force- output "obj" obj-- p' === v- v' === force + 0.1 * sin time--boundaryConditions :: (String -> BCMonad SXElement) -> (String -> BCMonad SXElement) -> BCMonad ()-boundaryConditions get0 getF = do- p0 <- get0 "p"- v0 <- get0 "v"-- pF <- getF "p"- vF <- getF "v"-- p0 === 0- v0 === 0---- p0 + 4 <== pF -- inequalities missing for now--- v0 === vF- pF === 1- vF === 0--mayer :: (Floating a, Monad m) => a -> (String -> m a) -> (String -> m a) -> m a-mayer endTime _get0 getF = do- p <- getF "p"- v <- getF "v"-- return (p**2 + v**2 + endTime/1000)--myOcp :: SXElement -> (String -> OcpMonad SXElement) -> OcpMonad ()-myOcp time get = do- v <- get "v"- u <- get "u"- force <- get "force"- obj <- get "obj"-- v**2 + u**2 <== 4 + time/100-- lagrangeTerm (obj + force*force*1e-4)--main :: IO ()-main = void $ withCallback go- where- n = 100- deg = 3- tbnds = (Just 4, Just 4)- go cb = solveStaticOcp Legendre ipoptSolver myDae mayer boundaryConditions myOcp tbnds n deg (Just cb')- where- cb' meta x = cb (x, meta)
+ examples/Rocket.hs view
@@ -0,0 +1,169 @@+{-# OPTIONS_GHC -Wall #-}+{-# Language TypeFamilies #-}+{-# Language DeriveFunctor #-}+{-# Language DeriveGeneric #-}+{-# Language DataKinds #-}++module Main ( main ) where++import GHC.Generics ( Generic, Generic1 )++import Data.Vector ( Vector )++import Accessors ( Lookup )++import Dyno.View.View ( J, jfill )+import Dyno.Nlp ( Bounds )+import Dyno.Ocp+import Dyno.Vectorize ( Vectorize, None(..), fill )+import Dyno.Solvers ( Solver(..), Opt(..), ipoptSolver )+import Dyno.NlpUtils ( solveNlp )+import Dyno.DirectCollocation.Formulate ( CollProblem(..), makeCollProblem )+import Dyno.DirectCollocation.Types ( CollTraj' )+import Dyno.DirectCollocation.Dynamic ( toMeta )+import Dyno.DirectCollocation.Quadratures ( QuadratureRoots(..) )+import Dynoplot.Callback ( withCallback )++rocketOcp :: OcpPhase' RocketOcp+rocketOcp =+ OcpPhase+ { ocpMayer = mayer+ , ocpLagrange = lagrange+ , ocpQuadratures = \_ _ _ _ _ _ _ -> None+ , ocpDae = dae+ , ocpBc = bc+ , ocpPathC = pathC+ , ocpBcBnds = bcBnds+ , ocpPathCBnds = pathCBnds+ , ocpXbnd = RocketX+ { xPos = (Just 0, Nothing)+ , xVel = (Just (-10), Just 0)+ , xMass = (Just 0.01, Nothing)+ , xThrust = (Just (-200), Just 200)+ }+ , ocpZbnd = fill (Nothing, Nothing)+ , ocpUbnd = RocketU+ { uThrustDot = (Just (-100), Just 100) }+ , ocpPbnd = fill (Nothing, Nothing)+ , ocpTbnd = (Just 4, Just 4)+ , ocpObjScale = Nothing+ , ocpTScale = Nothing+ , ocpXScale = Nothing+ , ocpZScale = Nothing+ , ocpUScale = Nothing+ , ocpPScale = Nothing+ , ocpResidualScale = Nothing+ , ocpBcScale = Nothing+ , ocpPathCScale = Nothing+ }++data RocketOcp+type instance X RocketOcp = RocketX+type instance O RocketOcp = RocketO+type instance R RocketOcp = RocketX+type instance U RocketOcp = RocketU+type instance C RocketOcp = RocketBc+type instance H RocketOcp = RocketPathC+type instance P RocketOcp = None+type instance Z RocketOcp = None+type instance Q RocketOcp = None++data RocketX a =+ RocketX+ { xPos :: a+ , xVel :: a+ , xMass :: a+ , xThrust :: a+ } deriving (Functor, Generic, Generic1)+data RocketU a =+ RocketU+ { uThrustDot :: a+ } deriving (Functor, Generic, Generic1)+data RocketO a =+ RocketO+ { oForce :: a+ } deriving (Functor, Generic, Generic1)+data RocketBc a =+ RocketBc+ { bcX0 :: RocketX a+ , bcXF :: RocketX a+ } deriving (Functor, Generic, Generic1)+data RocketPathC a = RocketPathC a deriving (Functor, Generic, Generic1)+instance Vectorize RocketX+instance Vectorize RocketU+instance Vectorize RocketO+instance Vectorize RocketBc+instance Vectorize RocketPathC+instance Lookup a => Lookup (RocketX a)+instance Lookup a => Lookup (RocketU a)+instance Lookup a => Lookup (RocketO a)+instance Lookup a => Lookup (RocketBc a)+instance Lookup a => Lookup (RocketPathC a)+++dae :: Floating a+ => RocketX a -> RocketX a -> None a -> RocketU a -> None a -> a+ -> (RocketX a, RocketO a)+dae (RocketX p' v' m' thrust') (RocketX _ v m thrust) _ (RocketU uThrust') _ _ =+ (residual, outputs)+ where+ residual = RocketX+ { xPos = p' - v+ , xVel = v' - force/m+ , xMass = m' - (-1e-2*thrust**2)+ , xThrust = thrust' - uThrust'+ }+ outputs = RocketO { oForce = force+ }+ g = 9.8+ force = thrust - m*g+++bc :: RocketX a -> RocketX a -> None a -> None a -> a -> RocketBc a+bc x0 xf _ _ _ = RocketBc x0 xf++bcBnds :: RocketBc Bounds+bcBnds =+ RocketBc+ { bcX0 = RocketX (Just 1, Just 1) (Just 0, Just 0) (Just 10, Just 10) (Nothing, Nothing)+ , bcXF = RocketX (Just 0, Just 0) (Just 0, Just 0) (Nothing, Nothing) (Nothing, Nothing)+ }++mayer :: Floating a => a -> RocketX a -> RocketX a -> None a -> None a -> a+mayer _endTime _ (RocketX _ _ mf _) _ _ = -mf -- endTime+++pathC :: Floating a => RocketX a -> None a -> RocketU a -> None a -> RocketO a -> a -> RocketPathC a+pathC _ _ _ _ _ = RocketPathC++pathCBnds :: RocketPathC Bounds+pathCBnds = RocketPathC (Nothing, Just 4)++lagrange :: Fractional a => RocketX a -> None a -> RocketU a -> None a -> RocketO a -> a -> a -> a+lagrange _ _ (RocketU u') _ _ _ _ = 1e-4*u'*u'+-- (1e-8*u*u + 1e-9*p*p + 1e-9*v*v + 1e-9*m*m)+-- (1e-6*u*u + 1e-6*p*p + 1e-6*v*v + 1e-6*m*m)++solver :: Solver+solver = ipoptSolver { options = [("expand", Opt True)] }++guess :: J (CollTraj' RocketOcp NCollStages CollDeg) (Vector Double)+guess = jfill 1++type NCollStages = 100+type CollDeg = 3++main :: IO ()+main = + withCallback $ \send -> do++ cp <- makeCollProblem Legendre rocketOcp guess+ let nlp = cpNlp cp+ meta = toMeta (cpMetaProxy cp)++ cb' traj = do+ plotPoints <- cpPlotPoints cp traj+ send (plotPoints, meta)++ _ <- solveNlp solver nlp (Just cb')+ return ()
examples/Sailboat.hs view
@@ -3,6 +3,7 @@ -- \Used with permission. {-# OPTIONS_GHC -Wall #-}+{-# Language TypeFamilies #-} {-# Language ScopedTypeVariables #-} {-# Language FlexibleInstances #-} {-# Language DeriveFunctor #-}@@ -16,14 +17,15 @@ import GHC.Generics ( Generic, Generic1 ) -import Data.Proxy ( Proxy(..) ) import Data.Vector ( Vector ) import qualified System.ZMQ4 as ZMQ import Linear -- ( V2(..) ) import qualified Data.List.NonEmpty as NE-import qualified Data.ByteString.Lazy as BSL+--import qualified Data.ByteString.Lazy as BSL+import qualified Data.ByteString as BS import qualified Data.ByteString.Char8 as BS8-import qualified Data.Binary as B+--import qualified Data.Binary as B+import qualified Data.Serialize as Ser import Text.Printf ( printf ) import Accessors ( Lookup )@@ -33,13 +35,24 @@ import Dyno.View.JV ( splitJV ) import Dyno.Solvers import Dyno.NlpUtils-import Dyno.Nlp+import Dyno.Nlp ( NlpOut(..) ) import Dyno.Ocp-import Dyno.DirectCollocation+import Dyno.DirectCollocation.Formulate+import Dyno.DirectCollocation.Types import Dyno.DirectCollocation.Quadratures ( QuadratureRoots(..) )-import Dyno.DirectCollocation.Formulate ( makeGuess ) import Dyno.DirectCollocation.Dynamic +data SailboatOcp+type instance X SailboatOcp = SbX+type instance Z SailboatOcp = SbZ+type instance U SailboatOcp = SbU+type instance P SailboatOcp = SbP+type instance R SailboatOcp = SbR+type instance O SailboatOcp = SbO+type instance C SailboatOcp = SbBc+type instance H SailboatOcp = None+type instance Q SailboatOcp = None+ data SbX a = SbX { xGamma :: a , xP :: V2 a , xV :: V2 a@@ -76,10 +89,10 @@ instance Lookup (SbO ()) ------------------------------ zmq helpers --------------------------------------newtype Packed = Packed { unPacked :: BSL.ByteString }+newtype Packed = Packed { unPacked :: BS.ByteString } -encodeSerial :: B.Binary a => a -> Packed-encodeSerial = Packed . B.encode+encodeSerial :: Ser.Serialize a => a -> Packed+encodeSerial = Packed . Ser.encode -------------------------------------------------------------------------- norm2sqr :: Num a => V2 a -> a@@ -196,7 +209,7 @@ pathc :: t -> t1 -> t2 -> t3 -> t4 -> t5 -> None a pathc _ _ _ _ _ _ = None -ocp :: OcpPhase SbX SbZ SbU SbP SbR SbO SbBc None None+ocp :: OcpPhase' SailboatOcp ocp = OcpPhase { ocpMayer = mayer , ocpLagrange = lagrange , ocpQuadratures = \_ _ _ _ _ _ _ -> None@@ -237,11 +250,12 @@ let send :: String -> Packed -> IO () send channel msg = ZMQ.sendMulti publisher (NE.fromList [ BS8.pack channel- , BSL.toStrict (unPacked msg)+ , unPacked msg+-- , BSL.toStrict (unPacked msg) ]) f send -initialGuess :: CollTraj SbX SbZ SbU SbP NCollStages CollDeg (Vector Double)+initialGuess :: CollTraj' SailboatOcp NCollStages CollDeg (Vector Double) initialGuess = makeGuess Legendre tf guessX (const SbZ) guessU SbP where tf = 20@@ -263,18 +277,14 @@ main :: IO () main = do- cp <- makeCollProblem Legendre ocp+ cp <- makeCollProblem Legendre ocp (cat initialGuess) let nlp = cpNlp cp ZMQ.withContext $ \context -> withPublisher context urlDynoPlot $ \sendDynoPlotMsg -> do -- withPublisher context urlOptTelem $ \sendOptTelemMsg -> do- let guess = cat initialGuess- proxy :: Proxy (CollTraj SbX SbZ SbU SbP NCollStages CollDeg)- proxy = Proxy- meta = toMeta (Proxy :: Proxy None) (Proxy :: Proxy SbO) proxy+ let meta = toMeta (cpMetaProxy cp) - callback :: J (CollTraj SbX SbZ SbU SbP NCollStages CollDeg) (Vector Double)- -> IO Bool+ callback :: J (CollTraj' SailboatOcp NCollStages CollDeg) (Vector Double) -> IO Bool callback traj = do plotPoints <- cpPlotPoints cp traj -- dynoplot@@ -317,7 +327,7 @@ -- sendOptTelemMsg "opt_telem" (encodeProto optTelemMsg) return True - (msg0,opt0') <- solveNlp solver (nlp { nlpX0 = guess }) (Just callback)+ (msg0,opt0') <- solveNlp solver nlp (Just callback) opt0 <- case msg0 of Left msg' -> error msg' Right _ -> return opt0' let CollTraj endTime' _ _ xf = split (xOpt opt0)
examples/Sofa/Common.hs view
@@ -17,7 +17,7 @@ import GHC.Generics ( Generic, Generic1 ) import qualified Data.Foldable as F-import Data.Binary+import Data.Serialize import Dyno.TypeVecs ( Vec, Dim ) import qualified Dyno.TypeVecs as TV@@ -52,8 +52,8 @@ , smMeanThetas :: [(Point Double, Double)] } deriving Generic -instance Binary SofaMessage-instance Binary a => Binary (Point a)+instance Serialize SofaMessage+instance Serialize a => Serialize (Point a) url :: String url = "tcp://127.0.0.1:5563"
examples/SofaExpando.hs view
@@ -13,10 +13,10 @@ import Data.Proxy ( Proxy(..) ) import Data.IORef ( newIORef, readIORef, writeIORef ) import qualified Data.Foldable as F-import Data.Binary+import Data.Serialize import qualified System.ZMQ4 as ZMQ import Data.ByteString.Char8 ( pack )-import Data.ByteString.Lazy ( toStrict )+--import Data.ByteString.Lazy ( toStrict ) import Dyno.Vectorize import Dyno.Nlp@@ -234,11 +234,12 @@ solver = ipoptSolver { options = [("ma86_order", Opt "metis"), ("max_iter", Opt (1000 :: Int))]} --solver = snoptSolver { options = [ ("detect_linear", Opt False) ] } -send :: Binary a => ZMQ.Socket ZMQ.Pub -> String -> a -> IO ()+send :: Serialize a => ZMQ.Socket ZMQ.Pub -> String -> a -> IO () send publisher chanName stuff = do let bs = encode stuff ZMQ.send publisher [ZMQ.SendMore] (pack chanName)- ZMQ.send publisher [] (toStrict bs)+ ZMQ.send publisher [] bs+-- ZMQ.send publisher [] (toStrict bs) main :: IO () main =
examples/SofaVisualizer.hs view
@@ -8,8 +8,9 @@ import Linear.Quaternion ( Quaternion(..) ) import Control.Monad ( when, forever ) import Data.ByteString.Char8 ( pack )-import Data.ByteString.Lazy ( fromStrict )-import Data.Binary+--import Data.ByteString.Lazy ( fromStrict )+--import Data.Binary+import Data.Serialize import qualified System.ZMQ4 as ZMQ import qualified Control.Concurrent.STM as STM import qualified Control.Concurrent as CC@@ -43,9 +44,12 @@ when mre $ do msg <- ZMQ.receive subscriber let decoded :: SofaMessage- decoded = case decodeOrFail (fromStrict msg) of- Left (_,_,err) -> error $ "decode failure: " ++ err- Right (_,_,t) -> t+ decoded = case decode msg of+ Left err -> error $ "decode failure: " ++ err+ Right t -> t+-- decoded = case decodeOrFail (fromStrict msg) of+-- Left (_,_,err) -> error $ "decode failure: " ++ err+-- Right (_,_,t) -> t writeChan decoded main :: IO ()
+ examples/Spring.hs view
@@ -0,0 +1,157 @@+{-# OPTIONS_GHC -Wall #-}+{-# Language TypeFamilies #-}+{-# Language DeriveFunctor #-}+{-# Language DeriveGeneric #-}+{-# Language DataKinds #-}++module Main ( SpringX(..), SpringU(..), main ) where++import GHC.Generics ( Generic, Generic1 )++import Data.Vector ( Vector )++import Accessors ( Lookup )++import Dyno.View.View ( J, jfill )+import Dyno.Nlp ( Bounds )+import Dyno.Ocp+import Dyno.Vectorize ( Vectorize, None(..), fill )+import Dyno.Solvers ( Solver(..), Opt(..), ipoptSolver )+import Dyno.NlpUtils ( solveNlp )+import Dyno.DirectCollocation.Formulate ( CollProblem(..), makeCollProblem )+import Dyno.DirectCollocation.Types ( CollTraj' )+import Dyno.DirectCollocation.Dynamic ( toMeta )+import Dyno.DirectCollocation.Quadratures ( QuadratureRoots(..) )+import Dynoplot.Callback ( withCallback )++springOcp :: OcpPhase' SpringOcp+springOcp =+ OcpPhase+ { ocpMayer = mayer+ , ocpLagrange = lagrange+ , ocpQuadratures = \_ _ _ _ _ _ _ -> None+ , ocpDae = dae+ , ocpBc = bc+ , ocpPathC = pathC+ , ocpBcBnds = bcBnds+ , ocpPathCBnds = pathCBnds+ , ocpXbnd = fill (Nothing, Nothing)+ , ocpZbnd = fill (Nothing, Nothing)+ , ocpUbnd = fill (Nothing, Nothing)+ , ocpPbnd = fill (Nothing, Nothing)+ , ocpTbnd = (Just 4, Just 4)+ , ocpObjScale = Nothing+ , ocpTScale = Nothing+ , ocpXScale = Nothing+ , ocpZScale = Nothing+ , ocpUScale = Nothing+ , ocpPScale = Nothing+ , ocpResidualScale = Nothing+ , ocpBcScale = Nothing+ , ocpPathCScale = Nothing+ }++data SpringOcp+type instance X SpringOcp = SpringX+type instance O SpringOcp = SpringO+type instance R SpringOcp = SpringX+type instance U SpringOcp = SpringU+type instance C SpringOcp = SpringBc+type instance H SpringOcp = SpringPathC+type instance P SpringOcp = None+type instance Z SpringOcp = None+type instance Q SpringOcp = None++data SpringX a =+ SpringX+ { xPos :: a+ , xVel :: a+ } deriving (Functor, Generic, Generic1)+data SpringU a =+ SpringU+ { uMotor :: a+ } deriving (Functor, Generic, Generic1)+data SpringO a =+ SpringO+ { oForce :: a+ , oObj :: a+ } deriving (Functor, Generic, Generic1)+data SpringBc a =+ SpringBc+ { bcX0 :: SpringX a+ , bcXF :: SpringX a+ } deriving (Functor, Generic, Generic1)+data SpringPathC a = SpringPathC a deriving (Functor, Generic, Generic1)+instance Vectorize SpringX+instance Vectorize SpringU+instance Vectorize SpringO+instance Vectorize SpringBc+instance Vectorize SpringPathC+instance Lookup a => Lookup (SpringX a)+instance Lookup a => Lookup (SpringU a)+instance Lookup a => Lookup (SpringO a)+instance Lookup a => Lookup (SpringBc a)+instance Lookup a => Lookup (SpringPathC a)++dae :: Floating a+ => SpringX a -> SpringX a -> None a -> SpringU a -> None a -> a+ -> (SpringX a, SpringO a)+dae (SpringX p' v') (SpringX p v) _ (SpringU u) _ t =+ (residual, outputs)+ where+ residual = SpringX (p' - v) (v' - force)+ outputs = SpringO { oForce = force+ , oObj = obj+ }+ k = 4+ b = 0.3+ + force = u - k * p - b * v + 0.1 * sin t+ obj = p**2 + v**2 + u**2++bc :: SpringX a -> SpringX a -> None a -> None a -> a -> SpringBc a+bc x0 xf _ _ _ = SpringBc x0 xf++bcBnds :: SpringBc Bounds+bcBnds =+ SpringBc+ { bcX0 = SpringX (Just 0, Just 0) (Just 0, Just 0)+ , bcXF = SpringX (Just 1, Just 1) (Just 0, Just 0)+ }++mayer :: Floating a => a -> SpringX a -> SpringX a -> None a -> None a -> a+mayer endTime _ (SpringX pf vf) _ _ = (pf**2 + vf**2 + endTime/1000)++pathC :: Floating a => SpringX a -> None a -> SpringU a -> None a -> SpringO a -> a -> SpringPathC a+pathC (SpringX _ v) _ (SpringU u) _ _ time =+ SpringPathC (v**2 + u**2 - time/100)++pathCBnds :: SpringPathC Bounds+pathCBnds = SpringPathC (Nothing, Just 4)++lagrange :: Fractional a => SpringX a -> None a -> SpringU a -> None a -> SpringO a -> a -> a -> a+lagrange _ _ _ _ (SpringO force obj) _ _ = obj + force*force*1e-4++solver :: Solver+solver = ipoptSolver { options = [("expand", Opt True)] }++guess :: J (CollTraj' SpringOcp NCollStages CollDeg) (Vector Double)+guess = jfill 1++type NCollStages = 100+type CollDeg = 3++main :: IO ()+main = + withCallback $ \send -> do++ cp <- makeCollProblem Legendre springOcp guess+ let nlp = cpNlp cp+ meta = toMeta (cpMetaProxy cp)++ cb' traj = do+ plotPoints <- cpPlotPoints cp traj+ send (plotPoints, meta)++ _ <- solveNlp solver nlp (Just cb')+ return ()
+ examples/beginner/SimpleQp.hs view
@@ -0,0 +1,48 @@+{-# OPTIONS_GHC -Wall #-}+{-# Language DeriveFunctor #-}+{-# Language DeriveGeneric #-}++module Main where++import GHC.Generics ( Generic, Generic1 )++import Dyno.Vectorize+import Dyno.NlpUtils+import Dyno.Solvers++-- minimize: x0**2 + 2*x1**2+-- subject to: 1 <= 2*x0 + x1 <= inf+-- -inf <= x0 <= 10+-- -20 <= x1 <= inf++-- design variable+data X a = X a a deriving (Functor, Generic, Generic1, Show)+instance Vectorize X++-- constraint+data G a = G a deriving (Functor, Generic, Generic1, Show)+instance Vectorize G++-- objective/constraint function+fg :: Num a => X a -> (a, G a)+fg (X x0 x1) = (f, g)+ where+ f = x0*x0 + 2*x1*x1+ g = G (2*x0 + x1)++-- design variable bounds+bx :: X (Maybe Double, Maybe Double)+bx = X (Nothing, Just 10) (Just (-20), Nothing)++-- constraint bounds+bg :: G (Maybe Double, Maybe Double)+bg = G (Just 1, Nothing)++-- initial guess+xguess :: X Double+xguess = X 2 3++main :: IO ()+main = do+ opt <- solveNlpV ipoptSolver fg bx bg xguess Nothing+ print opt
src/Dyno/AutoScaling.hs view
@@ -18,7 +18,7 @@ import Casadi.Sparsity ( getRow, getCol ) import Casadi.SX ( SX )-import Casadi.DMatrix ( DMatrix, ddata )+import Casadi.DMatrix ( DMatrix, dnonzeros ) import qualified Casadi.CMatrix as CM import Dyno.View.Unsafe.View ( mkJ, unJ )@@ -42,7 +42,7 @@ mat = unM $ M.sparse mat0 sp = CM.sparsity mat- dat = ddata mat+ dat = dnonzeros mat row = getRow sp col = getCol sp
− src/Dyno/DirectCollocation.hs
@@ -1,47 +0,0 @@-{-# OPTIONS_GHC -Wall #-}-{-# Language ScopedTypeVariables #-}--module Dyno.DirectCollocation- ( CollTraj(..)- , CollProblem(..)- , makeCollProblem- , solveOcp- ) where--import Data.Proxy-import Data.Vector ( Vector )--import Dyno.View.View ( J, jfill )-import Dyno.Vectorize ( Vectorize )-import Dyno.Ocp ( OcpPhase )-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 q .- (Vectorize x, Vectorize z, Vectorize u, Vectorize p,- 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 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 roots ocp- let nlp = cpNlp cp- toPlotPoints = cpPlotPoints cp- --_ <- 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- return res
src/Dyno/DirectCollocation/Dynamic.hs view
@@ -7,8 +7,8 @@ ( DynPlotPoints , CollTrajMeta(..) , addCollocationChannel+ , MetaProxy(..) , toMeta- , toMetaCov , dynPlotPoints , catDynPlotPoints , NameTree(..)@@ -19,12 +19,15 @@ import Data.Proxy ( Proxy(..) ) import Data.List ( mapAccumL ) import Data.Tree ( Tree(..) )+import Data.Vector.Cereal ()+import Data.Vector.Binary () 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.Binary ( Binary )+import Data.Serialize ( Serialize ) import Linear.V import Accessors ( AccessorTree(..), Lookup(..), accessors )@@ -72,31 +75,49 @@ (Vector (Vector (a, Vector a))) (Vector (Vector (a, Vector a))) (Vector (Vector (a, Vector a)))+ (Vector (Vector (a, Vector a))) deriving Generic -instance Binary a => Binary (DynPlotPoints a) +--instance Binary a => Binary (DynPlotPoints a) -- binary is slower than serial by 2x on this+instance Serialize a => Serialize (DynPlotPoints a)+ catDynPlotPoints :: V.Vector (DynPlotPoints a) -> DynPlotPoints a catDynPlotPoints pps = DynPlotPoints- (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)+ (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)+ (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,- Vectorize x, Vectorize z, Vectorize u, Vectorize o, Vectorize p)+ forall x z u p h o n deg a .+ ( Dim n, Dim deg, Real a, Fractional a, Show a+ , Vectorize x, Vectorize z, Vectorize u, Vectorize o, Vectorize p, Vectorize h+ ) => QuadratureRoots -> CollTraj x z u p n deg (Vector a)- -> Vec n (Vec deg (J (JV o) (Vector a), J (JV x) (Vector a)), J (JV x) (Vector a))+ -> Vec n ( Vec deg ( J (JV o) (Vector a)+ , J (JV x) (Vector a)+ , J (JV h) (Vector a)+ )+ , J (JV x) (Vector a)+ ) -> DynPlotPoints a-dynPlotPoints quadratureRoots (CollTraj tf' _ stages' xf) outputs =- DynPlotPoints xss' zss uss oss xdss+dynPlotPoints quadratureRoots (CollTraj tf' _ stages' xf) outputs+ -- if degree is one, each arc will be 1 point and won't get drawn+ -- see https://github.com/ghorn/dynobud/issues/72+ -- https://github.com/ghorn/Plot-ho-matic/issues/10+ -- https://github.com/timbod7/haskell-chart/issues/81+ | reflectDim (Proxy :: Proxy deg) == 1 =+ DynPlotPoints xss' (singleArc zss) (singleArc uss) (singleArc oss) (singleArc xdss) (singleArc hss)+ | otherwise = DynPlotPoints xss' zss uss oss xdss hss where+ singleArc :: Vector (Vector b) -> Vector (Vector b)+ singleArc = V.singleton . V.concat . V.toList nStages = size (Proxy :: Proxy (JVec n (JV Id))) tf,h :: a Id tf = splitJV tf'@@ -110,16 +131,16 @@ xss' = xss `V.snoc` (V.singleton (tf, unJ xf)) - xss,zss,uss,oss,xdss :: Vector (Vector (a, Vector a))- (xss,zss,uss,oss,xdss) = V.unzip5 xzuoxds+ xss,zss,uss,oss,xdss,hss :: Vector (Vector (a, Vector a))+ (xss,zss,uss,oss,xdss,hss) = V.unzip6 xzuoxdhs -- todo: check this final time tf'' against expected tf- (_tf'', xzuoxds) = T.mapAccumL f 0 $ V.zip (TV.unVec stages) (TV.unVec outputs)+ (_tf'', xzuoxdhs) = 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))+ , (Vec deg (J (JV o) (Vector a), J (JV x) (Vector a), J (JV h) (Vector a)), J (JV x) (Vector a)) ) -> ( a , ( V.Vector (a, V.Vector a)@@ -127,9 +148,10 @@ , 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))+ f t0 (CollStage x0 xzus', (xdohs, xnext)) = (tnext, (xs,zs,us,os,xds,hs)) where tnext = t0 + h xzus0 = fmap split (unJVec (split xzus')) :: Vec deg (CollPoint (JV x) (JV z) (JV u) (Vector a))@@ -137,15 +159,17 @@ xs :: V.Vector (a, V.Vector a) xs = (t0, unJ x0) `V.cons` xs' `V.snoc` (tnext,unJ xnext) - xs',zs,us,os,xds :: Vector (a, Vector a)- (xs',zs,us,os,xds) = V.unzip5 $ TV.unVec $ TV.tvzipWith3 g xzus0 xdos taus+ xs',zs,us,os,xds,hs :: Vector (a, Vector a)+ (xs',zs,us,os,xds,hs) = V.unzip6 $ TV.unVec $ TV.tvzipWith3 g xzus0 xdohs 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',pathc) tau =+ ( (t,unJ' "x" x)+ , (t,unJ' "z" z)+ , (t,unJ' "u" u)+ , (t,unJ' "o" o)+ , (t,unJ' "x'" x')+ , (t,unJ' "h" pathc)+ ) where t = t0 + h*tau @@ -155,6 +179,7 @@ | NameTreeLeaf Int deriving (Show, Eq, Generic) instance Binary NameTree+instance Serialize NameTree data CollTrajMeta = CollTrajMeta { ctmX :: NameTree , ctmZ :: NameTree@@ -162,8 +187,10 @@ , ctmP :: NameTree , ctmO :: NameTree , ctmQ :: NameTree+ , ctmH :: NameTree } deriving (Eq, Generic, Show) instance Binary CollTrajMeta+instance Serialize CollTrajMeta namesFromAccTree :: AccessorTree a -> NameTree namesFromAccTree x = (\(_,(_,y)) -> y) $ namesFromAccTree' 0 ("",x)@@ -178,13 +205,14 @@ type MetaTree a = Tree.Forest (String, String, Maybe ((DynPlotPoints a, CollTrajMeta) -> [[(a,a)]])) forestFromMeta :: CollTrajMeta -> MetaTree Double-forestFromMeta meta = [xTree,zTree,uTree,oTree,xdTree]+forestFromMeta meta = [xTree,zTree,uTree,oTree,xdTree,hTree] where- xTree = blah (\(DynPlotPoints x _ _ _ _ ) -> x) "differential states" (ctmX meta)- zTree = blah (\(DynPlotPoints _ z _ _ _ ) -> z) "algebraic variables" (ctmZ meta)- uTree = blah (\(DynPlotPoints _ _ u _ _ ) -> u) "controls" (ctmU meta)- oTree = blah (\(DynPlotPoints _ _ _ o _ ) -> o) "outputs" (ctmO meta)- xdTree = blah (\(DynPlotPoints _ _ _ _ xd) -> xd) "diff state derivatives" (ctmX meta)+ xTree = blah (\(DynPlotPoints x _ _ _ _ _) -> x) "differential states" (ctmX meta)+ zTree = blah (\(DynPlotPoints _ z _ _ _ _) -> z) "algebraic variables" (ctmZ meta)+ uTree = blah (\(DynPlotPoints _ _ u _ _ _) -> u) "controls" (ctmU meta)+ oTree = blah (\(DynPlotPoints _ _ _ o _ _) -> o) "outputs" (ctmO meta)+ xdTree = blah (\(DynPlotPoints _ _ _ _ xd _) -> xd) "diff state derivatives" (ctmX meta)+ hTree = blah (\(DynPlotPoints _ _ _ _ _ h) -> h) "path constraints" (ctmH meta) blah :: forall f c t . (Functor f, F.Foldable f)@@ -198,23 +226,22 @@ woo = F.toList . fmap (F.toList . fmap (\(t,x) -> (t, x V.! k))) -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)- => 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 ())- }+data MetaProxy x z u p o q h = MetaProxy -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)- => 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))+toMeta :: forall x z u p o q h .+ ( Lookup (x ()), Lookup (z ()), Lookup (u ()), Lookup (p ()), Lookup (o ()), Lookup (q ())+ , Lookup (h ())+ , Vectorize x, Vectorize z, Vectorize u, Vectorize p, Vectorize o, Vectorize q+ , Vectorize h+ )+ => MetaProxy x z u p o q h -> 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 ())+ , ctmH = namesFromAccTree $ accessors (fill () :: h ())+ }
src/Dyno/DirectCollocation/Export.hs view
@@ -16,8 +16,9 @@ import Dyno.View.Unsafe.View ( unJ ) +import Dyno.Nlp ( NlpOut(..) ) import Dyno.TypeVecs ( Vec )-import Dyno.Vectorize ( Vectorize, fill )+import Dyno.Vectorize ( Vectorize, Id(..), fill ) import Dyno.View.View ( View(..) ) import Dyno.View.JV ( JV, splitJV ) import Dyno.View.JVec ( JVec(..) )@@ -26,19 +27,22 @@ import Dyno.DirectCollocation.Quadratures ( timesFromTaus ) toMatlab ::- forall x z u p r c h o q n deg+ forall x z u p r o c h q n deg lol . ( Lookup (x Double), Vectorize x , Lookup (z Double), Vectorize z , Lookup (u Double), Vectorize u , Lookup (o Double), Vectorize o , Lookup (p Double), Vectorize p- , Dim deg- , Dim n+ , Lookup (h Double), Vectorize h+ , Dim n, Dim deg )- => CollProblem x z u p r c h o q n deg- -> CollTraj x z u p n deg (Vector Double)+ => CollProblem x z u p r o c h q n deg+ -> NlpOut (CollTraj x z u p n deg) lol (Vector Double) -> IO String-toMatlab cp ct@(CollTraj tf' p' stages' xf) = do+toMatlab cp nlpOut = do+ let ct@(CollTraj tf' p' stages' xf) = split (xOpt nlpOut)+ CollTraj lagTf' lagP' _ _ = split (lambdaXOpt nlpOut)+ outs <- cpOutputs cp (cat ct) let taus :: Vec deg Double@@ -69,7 +73,8 @@ os :: [o Double] xdots :: [x Double]- (os, xdots) = unzip $ F.concatMap (F.toList . fst) outs -- drop the interpolated value+ hs :: [h Double]+ (os, xdots, hs) = unzip3 $ F.concatMap (F.toList . fst) outs -- drop the interpolated value getXs (CollStage x0 xzus) = splitJV x0 : map (getX . split) (F.toList (unJVec (split xzus))) getZs (CollStage _ xzus) = map (getZ . split) (F.toList (unJVec (split xzus)))@@ -87,13 +92,11 @@ at :: (Vectorize xzu, Lookup (xzu Double)) => [(String, xzu Double -> Double)] at = flatten $ accessors (fill 0) - p = splitJV p'- woo :: String -> [xzu Double] -> String -> (xzu Double -> Double) -> String woo topName xzus name get = topName ++ "." ++ name ++ " = " ++ show (map get xzus) ++ ";" - wooP :: String -> (p Double -> Double) -> String- wooP name get = "params." ++ name ++ " = " ++ show (get p) ++ ";"+ wooP :: String -> p Double -> String -> (p Double -> Double) -> String+ wooP topName p name get = topName ++ "." ++ name ++ " = " ++ show (get p) ++ ";" ret :: String ret = init $ unlines $@@ -102,8 +105,11 @@ map (uncurry (woo "ret.algVars" zs)) at ++ map (uncurry (woo "ret.controls" us)) at ++ map (uncurry (woo "ret.outputs" os)) at ++- map (uncurry wooP) at ++- [ ""+ map (uncurry (woo "ret.pathConstraints" hs)) at +++ map (uncurry (wooP "ret.params" (splitJV p'))) at +++ map (uncurry (wooP "ret.lagrangeMultipliers.params" (splitJV lagP'))) at +++ [ "ret.lagrangeMultipliers.T = " ++ show (unId (splitJV lagTf'))+ , "" , "ret.tx = " ++ show xTimes , "ret.tzuo = " ++ show zuoTimes , "ret.N = " ++ show n
src/Dyno/DirectCollocation/Formulate.hs view
@@ -1,5 +1,5 @@ {-# OPTIONS_GHC -Wall #-}---{-# OPTIONS_GHC -fdefer-type-errors #-}+{-# Language TypeFamilies #-} {-# Language DeriveGeneric #-} {-# Language ScopedTypeVariables #-} {-# Language TypeOperators #-}@@ -9,8 +9,8 @@ module Dyno.DirectCollocation.Formulate ( CovTraj(..) , CollProblem(..)- , CollCovProblem(..) , makeCollProblem+ , CollCovProblem(..) , makeCollCovProblem , mkTaus , makeGuess@@ -48,29 +48,42 @@ import qualified Dyno.TypeVecs as TV import Dyno.LagrangePolynomials ( lagrangeDerivCoeffs ) import Dyno.Nlp ( Nlp(..), Bounds )-import Dyno.Ocp ( OcpPhase(..), OcpPhaseWithCov(..) )+import Dyno.Ocp import Dyno.DirectCollocation.Types-import Dyno.DirectCollocation.Dynamic ( DynPlotPoints, dynPlotPoints )+import Dyno.DirectCollocation.Dynamic ( MetaProxy(..), DynPlotPoints, dynPlotPoints ) import Dyno.DirectCollocation.Quadratures ( QuadratureRoots(..), mkTaus, interpolate, timesFromTaus ) import Dyno.DirectCollocation.Robust -data CollProblem x z u p r c h o q n deg =+data CollProblem x z u p r o c h q n deg = CollProblem- { cpNlp :: Nlp (CollTraj x z u p n deg) JNone (CollOcpConstraints n deg x r c h) MX+ { cpNlp :: Nlp (CollTraj x z u p n deg)+ JNone+ (CollOcpConstraints x r c h n deg) 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)+ , 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))+ , Vec n ( Vec deg ( J (JV o) (Vector Double)+ , J (JV x) (Vector Double)+ , J (JV h) (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))+ -> IO (Vec n ( Vec deg ( o Double+ , x Double+ , h Double+ )+ , x Double+ )+ ) , cpTaus :: Vec deg Double , cpRoots :: QuadratureRoots , cpEvalQuadratures :: Vec n (Vec deg Double) -> Double -> IO Double+ , cpMetaProxy :: MetaProxy x z u p o q h } makeCollProblem ::@@ -80,8 +93,9 @@ , 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+ -> J (CollTraj x z u p n deg) (Vector Double)+ -> IO (CollProblem x z u p r o c h q n deg)+makeCollProblem roots ocp guess = do let -- the collocation points taus :: Vec deg Double taus = mkTaus roots@@ -157,24 +171,46 @@ outputFun <- toMXFun "stageOutputs" $ outputFunction callInterpolate cijs taus dynFun -- prepare callbacks- 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')+ let f :: J (JV o) DMatrix -> J (JV x) DMatrix -> J (JV h) DMatrix+ -> (J (JV o) (Vector Double), J (JV x) (Vector Double), J (JV h) (Vector Double))+ f o' x' h' = (d2v o', d2v x', d2v h') callOutputFun :: J (JV p) (Vector Double) -> J (JV Id) (Vector Double) -> J (CollStage (JV x) (JV z) (JV u) deg) (Vector Double) -> J (JV Id) (Vector Double)- -> IO (Vec deg (J (JV o) (Vector Double), J (JV x) (Vector Double)), J (JV x) (Vector Double))+ -> IO ( Vec deg ( J (JV o) (Vector Double)+ , J (JV x) (Vector Double)+ , J (JV h) (Vector Double)+ )+ , J (JV x) (Vector Double)+ ) callOutputFun p h stage k = do+ let p' = v2d p (_ :*: xdot :*: out :*: xnext) <-- eval outputFun $ (v2d stage) :*: (v2d p) :*: (v2d h) :*: (v2d k)+ eval outputFun $ (v2d stage) :*: p' :*: (v2d h) :*: (v2d k)++ let stageTimes :: Vec deg (J (JV Id) DMatrix)+ stageTimes = fmap (\tau -> t0 + realToFrac tau * h') taus+ where+ t0 = h' * v2d k+ h' = v2d h+ CollStage _ collPoints = split stage+ hs <- eval pathStageConFun $ p' :*: (cat (JVec stageTimes)) :*: out :*: (v2d collPoints)+ let outs0 = unJVec (split out) :: Vec deg (J (JV o) DMatrix) xdots0 = unJVec (split xdot) :: Vec deg (J (JV x) DMatrix)- return (TV.tvzipWith f outs0 xdots0, d2v xnext)+ hs0 = unJVec (split hs) :: Vec deg (J (JV h) DMatrix)+ return (TV.tvzipWith3 f outs0 xdots0 hs0, d2v xnext) mapOutputFun :: J (CollTraj x z u p n deg) (Vector Double)- -> IO (Vec n (Vec deg (J (JV o) (Vector Double), J (JV x) (Vector Double)), J (JV x) (Vector Double)))+ -> IO (Vec n ( Vec deg ( J (JV o) (Vector Double)+ , J (JV x) (Vector Double)+ , J (JV h) (Vector Double)+ )+ , J (JV x) (Vector Double)+ )+ ) mapOutputFun ct = do let CollTraj tf p stages _ = split ct h = catJV $ Id (tf' / fromIntegral n)@@ -191,7 +227,10 @@ 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))+ , Vec n ( Vec deg ( J (JV o) (Vector Double)+ , J (JV x) (Vector Double)+ , J (JV h) (Vector Double)+ ) , J (JV x) (Vector Double) ) )@@ -204,15 +243,16 @@ 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))+ -> IO (Vec n (Vec deg (o Double, x Double, h Double), x Double)) getOutputs traj = do outputs <- mapOutputFun traj- let devec :: Vec deg (J (JV o) (Vector Double), J (JV x) (Vector Double))- -> Vec deg (o Double, x Double)- devec = fmap (\(x,y) -> (splitJV x, splitJV y))+ let devec :: Vec deg (J (JV o) (Vector Double), J (JV x) (Vector Double), J (JV h) (Vector Double))+ -> Vec deg (o Double, x Double, h Double)+ devec = fmap (\(x,y,z) -> (splitJV x, splitJV y, splitJV z)) return $ fmap (\(x,y) -> (devec x, splitJV y)) outputs - let nlp = Nlp {+ let nlp :: Nlp (CollTraj x z u p n deg) JNone (CollOcpConstraints x r c h n deg) MX+ nlp = Nlp { nlpFG = getFg taus (bcFun :: SXFun (J (JV x) :*: J (JV x) :*: J (JV q) :*: J (JV p) :*: J (JV Id)) (J (JV c)))@@ -231,7 +271,7 @@ (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+ , nlpX0 = guess :: J (CollTraj x z u p n deg) (Vector Double) , nlpP = cat JNone , nlpLamX0 = Nothing , nlpLamG0 = Nothing@@ -271,42 +311,53 @@ , cpTaus = taus , cpRoots = roots , cpEvalQuadratures = evalQuadratures+ , cpMetaProxy = MetaProxy } -data CollCovProblem x z u p r o c h n deg sx sw sh shr sc =+data CollCovProblem ocp n deg sx sw sh shr sc = CollCovProblem { ccpNlp :: Nlp- (CollTrajCov sx x z u p n deg)+ (CollTrajCov sx ocp n deg) JNone- (CollOcpCovConstraints n deg x r c h sh shr sc) MX- , ccpPlotPoints :: J (CollTrajCov sx x z u p n deg) (Vector Double) -> IO (DynPlotPoints Double)+ (CollOcpCovConstraints ocp n deg sh shr sc) MX+ , ccpPlotPoints :: J (CollTrajCov sx ocp n deg) (Vector Double) -> IO (DynPlotPoints Double) , ccpOutputs ::- J (CollTrajCov sx x z u p n deg) (Vector Double)- -> IO ( Vec n (Vec deg (o Double, x Double), x Double)+ J (CollTrajCov sx ocp n deg) (Vector Double)+ -> IO ( Vec n (Vec deg (O ocp Double, X ocp Double, H ocp Double), X ocp Double) , Vec n (J (Cov (JV sx)) (Vector Double)) , J (Cov (JV sx)) (Vector Double) ) , ccpSensitivities :: MXFun- (J (CollTraj x z u p n deg))+ (J (CollTraj' ocp n deg)) (CovarianceSensitivities (JV sx) (JV sw) n) , ccpCovariances :: MXFun- (J (CollTrajCov sx x z u p n deg)) (J (CovTraj sx n))+ (J (CollTrajCov sx ocp n deg)) (J (CovTraj sx n)) , ccpRoots :: QuadratureRoots } makeCollCovProblem ::- forall x z u p r o c h q sx sz sw sr sh shr sc deg n .+ forall ocp 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+ , x ~ X ocp+ , q ~ Q ocp+ , h ~ H ocp+ , c ~ C ocp+ , o ~ O ocp+ , r ~ R ocp+ , p ~ P ocp+ , u ~ U ocp+ , z ~ Z ocp ) => 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 roots ocp ocpCov = do+ -> OcpPhase' ocp+ -> OcpPhaseWithCov ocp sx sz sw sr sh shr sc+ -> J (CollTraj x z u p n deg) (Vector Double)+ -> IO (CollCovProblem ocp n deg sx sw sh shr sc)+makeCollCovProblem roots ocp ocpCov guess = do let -- the collocation points taus :: Vec deg Double taus = mkTaus roots@@ -322,7 +373,7 @@ lagrangeFun <- toSXFun "cov lagrange" $ \(x0:*:x1:*:x2:*:x3) -> sxCatJV $ Id $ ocpCovLagrange ocpCov (unId (sxSplitJV x0)) (sxSplitJV x1) x2 (unId (sxSplitJV x3)) - cp0 <- makeCollProblem roots ocp+ cp0 <- makeCollProblem roots ocp guess robustify <- mkRobustifyFunction (ocpCovProjection ocpCov) (ocpCovRobustifyPathC ocpCov) @@ -339,9 +390,9 @@ robustPathCUb = catJV rpathCUb -- the NLP- fg :: J (CollTrajCov sx x z u p n deg) MX+ fg :: J (CollTrajCov sx ocp n deg) MX -> J JNone MX- -> (J (JV Id) MX, J (CollOcpCovConstraints n deg x r c h sh shr sc) MX)+ -> (J (JV Id) MX, J (CollOcpCovConstraints ocp n deg sh shr sc) MX) fg = getFgCov taus computeCovariances gammas@@ -354,13 +405,13 @@ computeCovariancesFun' <- toMXFun "compute covariances" computeCovariances -- callbacks- let getPlotPoints :: J (CollTrajCov sx x z u p n deg) (Vector Double) -> IO (DynPlotPoints Double)+ let getPlotPoints :: J (CollTrajCov sx ocp n deg) (Vector Double) -> IO (DynPlotPoints Double) getPlotPoints collTrajCov = do let CollTrajCov _ collTraj = split collTrajCov cpPlotPoints cp0 collTraj - getOutputs :: J (CollTrajCov sx x z u p n deg) (Vector Double)- -> IO ( Vec n (Vec deg (o Double, x Double), x Double)+ getOutputs :: J (CollTrajCov sx ocp n deg) (Vector Double)+ -> IO ( Vec n (Vec deg (o Double, x Double, h Double), x Double) , Vec n (J (Cov (JV sx)) (Vector Double)) , J (Cov (JV sx)) (Vector Double) )@@ -420,21 +471,32 @@ } getFg ::- 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 q)+ forall x z 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 q+ )+ -- taus => Vec deg Double+ -- 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))+ -- lagQuadFun -> ((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)+ -- quadFun -> ((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)+ -- stageFun -> ((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)+ -- collTraj -> J (CollTraj x z u p n deg) MX+ -- parameter -> J JNone MX- -> (J (JV Id) MX, J (CollOcpConstraints n deg x r c h) MX)+ -- (objective, constraints)+ -> (J (JV Id) MX, J (CollOcpConstraints x r c h n deg) MX) getFg taus bcFun mayerFun lagQuadFun quadFun stageFun collTraj _ = (obj, cat g) where -- split up the design vars@@ -454,6 +516,14 @@ finalQuadratures :: J (JV q) MX finalQuadratures = F.sum $ TV.tvzipWith3 (oneStage quadFun) spstagesPoints outputs times'+ oneStage :: View qOrSomething+ => ((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 qOrSomething MX)+ -> J (JVec deg (CollPoint (JV x) (JV z) (JV u))) MX+ -> J (JVec deg (JV o)) MX+ -> J (JVec deg (JV Id)) MX+ -> J qOrSomething MX oneStage qfun stagePoints stageOutputs stageTimes = qfun (parm :*: stagePoints :*: stageOutputs :*: dt :*: stageTimes) @@ -508,13 +578,21 @@ getFgCov ::- forall z x u p r c h sx sh shr sc n deg .- (Dim deg, Dim n, Vectorize x, Vectorize z, Vectorize u, Vectorize p,- Vectorize h, Vectorize c, Vectorize r,- Vectorize sx, View sc, View sh, Vectorize shr)+ forall ocp x z u p r c h sx sh shr sc n deg .+ ( Dim deg, Dim n, Vectorize x, Vectorize z, Vectorize u, Vectorize p+ , Vectorize h, Vectorize c, Vectorize r+ , Vectorize sx, View sc, View sh, Vectorize shr+ , X ocp ~ x+ , Z ocp ~ z+ , U ocp ~ u+ , P ocp ~ p+ , R ocp ~ r+ , C ocp ~ c+ , H ocp ~ h+ ) -- taus => Vec deg Double- -> (J (CollTrajCov sx x z u p n deg) MX -> J (CovTraj sx n) MX)+ -> (J (CollTrajCov sx ocp n deg) MX -> J (CovTraj sx n) MX) -- gammas -> J (JV shr) MX -- robustify@@ -529,11 +607,11 @@ -- mayerFun -> SXFun (J (JV Id) :*: J (JV x) :*: J (JV x) :*: J (Cov (JV sx)) :*: J (Cov (JV sx))) (J (JV Id))- -> (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)+ -> (J (CollTraj' ocp n deg) MX -> J JNone MX -> (J (JV Id) MX, J (CollOcpConstraints' ocp n deg) MX) )- -> J (CollTrajCov sx x z u p n deg) MX+ -> J (CollTrajCov sx ocp n deg) MX -> J JNone MX- -> (J (JV Id) MX, J (CollOcpCovConstraints n deg x r c h sh shr sc) MX)+ -> (J (JV Id) MX, J (CollOcpCovConstraints ocp n deg sh shr sc) MX) getFgCov taus computeCovariances gammas robustify sbcFun shFun lagrangeFun mayerFun@@ -594,13 +672,12 @@ robustifiedPathC = TV.tvzipWith (robustify gammas parm) x0s covs ----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)+getBg :: forall x z u p r o c h q n deg .+ ( Dim n, Dim deg+ , Vectorize x, Vectorize r, Vectorize c, Vectorize h+ ) => OcpPhase x z u p r o c h q- -> CollOcpConstraints n deg x r c h (Vector Bounds)+ -> CollOcpConstraints x r c h n deg (Vector Bounds) getBg ocp = CollOcpConstraints { coCollPoints = jreplicate (jfill (Just 0, Just 0)) -- dae residual constraint@@ -609,6 +686,7 @@ , coBc = catJV (ocpBcBnds ocp) } where+ hbnds :: J (JV h) (Vector Bounds) hbnds = catJV (ocpPathCBnds ocp) evaluateQuadraturesFunction ::@@ -828,7 +906,7 @@ --- return dynamics constraints, outputs, and interpolated state+-- return path constraints at each collocation point pathStageConstraints :: forall x z u p o h deg . (Dim deg, View x, View z, View u, View p, View o, View h) => SXFun (J (JV Id) :*: J p :*: J o :*: J (CollPoint x z u))@@ -842,7 +920,7 @@ stageTimes = unJVec $ split stageTimes' cps = fmap split (unJVec (split collPoints)) :: Vec deg (CollPoint x z u MX) - -- dae constraints (dynamics)+ -- path constraints hs :: Vec deg (J h MX) hs = TV.tvzipWith3 applyH cps stageTimes (unJVec (split outputs)) @@ -885,7 +963,9 @@ -- | make an initial guess makeGuess :: forall x z u p deg n .- (Dim n, Dim deg, Vectorize x, Vectorize z, Vectorize u, Vectorize p)+ ( Dim n, Dim deg+ , Vectorize x, Vectorize z, Vectorize u, Vectorize p+ ) => QuadratureRoots -> Double -> (Double -> x Double) -> (Double -> z Double) -> (Double -> u Double) -> p Double@@ -921,7 +1001,9 @@ -- | make an initial guess makeGuessSim :: forall x z u p deg n .- (Dim n, Dim deg, Vectorize x, Vectorize z, Vectorize u, Vectorize p)+ ( Dim n, Dim deg+ , Vectorize x, Vectorize z, Vectorize u, Vectorize p+ ) => QuadratureRoots -> Double -> x Double
+ src/Dyno/DirectCollocation/Interpolate.hs view
@@ -0,0 +1,209 @@+{-# OPTIONS_GHC -Wall #-}+{-# Language ScopedTypeVariables #-}+{-# Language DeriveFunctor #-}+{-# Language DeriveFoldable #-}+{-# Language DeriveTraversable #-}+{-# Language PolyKinds #-}++module Dyno.DirectCollocation.Interpolate+ ( interpolateTraj+ , interpolateConstraints+ ) where++import qualified Data.Traversable as T+import Data.Proxy ( Proxy(..) )+import qualified Data.Vector as V+import qualified Data.Foldable as F+import Linear.V+import Linear ( lerp )++import Dyno.View.Unsafe.View ( unJ, mkJ )+import Dyno.View.View ( View(..), J )+import Dyno.View.JV ( JV )+import Dyno.View.JVec+import Dyno.TypeVecs ( Vec )+import Dyno.Vectorize ( Vectorize )+import qualified Dyno.TypeVecs as TV+import qualified Dyno.LagrangePolynomials as LP+import Dyno.DirectCollocation.Types ( CollTraj(..), CollStage(..), CollPoint(..), CollOcpConstraints(..) )+import Dyno.DirectCollocation.Quadratures ( QuadratureRoots, timesFromTaus, mkTaus )+++type TimeVal f deg = (Double, Vec deg (Double, J f (V.Vector Double)), Double)++-- a zipper to avoid quadratic lookup+data TimeZ deg f = TimeZ [TimeVal f deg] (TimeVal f deg) [TimeVal f deg]+++closestTime :: TimeZ deg f -> Double -> (TimeZ deg f, Vec deg (Double, J f (V.Vector Double)))+-- time too big and we have another value+closestTime (TimeZ ls mid@(_, _, t1) (r:rs)) t+ | t > t1 = closestTime (TimeZ (mid:ls) r rs) t+-- time too big and we don't have another value+closestTime tz@(TimeZ _ (t0, xs, t1) []) t+ | t > t1 + 0.5*(t1 - t0) = error $ "requested time which is too big " ++ show (t0, t, t1)+ | t > t1 = (tz, xs)+-- braket ok+closestTime tz@(TimeZ _ (t0, xs, _) _) t+ | t0 <= t = (tz, xs)+ | otherwise = error "time isn't increasing monotonically"+++interp ::+ forall f deg+ . (Dim deg, View f)+ => TimeZ deg f -> Double -> (TimeZ deg f, J f (V.Vector Double))+interp tz0 t = (tz, ret)+ where+ txs :: Vec deg (Double, J f (V.Vector Double))+ (tz, txs) = closestTime tz0 t++ ts :: Vec deg Double+ xs :: Vec deg (J f (V.Vector Double))+ (ts,xs) = TV.tvunzip txs++ ret :: J f (V.Vector Double)+ ret = mkJ $ LP.interpolate ts (fmap unJ xs) t+++type Point x z u = CollPoint (JV x) (JV z) (JV u)+newtype Times deg a = Times (a, Vec deg a) deriving (Functor, F.Foldable, T.Traversable)++-- | re-discretize a collocation trajectory using the lagrange interpolation polynomials+-- from the quadrature scheme+interpolateTraj ::+ forall x z u p n0 n1 deg0 deg1+ . ( Dim n0, Dim n1, Dim deg0, Dim deg1+ , Vectorize x, Vectorize z, Vectorize u+ )+ => Vec deg0 Double+ -> CollTraj x z u p n0 deg0 (V.Vector Double)+ -> QuadratureRoots+ -> CollTraj x z u p n1 deg1 (V.Vector Double)+interpolateTraj taus0 traj0 roots1 = traj0 { ctStages = cat (JVec (fmap cat stages1)) }+ where+ n0 = reflectDim (Proxy :: Proxy n0)+ n1 = reflectDim (Proxy :: Proxy n1)++ tf = 1.0 -- could be anything, returned traj doesn't use this it uses the correct tf+ dt0 = tf / fromIntegral n0+ dt1 = tf / fromIntegral n1++ taus1 :: Vec deg1 Double+ taus1 = mkTaus roots1++ times0' :: Vec n0 (Double, Vec deg0 Double)+ times0' = timesFromTaus 0 taus0 dt0++ stages0 :: Vec n0 (Vec deg0 (J (CollPoint (JV x) (JV z) (JV u)) (V.Vector Double)))+ stages0 = fmap (points . split) $ unJVec $ split (ctStages traj0)+ points (CollStage _ ps) = unJVec (split ps)++ times0 :: Vec n0 (TimeVal (Point x z u) deg0)+ times0 = TV.tvzipWith3 (\(t0,ts) t1 xs -> (t0, TV.tvzip ts xs, t1))+ times0' (TV.tvshiftl (fmap fst times0') tf) stages0++ tzip0 :: TimeZ deg0 (Point x z u)+ tzip0 = case F.toList times0 of+ [] -> error "can't interpolate with 0 length input"+ (x:xs) -> TimeZ [] x xs++ times1 :: Vec n1 (Double, Vec deg1 Double)+ times1 = timesFromTaus 0 taus1 dt1++ stages1 :: Vec n1 (CollStage (JV x) (JV z) (JV u) deg1 (V.Vector Double))+ stages1 = snd $ T.mapAccumL foo tzip0 times1++ foo :: TimeZ deg0 (Point x z u)+ -> (Double, Vec deg1 Double)++ -> ( TimeZ deg0 (Point x z u)+ , CollStage (JV x) (JV z) (JV u) deg1 (V.Vector Double)+ )+ foo timez0 (t0, ts) = (timezf, CollStage x0 (cat (JVec xzus)))+ where+ CollPoint x0 _ _ = split xzu0+ (timez1, xzu0) = interp timez0 t0+ (timezf, xzus) = T.mapAccumL interp timez1 ts+++-- | Re-discretize collocation constraints using the lagrange interpolation polynomials+-- from the quadrature scheme. This is useful for lagrange multipliers.+interpolateConstraints ::+ forall x r c h n0 n1 deg0 deg1+ . ( Dim n0, Dim n1, Dim deg0, Dim deg1+ , Vectorize x, Vectorize r, Vectorize c, Vectorize h+ )+ => Vec deg0 Double+ -> CollOcpConstraints x r c h n0 deg0 (V.Vector Double)+ -> QuadratureRoots+ -> CollOcpConstraints x r c h n1 deg1 (V.Vector Double)+interpolateConstraints taus0 con0 roots1 = con1+ where+ con1 = CollOcpConstraints+ { coCollPoints = go' (coCollPoints con0)+ , coPathC = go' (coPathC con0)+ , coContinuity = cat (JVec cont)+ , coBc = coBc con0+ }++ cont0 :: Vec n0 (J (JV x) (V.Vector Double))+ cont0 = unJVec $ split (coContinuity con0)++ tc0 :: Vec n0 (Double, J (JV x) (V.Vector Double))+ tc0 = TV.tvzip (fmap fst times0') cont0++ cont :: Vec n1 (J (JV x) (V.Vector Double))+ cont = snd $ T.mapAccumL linterp (F.toList tc0) (fmap fst times1)++ go' :: forall s+ . Vectorize s+ => J (JVec n0 (JVec deg0 (JV s))) (V.Vector Double)+ -> J (JVec n1 (JVec deg1 (JV s))) (V.Vector Double)+ go' x = cat $ JVec $ fmap (cat . JVec) (go (fmap (unJVec . split) (unJVec (split x))))++ n0 = reflectDim (Proxy :: Proxy n0)+ n1 = reflectDim (Proxy :: Proxy n1)++ tf = 1.0 -- could be anything+ dt0 = tf / fromIntegral n0+ dt1 = tf / fromIntegral n1++ taus1 :: Vec deg1 Double+ taus1 = mkTaus roots1++ times0' :: Vec n0 (Double, Vec deg0 Double)+ times0' = timesFromTaus 0 taus0 dt0++ times1 :: Vec n1 (Double, Vec deg1 Double)+ times1 = timesFromTaus 0 taus1 dt1++ go :: forall s+ . View s+ => Vec n0 (Vec deg0 (J s (V.Vector Double)))+ -> Vec n1 (Vec deg1 (J s (V.Vector Double)))+ go x0s = x1s+ where+ x1s :: Vec n1 (Vec deg1 (J s (V.Vector Double)))+ x1s = snd $ T.mapAccumL (T.mapAccumL interp) tzip0' (fmap snd times1)++ tzip0' :: TimeZ deg0 s+ tzip0' = case F.toList times00 of+ [] -> error "can't interpolate with 0 length input"+ (x:xs) -> TimeZ [] x xs++ times00 :: Vec n0 (TimeVal s deg0)+ times00 = TV.tvzipWith3 (\(t0,ts) t1 xs -> (t0, TV.tvzip ts xs, t1))+ times0' (TV.tvshiftl (fmap fst times0') tf) x0s++++linterp :: View s+ => [(Double, J s (V.Vector Double))]+ -> Double+ -> ([(Double, J s (V.Vector Double))], J s (V.Vector Double))+-- if t is too big and there are others available+linterp (_:others@((t1,_):_:_)) t+ | t > t1 = linterp others t+linterp acc@((t0,x0):(t1,x1):_) t = (acc, mkJ (lerp ((t - t0) / (t1 - t0)) (unJ x0) (unJ x1)))+linterp _ _ = error "linear interpolation ran out of nodes"
src/Dyno/DirectCollocation/Profile.hs view
@@ -13,14 +13,14 @@ import Dyno.View.View ( J ) import Dyno.Vectorize ( Vectorize )-import Dyno.Ocp ( OcpPhase )+import Dyno.Ocp 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.NlpUtils ( solveNlp )-import Dyno.Nlp ( Nlp(..), NlpOut(..) )+import Dyno.Nlp ( NlpOut(..) ) data ProfileReport = ProfileReport@@ -29,13 +29,14 @@ 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 x r c h n deg) (Vector Double) -> IO ProfileReport toProfileReport _ _ = return ProfileReport 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, Vectorize q)+ ( Vectorize x, Vectorize z, Vectorize u, Vectorize p+ , 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))@@ -52,16 +53,17 @@ profileOne :: 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 q,- Dim n, Dim deg)+ ( Vectorize x, Vectorize z, Vectorize u, Vectorize p+ , Vectorize r, Vectorize o, Vectorize c, Vectorize h, Vectorize q+ , Dim n, Dim deg+ ) => QuadratureRoots -> OcpPhase x z u p r o c h q -> J (CollTraj x z u p n deg) (Vector Double) -> Solver -> IO ProfileReport profileOne roots ocp guess solver = do- cp <- makeCollProblem roots ocp+ cp <- makeCollProblem roots ocp guess let nlp = cpNlp cp- x <- solveNlp solver (nlp { nlpX0 = guess }) Nothing+ x <- solveNlp solver nlp Nothing uncurry toProfileReport x
src/Dyno/DirectCollocation/Quadratures.hs view
@@ -1,7 +1,7 @@ {-# OPTIONS_GHC -Wall #-} {-# Language ScopedTypeVariables #-}-{-# Language FlexibleContexts #-} {-# Language DeriveGeneric #-}+{-# Language FlexibleContexts #-} {-# Language PolyKinds #-} module Dyno.DirectCollocation.Quadratures@@ -18,6 +18,7 @@ import qualified Data.Vector as V import qualified Data.Foldable as F import Data.Binary ( Binary )+import Data.Serialize ( Serialize ) import Linear.V import JacobiRoots ( shiftedLegendreRoots, shiftedRadauRoots )@@ -29,6 +30,7 @@ data QuadratureRoots = Legendre | Radau deriving (Show, Eq, Ord, Enum, Generic) instance Binary QuadratureRoots+instance Serialize QuadratureRoots mkTaus :: forall deg a
src/Dyno/DirectCollocation/Robust.hs view
@@ -1,4 +1,5 @@ {-# OPTIONS_GHC -Wall #-}+{-# Language TypeFamilies #-} {-# Language ScopedTypeVariables #-} {-# Language TypeOperators #-} {-# Language DeriveGeneric #-}@@ -27,6 +28,7 @@ import qualified Dyno.View.Unsafe.M as M ( mkM, blockSplit ) import Dyno.SXElement ( SXElement, sxSplitJV, sxCatJV )+import Dyno.Ocp import Dyno.View.View ( View(..), J, JNone(..), JTuple(..), fromDMatrix ) import Dyno.View.JV ( JV, catJV', splitJV' ) import Dyno.View.HList ( (:*:)(..) )@@ -65,8 +67,9 @@ mkComputeSensitivities :: forall x z u p sx sz sw sr deg n .- (Dim deg, Dim n, Vectorize x, Vectorize p, Vectorize u, Vectorize z,- Vectorize sr, Vectorize sw, Vectorize sz, Vectorize sx)+ ( Dim deg, Dim n, Vectorize x, Vectorize p, Vectorize u, Vectorize z+ , Vectorize sr, Vectorize sw, Vectorize sz, Vectorize sx+ ) => QuadratureRoots -> (x Sxe -> x Sxe -> z Sxe -> u Sxe -> p Sxe -> Sxe -> sx Sxe -> sx Sxe -> sz Sxe -> sw Sxe@@ -139,18 +142,24 @@ -- todo: calculate by first multiplying all the Fs mkComputeCovariances ::- forall z x u p sx sw n deg .- (Dim deg, Dim n, Vectorize x, Vectorize z, Vectorize u, Vectorize p,- Vectorize sx, Vectorize sw)+ forall ocp x z u p sx sw n deg .+ ( Dim deg, Dim n+ , Vectorize x, Vectorize z, Vectorize u, Vectorize p+ , Vectorize sx, Vectorize sw+ , X ocp ~ x+ , Z ocp ~ z+ , U ocp ~ u+ , P ocp ~ p+ ) => (M (JV sx) (JV sx) MX -> M (JV sx) (JV sw) MX -> J (Cov (JV sw)) MX -> J (JV Id) MX -> M (JV sx) (JV sx) MX) -> (J (CollTraj x z u p n deg) MX -> CovarianceSensitivities (JV sx) (JV sw) n MX) -> J (Cov (JV sw)) DMatrix- -> IO (J (CollTrajCov sx x z u p n deg) MX -> J (CovTraj sx n) MX)+ -> IO (J (CollTrajCov sx ocp n deg) MX -> J (CovTraj sx n) MX) mkComputeCovariances c2d computeSens qc' = do propOneCovFun <- toMXFun "propogate one covariance" (propOneCov c2d) - let computeCovs :: J (CollTrajCov sx x z u p n deg) MX -> J (CovTraj sx n) MX+ let computeCovs :: J (CollTrajCov sx ocp n deg) MX -> J (CovTraj sx n) MX computeCovs collTrajCov = cat covTraj where CollTrajCov p0 collTraj = split collTrajCov
src/Dyno/DirectCollocation/Types.hs view
@@ -1,13 +1,16 @@ {-# OPTIONS_GHC -Wall #-} {-# Language ScopedTypeVariables #-} {-# Language DeriveGeneric #-}+{-# Language FlexibleContexts #-} {-# Language PolyKinds #-} module Dyno.DirectCollocation.Types ( CollTraj(..)+ , CollTraj' , CollStage(..) , CollPoint(..) , CollStageConstraints(..)+ , CollOcpConstraints' , CollOcpConstraints(..) , CollTrajCov(..) , CollOcpCovConstraints(..)@@ -16,7 +19,9 @@ , fmapCollTraj , fmapCollTraj' , fmapStage+ , fmapStageJ , fmapCollPoint+ , fmapCollPointJ , fillCollConstraints , getXzus ) where@@ -27,6 +32,8 @@ import Linear.V ( Dim(..) ) import Data.Vector ( Vector ) +import Dyno.Ocp+import Dyno.View.Viewable ( Viewable ) import Dyno.View.View ( View(..), J, jfill ) import Dyno.View.JVec ( JVec(..), jreplicate ) import Dyno.View.Cov ( Cov )@@ -34,30 +41,40 @@ import Dyno.Vectorize ( Vectorize(..), Id ) +-- | CollTraj using type families to compress type parameters+type CollTraj' ocp n deg = CollTraj (X ocp) (Z ocp) (U ocp) (P ocp) n deg+ -- design variables data CollTraj x z u p n deg a =- CollTraj (J (JV Id) a) (J (JV p) a) (J (JVec n (CollStage (JV x) (JV z) (JV u) deg)) a) (J (JV x) a)- deriving (Eq, Generic, Show)- -- endtime, params, coll stages, xf+ CollTraj+ { ctTf :: J (JV Id) a+ , ctP :: J (JV p) a+ , ctStages :: J (JVec n (CollStage (JV x) (JV z) (JV u) deg)) a+ , ctXf :: J (JV x) a+ } deriving (Eq, Generic, Show) -- design variables-data CollTrajCov sx x z u p n deg a =- CollTrajCov (J (Cov (JV sx)) a) (J (CollTraj x z u p n deg) a)+data CollTrajCov sx ocp n deg a =+ CollTrajCov (J (Cov (JV sx)) a) (J (CollTraj' ocp n deg) a) deriving (Eq, Generic, Show)- -- endtime, params, coll stages, xf -data CollStage x z u deg a = CollStage (J x a) (J (JVec deg (CollPoint x z u)) a)- deriving (Eq, Generic, Show)+data CollStage x z u deg a =+ CollStage (J x a) (J (JVec deg (CollPoint x z u)) a)+ deriving (Eq, Generic, Show) -data CollPoint x z u a = CollPoint (J x a) (J z a) (J u a)- deriving (Eq, Generic, Show)+data CollPoint x z u a =+ CollPoint (J x a) (J z a) (J u a)+ deriving (Eq, Generic, Show) -- constraints data CollStageConstraints x deg r a = CollStageConstraints (J (JVec deg (JV r)) a) (J (JV x) a) deriving (Eq, Generic, Show) -data CollOcpConstraints n deg x r c h a =+-- | CollOcpConstraints using type families to compress type parameters+type CollOcpConstraints' ocp n deg = CollOcpConstraints (X ocp) (R ocp) (C ocp) (H ocp) n deg++data CollOcpConstraints x r c h n deg a = CollOcpConstraints { coCollPoints :: J (JVec n (JVec deg (JV r))) a , coContinuity :: J (JVec n (JV x)) a@@ -65,9 +82,9 @@ , coBc :: J (JV c) a } deriving (Eq, Generic, Show) -data CollOcpCovConstraints n deg x r c h sh shr sc a =+data CollOcpCovConstraints ocp n deg sh shr sc a = CollOcpCovConstraints- { cocNormal :: J (CollOcpConstraints n deg x r c h) a+ { cocNormal :: J (CollOcpConstraints' ocp n deg ) a , cocCovPathC :: J (JVec n sh) a , cocCovRobustPathC :: J (JVec n (JV shr)) a , cocSbc :: J sc a@@ -76,17 +93,22 @@ -- View instances instance (View x, View z, View u) => View (CollPoint x z u) instance (View x, View z, View u, Dim deg) => View (CollStage x z u deg)-instance (Vectorize x, Vectorize z, Vectorize u, Vectorize p, Dim n, Dim deg) =>- View (CollTraj x z u p n deg)-instance (Vectorize sx, Vectorize x, Vectorize z, Vectorize u, Vectorize p, Dim n, Dim deg) =>- View (CollTrajCov sx x z u p n deg)+instance ( Vectorize x, Vectorize z, Vectorize u, Vectorize p+ , Dim n, Dim deg+ ) => View (CollTraj x z u p n deg)+instance ( Vectorize (X ocp), Vectorize (Z ocp), Vectorize (U ocp), Vectorize (P ocp)+ , Vectorize sx+ , Dim n, Dim deg+ ) => View (CollTrajCov sx ocp n deg) instance (Vectorize x, Vectorize r, Dim deg) => View (CollStageConstraints x deg r)-instance (Vectorize x, Vectorize r, Dim n, Dim deg, Vectorize c, Vectorize h) =>- View (CollOcpConstraints n deg x r c h)-instance ( Vectorize x, Vectorize r, Dim n, Dim deg, Vectorize c, Vectorize h+instance ( Vectorize x, Vectorize r, Vectorize c, Vectorize h+ , Dim n, Dim deg+ ) => View (CollOcpConstraints x r c h n deg)+instance ( Vectorize (X ocp), Vectorize (R ocp), Vectorize (C ocp), Vectorize (H ocp)+ , Dim n, Dim deg , View sh, Vectorize shr, View sc- ) => View (CollOcpCovConstraints n deg x r c h sh shr sc)+ ) => View (CollOcpCovConstraints ocp n deg sh shr sc) getXzus ::@@ -105,9 +127,9 @@ fillCollConstraints :: forall x r c h n deg a .- (Vectorize x, Vectorize r, Vectorize h, Vectorize c,- Dim n, Dim deg, Show a)- => x a -> r a -> c a -> h a -> CollOcpConstraints n deg x r c h (Vector a)+ ( Vectorize x, Vectorize r, Vectorize c, Vectorize h+ , Dim n, Dim deg )+ => x a -> r a -> c a -> h a -> CollOcpConstraints x r c h n deg (Vector a) fillCollConstraints x r c h = CollOcpConstraints { coCollPoints = jreplicate $ jreplicate $ catJV r@@ -119,17 +141,19 @@ fillCollTraj :: forall x z u p n deg a .- (Vectorize x, Vectorize z, Vectorize u, Vectorize p,- Dim n, Dim deg, Show a)- => x a -> z a -> u a -> p a -> a -> CollTraj x z u p n deg (Vector a)+ ( Vectorize x, Vectorize z, Vectorize u, Vectorize p+ , Dim n, Dim deg )+ => x a -> z a -> u a -> p a -> a+ -> CollTraj x z u p n deg (Vector a) fillCollTraj x = fillCollTraj' x x --- | first argument maps over the non-collocation points+-- | first x argument fills 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)+ ( Vectorize x, Vectorize z, Vectorize u, Vectorize p+ , Dim n, Dim deg )+ => 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')@@ -141,39 +165,37 @@ (split (jfill () :: J (CollTraj x z u p n deg) (Vector ()))) fmapCollTraj ::- forall x1 x2 z1 z2 u1 u2 p1 p2 n deg a b .- ( Vectorize x1, Vectorize x2- , Vectorize z1, Vectorize z2- , Vectorize u1, Vectorize u2- , Vectorize p1, Vectorize p2- , Dim n, Dim deg- , Show a, Show b )- => (x1 a -> x2 b)- -> (z1 a -> z2 b)- -> (u1 a -> u2 b)- -> (p1 a -> p2 b)+ forall x0 z0 u0 p0 x1 z1 u1 p1 n deg a b .+ ( Vectorize x0, Vectorize x1+ , Vectorize z0, Vectorize z1+ , Vectorize u0, Vectorize u1+ , Vectorize p0, Vectorize p1+ , Dim n, Dim deg )+ => (x0 a -> x1 b)+ -> (z0 a -> z1 b)+ -> (u0 a -> u1 b)+ -> (p0 a -> p1 b) -> (a -> b)- -> CollTraj x1 z1 u1 p1 n deg (Vector a)- -> CollTraj x2 z2 u2 p2 n deg (Vector b)+ -> CollTraj x0 z0 u0 p0 n deg (Vector a)+ -> CollTraj x1 z1 u1 p1 n deg (Vector b) fmapCollTraj fx = fmapCollTraj' fx fx --- | first argument maps over the non-collocation points+-- | first x 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)+ forall x0 z0 u0 p0 x1 z1 u1 p1 n deg a b .+ ( Vectorize x0, Vectorize x1+ , Vectorize z0, Vectorize z1+ , Vectorize u0, Vectorize u1+ , Vectorize p0, Vectorize p1+ , Dim n, Dim deg )+ => (x0 a -> x1 b)+ -> (x0 a -> x1 b)+ -> (z0 a -> z1 b)+ -> (u0 a -> u1 b)+ -> (p0 a -> p1 b) -> (a -> b)- -> CollTraj x1 z1 u1 p1 n deg (Vector a)- -> CollTraj x2 z2 u2 p2 n deg (Vector b)+ -> CollTraj x0 z0 u0 p0 n deg (Vector a)+ -> CollTraj x1 z1 u1 p1 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@@ -186,46 +208,69 @@ -> J (JV f1) (Vector a) -> J (JV f2) (Vector b) fj f = catJV . f . splitJV -fmapJVec :: (View f, View g, Show a, Show b)- => (f (Vector a) -> g (Vector b)) -> JVec deg f (Vector a) -> JVec deg g (Vector b)+fmapJVec :: (View f, View g, Viewable a, Viewable b)+ => (f a -> g b) -> JVec deg f a -> JVec deg g b fmapJVec f = JVec . fmap (cat . f . split) . unJVec fmapStage :: forall x1 x2 z1 z2 u1 u2 deg a b . ( Vectorize x1, Vectorize x2 , Vectorize z1, Vectorize z2 , Vectorize u1, Vectorize u2- , Dim deg- , Show a, Show b )+ , Dim deg ) => (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' fx fz fu (CollStage x0 points0) = CollStage (fj fx' x0) points1+fmapStage fx' fx fz fu = fmapStageJ (fj fx') (fj fx) (fj fz) (fj fu) where- points1 = cat $ fmapJVec (fmapCollPoint fx fz fu) (split points0)- fj :: (Vectorize f1, Vectorize f2) => (f1 a -> f2 b) -> J (JV f1) (Vector a) -> J (JV f2) (Vector b) fj f = catJV . f . splitJV +fmapStageJ :: forall x1 x2 z1 z2 u1 u2 deg a b .+ ( Viewable a, Viewable b+ , View x1, View x2+ , View z1, View z2+ , View u1, View u2+ , Dim deg )+ => (J x1 a -> J x2 b)+ -> (J x1 a -> J x2 b)+ -> (J z1 a -> J z2 b)+ -> (J u1 a -> J u2 b)+ -> CollStage x1 z1 u1 deg a+ -> CollStage x2 z2 u2 deg b+fmapStageJ fx' fx fz fu (CollStage x0 points0) = CollStage (fx' x0) points1+ where+ points1 = cat $ fmapJVec (fmapCollPointJ fx fz fu) (split points0)+ fmapCollPoint :: forall x1 x2 z1 z2 u1 u2 a b . ( Vectorize x1, Vectorize x2 , Vectorize z1, Vectorize z2- , Vectorize u1, Vectorize u2- , Show a, Show b )+ , Vectorize u1, Vectorize u2 ) => (x1 a -> x2 b) -> (z1 a -> z2 b) -> (u1 a -> u2 b) -> CollPoint (JV x1) (JV z1) (JV u1) (Vector a) -> CollPoint (JV x2) (JV z2) (JV u2) (Vector b)-fmapCollPoint fx fz fu (CollPoint x z u) = CollPoint (fj fx x) (fj fz z) (fj fu u)+fmapCollPoint fx fz fu = fmapCollPointJ (fj fx) (fj fz) (fj fu) where fj :: (Vectorize f1, Vectorize f2) => (f1 a -> f2 b) -> J (JV f1) (Vector a) -> J (JV f2) (Vector b) fj f = catJV . f . splitJV++fmapCollPointJ :: forall x1 x2 z1 z2 u1 u2 a b .+ ( View x1, View x2+ , View z1, View z2+ , View u1, View u2 )+ => (J x1 a -> J x2 b)+ -> (J z1 a -> J z2 b)+ -> (J u1 a -> J u2 b)+ -> CollPoint x1 z1 u1 a+ -> CollPoint x2 z2 u2 b+fmapCollPointJ fx fz fu (CollPoint x z u) = CollPoint (fx x) (fz z) (fu u)
src/Dyno/LagrangePolynomials.lhs view
@@ -102,22 +102,34 @@ \begin{code} {-# OPTIONS_GHC -Wall #-}-{-# Language FlexibleContexts #-}-{-# Language GADTs #-}+{-# Language PolyKinds #-} -module Dyno.LagrangePolynomials ( lagrangeDerivCoeffs, lagrangeXis, runComparison ) where+module Dyno.LagrangePolynomials+ ( lagrangeDerivCoeffs, lagrangeXis, runComparison+ , interpolate+ ) where +import qualified Data.Foldable as F import qualified Data.Vector as V+import Linear ( Additive, (^*), sumV ) import Casadi.SXFunction ( sxFunction ) import Casadi.Function ( evalDMatrix ) import Casadi.SharedObject ( soInit ) import Casadi.SX ( SX, ssym, sgradient )-import Casadi.DMatrix ( DMatrix, ddata )-import Casadi.CMatrix ( dense )+import Casadi.DMatrix ( DMatrix, dnonzeros )+import Casadi.CMatrix ( densify ) import Dyno.TypeVecs ++interpolate :: (Additive f, Fractional a) => Vec deg a -> Vec deg (f a) -> a -> f a+interpolate taus0 xs0 tau1 = sumV [x ^* (lagrangeXis taus0' tau1 k) | (k,x) <- zip [0..] xs0']+ where+ taus0' = F.toList taus0+ xs0' = F.toList xs0++ lagrangeXis :: Fractional a => [a] -> a -> Int -> a lagrangeXis taus tau j = product [(tau - tk) / (tj - tk) | k <- [0..deg]@@ -281,7 +293,7 @@ putStrLn "numeric:" vals' <- V.mapM (\tau_i -> evalDMatrix zdotAlg (V.fromList (tau_i : sampleTaus'))) (V.fromList sampleTaus') let d2d :: DMatrix -> Double- d2d x = case V.toList (ddata (dense x)) of+ d2d x = case V.toList (dnonzeros (densify x)) of [y] -> y ys -> error $ "d2d: need length 1, got length " ++ show (length ys)
src/Dyno/Nlp.hs view
@@ -14,6 +14,7 @@ import Casadi.DMatrix ( DMatrix ) import qualified Data.Vector as V import Data.Binary ( Binary )+import Data.Serialize ( Serialize ) import Dyno.Vectorize ( Id ) import Dyno.View.View ( View(..), J )@@ -57,6 +58,7 @@ , lambdaGOpt :: J g a } deriving (Eq, Show, Generic) instance (View x, View g, Binary a, Viewable a) => Binary (NlpOut x g a)+instance (View x, View g, Serialize a, Viewable a) => Serialize (NlpOut x g a) -- | Karush–Kuhn–Tucker (KKT) matrix@@ -71,3 +73,4 @@ , kktF :: J (JV Id) DMatrix } deriving (Generic, Eq, Show) instance (View x, View g) => Binary (KKT x g)+instance (View x, View g) => Serialize (KKT x g)
src/Dyno/NlpSolver.hs view
@@ -77,8 +77,8 @@ import qualified Casadi.Core.Classes.IOInterfaceFunction as C import Casadi.Callback ( makeCallback )-import Casadi.DMatrix ( DMatrix, ddata )-import Casadi.Function ( Function, externalFunction )+import Casadi.DMatrix ( DMatrix, dnonzeros )+import Casadi.Function ( Function, externalFunction, generateCode ) import qualified Casadi.Option as Op import qualified Casadi.GenericC as Gen import Casadi.SharedObject ( soInit )@@ -167,7 +167,7 @@ nlpState <- ask dmat <- liftIO $ C.ioInterfaceFunction_input__0 (isSolver nlpState) name let scale = scaleFun (isScale nlpState)- return (mkJ $ ddata $ unJ $ scale (mkJ dmat))+ return (mkJ $ dnonzeros $ unJ $ scale (mkJ dmat)) getX0 :: View x => NlpSolver x p g (VD x) getX0 = getInput xbarToX "x0"@@ -201,7 +201,7 @@ nlpState <- ask dmat <- liftIO $ C.ioInterfaceFunction_output__0 (isSolver nlpState) name let scale = scaleFun (isScale nlpState)- return (mkJ $ ddata $ unJ $ scale (mkJ dmat))+ return (mkJ $ dnonzeros $ unJ $ scale (mkJ dmat)) getF :: NlpSolver x p g (VD (JV Id)) getF = getOutput fbarToF "f"@@ -487,8 +487,8 @@ generateAndCompile :: String -> Function -> IO Function generateAndCompile name f = do putStrLn $ "generating " ++ name ++ ".c"--- writeFile (name ++ ".c") (generateCode f)- C.function_generateCode__3 f (name ++ ".c") True+ writeFile (name ++ ".c") (generateCode f True)+-- C.function_generateCode__1 f (name ++ ".c") True let cmd = "clang" args = ["-fPIC","-shared","-Wall","-Wno-unused-variable",name++".c","-o",name++".so"] putStrLn (showCommandForUser cmd args)@@ -553,7 +553,7 @@ callbackRet <- case callback' of Nothing -> return True Just callback -> do- xval <- fmap (d2v . xbarToX scale . mkJ . CM.dense) $+ xval <- fmap (d2v . xbarToX scale . mkJ . CM.densify) $ C.ioInterfaceFunction_output__2 function' 0 callback xval interrupt <- readIORef intref
src/Dyno/NlpUtils.hs view
@@ -12,6 +12,7 @@ ) where import Control.Applicative ( Applicative(..) )+import Data.Maybe ( fromMaybe ) import qualified Data.Traversable as T import Control.Monad ( when, void ) import Data.Vector ( Vector )@@ -57,17 +58,19 @@ , 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+ -> Maybe (J p (Vector Double)) -> 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+ solverStuff pscale 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)@@ -75,7 +78,11 @@ where JTuple x p = split xp - runNlpSolver solverStuff fg Nothing (nlpScaleG nlp) (nlpScaleF nlp) callback $ do+ xpscale :: Maybe (J (JTuple x p) (Vector Double))+ xpscale = case (nlpScaleX nlp, pscale) of+ (Nothing, Nothing) -> Nothing+ (xs, ps) -> Just $ cat $ JTuple (fromMaybe (jfill 1) xs) (fromMaybe (jfill 1) ps)+ runNlpSolver solverStuff fg xpscale (nlpScaleG nlp) (nlpScaleF nlp) callback $ do let (lbx,ubx) = unzipJ (nlpBX nlp) (lbg,ubg) = unzipJ (nlpBG nlp) p0 = nlpP nlp
src/Dyno/Ocp.hs view
@@ -1,11 +1,19 @@ {-# OPTIONS_GHC -Wall #-} {-# Language TypeFamilies #-}-{-# Language FlexibleInstances #-} module Dyno.Ocp ( OcpPhase(..) , OcpPhaseWithCov(..)- , OcpPhaseClass(..)+ , OcpPhase'+ , X+ , Z+ , U+ , P+ , R+ , O+ , C+ , H+ , Q ) where import Data.Vector ( Vector )@@ -15,6 +23,7 @@ import Dyno.View.Cov ( Cov ) import Dyno.Nlp ( Bounds ) import Dyno.SXElement ( SXElement )+--import Dyno.Vectorize import Casadi.SX ( SX ) import Casadi.DMatrix ( DMatrix )@@ -22,27 +31,28 @@ type Sx a = J a SX type Sxe = SXElement -class OcpPhaseClass a where- type X a :: * -> *- type Z a :: * -> *- type U a :: * -> *- type P a :: * -> *- type R a :: * -> *- type O a :: * -> *- type C a :: * -> *- type H a :: * -> *- type Q a :: * -> *+-- | differential state+type family X a :: * -> *+-- | algebraic variable+type family Z a :: * -> *+-- | control+type family U a :: * -> *+-- | parameter+type family P a :: * -> *+-- | dae residual+type family R a :: * -> *+-- | output+type family O a :: * -> *+-- | boundary condition+type family C a :: * -> *+-- | path constraint+type family H a :: * -> *+-- | quadrature state+type family Q a :: * -> * -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+-- | OcpPhase using type families to compress type parameters+type OcpPhase' ocp = OcpPhase (X ocp) (Z ocp) (U ocp) (P ocp) (R ocp) (O ocp) (C ocp) (H ocp) (Q ocp)+ -- | One stage of an optimal control problem, solvable as a stand-alone optimal control problem. --
+ src/Dyno/OcpHomotopy.hs view
@@ -0,0 +1,184 @@+{-# OPTIONS_GHC -Wall #-}+{-# Language ScopedTypeVariables #-}+{-# Language PolyKinds #-}++module Dyno.OcpHomotopy+ ( runOcpHomotopy+ ) where++import Data.Vector ( Vector )+import qualified Data.Traversable as T++import Casadi.MX ( MX )++import Dyno.Ocp+import Dyno.Vectorize ( Vectorize, Id(..), None(..) )+import Dyno.View.View ( View(..), J, JTuple(..), JNone(..) )+import Dyno.View.JV ( JV, catJV, catJV' )+import Dyno.View.Viewable ( Viewable )+import Dyno.TypeVecs ( Dim )+import Dyno.Solvers ( Solver )+import Dyno.Nlp ( Nlp(..), NlpOut(..) )+import Dyno.NlpUtils ( HomotopyParams(..), solveNlp, solveNlpHomotopy )+import Dyno.DirectCollocation.Types ( CollTraj(..), CollOcpConstraints )+import Dyno.DirectCollocation.Formulate ( CollProblem(..), makeCollProblem )+import Dyno.DirectCollocation.Quadratures ( QuadratureRoots )+++tupleToCollTraj ::+ forall x z u p n deg a+ . ( Dim deg, Dim n, Viewable a, Vectorize x, Vectorize z, Vectorize u, Vectorize p )+ => JTuple (CollTraj x z u None n deg) (JV p) a+ -> J (CollTraj x z u p n deg) a+tupleToCollTraj (JTuple x0 p) = cat x1+ where+ x1 :: CollTraj x z u p n deg a+ x1 = CollTraj tf p stages0 xf++ CollTraj tf _ stages0 xf = split x0++collTrajToTuple ::+ forall x z u p n deg a .+ ( Viewable a+ , Vectorize x, Vectorize z, Vectorize u, Vectorize p+ , Dim deg, Dim n )+ => J (CollTraj x z u p n deg) a+ -> JTuple (CollTraj x z u None n deg) (JV p) a+collTrajToTuple x0 = JTuple (cat x1) pfp+ where+ x1 :: CollTraj x z u None n deg a+ x1 = CollTraj tf (catJV' None) stages0 xf++ CollTraj tf pfp stages0 xf = split x0+++convertNlp ::+ forall x z u p r c h n deg a+ . ( Viewable a+ , Vectorize x, Vectorize z, Vectorize u, Vectorize p+ , Dim deg, Dim n+ )+ => Nlp (CollTraj x z u p n deg) JNone (CollOcpConstraints x r c h n deg) a+ -> Nlp (CollTraj x z u None n deg) (JV p) (CollOcpConstraints x r c h n deg) a+convertNlp nlp0 = nlp+ where+ nlp = Nlp { nlpX0 = x0+ , nlpBG = nlpBG nlp0+ , nlpP = fp0+ , nlpFG = fg+ , nlpBX = bx+ , nlpLamX0 = fmap ((\(JTuple ret _) -> ret) . collTrajToTuple) (nlpLamX0 nlp0)+ , nlpLamG0 = nlpLamG0 nlp0+ , nlpScaleF = nlpScaleF nlp0+ , nlpScaleX = fmap ((\(JTuple ret _) -> ret) . collTrajToTuple) (nlpScaleX nlp0)+ , nlpScaleG = nlpScaleG nlp0+ }+ fg :: J (CollTraj x z u None n deg) a -> J (JV p) a+ -> (J (JV Id) a, J (CollOcpConstraints x r c h n deg) a)+ fg x fp = (f,g')+ where+ (f,g') = nlpFG nlp0 x' (cat JNone)+ x' :: J (CollTraj x z u p n deg) a+ x' = tupleToCollTraj (JTuple x fp)++ JTuple x0 fp0 = collTrajToTuple (nlpX0 nlp0)+ JTuple bx _ = collTrajToTuple (nlpBX nlp0)++++runOcpHomotopy ::+ forall x z u p r o c h q n deg t+ . ( Dim n, Dim deg+ , Vectorize x, Vectorize z, Vectorize u, Vectorize p+ , Vectorize r, Vectorize o, Vectorize c, Vectorize h, Vectorize q+ , T.Traversable t )+ => Double -> HomotopyParams+ -> OcpPhase x z u p r o c h q+ -> J (CollTraj x z u p n deg) (Vector Double)+ -> QuadratureRoots -> Bool -> Bool -> Solver -> Solver -> p Double -> t (p Double)+ -> (CollProblem x z u p r o c h q n deg+ -> IO ([String] -> J (CollTraj x z u p n deg) (Vector Double) -> IO Bool)+ )+ -> IO (t (NlpOut (CollTraj x z u p n deg)+ (CollOcpConstraints x r c h n deg)+ (Vector Double)))+runOcpHomotopy step0 homotopyParams ocpHomotopy guess roots useStartupCallback useHomotopyCallback+ startupSolver homotopySolver param0 nominalParams makeCallback = do+ cp0 <- makeCollProblem roots ocpHomotopy guess+ callbackHeh <- makeCallback cp0+ let nlp0 = cpNlp cp0+ let nlpHomotopy :: Nlp+ (CollTraj x z u None n deg)+ (JV p)+ (CollOcpConstraints x r c h n deg)+ MX+ nlpHomotopy = (convertNlp nlp0) {nlpP = catJV param0}++ let callback :: [String]+ -> J (CollTraj x z u p n deg) (Vector Double)+ -> IO Bool+ callback moarMsgs traj = callbackHeh moarMsgs traj++ scb = if useStartupCallback then Just (callback ["homotopy startup solve"]) else Nothing++ putStrLn "running startup solver..."+ (msg0,opt0') <- solveNlp startupSolver nlp0 scb++ opt0 <- case msg0 of+ Left msg' -> error msg'+ Right _ -> return opt0'++ let homoGuessX :: J (CollTraj x z u None n deg) (Vector Double)+ JTuple homoGuessX _ = collTrajToTuple $ xOpt opt0+ JTuple homoGuessLX _ = collTrajToTuple $ lambdaXOpt opt0+ homoGuessLG :: J (CollOcpConstraints x r c h n deg) (Vector Double)+ homoGuessLG = lambdaGOpt opt0++ pFinals :: t (J (JV p) (Vector Double))+ pFinals = fmap catJV nominalParams++ homoCallback :: J (JTuple (CollTraj x z u None n deg) (JV p)) (Vector Double)+ -> IO Bool+ homoCallback traj0 = callback [ "homotopy stepping"+ ] (tupleToCollTraj (split traj0))++ putStrLn "\ninitial solve done, starting homotopy steps"+ let hcb = if useHomotopyCallback then Just homoCallback else Nothing+ pscale :: Maybe (J (JV p) (Vector Double))+ pscale = fmap catJV (ocpPScale ocpHomotopy)+ opt1s <- solveNlpHomotopy step0 homotopyParams+ homotopySolver+ pscale+ (nlpHomotopy { nlpX0 = homoGuessX+ , nlpLamX0 = Just homoGuessLX+ , nlpLamG0 = Just homoGuessLG+ })+ pFinals+ hcb Nothing++ let f :: NlpOut (JTuple (CollTraj x z u None n deg) (JV p))+ (CollOcpConstraints x r c h n deg)+ (Vector Double)+ -> NlpOut (CollTraj x z u p n deg)+ (CollOcpConstraints x r c h n deg)+ (Vector Double)+ f nlpOut =+ NlpOut+ { fOpt = fOpt nlpOut+ , xOpt = g (xOpt nlpOut)+ , gOpt = gOpt nlpOut+ , lambdaXOpt = g (lambdaXOpt nlpOut)+ , lambdaGOpt = lambdaGOpt nlpOut+ }+ where+ g :: J (JTuple (CollTraj x z u None n deg) (JV p)) (Vector Double)+ -> J (CollTraj x z u p n deg) (Vector Double)+ g = tupleToCollTraj . split++ ret :: t (NlpOut+ (CollTraj x z u p n deg)+ (CollOcpConstraints x r c h n deg)+ (Vector Double))+ ret = fmap f opt1s++ return ret
src/Dyno/TypeVecs.hs view
@@ -51,22 +51,38 @@ import qualified Data.Traversable as T import qualified Data.Vector as V import Data.Vector.Binary () -- instances-import Data.Binary ( Binary(..) )+import Data.Vector.Cereal () -- instances+import qualified Data.Binary as B+import qualified Data.Serialize as S import Linear.Vector import Linear.V ( Dim(..) ) import Data.Proxy import Data.Reflection as R import Data.Distributive ( Distributive(..) ) +import Accessors ( Lookup(..), AccessorTree(..) )+ import Dyno.Vectorize -- 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, Binary a) => Binary (Vec n a) where- put = put . unVec- get = fmap mkVec get+instance (Dim n, B.Binary a) => B.Binary (Vec n a) where+ put = B.put . unVec+ get = fmap mkVec B.get+instance (Dim n, S.Serialize a) => S.Serialize (Vec n a) where+ put = S.put . unVec+ get = fmap mkVec S.get +instance (Lookup a, Dim n) => Lookup (Vec n a) where+ toAccessorTree vec f = Data ("Vec " ++ show n, "Vec " ++ show n) $ map child (take n [0..])+ where+ n = reflectDim (Proxy :: Proxy n)+ child k = ("v" ++ show k, toAccessorTree (getK vec) (getK . f))+ where+ getK :: Vec n a -> a+ getK (MkVec v) = v V.! k+ instance Dim n => Distributive (Vec n) where distribute f = mkVec $ V.generate (reflectDim (Proxy :: Proxy n)) $ \i -> fmap (\v -> V.unsafeIndex (vectorize v) i) f@@ -114,6 +130,7 @@ n = reflectDim (Proxy :: Proxy n) n' = V.length x +-- todo: put these in unsafe module mkVec :: forall n a . Dim n => V.Vector a -> Vec n a mkVec x | n == n' = MkVec x
src/Dyno/Vectorize.hs view
@@ -8,6 +8,11 @@ {-# LANGUAGE DeriveFunctor #-} {-# LANGUAGE DeriveFoldable #-} {-# LANGUAGE DeriveTraversable #-}+-- these last nasty ones are for instance Vectorize f => Applicative/Additive/Metric/etc f+{-# OPTIONS_GHC -fno-warn-orphans #-}+{-# LANGUAGE FlexibleInstances #-}+{-# LANGUAGE UndecidableInstances #-}+{-# LANGUAGE OverlappingInstances #-} module Dyno.Vectorize ( Vectorize(..)@@ -25,10 +30,12 @@ import GHC.Generics -import Control.Applicative ( Applicative(..) )+import Control.Applicative ( Applicative(..), (<$>) ) import qualified Data.Vector as V import Data.Foldable ( Foldable )+import qualified Data.Foldable as F import Data.Traversable ( Traversable )+import qualified Data.Traversable as T import Data.Proxy ( Proxy(..) ) import qualified Linear @@ -117,6 +124,26 @@ --vlength :: Vectorize f => Proxy f -> Int --vlength = const (gvlength (Proxy :: Proxy (Rep1 f)))+++-- undecidable, overlapping, orphan instances to get rid of boilerplate+instance Vectorize f => Applicative f where+ pure = fill+ x0 <*> x1 = devectorize (V.zipWith id (vectorize x0) (vectorize x1))+instance Vectorize f => Linear.Additive f where+ zero = fill 0+instance Vectorize f => Linear.Metric f where+ dot x0 x1 = V.sum $ V.zipWith (*) (vectorize x0) (vectorize x1)+instance (Vectorize f, Eq a) => Eq (f a) where+ x == y = (vectorize x) == (vectorize y)+ x /= y = (vectorize x) /= (vectorize y)+instance (Vectorize f, Ord a) => Ord (f a) where+ compare x y = compare (vectorize x) (vectorize y)+instance Vectorize f => Foldable f where+ foldMap f x = F.foldMap f (vectorize x)+ foldr f acc0 x = F.foldr f acc0 (vectorize x)+instance Vectorize f => Traversable f where+ traverse f x = devectorize <$> T.traverse f (vectorize x) vlength :: Vectorize f => Proxy f -> Int vlength = V.length . vectorize . (empty `asFunctorOf`)
src/Dyno/View/Cov.hs view
@@ -102,5 +102,5 @@ fromMat (UnsafeM c) = fromMatrix c fromMatrix :: (View f, CMatrix a, Viewable a) => a -> J (Cov f) a-fromMatrix x = mkJ $ CM.getNZ (CM.triu (CM.dense x)) slice'+fromMatrix x = mkJ $ CM.getNZ (CM.triu (CM.densify x)) slice' --fromMatrix x = mkJ $ CM.getNZ (CM.triu x) slice'
− src/Dyno/View/CustomFunction.hs
@@ -1,160 +0,0 @@-{-# OPTIONS_GHC -Wall #-}-{-# LANGUAGE ScopedTypeVariables #-}-{-# LANGUAGE RankNTypes #-}--module Dyno.View.CustomFunction- ( CustomFun(..)- , DerivGen(..)- , toCustomFun- ) where--import Control.Monad ( zipWithM )-import Data.Proxy-import qualified Data.Vector as V-import Data.Vector ( Vector )-import qualified Data.Foldable as F--import Casadi.Sparsity ( Sparsity, dense )-import Casadi.Option ( Opt(..), setOption )-import Casadi.SharedObject ( soInit )-import Casadi.DMatrix ( DMatrix )-import Casadi.CMatrix ( sparsity )--import qualified Dyno.TypeVecs as TV-import Dyno.TypeVecs ( Vec, Dim, reifyDim )-import Dyno.View.Scheme ( Scheme(..) )-import Dyno.View.Fun ( Fun(..) )-import Casadi.Callback ( makeCustomEvaluate, makeDerivativeGenerator )-import qualified Casadi.Core.Classes.Function as C-import qualified Casadi.Core.Classes.CustomFunction as C-import qualified Casadi.Core.Classes.IOInterfaceFunction as C-import Casadi.Core.Classes.DerivativeGenerator ( DerivativeGenerator )---data CustomFun f g =- CustomFun- { cfFun :: f DMatrix -> IO (g DMatrix)- , cfSparsityIn :: Maybe (f Sparsity)- , cfSparsityOut :: Maybe (g Sparsity)- , cfDerivGenerator :: Maybe (DerivGen f g)- , cfOptions :: [(String, Opt)]- }--data DerivGen f g =- DerivGen- { dgGetSeeds :: forall nfwd nadj- . (Dim nfwd, Dim nadj)- => f DMatrix -> Vec nfwd (f DMatrix) -> Vec nadj (g DMatrix)- -> IO (g DMatrix, Vec nfwd (g DMatrix), Vec nadj (f DMatrix))- , dgOptions :: [(String, Opt)]- , dgFwdSparsity :: Maybe (f Sparsity)- , dgAdjSparsity :: Maybe (g Sparsity)- }--groupsOf :: Int -> [a] -> [[a]]-groupsOf _ [] = []-groupsOf k vs- | length vs0 == k = vs0 : groupsOf k vs1- | otherwise = error "groupsOf not divisible"- where- (vs0, vs1) = splitAt k vs--toDerivGen :: forall f g . (Scheme f, Scheme g) => DerivGen f g -> IO DerivativeGenerator-toDerivGen dg = makeDerivativeGenerator $ \originalFun nfwd nadj -> do- let f fun = do- numIn <- C.ioInterfaceFunction_getNumInputs fun- inputs <- mapM (C.ioInterfaceFunction_getInput__2 fun) (take numIn [0..])-- let nf = numFields (Proxy :: Proxy f)- ng = numFields (Proxy :: Proxy g)-- let f' :: forall nfwd nadj- . (Dim nfwd, Dim nadj)- => Proxy nfwd -> Proxy nadj -> IO (Vector DMatrix)- f' _ _ = do- let (inputs0', inputs12') = splitAt nf inputs- (inputs1', inputs2') = splitAt (nfwd*nf) inputs12'-- inputs0 :: f DMatrix- inputs0 = fromVector (V.fromList inputs0')-- inputs1 :: Vec nfwd (f DMatrix)- inputs1 = TV.mkVec' (map (fromVector . V.fromList) (groupsOf nf inputs1'))-- inputs2 :: Vec nadj (g DMatrix)- inputs2 = TV.mkVec' (map (fromVector . V.fromList) (groupsOf ng inputs2'))- (out0, out1, out2) <- dgGetSeeds dg inputs0 inputs1 inputs2- let out0' = toVector out0- out1' = V.concat $ F.toList (fmap toVector out1)- out2' = V.concat $ F.toList (fmap toVector out2)- return (V.concat [out0', out1', out2'])-- outs <- reifyDim nfwd $ \pnfwd ->- reifyDim nadj $ \pnadj -> f' pnfwd pnadj- _ <- zipWithM (C.ioInterfaceFunction_setOutput__2 fun) (V.toList outs) [0..]- return ()-- ce <- makeCustomEvaluate f--- numIn <- C.ioInterfaceFunction_getNumInputs originalFun- numOut <- C.ioInterfaceFunction_getNumOutputs originalFun- spIns0 <- mapM (fmap sparsity . (C.ioInterfaceFunction_getInput__2 originalFun)) (take numIn [0..])- spOuts0 <- mapM (fmap sparsity . (C.ioInterfaceFunction_getOutput__2 originalFun)) (take numOut [0..])-- let spFwd = case dgFwdSparsity dg of--- Just sp -> toVector sp- _ -> V.fromList $ map (uncurry dense) $ sizeList (Proxy :: Proxy f)- spAdj = case dgAdjSparsity dg of--- Just sp -> toVector sp- _ -> V.fromList $ map (uncurry dense) $ sizeList (Proxy :: Proxy g)-- -- TODO: this is only right when everything's dense because it depends on jac sparsity!!!- spIns = V.concat [ V.fromList spIns0- , V.concat (replicate nfwd spFwd)- , V.concat (replicate nadj spAdj)- ]- spOuts = V.concat [ V.fromList spOuts0- , V.concat (replicate nfwd spAdj)- , V.concat (replicate nadj spFwd)- ]- cf <- C.customFunction__1 ce spIns spOuts- mapM_ (\(n,Opt o) -> setOption cf n o) (dgOptions dg)- - soInit cf- - return (C.castFunction cf)--toCustomFun ::- forall f g- . (Scheme f, Scheme g)- => CustomFun f g- -> IO (Fun f g)-toCustomFun customFun = do- ce <- makeCustomEvaluate $ \fun -> do- numIn <- C.ioInterfaceFunction_getNumInputs fun- inputs <- mapM (C.ioInterfaceFunction_getInput__2 fun) (take numIn [0..])- outputs <- cfFun customFun $ fromVector (V.fromList inputs)- _ <- zipWithM (C.ioInterfaceFunction_setOutput__2 fun) (V.toList (toVector outputs)) [0..]- return ()-- let spIn :: Vector Sparsity- spIn = case cfSparsityIn customFun of- Just spIn' -> toVector spIn'- Nothing -> V.fromList $ map (uncurry dense) $ sizeList (Proxy :: Proxy f)- spOut :: Vector Sparsity- spOut = case cfSparsityOut customFun of- Just spOut' -> toVector spOut'- Nothing -> V.fromList $ map (uncurry dense) $ sizeList (Proxy :: Proxy g)-- cf <- C.customFunction__1 ce spIn spOut- - mapM_ (\(n,Opt o) -> setOption cf n o) (cfOptions customFun)- case cfDerivGenerator customFun of- Nothing -> return ()- Just dg -> do- dgen <- toDerivGen dg- setOption cf "derivative_generator" dgen- soInit cf- - return (Fun (C.castFunction cf))
src/Dyno/View/HList.hs view
@@ -5,7 +5,6 @@ {-# LANGUAGE PolyKinds #-} {-# LANGUAGE TypeOperators #-} {-# LANGUAGE ScopedTypeVariables #-}-{-# LANGUAGE TypeFamilies #-} {-# LANGUAGE DefaultSignatures #-} {-# LANGUAGE TypeOperators #-} {-# LANGUAGE MultiParamTypeClasses #-}
src/Dyno/View/M.hs view
@@ -49,7 +49,7 @@ import qualified Data.Vector as V import Data.Proxy ( Proxy(..) ) import Casadi.CMatrix ( CMatrix )-import Casadi.DMatrix ( DMatrix, ddata, dsparsify )+import Casadi.DMatrix ( DMatrix, dnonzeros, dsparsify ) import qualified Casadi.CMatrix as CM import qualified Data.Packed.Matrix as HMat import qualified Numeric.LinearAlgebra.HMatrix as HMat@@ -70,7 +70,7 @@ 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)+dense (UnsafeM m) = mkM (CM.densify 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)@@ -287,7 +287,7 @@ => M n m DMatrix -> HMat.Matrix Double toHMat (UnsafeM d) = HMat.trans $ (m HMat.>< n) (V.toList v) where- v = ddata (CM.dense d)+ v = dnonzeros (CM.densify d) n = size (Proxy :: Proxy n) m = size (Proxy :: Proxy m)
src/Dyno/View/Scheme.hs view
@@ -23,25 +23,15 @@ import Data.Vector ( Vector ) import Casadi.CMatrix ( CMatrix )-import qualified Casadi.CMatrix as CM -import Dyno.View.Unsafe.View ( unsafeUnJ, mkJ )-import Dyno.View.Unsafe.M ( M(UnsafeM), unM )+import Dyno.View.Unsafe.View ( unsafeUnJ, mkJ' )+import Dyno.View.Unsafe.M ( unM, mkM' )+import qualified Dyno.View.M as M import Dyno.View.View ( View(..), J ) import Dyno.View.Viewable ( Viewable )---import Dyno.Nats---import Dyno.View.JVec ( JVec ) ---data MyScheme a = MyScheme (J (JVec D3 (JV Id)) a) (J (JVec D2 (JV Id)) a) deriving (Generic, Generic1, Show)---instance Scheme MyScheme ---go :: MyScheme MX---go = fromVector (V.fromList [400,500])------og :: V.Vector MX---og = toVector go- class FunctionIO (f :: * -> *) where fromMat :: (CMatrix a, Viewable a) => a -> Either String (f a) toFioMat :: f a -> a@@ -57,7 +47,7 @@ toVector = V.singleton . toFioMat sizeList p = [matSizes p] -instance (View f, View g) => Scheme (M f g) where+instance (View f, View g) => Scheme (M.M f g) where numFields = const 1 fromVector v = case V.toList v of [m] -> case fromMat m of Left err -> error $ "Scheme fromVector M error: " ++ err@@ -69,33 +59,12 @@ instance View f => FunctionIO (J f) where toFioMat = unsafeUnJ- fromMat x- | n1 /= n1' = mismatch- | n1 /= 0 && n2 /= n2' = mismatch- | n1 == 0 && not (n2 `elem` [0,1]) = mismatch- | otherwise = Right (mkJ x)- where- mismatch = Left $ "length mismatch: typed size: " ++ show (n1',n2') ++- ", actual size: " ++ show (n1,n2)- n1' = size (Proxy :: Proxy f)- n2' = 1- n1 = CM.size1 x- n2 = CM.size2 x+ fromMat = mkJ' matSizes = const (size (Proxy :: Proxy f), 1) -instance (View f, View g) => FunctionIO (M f g) where+instance (View f, View g) => FunctionIO (M.M f g) where toFioMat = unM- fromMat x- | n2 /= n2' = mismatch- | n1 /= n1' = mismatch- | otherwise = Right (UnsafeM x)- where- mismatch = Left $ "length mismatch: typed size: " ++ show (n1',n2') ++- ", actual size: " ++ show (n1,n2)- n1' = size (Proxy :: Proxy f)- n2' = size (Proxy :: Proxy g)- n1 = CM.size1 x- n2 = CM.size2 x+ fromMat = mkM' matSizes = const (size (Proxy :: Proxy f), size (Proxy :: Proxy g)) class Scheme (f :: * -> *) where@@ -193,10 +162,26 @@ reproxy = const (Proxy,Proxy) (px, py) = reproxy pxy -instance GFromVector f a => GFromVector (M1 i d f) a where+instance (Datatype d, GFromVector f a) => GFromVector (D1 d f) a where+ gfromVector name vs p = ret+ where+ ret = M1 $ gfromVector (name ++ "," ++ dname) vs $ reproxy p+ dname = datatypeName ret+ reproxy :: Proxy (D1 d f a) -> Proxy (f a)+ reproxy = const Proxy++instance (Constructor c, GFromVector f a) => GFromVector (C1 c f) a where+ gfromVector name vs p = ret+ where+ ret = M1 $ gfromVector (name ++ "," ++ cname) vs $ reproxy p+ cname = conName ret+ reproxy :: Proxy (C1 c f a) -> Proxy (f a)+ reproxy = const Proxy++instance (GFromVector f a) => GFromVector (S1 s f) a where gfromVector name vs = M1 . gfromVector name vs . reproxy where- reproxy :: Proxy (M1 i d f p) -> Proxy (f p)+ reproxy :: Proxy (S1 s f a) -> Proxy (f a) reproxy = const Proxy instance (FunctionIO f, Viewable a) => GFromVector (Rec0 (f a)) a where@@ -223,7 +208,7 @@ instance View f => GToVector (Rec0 (J f a)) a where gtoVector = Seq.singleton . unsafeUnJ . unK1 -instance (View f, View g) => GToVector (Rec0 (M f g a)) a where+instance (View f, View g) => GToVector (Rec0 (M.M f g a)) a where gtoVector = Seq.singleton . unM . unK1 --instance GToVector U1 a where
src/Dyno/View/Unsafe/M.hs view
@@ -15,7 +15,8 @@ import GHC.Generics ( Generic ) import Data.Proxy-import Data.Binary ( Binary(..) )+import qualified Data.Binary as B+import qualified Data.Serialize as S import qualified Data.Foldable as F import qualified Data.Vector as V import Data.Vector ( Vector )@@ -30,9 +31,13 @@ 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 (View f, View g) => B.Binary (M f g DMatrix) where+ put = B.put . unM+ get = fmap mkM B.get++instance (View f, View g) => S.Serialize (M f g DMatrix) where+ put = S.put . unM+ get = fmap mkM S.get instance Show a => Show (M f g a) where showsPrec p (UnsafeM x) = showsPrec p x
src/Dyno/View/Unsafe/View.hs view
@@ -21,7 +21,8 @@ import qualified Data.Sequence as Seq import Data.Proxy ( Proxy(..) ) import qualified Data.Vector as V-import Data.Binary ( Binary(..) )+import qualified Data.Binary as B+import qualified Data.Serialize as S import qualified Casadi.CMatrix as CM @@ -29,9 +30,12 @@ newtype J (f :: * -> *) (a :: *) = UnsafeJ { unsafeUnJ :: a } deriving (Eq, Generic) -instance (View f, Binary a, Viewable a) => Binary (J f a) where- put = put . unJ- get = fmap mkJ get+instance (View f, B.Binary a, Viewable a) => B.Binary (J f a) where+ put = B.put . unJ+ get = fmap mkJ B.get+instance (View f, S.Serialize a, Viewable a) => S.Serialize (J f a) where+ put = S.put . unJ+ get = fmap mkJ S.get instance Show a => Show (J f a) where showsPrec p (UnsafeJ x) = showsPrec p x@@ -81,8 +85,8 @@ mkJ' x | ny' == 1 && nx == nx' = Right (UnsafeJ x) | ny' == 0 && nx == nx' = Right (UnsafeJ (vrecoverDimension x 0))- | otherwise = Left $ "mkJ length mismatch: typed size: " ++ show nx ++- ", actual size: " ++ show nx'+ | otherwise = Left $ "mkJ length mismatch: typed size: " ++ show (nx,1::Int) +++ ", actual size: " ++ show (nx',ny') where nx = size (Proxy :: Proxy f) nx' = vsize1 x
src/Dyno/View/View.hs view
@@ -53,7 +53,7 @@ v2d = mkJ . CM.fromDVector . unJ d2v :: View f => J f DMatrix.DMatrix -> J f (V.Vector Double)-d2v = mkJ . DMatrix.ddata . CM.dense . unJ+d2v = mkJ . DMatrix.dnonzeros . CM.densify . unJ fmapJ :: View f => (a -> b) -> J f (Vector a) -> J f (Vector b) fmapJ f = mkJ . V.map f . unJ
src/Dyno/View/Viewable.hs view
@@ -1,5 +1,4 @@ {-# OPTIONS_GHC -Wall #-}-{-# Language TypeFamilies #-} module Dyno.View.Viewable ( Viewable(..)
tests/IntegrationTests.hs view
@@ -1,15 +1,12 @@ {-# OPTIONS_GHC -Wall #-} {-# Language ScopedTypeVariables #-}-{-# Language RankNTypes #-}-{-# Language FlexibleInstances #-}-{-# Language DeriveFunctor #-}-{-# Language DeriveGeneric #-} {-# Language DataKinds #-}+{-# Language TypeFamilies #-} {-# Language PolyKinds #-}-{-# Language GADTs #-}+{-# Language DeriveFunctor #-} {-# Language DeriveGeneric #-}-{-# Language FlexibleInstances #-}-{-# Language PolyKinds #-}+{-# Language RankNTypes #-}+{-# Language FlexibleContexts #-} module IntegrationTests ( integrationTests@@ -32,7 +29,7 @@ import Dyno.View.JV ( splitJV ) import Dyno.TypeVecs ( Dim ) import Dyno.Solvers-import Dyno.Nlp ( Nlp(..), NlpOut(..) )+import Dyno.Nlp ( NlpOut(..) ) import Dyno.NlpUtils import Dyno.Ocp@@ -56,36 +53,59 @@ --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)++data IntegrationOcp x p+type instance X (IntegrationOcp x p) = x+type instance Z (IntegrationOcp x p) = None+type instance U (IntegrationOcp x p) = None+type instance P (IntegrationOcp x p) = p+type instance R (IntegrationOcp x p) = x+type instance O (IntegrationOcp x p) = None+type instance C (IntegrationOcp x p) = x+type instance H (IntegrationOcp x p) = None+type instance Q (IntegrationOcp x p) = None++runIntegration ::+ forall x p deg n+ . ( Vectorize x, Vectorize p, Dim deg, Dim n )+ => Proxy n -> Proxy deg+ -> QuadratureRoots+ -> (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- }+ -> IO (Either String (x Double))+runIntegration _ _ roots ode x0 p tf = do+ let ocp :: OcpPhase' (IntegrationOcp x p)+ ocp = 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+ }+ let guess :: J (CollTraj x None None p n deg) (Vector Double)+ guess = cat $ makeGuessSim roots tf x0 (\x _ -> ode x p 0) (\_ _ -> None) p+ cp <- makeCollProblem roots ocp guess :: IO (CollProblem x None None p x None x None None n deg)+ (msg, opt') <- solveNlp solver (cpNlp cp) Nothing+ return $ case msg of+ Left m -> Left m+ Right _ -> Right (toXf (xOpt opt')) @@ -128,8 +148,9 @@ 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 :: ( 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@@ -175,22 +196,3 @@ ( 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
@@ -27,4 +27,4 @@ } my_test_opts :: TestOptions' Maybe-my_test_opts = mempty { topt_timeout = Just (Just 2000000) }+my_test_opts = mempty { topt_timeout = Just (Just 5000000) }
tests/VectorizeTests.hs view
@@ -49,7 +49,7 @@ , dProxy :: Proxy (n :: k) } -> Dims instance Show Dims where- show (Dims _ p) = "D" ++ show (reflectDim p)+ show (Dims _ p) = show (reflectDim p) instance Arbitrary Dims where arbitrary = elements [ d0, d1, d2, d3, d4, d10, d100 ]
tests/ViewTests.hs view
@@ -14,7 +14,8 @@ import GHC.Generics ( Generic1 ) import Data.Proxy ( Proxy(..) )-import Data.Binary ( encode, decodeOrFail )+import qualified Data.Binary as B+import qualified Data.Serialize as S import qualified Data.Traversable as T import qualified Data.Packed.Matrix as Mat import qualified Numeric.LinearAlgebra ( ) -- for Eq Matrix@@ -361,20 +362,34 @@ m2 = toMat m1 :: M f f DMatrix return $ beEqual m0 m2 -prop_serializeDeserialize :: Test-prop_serializeDeserialize =- testProperty "(M f g DMatrix): deserialize . serialize" $+prop_serializeDeserializeBinary :: Test+prop_serializeDeserializeBinary =+ testProperty "(M f g DMatrix): Binary 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+ let m1 = B.encode m0 return $- case decodeOrFail m1 of+ case B.decodeOrFail m1 of Left (_,_,msg) -> counterexample ("deserialization failure " ++ show msg) False Right (_,_,m2) -> beEqual m0 m2 +prop_serializeDeserializeCereal :: Test+prop_serializeDeserializeCereal =+ testProperty "(M f g DMatrix): Cereal 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 = S.encode m0+ return $+ case S.decode m1 of+ Left msg -> counterexample ("deserialization failure " ++ show msg) False+ Right m2 -> beEqual m0 m2+ prop_vsplitTup :: Test prop_vsplitTup = testProperty "vcatTup . vsplitTup" $@@ -452,7 +467,8 @@ , prop_fromToHMat , prop_covFromToMat , prop_covToFromMat- , prop_serializeDeserialize+ , prop_serializeDeserializeBinary+ , prop_serializeDeserializeCereal , prop_vsplitTup , prop_hsplitTup , prop_vsplitTrip