diff --git a/README.md b/README.md --- a/README.md +++ b/README.md @@ -3,10 +3,9 @@ This library has a few distinct features, which may later be broken into separate packages: * high-level, strongly-typed interface to CasADi * NLP modeling/solving (examples/Basic.hs, examples/BasicJ.hs) -* monadic NLP modeling DSL (examples/StaticExample.hs) * OCP modeling/solving (examles/Glider.hs) -* monadic OCP modeling DSL (examples/OcpM.hs, examples/Rocket.hs) -* live plotter for OCP solving (examples/Plotter.hs) +* proof of concept monadic NLP/OCP modeling DSL (examples/{NlpDsl.hs,OcpDslRocket.hs}) +* live plotter for OCP solving (examples/Dynoplot.hs) This package is built on top of CasADi (www.casadi.org). You will have to install the CasADi C++ libraries and the casadi-bindings haskell package. diff --git a/dynobud.cabal b/dynobud.cabal --- a/dynobud.cabal +++ b/dynobud.cabal @@ -1,5 +1,5 @@ name: dynobud -version: 1.0.0.0 +version: 1.1.0.0 synopsis: your dynamic optimization buddy description: See readme at license: LGPL-3 @@ -20,12 +20,7 @@ library exposed-modules: Dyno.LagrangePolynomials Dyno.TypeVecs - Dyno.Nats Dyno.MultipleShooting - Dyno.Dae - Dyno.Models.Aircraft - Dyno.Models.AeroCoeffs - Dyno.Models.Betty Dyno.Ocp Dyno.DirectCollocation Dyno.DirectCollocation.Dynamic @@ -34,32 +29,27 @@ Dyno.DirectCollocation.Integrate Dyno.DirectCollocation.Profile Dyno.DirectCollocation.Quadratures - Dyno.DirectCollocation.Reify Dyno.DirectCollocation.Robust Dyno.DirectCollocation.Types - Dyno.Cov Dyno.SXElement - Dyno.View - Dyno.View.CasadiMat + Dyno.View.Cov Dyno.View.CustomFunction Dyno.View.Fun Dyno.View.FunJac Dyno.View.HList Dyno.View.JV + Dyno.View.JVec Dyno.View.M - Dyno.View.NumInstances Dyno.View.Scheme Dyno.View.Symbolic + Dyno.View.Unsafe.View + Dyno.View.Unsafe.M Dyno.View.View Dyno.View.Viewable Dyno.Vectorize Dyno.Nlp - Dyno.NlpMonad Dyno.NlpScaling Dyno.NlpSolver - Dyno.OcpMonad - Dyno.Interface.LogsAndErrors - Dyno.Interface.Types Dyno.Solvers -- Dyno.Sqp.Sqp -- Dyno.Sqp.LineSearch @@ -70,30 +60,25 @@ Dyno.Server.Server other-modules: + build-depends: base >=4.6 && < 5, + casadi-bindings-core >= 2.2.0.2, + casadi-bindings >= 2.2.0.4, +-- casadi-bindings-internal, + jacobi-roots >=0.2 && <0.3, + spatial-math >= 0.2.1.0, vector >=0.10, data-default, mtl >=2.2.1, containers >=0.5, - jacobi-roots >=0.2 && <0.3, hmatrix, - unordered-containers >=0.2, - casadi-bindings-internal, - casadi-bindings-core >= 2.2.0.2, - casadi-bindings >= 2.2.0.2, - transformers >=0.3, - primitive >=0.5 && <0.6, - ghc-prim >=0.3 && <0.4, linear >= 1.3.1.1, - spatial-math >= 0.2.1.0, - tagged >= 0.6, reflection >= 1.3.2, lens, cereal, - glib, time, - stm, gtk >= 0.13, + glib, Chart-cairo >= 1.3.3, cairo, Chart >= 1.3.3, @@ -122,6 +107,7 @@ default-language: Haskell2010 build-depends: dynobud , base >=4.6 && < 5 + , casadi-bindings , vector , linear , Chart >= 1.3.3 @@ -132,13 +118,14 @@ ghc-options: -O2 -executable plotSofa +executable sofa-viz-2000 if flag(examples) Buildable: True else Buildable: False hs-source-dirs: examples - main-is: PlotSofa.hs + main-is: SofaVisualizer.hs + other-modules: Sofa.Common default-language: Haskell2010 build-depends: dynobud, base >=4.6 && < 5, @@ -152,13 +139,14 @@ vector ghc-options: -O2 -threaded -executable sofaTime +executable sofa-expand-o-matic if flag(examples) Buildable: True else Buildable: False hs-source-dirs: examples - main-is: Sofa.hs + main-is: SofaExpando.hs + other-modules: Sofa.Common default-language: Haskell2010 build-depends: dynobud, vector, @@ -180,6 +168,7 @@ default-language: Haskell2010 build-depends: dynobud, vector, + casadi-bindings, base >= 4.6 && < 5 ghc-options: -threaded -O2 @@ -197,51 +186,75 @@ ghc-options: -threaded -O2 -executable ocpMonad +executable ocpDslSpring if flag(examples) Buildable: True else Buildable: False hs-source-dirs: examples - main-is: OcpM.hs + main-is: OcpDslSpring.hs + other-modules: ExampleDsl.OcpMonad + ExampleDsl.LogsAndErrors + ExampleDsl.Types default-language: Haskell2010 - build-depends: dynobud, - vector >=0.10, - base >=4.6 && < 5, - zeromq4-haskell, - cereal, - bytestring - + build-depends: base >=4.6 && < 5 + , dynobud + , casadi-bindings + , vector + , containers + , unordered-containers + , lens + , bytestring + , zeromq4-haskell + , cereal + , mtl ghc-options: -threaded -O2 -executable rocket +executable ocpDslRocket if flag(examples) Buildable: True else Buildable: False hs-source-dirs: examples - main-is: Rocket.hs + main-is: OcpDslRocket.hs + other-modules: ExampleDsl.OcpMonad + ExampleDsl.LogsAndErrors + ExampleDsl.Types default-language: Haskell2010 - build-depends: dynobud, - vector >=0.10, - base >=4.6 && < 5, - zeromq4-haskell, - cereal, - bytestring + build-depends: base >=4.6 && < 5 + , dynobud + , casadi-bindings + , vector + , containers + , unordered-containers + , lens + , bytestring + , zeromq4-haskell + , cereal + , mtl ghc-options: -threaded -O2 -executable staticExample +executable nlpDsl if flag(examples) Buildable: True else Buildable: False hs-source-dirs: examples - main-is: StaticExample.hs + main-is: NlpDsl.hs + other-modules: ExampleDsl.NlpMonad + ExampleDsl.LogsAndErrors + ExampleDsl.Types default-language: Haskell2010 - build-depends: dynobud, - vector >=0.10, - base >=4.6 && < 5 + build-depends: base >=4.6 && < 5 + , dynobud + , casadi-bindings + , vector + , linear + , containers + , unordered-containers + , lens + , mtl ghc-options: -threaded -O2 executable basic @@ -267,6 +280,7 @@ default-language: Haskell2010 build-depends: dynobud, vector >=0.10, + casadi-bindings >=0.10, base >=4.6 && < 5 ghc-options: -threaded -O2 @@ -290,6 +304,9 @@ Buildable: False hs-source-dirs: examples main-is: Glider.hs + other-modules: Glider.Aircraft + Glider.AeroCoeffs + Glider.Betty default-language: Haskell2010 build-depends: dynobud, base >=4.6 && < 5, @@ -326,7 +343,7 @@ else Buildable: False hs-source-dirs: examples - main-is: Plotter.hs + main-is: Dynoplot.hs default-language: Haskell2010 build-depends: dynobud, base >=4.6 && < 5, diff --git a/examples/Basic.hs b/examples/Basic.hs --- a/examples/Basic.hs +++ b/examples/Basic.hs @@ -7,6 +7,8 @@ module Main where +import GHC.Generics ( Generic1 ) + import Dyno.Vectorize import Dyno.Nlp import Dyno.NlpSolver diff --git a/examples/BasicJ.hs b/examples/BasicJ.hs --- a/examples/BasicJ.hs +++ b/examples/BasicJ.hs @@ -1,28 +1,36 @@ -- | Minimize the Rosenbrock function (plus a trivial constraint) using -- the more complicated NLP' interface. +-- Unfortunately, at the moment there only types here are (JV ) compound types +-- so the use of Views aren't fully illustrated. +-- todo: comment up the multiple shooting code as an example {-# OPTIONS_GHC -Wall #-} +{-# Language DeriveFunctor #-} {-# Language DeriveGeneric #-} module Main where -import GHC.Generics ( Generic ) +import GHC.Generics ( Generic, Generic1 ) + import Data.Vector ( Vector ) import qualified Data.Vector as V -import Dyno.View +import Casadi.MX ( MX ) +import Dyno.View.View +import Dyno.View.JV ( JV, catJV, catJV', splitJV' ) +import Dyno.Vectorize import Dyno.Nlp import Dyno.NlpSolver import Dyno.Solvers -data X a = X (J S a) (J S a) deriving (Generic, Show) -data G a = G (J S a) deriving (Generic, Show) +data X a = X a a deriving (Functor, Generic, Generic1, Show) +data G a = G a deriving (Functor, Generic, Generic1, Show) -instance View X -instance View G +instance Vectorize X +instance Vectorize G -myNlp :: Nlp' X JNone G MX +myNlp :: Nlp' (JV X) JNone (JV G) MX myNlp = Nlp' { nlpFG' = fg , nlpBX' = bx , nlpBG' = bg @@ -35,24 +43,24 @@ , nlpScaleG' = Nothing } where - x0 :: J X (V.Vector Double) - x0 = cat $ X (-8) (-8) + x0 :: J (JV X) (V.Vector Double) + x0 = catJV $ X (-8) (-8) - bx :: J X (Vector Bounds) - bx = mkJ $ - V.fromList [ (Just (-21), Just 0.5) - , (Just (-2), Just 2) - ] - bg :: J G (Vector Bounds) - bg = mkJ $ (V.singleton (Just (-10), Just 10)) + bx :: J (JV X) (Vector Bounds) + bx = catJV $ + X (Just (-21), Just 0.5) + (Just (-2), Just 2) - fg :: J X MX -> J JNone MX -> (J S MX, J G MX) - fg xy _ = (f, cat g) + bg :: J (JV G) (Vector Bounds) + bg = catJV $ G (Just (-10), Just 10) + + fg :: J (JV X) MX -> J JNone MX -> (J (JV Id) MX, J (JV G) MX) + fg xy _ = (f, catJV' g) where f = (1-x)**2 + 100*(y - x**2)**2 g = G x - X x y = split xy + X x y = splitJV' xy main :: IO () main = do diff --git a/examples/DaeColl.hs b/examples/DaeColl.hs --- a/examples/DaeColl.hs +++ b/examples/DaeColl.hs @@ -2,15 +2,18 @@ {-# Language FlexibleInstances #-} {-# Language DeriveFunctor #-} {-# Language DeriveGeneric #-} +{-# Language DataKinds #-} +{-# Language PolyKinds #-} module Main where +import GHC.Generics ( Generic, Generic1 ) + import Data.Vector ( Vector ) import Dyno.Vectorize -import Dyno.View +import Dyno.View.View ( J, jfill ) import Dyno.TypeVecs -import Dyno.Nats import Dyno.Solvers --import Dyno.Sqp.Sqp --import Dyno.Sqp.LineSearch @@ -67,7 +70,7 @@ fx = torque*y fy = -torque*x + m*9.8 -pendOcp :: OcpPhase PendX PendZ PendU PendP PendR PendO (Vec D8) None +pendOcp :: OcpPhase PendX PendZ PendU PendP PendR PendO (Vec 8) None pendOcp = OcpPhase { ocpMayer = mayer , ocpLagrange = lagrange , ocpDae = pendDae @@ -104,7 +107,7 @@ ubnd :: PendU Bounds ubnd = PendU (Just (-40), Just 40) -bc :: Floating a => PendX a -> PendX a -> Vec D8 a +bc :: Floating a => PendX a -> PendX a -> Vec 8 a bc (PendX x0 y0 vx0 vy0) (PendX xf yf vxf vyf) = mkVec' [ x0 @@ -117,8 +120,8 @@ , vyf ] -type NCollStages = D80 -type CollDeg = D3 +type NCollStages = 80 +type CollDeg = 3 guess :: J (CollTraj PendX PendZ PendU PendP NCollStages CollDeg) (Vector Double) guess = jfill 1 diff --git a/examples/Dynoplot.hs b/examples/Dynoplot.hs new file mode 100644 --- /dev/null +++ b/examples/Dynoplot.hs @@ -0,0 +1,55 @@ +{-# OPTIONS_GHC -Wall #-} +{-# Language DeriveDataTypeable #-} + +module Main ( main ) where + +import qualified Control.Concurrent as CC +import Control.Monad ( when, forever ) +import Data.ByteString.Char8 ( pack ) +import Data.Serialize +import qualified System.ZMQ4 as ZMQ +import System.Console.CmdArgs ( (&=), Data, Typeable ) +import qualified System.Console.CmdArgs as CA + +import Dyno.Server.Server ( runPlotter, newChannel ) +import Dyno.DirectCollocation.Dynamic ( DynPlotPoints, CollTrajMeta ) + +import Dynoplot.Channel ( dynoplotUrl, dynoplotChannelName ) + +sub :: String -> ((DynPlotPoints Double, CollTrajMeta) -> IO ()) -> String -> IO () +sub ip' writeChan name = ZMQ.withContext $ \context -> + ZMQ.withSocket context ZMQ.Sub $ \subscriber -> do + ZMQ.connect subscriber ip' + ZMQ.subscribe subscriber (pack name) + forever $ do + _ <- ZMQ.receive subscriber + mre <- ZMQ.moreToReceive subscriber + when mre $ do + msg <- ZMQ.receive subscriber + let decoded :: (DynPlotPoints Double, CollTrajMeta) + decoded = case decode msg of + Left err -> error err + Right t -> t + writeChan decoded + +main :: IO () +main = do + args <- CA.cmdArgs (myargs &= CA.program "dynoplot") + let ip' = ip args + channel' = channel args + putStrLn $ "using ip \""++ip'++"\"" + putStrLn $ "using channel \""++channel'++"\"" + + (c0, writeMe) <- newChannel channel' + + listenerTid0 <- CC.forkIO (sub ip' writeMe channel') + runPlotter c0 [listenerTid0] + +data VisArgs = VisArgs { ip :: String + , channel :: String + } deriving (Show, Data, Typeable) + +myargs :: VisArgs +myargs = VisArgs { ip = dynoplotUrl &= CA.help "an IP address" &= CA.typ "ADDRESS" + , channel = dynoplotChannelName &= CA.help "zmq channel name" + } &= CA.summary "plotter for dynobud OCPs" diff --git a/examples/ExampleDsl/LogsAndErrors.hs b/examples/ExampleDsl/LogsAndErrors.hs new file mode 100644 --- /dev/null +++ b/examples/ExampleDsl/LogsAndErrors.hs @@ -0,0 +1,58 @@ +{-# OPTIONS_GHC -Wall #-} +{-# Language PackageImports #-} +{-# Language FlexibleContexts #-} + +module ExampleDsl.LogsAndErrors + ( ErrorMessage (..) + , LogMessage (..) + , countLogs + , debug + , warn + , err + , impossible + ) where + +import "mtl" Control.Monad.Except ( MonadError, throwError ) +import "mtl" Control.Monad.Writer ( MonadWriter, tell ) + +data LogMessage = Debug String + | Warning String + | Error String + | Impossible String + +instance Show LogMessage where + show (Debug x) = "Debug: " ++ x + show (Warning x) = "Warning: " ++ x + show (Error x) = "Error: " ++ x + show (Impossible x) = "\"Impossible\" Error: " ++ x + +countLogs' :: (Int,Int,Int,Int) -> [LogMessage] -> (Int,Int,Int,Int) +countLogs' x [] = x +countLogs' (a,b,c,d) (Debug _:xs) = countLogs' (a+1, b, c, d) xs +countLogs' (a,b,c,d) (Warning _:xs) = countLogs' ( a, b+1, c, d) xs +countLogs' (a,b,c,d) (Error _:xs) = countLogs' ( a, b, c+1, d) xs +countLogs' (a,b,c,d) (Impossible _:xs) = countLogs' ( a, b, c, d+1) xs + +countLogs :: [LogMessage] -> (Int,Int,Int,Int) +countLogs = countLogs' (0,0,0,0) + +newtype ErrorMessage = ErrorMessage String -- deriving Error +instance Show ErrorMessage where + show (ErrorMessage msg) = msg + +logMessage :: MonadWriter [t] m => t -> m () +logMessage x = tell [x] + +debug :: MonadWriter [LogMessage] m => String -> m () +debug = logMessage . Debug + +warn :: MonadWriter [LogMessage] m => String -> m () +warn = logMessage . Warning + +err :: (MonadError ErrorMessage m, MonadWriter [LogMessage] m) => + String -> m a +err x = logMessage (Error x) >> throwError (ErrorMessage x) + +impossible :: (MonadError ErrorMessage m, MonadWriter [LogMessage] m) => + String -> m b +impossible x = logMessage (Impossible x) >> throwError (ErrorMessage ("\"impossible error\": " ++ x)) diff --git a/examples/ExampleDsl/NlpMonad.hs b/examples/ExampleDsl/NlpMonad.hs new file mode 100644 --- /dev/null +++ b/examples/ExampleDsl/NlpMonad.hs @@ -0,0 +1,236 @@ +{-# OPTIONS_GHC -Wall #-} +{-# Language ScopedTypeVariables #-} +{-# Language PackageImports #-} +{-# Language GeneralizedNewtypeDeriving #-} +{-# Language RankNTypes #-} + +module ExampleDsl.NlpMonad + ( NlpMonad + , (===) + , (<==) + , (>==) + , bound + , minimize + , designVar + , solveStaticNlp + ) where + +import Control.Applicative ( Applicative ) +import Control.Monad ( when ) +import "mtl" Control.Monad.Reader ( MonadIO(..) ) +import "mtl" Control.Monad.Except ( ExceptT, MonadError, runExceptT ) +import "mtl" Control.Monad.State ( StateT, MonadState, runStateT, get, put ) +import "mtl" Control.Monad.Writer ( WriterT, MonadWriter, runWriterT ) +import qualified Data.Foldable as F +import qualified Data.HashSet as HS +import qualified Data.Sequence as S +import qualified Data.Map.Strict as M +import Data.Sequence ( (|>) ) +import Data.Vector ( Vector ) +import qualified Data.Vector as V +import Linear.V ( Dim(..) ) +import Data.Proxy + +import Casadi.SharedObject ( soInit ) +import Casadi.MX ( MX ) +import Casadi.SXFunction +import Casadi.Function +import Casadi.CMatrix ( veccat ) + +import Dyno.View.Unsafe.View ( mkJ, unJ ) + +import Dyno.SXElement ( SXElement, sxElementSym, sxElementToSX ) +import Dyno.Vectorize ( Id, fill ) +import Dyno.TypeVecs ( Vec ) +import Dyno.View.View ( View(..), J, JNone(..), jfill ) +import Dyno.View.JV ( JV ) +import Dyno.View.JVec ( JVec ) +import qualified Dyno.TypeVecs as TV +import Dyno.NlpSolver ( NlpSolverStuff, solveNlp' ) +import Dyno.Nlp ( Nlp'(..), NlpOut'(..), Bounds) + +import ExampleDsl.LogsAndErrors +import ExampleDsl.Types + +--withEllipse :: Int -> String -> String +--withEllipse n blah +-- | length blah <= n = blah +-- | otherwise = take n blah ++ "..." + +newtype NlpMonad a = + NlpMonad + { runNlp :: ExceptT ErrorMessage (WriterT [LogMessage] (StateT NlpMonadState IO)) a + } deriving ( Functor + , Applicative + , Monad + , MonadError ErrorMessage + , MonadState NlpMonadState + , MonadWriter [LogMessage] + , MonadIO + ) + +emptySymbolicNlp :: NlpMonadState +emptySymbolicNlp = NlpMonadState S.empty HS.empty S.empty ObjectiveUnset HomotopyParamUnset + +build :: NlpMonad a -> IO (Either ErrorMessage a, [LogMessage], NlpMonadState) +build = build' emptySymbolicNlp + where + build' :: NlpMonadState -> NlpMonad a -> IO (Either ErrorMessage a, [LogMessage], NlpMonadState) + build' nlp0 builder = do + ((result,logs),state) <- flip runStateT nlp0 . runWriterT . runExceptT . runNlp $ builder + return (result, logs, state) + +designVar :: String -> NlpMonad SXElement +designVar name = do + debug $ "adding design variable \""++name++"\"" + state0 <- get + let map0 = nlpXSet state0 + sym <- liftIO (sxElementSym name) + when (HS.member name map0) $ err $ name ++ " already in symbol map" + let state1 = state0 { nlpX = nlpX state0 |> (name, sym) + , nlpXSet = HS.insert name map0 + } + put state1 + return sym + +infix 4 === +(===) :: SXElement -> SXElement -> NlpMonad () +(===) lhs rhs = do + debug $ "adding equality constraint: " +-- ++ withEllipse 30 (show lhs) ++ " == " ++ withEllipse 30 (show rhs) + state0 <- get + put $ state0 { nlpConstraints = nlpConstraints state0 |> Eq2 lhs rhs } + +infix 4 <== +(<==) :: SXElement -> SXElement -> NlpMonad () +(<==) lhs rhs = do + debug $ "adding inequality constraint: " +-- ++ withEllipse 30 (show lhs) ++ " <= " ++ withEllipse 30 (show rhs) + state0 <- get + put $ state0 { nlpConstraints = nlpConstraints state0 |> Ineq2 lhs rhs } + +infix 4 >== +(>==) :: SXElement -> SXElement -> NlpMonad () +(>==) lhs rhs = do + debug $ "adding inequality constraint: " +-- ++ withEllipse 30 (show lhs) ++ " >= " ++ withEllipse 30 (show rhs) + state0 <- get + put $ state0 { nlpConstraints = nlpConstraints state0 |> Ineq2 rhs lhs } + +bound :: SXElement -> (Double,Double) -> NlpMonad () +bound mid (lhs, rhs) = do + debug $ "adding inequality bound: " -- ++ +-- withEllipse 30 (show lhs) ++ " <= " ++ +-- withEllipse 30 (show mid) ++ " <= " ++ +-- withEllipse 30 (show rhs) + state0 <- get + put $ state0 { nlpConstraints = nlpConstraints state0 |> Ineq3 mid (lhs, rhs) } + +minimize :: SXElement -> NlpMonad () +minimize obj = do + debug $ "setting objective function: " -- ++ withEllipse 30 (show obj) + state0 <- get + case nlpObj state0 of + Objective _x -> err $ init $ unlines + [ "you set the objective function twice" +-- , " old val: " ++ show x +-- , " new val: " ++ show obj + ] + ObjectiveUnset -> put $ state0 { nlpObj = Objective obj } + + +constr :: Constraint SXElement -> (SXElement, Bounds) +constr (Eq2 lhs rhs) = (lhs - rhs, (Just 0, Just 0)) +constr (Ineq2 lhs rhs) = (lhs - rhs, (Nothing, Just 0)) +constr (Ineq3 x (lhs,rhs)) = (x, (Just lhs, Just rhs)) + + +toG :: Dim ng => S.Seq (Constraint SXElement) -> Vec ng (SXElement, Bounds) +toG nlpConstraints' = TV.mkVec $ V.fromList $ F.toList $ fmap constr nlpConstraints' + +buildNlp :: forall nx ng . + (Dim nx, Dim ng) => NlpMonadState -> IO (Nlp' (JVec nx (JV Id)) JNone (JVec ng (JV Id)) MX) +buildNlp state = do + obj <- case nlpObj state of + Objective obj' -> return obj' + ObjectiveUnset -> error "solveNlp: objective unset" + + let inputs :: Vector SXElement + inputs = V.fromList $ map snd $ F.toList (nlpX state) + + g :: Vec ng SXElement + gbnd :: Vec ng Bounds + (g, gbnd) = TV.tvunzip $ toG (nlpConstraints state) + + xbnd :: Vec nx Bounds + xbnd = fill (Nothing, Nothing) + + svector = veccat . fmap sxElementToSX + + sxfun <- sxFunction (V.fromList [svector inputs]) (V.fromList [svector (V.singleton obj), svector (TV.unVec g)]) + soInit sxfun + let fg :: J (JVec nx (JV Id)) MX -> J JNone MX -> (J (JV Id) MX, J (JVec ng (JV Id)) MX) + fg x _ = (mkJ (ret V.! 0), mkJ (ret V.! 1)) + where + ret = callMX sxfun (V.singleton (unJ x)) + + return Nlp' { nlpFG' = fg + , nlpBX' = mkJ (TV.unVec xbnd) + , nlpBG' = mkJ (TV.unVec gbnd) + , nlpX0' = jfill 0 + , nlpP' = cat JNone + , nlpScaleF' = Nothing + , nlpScaleX' = Nothing + , nlpScaleG' = Nothing + , nlpLamX0' = Nothing + , nlpLamG0' = Nothing + } + + +reifyNlp :: + forall r . + NlpMonad () -> Maybe (Vector Double -> IO Bool) -> M.Map String Double + -> (forall x g . (View x, View g) + => Nlp' x JNone g MX -> Maybe (J x (Vector Double) -> IO Bool) -> NlpMonadState -> IO r) + -> IO r +reifyNlp nlpmonad cb x0map f = do + (ret,logs,state) <- build nlpmonad + case ret of + Right _ -> return () + Left err' -> error $ unlines $ map show logs ++ [show err'] + + let nx = S.length (nlpX state) + ng = S.length (nlpConstraints state) + + lookupGuess = flip (M.findWithDefault 0) x0map + x0 = V.fromList $ map (lookupGuess . fst) $ F.toList (nlpX state) + + TV.reifyDim nx $ \(Proxy :: Proxy nx) -> +-- TV.reifyDim np $ \(Proxy :: Proxy np) -> + TV.reifyDim ng $ \(Proxy :: Proxy ng) -> do + nlp0 <- buildNlp state :: IO (Nlp' (JVec nx (JV Id)) JNone (JVec ng (JV Id)) MX) + let nlp = nlp0 { nlpX0' = mkJ x0 } + f nlp (fmap (. unJ) cb) state + + +solveStaticNlp :: + NlpSolverStuff + -> NlpMonad () -> [(String,Double)] -> Maybe (Vector Double -> IO Bool) + -> IO (Either String String, Double, [(String,Double)]) +solveStaticNlp solverStuff nlp x0' callback = reifyNlp nlp callback x0 foo + where + x0 = M.fromListWithKey errlol x0' + errlol name xx yy = + error $ "solveStaticNlp: initial guess has variable \"" ++ name ++ "\" more than once: " ++ + show (xx,yy) + + foo :: + (View x, View p, View g) => + Nlp' x p g MX -> Maybe (J x (Vector Double) -> IO Bool) -> NlpMonadState -> + IO (Either String String, Double, [(String,Double)]) + foo nlp' cb' state = do + (ret,nlpOut) <- solveNlp' solverStuff nlp' cb' + let fopt = V.head (unJ (fOpt' nlpOut)) :: Double + xopt = F.toList $ unJ (xOpt' nlpOut) :: [Double] + xnames = map fst (F.toList (nlpX state)) :: [String] + return (ret, fopt, zip xnames xopt) diff --git a/examples/ExampleDsl/OcpMonad.hs b/examples/ExampleDsl/OcpMonad.hs new file mode 100644 --- /dev/null +++ b/examples/ExampleDsl/OcpMonad.hs @@ -0,0 +1,496 @@ +{-# OPTIONS_GHC -Wall #-} +{-# Language ScopedTypeVariables #-} +{-# Language PackageImports #-} +{-# Language MultiParamTypeClasses #-} +{-# Language FunctionalDependencies #-} +{-# Language GeneralizedNewtypeDeriving #-} +{-# Language FlexibleContexts #-} +{-# Language RankNTypes #-} + +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.NlpSolver ( NlpSolverStuff ) +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) + -> Int -> Int + -> (forall x z u p r o c h . + (Vectorize x, Vectorize z, Vectorize u, Vectorize p, Vectorize r, Vectorize o, Vectorize c, Vectorize h) + => OcpPhase x z u p r o c h -> CollTrajMeta -> IO ret) + -> IO ret +reifyOcpPhase daeMonad mayerMonad bcMonad ocpMonad tbnds n deg f = do + time <- sxElementSym "_t" + endT <- sxElementSym "T" + let time' = sxElementToSX time + endT' = sxElementToSX endT + dae' <- buildDae (daeMonad time) + let dae :: DaeState + dae = case dae' of + (Left errmsg, _, _) -> error $ "buildOcpPhase: buildDae failure: " ++ show errmsg + (_, _, daeState) -> daeState + + xdotnames, xnames, znames, unames, pnames :: Vector String + xdots, xs, zs, us, ps :: Vector SXElement + (xdotnames,xdots) = V.unzip $ V.fromList $ F.toList $ _daeXDot dae + (xnames,xs) = V.unzip $ V.fromList $ F.toList $ _daeX dae + (znames,zs) = V.unzip $ V.fromList $ F.toList $ _daeZ dae + (unames,us) = V.unzip $ V.fromList $ F.toList $ _daeU dae + (pnames,ps) = V.unzip $ V.fromList $ F.toList $ _daeP dae + + xdots' = svector xdots + xs' = svector xs + zs' = svector zs + us' = svector us + ps' = svector ps + + daeResidual :: Vector SXElement + daeResidual = V.map (uncurry (-)) $ V.fromList $ F.toList $ daeConstraints dae + + onames :: Vector String + osOut :: Vector SXElement + (onames, osOut) = V.unzip $ V.fromList $ M.toList $ _daeO dae + os <- V.mapM sxElementSym onames :: IO (Vector SXElement) + let os' = svector os + + lookupThingy :: String -> OcpMonad SXElement + lookupThingy name = do + debug $ "ocp monad: looking up \"" ++ name ++ "\"" + case M.lookup name varmap of + Nothing -> err $ "ocp monad: nothing named \"" ++ name ++ "\"" + Just expr -> do + debug $ "ocp monad: found \"" ++ name ++ "\"" + --debug $ "ocp monad: found \"" ++ name ++ "\": " ++ show expr + return expr + where + varmap :: M.Map String SXElement + varmap = M.fromList $ F.toList $ V.concat + [ V.zip xdotnames xdots + , V.zip xnames xs + , V.zip znames zs + , V.zip unames us + , V.zip pnames ps + , V.zip onames os + ] + + ocp' <- flip runStateT emptySymbolicOcp $ runWriterT $ runExceptT (runOcp (ocpMonad time lookupThingy)) + let ocp :: OcpState + ocp = case ocp' of + ((Left errmsg, logs),_) -> + error $ unlines $ ("" : map show logs) ++ ["","ocp monad failure: " ++ show errmsg] + ((Right _, _), ocpState) -> ocpState + + obj = case ocpLagrangeObj ocp of + ObjectiveUnset -> 0 + Objective obj' -> obj' + + lagFunSX <- sxFunction (V.fromList [xs',zs',us',ps',os',time',endT']) (V.fromList [svector (V.singleton obj)]) + setOption lagFunSX "name" "lagrange" + soInit lagFunSX + + let pathConstraints :: [SXElement] + pathConstraintBnds :: [(Maybe Double, Maybe Double)] + (pathConstraints, pathConstraintBnds) = unzip $ map constr (F.toList (ocpPathConstraints ocp)) + + pathcFunSX <- sxFunction (V.fromList [xs',zs',us',ps',os',time']) + (V.singleton (svector (V.fromList pathConstraints))) + setOption pathcFunSX "name" "pathConstraints" + soInit pathcFunSX + + + daeFunSX <- sxFunction (V.fromList [xdots', xs', zs', us', ps', time']) + (V.fromList [svector daeResidual, svector osOut]) + setOption pathcFunSX "name" "daeResidualAndOutputs" + soInit daeFunSX + + -- run the mayer function + x0s <- mapM (sxElementSym . (++ "_0")) (F.toList xnames) + xFs <- mapM (sxElementSym . (++ "_F")) (F.toList xnames) + let lookupState :: M.Map String SXElement -> String + -> ExceptT ErrorMessage (Writer [LogMessage]) SXElement + lookupState xmap name = do + debug $ "mayer monad: looking up \"" ++ name ++ "\"" + case M.lookup name xmap of + Nothing -> err $ "mayer monad: no state named \"" ++ name ++ "\"" + Just expr -> do + debug $ "mayer monad: found \"" ++ name ++ "\"" + return expr + + xmap0 :: M.Map String SXElement + xmap0 = M.fromList $ zip (F.toList xnames) x0s + + xmapF :: M.Map String SXElement + xmapF = M.fromList $ zip (F.toList xnames) xFs + + mayerObj :: SXElement + mayerObj = case runWriter (runExceptT (mayerMonad endT (lookupState xmap0) (lookupState xmapF))) of + (Left errmsg, logs) -> + error $ unlines $ ("" : map show logs) ++ ["","mayer monad failure: " ++ show errmsg] + (Right ret, _) -> ret + mayerFunSX <- sxFunction (V.fromList [svector (V.singleton endT), svector (V.fromList x0s), svector (V.fromList xFs)]) + (V.singleton (svector (V.singleton mayerObj))) + setOption mayerFunSX "name" "mayer" + soInit mayerFunSX + + + let lookupState0 :: String -> BCMonad SXElement + lookupState0 name = do + debug $ "boundary condition monad: looking up initial \"" ++ name ++ "\"" + case M.lookup name xmap0 of + Nothing -> err $ "boundary condition monad: no state named \"" ++ name ++ "\"" + Just expr -> do + debug $ "boundary condition monad: found \"" ++ name ++ "\"" + return expr + + lookupStateF :: String -> BCMonad SXElement + lookupStateF name = do + debug $ "boundary condition monad: looking up final \"" ++ name ++ "\"" + case M.lookup name xmapF of + Nothing -> err $ "boundary condition monad: no state named \"" ++ name ++ "\"" + Just expr -> do + debug $ "boundary condition monad: found \"" ++ name ++ "\"" + return expr + bcs' <- flip runStateT S.empty $ runWriterT (runExceptT (runBc $ bcMonad lookupState0 lookupStateF)) + let bcs :: Vector SXElement + bcbnds :: Vector Bounds + (bcs,bcbnds) = case bcs' of + ((Left errmsg, logs),_) -> + error $ unlines $ ("" : map show logs) ++ ["","boundary condition monad failure: " ++ show errmsg] + ((Right _,_), ret) -> V.unzip $ V.fromList $ map constr $ F.toList ret + bcFunSX <- sxFunction (V.fromList [svector (V.fromList x0s), svector (V.fromList xFs)]) + (V.singleton (svector bcs)) + setOption bcFunSX "name" "boundaryConditions" + soInit bcFunSX + + let meta = CollTrajMeta + { ctmX = NameTreeNode ("", "") (zip (F.toList xnames) (map NameTreeLeaf [0..])) + , ctmZ = NameTreeNode ("", "") (zip (F.toList znames) (map NameTreeLeaf [0..])) + , ctmU = NameTreeNode ("", "") (zip (F.toList unames) (map NameTreeLeaf [0..])) + , ctmP = NameTreeNode ("", "") (zip (F.toList pnames) (map NameTreeLeaf [0..])) + , ctmO = NameTreeNode ("", "") (zip (F.toList onames) (map NameTreeLeaf [0..])) + , ctmN = n + , ctmDeg = deg + , ctmNx = V.length xnames + , ctmNz = V.length znames + , ctmNu = V.length unames + , ctmNp = V.length pnames + , ctmNo = V.length onames + , ctmNsx = 0 + , ctmQuadRoots = Legendre -- TODO: make this an input + } + TV.reifyDim (ctmNx meta) $ \(Proxy :: Proxy nx) -> + TV.reifyDim (ctmNz meta) $ \(Proxy :: Proxy nz) -> + TV.reifyDim (ctmNu meta) $ \(Proxy :: Proxy nu) -> + TV.reifyDim (ctmNp meta) $ \(Proxy :: Proxy np) -> + TV.reifyDim (V.length daeResidual) $ \(Proxy :: Proxy nr) -> + TV.reifyDim (V.length onames) $ \(Proxy :: Proxy no) -> + TV.reifyDim (V.length bcs) $ \(Proxy :: Proxy nc) -> + TV.reifyDim (length pathConstraints) $ \(Proxy :: Proxy nh) -> do + -- TV.reifyDim ncov $ \(Proxy :: Proxy ncov) -> do + -- TV.reifyDim nsh $ \(Proxy :: Proxy nsh) -> do + -- TV.reifyDim nsc $ \(Proxy :: Proxy nsc) -> do + + let daeFun :: Vec nx SXElement -> Vec nx SXElement -> Vec nz SXElement -> Vec nu SXElement + -> Vec np SXElement -> SXElement + -> (Vec nr SXElement, Vec no SXElement) + daeFun x' x z u p t = (devec (rets V.! 0), devec (rets V.! 1)) + where + rets = callSX daeFunSX (V.fromList [vec x', vec x, vec z, vec u, vec p, sxElementToSX t]) + + lagrangeFun :: Vec nx SXElement -> Vec nz SXElement -> Vec nu SXElement -> Vec np SXElement -> Vec no SXElement -> SXElement -> SXElement -> SXElement + lagrangeFun x z u p o t tf = + sxToSXElement $ V.head $ callSX lagFunSX $ + (V.fromList [vec x, vec z, vec u, vec p, vec o, sxElementToSX t, sxElementToSX tf]) + --Left errmsg -> error $ "toOcpPhase: lagrangeFun: " ++ errmsg ++ + -- "\ninputs: " ++ show (xnames ++ znames ++ unames ++ pnames) ++ show onames ++ + -- "\nnumeric inputs x: " ++ show (V.length x) ++ + -- "\nnumeric inputs z: " ++ show (V.length z) ++ + -- "\nnumeric inputs u: " ++ show (V.length u) ++ + -- "\nnumeric inputs p: " ++ show (V.length p) ++ + -- "\nnumeric inputs o: " ++ show (V.length o) + + pathConstraintFun :: Vec nx SXElement -> Vec nz SXElement -> Vec nu SXElement + -> Vec np SXElement -> Vec no SXElement -> SXElement -> Vec nh SXElement + pathConstraintFun x z u p o t = + devec $ V.head $ callSX pathcFunSX (V.fromList [vec x, vec z, vec u, vec p, vec o, sxElementToSX t]) + + mayerFun :: SXElement -> Vec nx SXElement -> Vec nx SXElement + -> SXElement + mayerFun endT'' x0 xF = sxToSXElement $ V.head $ callSX mayerFunSX (V.fromList [sxElementToSX endT'', vec x0, vec xF]) + + bcFun :: Vec nx SXElement -> Vec nx SXElement -> Vec nc SXElement + bcFun x0 xF = devec $ V.head $ callSX bcFunSX (V.fromList [vec x0, vec xF]) + + ocpPhase = + OcpPhase { ocpMayer = mayerFun + , ocpLagrange = lagrangeFun + , ocpDae = daeFun + , ocpBc = bcFun + , ocpBcBnds = devectorize bcbnds + , ocpPathC = pathConstraintFun + , ocpPathCBnds = devectorize (V.fromList pathConstraintBnds) + , ocpXbnd = fill (Nothing, Nothing) + , ocpZbnd = fill (Nothing, Nothing) + , ocpUbnd = fill (Nothing, Nothing) + , ocpPbnd = fill (Nothing, Nothing) + , ocpTbnd = tbnds + , ocpObjScale = Nothing + , ocpTScale = Nothing + , ocpXScale = Nothing + , ocpZScale = Nothing + , ocpUScale = Nothing + , ocpPScale = Nothing + , ocpResidualScale = Nothing + , ocpBcScale = Nothing + , ocpPathCScale = Nothing + } + f ocpPhase meta + +vec :: Vectorize f => f SXElement -> SX +vec = svector . vectorize + +devec :: Vectorize f => SX -> f SXElement +devec = sxSplitJV . mkJ + +solveStaticOcp :: + NlpSolverStuff + -> (SXElement -> DaeMonad ()) + -> (forall a m . (Floating a, Monad m) => a -> (String -> m a) -> (String -> m a) -> m a) + -> ((String -> BCMonad SXElement) -> (String -> BCMonad SXElement) -> BCMonad ()) + -> (SXElement -> (String -> OcpMonad SXElement) -> OcpMonad ()) + -> (Maybe Double, Maybe Double) + -> Int -> Int + -> Maybe (CollTrajMeta -> DynPlotPoints Double -> IO Bool) + -> IO (Either String String) +solveStaticOcp solverStuff dae mayer bc ocp tbnds n deg cb = + reifyOcpPhase dae mayer bc ocp tbnds n deg woo + where + woo ocpphase meta = solveOcp solverStuff n deg (cb <*> pure meta) ocpphase diff --git a/examples/ExampleDsl/Types.hs b/examples/ExampleDsl/Types.hs new file mode 100644 --- /dev/null +++ b/examples/ExampleDsl/Types.hs @@ -0,0 +1,86 @@ +{-# OPTIONS_GHC -Wall -ddump-splices #-} + +module ExampleDsl.Types + ( Constraint(..) + , Objective(..) + , HomotopyParam(..) + , NlpMonadState(..) + , OcpState(..) + , DaeState(..) + , daeX + , daeXDot + , daeZ + , daeU + , daeP + , daeO + ) where + +import qualified Data.HashSet as HS +import qualified Data.Sequence as S +import qualified Data.Map as M +import Control.Lens +import Data.Functor ( (<$>) ) + +import Dyno.SXElement ( SXElement ) + +data Constraint a = Eq2 a a + | Ineq2 a a + | Ineq3 a (Double, Double) + +data Objective a = ObjectiveUnset | Objective a +data HomotopyParam a = HomotopyParamUnset | HomotopyParam a + +data NlpMonadState = + NlpMonadState + { nlpX :: S.Seq (String, SXElement) + , nlpXSet :: HS.HashSet String + , nlpConstraints :: S.Seq (Constraint SXElement) + , nlpObj :: Objective SXElement + , nlpHomoParam :: HomotopyParam SXElement + } + +data OcpState = OcpState { ocpPathConstraints :: S.Seq (Constraint SXElement) + , ocpLagrangeObj :: Objective SXElement + , ocpHomoParam :: HomotopyParam SXElement + } + +data DaeState = DaeState { _daeXDot :: S.Seq (String, SXElement) + , _daeX :: S.Seq (String, SXElement) + , _daeZ :: S.Seq (String, SXElement) + , _daeU :: S.Seq (String, SXElement) + , _daeP :: S.Seq (String, SXElement) + , _daeO :: M.Map String SXElement + , daeNameSet :: HS.HashSet String + , daeConstraints :: S.Seq (SXElement, SXElement) + } + +--makeLenses ''DaeState +daeXDot :: Lens' DaeState (S.Seq (String, SXElement)) +daeXDot f (DaeState xdot' x z u p o ss c) = + (\xdot -> DaeState xdot x z u p o ss c) <$> f xdot' +{-# INLINE daeXDot #-} + +daeX :: Lens' DaeState (S.Seq (String, SXElement)) +daeX f (DaeState xdot x' z u p o ss c) = + (\x -> DaeState xdot x z u p o ss c) <$> f x' +{-# INLINE daeX #-} + +daeZ :: Lens' DaeState (S.Seq (String, SXElement)) +daeZ f (DaeState xdot x z' u p o ss c) = + (\z -> DaeState xdot x z u p o ss c) <$> f z' +{-# INLINE daeZ #-} + +daeU :: Lens' DaeState (S.Seq (String, SXElement)) +daeU f (DaeState xdot x z u' p o ss c) = + (\u -> DaeState xdot x z u p o ss c) <$> f u' +{-# INLINE daeU #-} + +daeP :: Lens' DaeState (S.Seq (String, SXElement)) +daeP f (DaeState xdot x z u p' o ss c) = + (\p -> DaeState xdot x z u p o ss c) <$> f p' +{-# INLINE daeP #-} + +daeO :: Lens' DaeState (M.Map String SXElement) +daeO f (DaeState xdot x z u p o' ss c) = + (\o -> DaeState xdot x z u p o ss c) <$> f o' +{-# INLINE daeO #-} diff --git a/examples/Glider.hs b/examples/Glider.hs --- a/examples/Glider.hs +++ b/examples/Glider.hs @@ -1,12 +1,14 @@ {-# OPTIONS_GHC -Wall #-} +{-# Language DataKinds #-} module Main ( main ) where +import Data.Proxy ( Proxy(..) ) import Linear import Data.Vector ( Vector ) import Dyno.Vectorize -import Dyno.View +import Dyno.View.View import Dyno.Solvers --import Dyno.Sqp.Sqp --import Dyno.Sqp.LineSearch @@ -17,16 +19,14 @@ import Dyno.DirectCollocation import Dyno.DirectCollocation.Dynamic ( toMeta ) -import Dyno.Models.Aircraft -import Dyno.Models.AeroCoeffs -import Dyno.Models.Betty -import Dyno.Nats +import Glider.Aircraft +import Glider.AeroCoeffs +import Glider.Betty -import GliderShared -import ServerSender ( withCallback ) +import Dynoplot.Callback ( withCallback ) -type NCollStages = D100 -type CollDeg = D2 +type NCollStages = 100 +type CollDeg = 2 mayer :: Floating a => a -> AcX a -> AcX a -> a mayer _ _ _ = 0 @@ -108,25 +108,21 @@ main :: IO () main = do - putStrLn $ "using ip \""++gliderUrl++"\"" - putStrLn $ "using channel \""++gliderChannelName++"\"" - cp <- makeCollProblem ocp let nlp = cpNlp cp - toDyn = cpCallback cp - withCallback gliderUrl gliderChannelName $ \cb -> do + withCallback $ \cb -> do let guess = jfill 1 cb' :: J (CollTraj AcX None AcU None NCollStages CollDeg) (Vector Double) -> IO Bool cb' traj = do - (dyn,_) <- toDyn traj + plotPoints <- cpPlotPoints cp traj let proxy :: Proxy (CollTraj AcX None AcU None NCollStages CollDeg) proxy = Proxy - cb ([dyn], toMeta (cpRoots cp) (Proxy :: Proxy None) proxy) + cb (plotPoints, toMeta (cpRoots cp) (Proxy :: Proxy None) proxy) - (msg,opt') <- solveNlp' ipoptSolver (nlp { nlpX0' = guess }) (Just cb') - opt <- case msg of Left msg' -> error msg' - Right _ -> return opt' + (msg,_) <- solveNlp' ipoptSolver (nlp { nlpX0' = guess }) (Just cb') + case msg of Left msg' -> putStrLn $ "optimization failed, message: " ++ msg' + Right _ -> putStrLn "optimization succeeded" -- let xopt = xOpt opt -- lambda = lambdaOpt opt -- diff --git a/examples/Glider/AeroCoeffs.hs b/examples/Glider/AeroCoeffs.hs new file mode 100644 --- /dev/null +++ b/examples/Glider/AeroCoeffs.hs @@ -0,0 +1,271 @@ +{-# OPTIONS_GHC -Wall #-} +{-# Language DeriveFunctor #-} +{-# Language DeriveFoldable #-} +{-# Language DeriveGeneric #-} + +module Glider.AeroCoeffs where + +import GHC.Generics ( Generic, Generic1 ) + +import Data.Foldable ( Foldable ) +import Linear + +import Dyno.Server.Accessors ( Lookup(..) ) +import Dyno.Vectorize + +atan2' :: Floating a => a -> a -> a +atan2' y x = 2 * atan (y / (sqrt(x*x + y*y + 1e-15) + x) ) + +data ControlSurfaces a = + ControlSurfaces { csElev :: a + , csRudder :: a + , csAil :: a + , csFlaps :: a + } deriving (Eq, Functor, Foldable, Generic, Generic1, Show) +instance Vectorize ControlSurfaces +instance (Lookup a, Generic a) => Lookup (ControlSurfaces a) + +data AeroForceCoeffs a = + AeroForceCoeffs { af_cL0 :: a + , af_cL_A :: a + , af_cL_elev :: a + , af_cL_flaps :: a + + , af_cD0 :: a + , af_cD_A :: a + , af_cD_A2 :: a + , af_cD_B2 :: a + + , af_cD_elev :: a + , af_cD_elev2 :: a + , af_cD_A_elev :: a + + , af_cD_flaps :: a + , af_cD_flaps2 :: a + , af_cD_A_flaps :: a + + , af_cD_rudder :: a + , af_cD_rudder2 :: a + , af_cD_B_rudder :: a + + , af_cD_ail :: a + , af_cD_ail2 :: a + , af_cD_B_ail :: a + + , af_cY_B :: a + , af_cY_rudder :: a + } deriving (Functor, Generic, Generic1, Show) +instance Vectorize AeroForceCoeffs + +data AeroMomentCoeffs a = + AeroMomentCoeffs { am_cm0 :: a + + , am_cl_p :: a + , am_cl_q :: a + , am_cl_r :: a + + , am_cm_p :: a + , am_cm_q :: a + , am_cm_r :: a + + , am_cn_p :: a + , am_cn_q :: a + , am_cn_r :: a + + , am_cl_B :: a + , am_cl_AB :: a + , am_cm_A :: a + , am_cn_B :: a + , am_cn_AB :: a + + , am_cl_ail :: a + , am_cm_elev + , am_cm_flaps :: a + , am_cn_rudder :: a + } deriving (Functor, Generic, Generic1, Show) +instance Vectorize AeroMomentCoeffs + +data AeroRefs a = + AeroRefs { ar_sref :: a + , ar_bref :: a + , ar_cref :: a + } deriving (Functor, Generic, Generic1, Show) +instance Vectorize AeroRefs + + +-- | Compute aerodynamic forces/moments in the body frame. +-- Parameters: +-- dcm_n2b: rotation matrix rotating vectors expressed in NED to vectors expressed in body +-- v_bw_b: body velocity in the wind frame, expressed in the body frame +-- w_bn_b: body angular velocity w.r.t. NED +aeroForcesMoments :: Floating a => AeroForceCoeffs a -> AeroMomentCoeffs a -> AeroRefs a -> + V3 a -> V3 a -> ControlSurfaces a -> (V3 a, V3 a) +aeroForcesMoments forceCoeffs momentCoeffs refs v_bw_b w_bn_b controlSurfaces = (forces, moments) + where + V3 cL cD cY = aeroForceCoeffs alpha beta controlSurfaces forceCoeffs + c_lmn = aeroMomentCoeffs alpha beta airspeed w_bn_b controlSurfaces momentCoeffs refs + + -- alpha/beta + alpha = atan2' v_bw_b_z v_bw_b_x + beta = asin (v_bw_b_y / airspeed) + V3 v_bw_b_x v_bw_b_y v_bw_b_z = v_bw_b + + airspeedSquared = quadrance v_bw_b + airspeed = sqrt airspeedSquared + + moments = rho_sref_v2*^(V3 bref cref bref)*c_lmn + forces = dragForce + liftForce + sideForce + + dragForce = (-rho_sref_v*cD) *^ v_bw_b + liftForce = rho_sref_v*cL *^ e_b2L_b_v + sideForce = rho_sref*cY *^ e_b2Y_b_v2 + + -- y axis of aircraft expressed in body frame + e_b2y_b = V3 0 1 0 + + -- lift axis normalized to airspeed + e_b2L_b_v = cross e_b2y_b v_bw_b + + -- sideforces axis normalized to airspeed^2 + e_b2Y_b_v2 = cross e_b2L_b_v (-v_bw_b) + + rho_sref = 0.5*rho*sref + rho_sref_v2 = rho_sref*airspeedSquared + rho_sref_v = rho_sref*airspeed + + sref = ar_sref refs + bref = ar_bref refs + cref = ar_cref refs + + rho = 1.23 + +aeroForceCoeffs :: Num a => a -> a -> ControlSurfaces a -> AeroForceCoeffs a -> V3 a +aeroForceCoeffs alpha beta controlSurfaces coeffs = V3 cL cD cY + where + cL_wing = cL_A'*alpha + cL0' + cD_wing = cD_A'*alpha + cD_A2'*alpha*alpha + cD_B2'*beta*beta + cD0' + cY_wing = cY_B'*beta + + cL_elev = cL_elev' * elev + cD_elev = cD_elev2' * elev * elev + cD_A_elev' * elev * alpha + cD_elev' * elev + + cD_ail = cD_ail2'*ail*ail + cD_B_ail'*beta*ail + cD_ail'*ail + + cL_flaps = cL_flaps'*flaps + cD_flaps = cD_flaps2'*flaps*flaps + cD_A_flaps'*alpha*flaps + cD_flaps'*flaps + + cY_rudder = cY_rudder'*rudder + cD_rudder = cD_rudder2'*rudder*rudder + cD_B_rudder'*beta*rudder + cD_rudder'*rudder + + cL = cL_wing + cL_elev + cL_flaps + cD = cD_wing + cD_elev + cD_ail + cD_flaps + cD_rudder + cY = cY_wing + cY_rudder + + -- inputs + elev = csElev controlSurfaces + rudder = csRudder controlSurfaces + ail = csAil controlSurfaces + flaps = csFlaps controlSurfaces + + -- unpack aero coeffs + cL_A' = af_cL_A coeffs + cL0' = af_cL0 coeffs + cD_A' = af_cD_A coeffs + cD_A2' = af_cD_A2 coeffs + cD_B2' = af_cD_B2 coeffs + cD0' = af_cD0 coeffs + cY_rudder' = af_cY_rudder coeffs + cD_rudder2' = af_cD_rudder2 coeffs + cD_flaps2' = af_cD_flaps2 coeffs + cD_elev2' = af_cD_elev2 coeffs + cD_flaps' = af_cD_flaps coeffs + cD_A_flaps' = af_cD_A_flaps coeffs + cD_A_elev' = af_cD_A_elev coeffs + cD_elev' = af_cD_elev coeffs + cD_ail2' = af_cD_ail2 coeffs + cD_ail' = af_cD_ail coeffs + cD_B_ail' = af_cD_B_ail coeffs + cD_B_rudder' = af_cD_B_rudder coeffs + cD_rudder' = af_cD_rudder coeffs + cL_elev' = af_cL_elev coeffs + cL_flaps' = af_cL_flaps coeffs + cY_B' = af_cY_B coeffs + + +aeroMomentCoeffs :: Fractional a => a -> a -> a -> V3 a -> ControlSurfaces a -> AeroMomentCoeffs a -> AeroRefs a -> V3 a +aeroMomentCoeffs alpha beta airspeed w_bn_b controlSurfaces coeffs refs = + momentCoeffs0 + momentCoeffs_pqr + momentCoeffs_AB + momentCoeffs_surf + where + elev = csElev controlSurfaces + rudder = csRudder controlSurfaces + ail = csAil controlSurfaces + flaps = csFlaps controlSurfaces + + w_bn_b_hat = (V3 bref cref bref) * w_bn_b ^* (0.5/airspeed) + + momentCoeffs0 = V3 cm0 0 0 + + momentCoeffs_pqr = + (V3 + (V3 cl_p cl_q cl_r) + (V3 cm_p cm_q cm_r) + (V3 cn_p cn_q cn_r)) !* w_bn_b_hat + + momentCoeffs_AB = + (V3 + (V3 0 cl_B cl_AB) + (V3 cm_A 0 0) + (V3 0 cn_B cn_AB)) !* (V3 alpha beta (alpha*beta)) + + momentCoeffs_surf = + V3 + (cl_ail * ail) + (cm_elev * elev + cm_flaps * flaps) + (cn_rudder * rudder) + + -- unpack aero coeffs + cm0 = am_cm0 coeffs + + cl_p = am_cl_p coeffs + cl_q = am_cl_q coeffs + cl_r = am_cl_r coeffs + + cm_p = am_cm_p coeffs + cm_q = am_cm_q coeffs + cm_r = am_cm_r coeffs + + cn_p = am_cn_p coeffs + cn_q = am_cn_q coeffs + cn_r = am_cn_r coeffs + + cl_B = am_cl_B coeffs + cl_AB = am_cl_AB coeffs + cm_A = am_cm_A coeffs + cn_B = am_cn_B coeffs + cn_AB = am_cn_AB coeffs + + cl_ail = am_cl_ail coeffs + cm_elev = am_cm_elev coeffs + cm_flaps = am_cm_flaps coeffs + cn_rudder = am_cn_rudder coeffs + + bref = ar_bref refs + cref = ar_cref refs + +trans :: V3 (V3 a) -> V3 (V3 a) +trans (V3 + (V3 e11 e12 e13) + (V3 e21 e22 e23) + (V3 e31 e32 e33)) + = + V3 + (V3 e11 e21 e31) + (V3 e12 e22 e32) + (V3 e13 e23 e33) + +skew :: Num a => V3 a -> V3 (V3 a) +skew (V3 x y z) = + V3 + (V3 0 (-z) y ) + (V3 z 0 (-x)) + (V3 (-y) x 0 ) diff --git a/examples/Glider/Aircraft.hs b/examples/Glider/Aircraft.hs new file mode 100644 --- /dev/null +++ b/examples/Glider/Aircraft.hs @@ -0,0 +1,67 @@ +{-# OPTIONS_GHC -Wall -fno-warn-orphans #-} +{-# Language ScopedTypeVariables #-} +{-# Language DeriveFunctor #-} +{-# Language DeriveGeneric #-} + +module Glider.Aircraft ( AcX(..), AcU(..), aircraftDae ) where + +import GHC.Generics ( Generic, Generic1 ) + +import Linear + +import Dyno.Vectorize +import Dyno.Server.Accessors ( Lookup(..) ) + +import Glider.AeroCoeffs + +data AcX a = AcX { ac_r_n2b_n :: V3 a + , ac_v_bn_b :: V3 a + , ac_R_n2b :: M33 a + , ac_w_bn_b :: V3 a + , ac_u :: AcU a + } deriving (Eq, Functor, Generic, Generic1, Show) +data AcU a = AcU { acSurfaces :: ControlSurfaces a + } deriving (Eq, Functor, Generic, Generic1, Show) +newtype AcZ a = AcZ (None a) deriving (Eq, Functor, Generic, Generic1, Show) +newtype AcR a = AcR (AcX a) deriving (Eq, Functor, Generic, Generic1, Show) +newtype AcP a = AcP (None a) deriving (Eq, Functor, Generic, Generic1, Show) + +instance Vectorize AcX +instance Vectorize AcZ +instance Vectorize AcU +instance Vectorize AcP +instance Vectorize AcR + +instance (Lookup a, Generic a) => Lookup (AcX a) +instance (Lookup a, Generic a) => Lookup (AcZ a) +instance (Lookup a, Generic a) => Lookup (AcU a) +instance (Lookup a, Generic a) => Lookup (AcP a) +instance (Lookup a, Generic a) => Lookup (AcR a) + +subCs :: Num a => ControlSurfaces a -> ControlSurfaces a -> ControlSurfaces a +subCs (ControlSurfaces x0 x1 x2 x3) (ControlSurfaces y0 y1 y2 y3) = + ControlSurfaces (x0-y0) (x1-y1) (x2-y2) (x3-y3) + +aircraftDae :: forall a. Floating a => + (a, M33 a) -> AeroForceCoeffs a -> AeroMomentCoeffs a -> AeroRefs a -> + AcX a -> AcX a -> AcU a -> AcX a +aircraftDae + (mass, inertia) + forceCoeffs + momentCoeffs + refs + (AcX r_n2b_n' v_bn_b' dcm_n2b' w_bn_b' (AcU controlSurfaces')) + (AcX _ v_bn_b dcm_n2b w_bn_b (AcU controlSurfaces)) + (AcU controlSurfaces'') = daeResidual + where + v_bw_b = v_bn_b -- no relative wind + (aero_forces_body, moments_body) = aeroForcesMoments forceCoeffs momentCoeffs refs v_bw_b w_bn_b controlSurfaces + forces_body = aero_forces_body + dcm_n2b !* (V3 0 0 (9.81*mass)) + + daeResidual = + AcX { ac_r_n2b_n = (trans dcm_n2b) !* v_bn_b - r_n2b_n' + , ac_v_bn_b = v_bn_b' + cross w_bn_b v_bn_b - forces_body ^/ mass + , ac_R_n2b = (trans (skew w_bn_b)) !*! dcm_n2b - dcm_n2b' + , ac_w_bn_b = inertia !* w_bn_b' + cross w_bn_b (inertia !* w_bn_b) - moments_body + , ac_u = AcU $ subCs controlSurfaces'' controlSurfaces' + } diff --git a/examples/Glider/Betty.hs b/examples/Glider/Betty.hs new file mode 100644 --- /dev/null +++ b/examples/Glider/Betty.hs @@ -0,0 +1,72 @@ +{-# OPTIONS_GHC -Wall #-} + +module Glider.Betty + ( bettyFc + , bettyMc + , bettyRefs + , bettyInertia + , bettyMass + ) where + +import Linear + +import Glider.AeroCoeffs + +bettyFc :: Floating a => AeroForceCoeffs a +bettyFc = AeroForceCoeffs + { af_cL0 = 0.203530 + , af_cL_A = 5.786876 + + , af_cD_A = 0.018751 + , af_cD_A2 = 1.529989 + , af_cD_B2 = -0.16247 + , af_cD0 = 0.008767 + + , af_cY_B = -0.239789 + + -- control surface forces + , af_cL_elev = -0.0105*180/pi + , af_cL_flaps = 0.0184*180/pi + , af_cY_rudder = 0.0035*180/pi + , af_cD_flaps2 = 3.03874e-05, af_cD_A_flaps = 0.000101404, af_cD_flaps = 0.000208995 + , af_cD_elev2 = 4.19816e-05, af_cD_A_elev = -9.79647e-05, af_cD_elev = 4.52856e-05 + , af_cD_ail2 = 5.60583e-05, af_cD_B_ail = -6.73139e-06, af_cD_ail = 0 + , af_cD_rudder2 = 2.03105e-05, af_cD_B_rudder = 5.55453e-05, af_cD_rudder = 0 + } + +bettyMc :: Floating a => AeroMomentCoeffs a +bettyMc = AeroMomentCoeffs + { am_cl_p = -0.576, am_cl_q = 0.0, am_cl_r = 0.0707 + , am_cm_p = 0.0, am_cm_q = -15.5, am_cm_r = 0.0 + , am_cn_p = -0.036, am_cn_q = 0.0, am_cn_r = -0.0667 + + , am_cl_B = -0.051808 + , am_cl_AB = -0.208344 + , am_cm_A = -0.450643 + -- cm0 valid for CG/bridle location 0.1 meters behind main wing leading edge + , am_cm0 = 0.028980 + , am_cn_B = 0.037183 + , am_cn_AB = -0.028933 + + -- control surface moments + , am_cl_ail = 0.0073*180/pi + , am_cm_elev = 0.0352*180/pi + , am_cm_flaps = 0.0026*180/pi + , am_cn_rudder = 0.001176*180/pi + } + +bettyRefs :: Fractional a => AeroRefs a +bettyRefs = AeroRefs { ar_sref = 0.684 + , ar_bref = 2.904 -- sqrt(sref*AR), + , ar_cref = 0.2512 -- sqrt(sref/AR), + } + +bettyInertia :: Fractional a => M33 a +bettyInertia = + V3 + (V3 0.565 0 0) + (V3 0 0.161 0) + (V3 0 0 0.723) + +bettyMass :: Fractional a => a +bettyMass = 7.5 diff --git a/examples/Homotopy.hs b/examples/Homotopy.hs --- a/examples/Homotopy.hs +++ b/examples/Homotopy.hs @@ -1,37 +1,40 @@ --- | Minimize the Rosenbrock function (plus a trivial constraint) using --- the more complicated NLP' interface. - {-# OPTIONS_GHC -Wall #-} +{-# Language DeriveFunctor #-} {-# Language DeriveGeneric #-} module Main where -import GHC.Generics ( Generic ) +import GHC.Generics ( Generic, Generic1 ) + import Data.Vector ( Vector ) import qualified Data.Vector as V import Text.Printf ( printf ) -import Dyno.View -import Dyno.Nlp -import Dyno.NlpSolver +import Casadi.MX ( MX ) + +import Dyno.View.View ( J ) +import Dyno.View.JV ( JV, catJV, catJV', splitJV, splitJV' ) +import Dyno.Vectorize ( Vectorize, Id ) +import Dyno.Nlp ( Nlp'(..), Bounds ) +import Dyno.NlpSolver ( Opt(..), solveNlpHomotopy' ) import Dyno.Solvers -data P a = P (J S a) (J S a) deriving (Generic, Show) -data X a = X (J S a) (J S a) deriving (Generic, Show) -data G a = G (J S a)-- (J S a) - deriving (Generic, Show) +data P a = P a a deriving (Functor, Generic, Generic1, Show) +data X a = X a a deriving (Functor, Generic, Generic1, Show) +data G a = G a -- (J (JV Id) a) + deriving (Functor, Generic, Generic1, Show) -instance View X -instance View G -instance View P +instance Vectorize X +instance Vectorize G +instance Vectorize P -myNlp :: Nlp' X P G MX +myNlp :: Nlp' (JV X) (JV P) (JV G) MX myNlp = Nlp' { nlpFG' = fg , nlpBX' = bx , nlpBG' = bg , nlpX0' = x0 - , nlpP' = cat $ P (-2) 0 + , nlpP' = catJV $ P (-2) 0 , nlpLamX0' = Nothing , nlpLamG0' = Nothing , nlpScaleF' = Nothing @@ -39,45 +42,42 @@ , nlpScaleG' = Nothing } where - x0 :: J X (V.Vector Double) - x0 = cat $ X (-8) (-8) + x0 :: J (JV X) (V.Vector Double) + x0 = catJV $ X (-8) (-8) - bx :: J X (Vector Bounds) - bx = mkJ $ - V.fromList [ (Just (-3), Just 3) - , (Just (-3), Just 3) - ] - bg :: J G (Vector Bounds) - bg = mkJ $ (V.singleton (Nothing, Just 0)) + bx :: J (JV X) (Vector Bounds) + bx = catJV $ X (Just (-3), Just 3) (Just (-3), Just 3) + bg :: J (JV G) (Vector Bounds) + bg = catJV (G (Nothing, Just 0)) - fg :: J X MX -> J P MX -> (J S MX, J G MX) - fg xy pxy = (f, cat g) + fg :: J (JV X) MX -> J (JV P) MX -> (J (JV Id) MX, J (JV G) MX) + fg xy pxy = (f, catJV' g) where - X x y = split xy - P px py = split pxy + X x y = splitJV' xy + P px _ = splitJV' pxy f = (1-x)**2 + 100*(y - x**2)**2 -- g = G x -- f = (x - px)**2 + (y - py)**2 g = G (x - px) +solver :: NlpSolverStuff --solver = ipoptSolver {options = [ --("max_iter", Opt (5 :: Int)) -- ("print_level", Opt (0 :: Int)) -- , ("print_time", Opt False) -- ]} solver = snoptSolver {options = [ ("print_time", Opt False) - , ("_isumm", Opt (0 :: Int)) +-- , ("_isumm", Opt (0 :: Int)) -- , ("max_iter", Opt (5 :: Int)) -- , ("_start", Opt "Warm") ]} main :: IO () main = do - let cbp :: J X (Vector Double) -> J P (Vector Double) -> Double -> IO () + let cbp :: J (JV X) (Vector Double) -> J (JV P) (Vector Double) -> Double -> IO () cbp xy pxy alpha = do - let X x y = split xy - P px py = split pxy - --printf "X: (%.3f,%.3f), P: (%.3f, %.3f), a: %.4f\n" - -- (V.head (unJ x)) (V.head (unJ y)) (V.head (unJ px)) (V.head (unJ py)) alpha + let X x y = splitJV xy + P px py = splitJV pxy + printf "X: (%.3f,%.3f), P: (%.3f, %.3f), a: %.4f\n" x y px py alpha return () - opt <- solveNlpHomotopy' 1e-3 (0.6, 2, 10, 20) solver myNlp (cat (P (2) (0))) Nothing (Just cbp) + opt <- solveNlpHomotopy' 1e-3 (0.6, 2, 10, 20) solver myNlp (catJV (P 2 0)) Nothing (Just cbp) print opt diff --git a/examples/MultipleShooting.hs b/examples/MultipleShooting.hs --- a/examples/MultipleShooting.hs +++ b/examples/MultipleShooting.hs @@ -2,12 +2,15 @@ {-# Language ScopedTypeVariables #-} {-# Language DeriveGeneric #-} {-# Language DeriveFunctor #-} +{-# Language DataKinds #-} +{-# Language PolyKinds #-} module Main ( main ) where -import GHC.Generics ( Generic ) +import GHC.Generics ( Generic, Generic1 ) + import qualified Data.Vector as V import qualified Data.Foldable as F import Control.Applicative ( Applicative(..) ) @@ -20,14 +23,15 @@ import Data.Colour.Names import Control.Lens +import Casadi.MX ( MX ) + import Dyno.View.View import Dyno.View.JV +import Dyno.View.JVec import Dyno.Nlp import Dyno.NlpSolver import Dyno.Solvers import Dyno.Vectorize -import Dyno.View.CasadiMat ( MX ) -import Dyno.Nats import Dyno.MultipleShooting -- state/control/parameter definitions @@ -73,7 +77,7 @@ -- run the thing main :: IO () main = do - myNlp <- makeMsNlp ocp :: IO (Nlp' (MsDvs X U P D40) JNone (MsConstraints X D40) MX) + myNlp <- makeMsNlp ocp :: IO (Nlp' (MsDvs X U P 40) JNone (MsConstraints X 40) MX) (msg,opt') <- solveNlp' ipoptSolver myNlp Nothing opt <- case msg of Left err -> error err @@ -85,21 +89,37 @@ (xs', us) = unzip $ map splitXU $ F.toList $ unJVec $ split (dvXus xopt) xf = splitJV (dvXf xopt) xs = xs' ++ [xf] - renderableToWindow (chart [ ("u", (map (\(U u) -> u) us) ++ [0]) - , ("p", map (\(X p _) -> p) xs) - , ("v", map (\(X _ v) -> v) xs) - ]) 600 600 + renderable :: Renderable () + renderable = charts [ ("u", zip [0..] (map (\(U u) -> u) us)) + , ("p", zip [0..] (map (\(X p _) -> p) xs)) + , ("v", zip [0..] (map (\(X _ v) -> v) xs)) + ] + renderableToWindow renderable 600 600 -chart :: [(String, [Double])] -> Renderable () -chart vals = toRenderable layout + +charts :: [(String,[(Double,Double)])] -> Renderable () +charts vals = toRenderable slayouts where - points :: (String, [Double]) -> PlotPoints Double Double - points (name, ys) = plot_points_style .~ filledCircles 2 (opaque red) - $ plot_points_values .~ (zip [0..] ys) - $ plot_points_title .~ name - $ def + plots :: (String, [(Double, Double)]) -> StackedLayout Double + plots (name, xys) = StackedLayout layout + where + lines' :: PlotLines Double Double + lines' = plot_lines_values .~ [xys] + $ plot_lines_title .~ name + $ def - layout :: Layout Double Double - layout = layout_title .~ "a plot" - $ layout_plots .~ (map (toPlot . points) vals) - $ def + points :: PlotPoints Double Double + points = plot_points_style .~ filledCircles 2 (opaque red) + $ plot_points_values .~ [(x,y) | (x,y) <- xys] + $ plot_points_title .~ name + $ def + + layout :: Layout Double Double + layout = layout_title .~ name + $ layout_plots .~ [toPlot lines', toPlot points] + $ def + + slayouts :: StackedLayouts Double + slayouts = slayouts_compress_legend .~ False + $ slayouts_layouts .~ (map plots vals) + $ def diff --git a/examples/NlpDsl.hs b/examples/NlpDsl.hs new file mode 100644 --- /dev/null +++ b/examples/NlpDsl.hs @@ -0,0 +1,36 @@ +{-# OPTIONS_GHC -Wall #-} + +module Main where + +import Dyno.Solvers + +import ExampleDsl.NlpMonad + +rosen :: NlpMonad () +rosen = do + x1 <- designVar "x1" + x2 <- designVar "x2" + x3 <- designVar "x3" + x4 <- designVar "x4" + + 0 + x1**2 + x2**2 + x3 === 2 + 0 + x2**4 + x4 === 4 + 0 + 2*x1 + 4*x2 >== 0.0 + x3 >== 0 + x4 >== 0 + + minimize $ (x1 + x2 + x3)**2 + 3*x3 + 5*x4 + + +main :: IO () +main = do + let guess = [ ("x1", 0.1) + , ("x2", 0.125) + , ("x3", 0.666666) + , ("x4", 0.142857) + ] + + (status, fopt, xopt) <- solveStaticNlp ipoptSolver rosen guess Nothing + print status + putStrLn $ "value: " ++ show fopt + mapM_ (\(n,v) -> putStrLn $ n ++ ": " ++ show v) xopt diff --git a/examples/OcpDslRocket.hs b/examples/OcpDslRocket.hs new file mode 100644 --- /dev/null +++ b/examples/OcpDslRocket.hs @@ -0,0 +1,91 @@ +{-# OPTIONS_GHC -Wall #-} + +module Main ( main ) where + +import Control.Monad ( void ) +--import Control.Concurrent ( threadDelay ) + +import Dyno.Solvers + +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 ipoptSolver myDae mayer boundaryConditions myOcp tbnds n deg (Just cb') + where + cb' meta x = cb (x, meta) + --cb' meta x = threadDelay 200000 >> cb (x, meta) diff --git a/examples/OcpDslSpring.hs b/examples/OcpDslSpring.hs new file mode 100644 --- /dev/null +++ b/examples/OcpDslSpring.hs @@ -0,0 +1,71 @@ +{-# OPTIONS_GHC -Wall #-} + +module Main ( main ) where + +import Control.Monad ( void ) + +import Dyno.Solvers + +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 ipoptSolver myDae mayer boundaryConditions myOcp tbnds n deg (Just cb') + where + cb' meta x = cb (x, meta) diff --git a/examples/OcpM.hs b/examples/OcpM.hs deleted file mode 100644 --- a/examples/OcpM.hs +++ /dev/null @@ -1,72 +0,0 @@ -{-# OPTIONS_GHC -Wall #-} - -module Main ( main ) where - -import Control.Monad ( void ) - -import Dyno.OcpMonad -import Dyno.Solvers -import ServerSender -import GliderShared - -myDae :: SXElement -> DaeMonad () -myDae time = do - (p,p') <- diffState "p" - (v,v') <- diffState "v" - u <- control "u" - - let k = 4 - b = 0.3 - - force = u - k * p - b * v - obj = p**2 + v**2 + u**2 - output "force" force - output "obj" obj - - p' === v - v' === force + 0.1 * sin time - -boundaryConditions :: (String -> BCMonad SXElement) -> (String -> BCMonad SXElement) -> BCMonad () -boundaryConditions get0 getF = do - p0 <- get0 "p" - v0 <- get0 "v" - - pF <- getF "p" - vF <- getF "v" - - p0 === 0 - v0 === 0 - --- p0 + 4 <== pF -- inequalities missing for now --- v0 === vF - pF === 1 - vF === 0 - -mayer :: (Floating a, Monad m) => a -> (String -> m a) -> (String -> m a) -> m a -mayer endTime get0 getF = do - p <- getF "p" - v <- getF "v" - - return (p**2 + v**2 + endTime/1000) - -myOcp :: SXElement -> (String -> OcpMonad SXElement) -> OcpMonad () -myOcp time get = do - p <- get "p" - v <- get "v" - u <- get "u" - force <- get "force" - obj <- get "obj" - - v**2 + u**2 <== 4 + time/100 - - lagrangeTerm obj - -main :: IO () -main = void $ withCallback gliderUrl gliderChannelName go - where - n = 100 - deg = 3 - tbnds = (Just 4, Just 4) - go cb = solveStaticOcp ipoptSolver myDae mayer boundaryConditions myOcp tbnds n deg (Just cb') - where - cb' meta x = cb (x, meta) diff --git a/examples/PlotSofa.hs b/examples/PlotSofa.hs deleted file mode 100644 --- a/examples/PlotSofa.hs +++ /dev/null @@ -1,171 +0,0 @@ -{-# OPTIONS_GHC -Wall #-} -{-# Language CPP #-} - -module Main ( main ) where - -import qualified Data.Foldable as F -import Linear.V3 ( V3(..) ) -import Linear.Quaternion ( Quaternion(..) ) -import Control.Monad ( when, forever ) -import Data.ByteString.Char8 ( pack ) -import Data.Serialize -import qualified System.ZMQ4 as ZMQ -import qualified Control.Concurrent.STM as STM -import qualified Control.Concurrent as CC -import Text.Printf - -import Vis - -import SofaShared - ---type M22 = ((Double,Double),(Double,Double)) - - ---sub :: ((DynCollTraj (Vector Double), CollTrajMeta, [M22], M22) -> IO ()) -> IO () ---sub writeChan = ZMQ.withContext $ \context -> --- ZMQ.withSocket context ZMQ.Sub $ \subscriber -> do --- ZMQ.connect subscriber url --- ZMQ.subscribe subscriber (pack channelName) --- forever $ do --- _ <- ZMQ.receive subscriber --- mre <- ZMQ.moreToReceive subscriber --- when mre $ do - -sub :: (SofaMessage -> IO ()) -> IO () -sub writeChan = ZMQ.withContext $ \context -> - ZMQ.withSocket context ZMQ.Sub $ \subscriber -> do - ZMQ.connect subscriber url - ZMQ.subscribe subscriber (pack sofaChannel) - forever $ do - _ <- ZMQ.receive subscriber - mre <- ZMQ.moreToReceive subscriber - when mre $ do - msg <- ZMQ.receive subscriber - let decoded :: SofaMessage - decoded = case decode msg of - Left err -> error err - Right t -> t - writeChan decoded - -main :: IO () -main = do - -- keep reading from tcp and storing results in the queue - trajChan <- STM.atomically STM.newTQueue - _ <- CC.forkIO (sub (STM.atomically . (STM.writeTQueue trajChan))) - - -- keep parsing results from the queue into nice form - trajMVar <- CC.newMVar (VisObjects [], [], []) - - let getLastValue = do - val <- STM.atomically (STM.readTQueue trajChan) - empty' <- STM.atomically (STM.isEmptyTQueue trajChan) - if empty' then return val else getLastValue - - parserThread = do - sofaX <- getLastValue - CC.modifyMVar_ trajMVar $ \(_,_,xs) -> do - let (mainvis, stages) = toVisObjects sofaX - return (mainvis, stages, xs) - parserThread - - _ <- CC.forkIO parserThread - - animateIO (defaultOpts { optWindowName = "sofa lol" }) (animateFun trajMVar) - -multiplyList :: Int -> Int -> [a] -> [a] -multiplyList _ _ [] = [] -multiplyList k 0 (_:xs) = multiplyList k k xs -multiplyList k j xs@(x0:_) = x0 : multiplyList k (j-1) xs - -animateFun :: CC.MVar (VisObject Double, [VisObject Double], [VisObject Double]) - -> Float -> IO (VisObject Double) -animateFun mv = const $ do - (mainvis, stages, plotstages) <- CC.takeMVar mv - case plotstages of - (s0:ss) -> do - CC.putMVar mv (mainvis, stages, ss) - return $ VisObjects [mainvis, s0] - []-> do - let n = max 1 $ 400 `div` length stages - CC.putMVar mv (mainvis, stages, multiplyList n n stages) - return mainvis - -linspace :: Fractional a => a -> a -> Int -> [a] -linspace x0 xf n = xs - where - h = (xf-x0)/(fromIntegral n - 1) - xs = map (\k -> x0 + h*(fromIntegral k)) (take n [(0::Int)..]) - -qy :: Quaternion Double -qy = Quaternion 0 $ V3 1 0 0 - -toVisObjects :: SofaMessage -> (VisObject Double, [VisObject Double]) -toVisObjects (SofaMessage iters r points stages) = - ( RotQuat qy $ VisObjects [walls, txt, shape0 - , VisObjects (allPoints (-1)) - , Trans (V3 (-2) (-2) 0) axes - ] - , map (RotQuat qy . Trans (V3 1 1 0)) (allPoints 0) - ) - where - walls = VisObjects - [ Line [ V3 (-4) 1 0 - , V3 1 1 0 - , V3 1 (-4) 0 - ] (makeColor 1 1 1 1) - , Line [ V3 (-4) 2 0 - , V3 2 2 0 - , V3 2 (-4) 0 - ] (makeColor 1 1 1 1) - , Line [ V3 (-4) 0 0 - , V3 0 0 0 - , V3 0 (-4) 0 - ] (makeColor 1 1 1 1) - ] - - axes = Axes (0.5, 15) - npoints = length points - nsteps = length stages - shape0 = Line' $ - zipWith (\(Point x y) c -> ((V3 x y 0) - (V3 2 2 0), c)) - (points ++ [head points]) - (colors (npoints + 1)) - drawOne :: [Point Double] -> Double -> Color -> VisObject Double - drawOne ps@(p0:_) z = - Line - (map (\(Point x y) -> (V3 x y z)) (ps ++ [p0])) - drawOne _ _ = const (VisObjects []) - - area = 0.5 * (F.sum $ zipWithNext' cross points) - - allPoints :: Double -> [VisObject Double] - allPoints maxheight = zipWith3 (\c so z -> drawOne (stagePoints so) z c) - (colors (nsteps + 1)) - stages - (linspace 0 maxheight (nsteps + 1)) - - colors :: Int -> [Color] - colors k = fmap (\gamma -> makeColor 0 gamma (1 - gamma) 1) (gammas k) - - gammas :: Int -> [Float] - gammas k = linspace 0 1 k - - stagePoints :: (Point Double, Double) -> [Point Double] - stagePoints (mean, theta) = fmap rot points - where - rot :: Point Double -> Point Double - rot (Point x y) = mean + Point (x*cos(theta) + y*sin(theta)) (-x*sin(theta) + y*cos(theta)) - - - messages = [ show npoints ++ " segments" - , show nsteps ++ " stages" - , printf "segment length: %.4f" r - , printf "area: %.4f" area - , "iteration: " ++ show iters - ] - txt = VisObjects $ - zipWith (\s k -> Text2d s (30,fromIntegral $ 30*k) TimesRoman24 (makeColor 1 1 1 1)) - messages (reverse [1..length messages]) --- trajLine = Line (zipWith (\x y -> V3 x y 0) (concat xs0) (concat ys0)) (makeColor 1 0 0 0.4) --- trajDots = Points (zipWith (\x y -> V3 x y 0) xsCollPts ysCollPts) (Just 1) red --- trajDots' = Points (zipWith (\x y -> V3 x y 0) xsBigPts ysBigPts) (Just 2) red diff --git a/examples/Plotter.hs b/examples/Plotter.hs deleted file mode 100644 --- a/examples/Plotter.hs +++ /dev/null @@ -1,56 +0,0 @@ -{-# OPTIONS_GHC -Wall #-} -{-# Language DeriveDataTypeable #-} - -module Main ( main ) where - -import qualified Control.Concurrent as CC -import Control.Monad ( when, forever ) -import Data.ByteString.Char8 ( pack ) -import Data.Serialize -import Data.Vector ( Vector ) -import qualified System.ZMQ4 as ZMQ -import System.Console.CmdArgs ( (&=), Data, Typeable ) -import qualified System.Console.CmdArgs as CA - -import Dyno.Server.Server ( runPlotter, newChannel ) -import Dyno.DirectCollocation.Dynamic - -import GliderShared ( gliderUrl, gliderChannelName ) - -sub :: String -> (([DynCollTraj (Vector Double)], CollTrajMeta) -> IO ()) -> String -> IO () -sub ip' writeChan name = ZMQ.withContext $ \context -> - ZMQ.withSocket context ZMQ.Sub $ \subscriber -> do - ZMQ.connect subscriber ip' - ZMQ.subscribe subscriber (pack name) - forever $ do - _ <- ZMQ.receive subscriber - mre <- ZMQ.moreToReceive subscriber - when mre $ do - msg <- ZMQ.receive subscriber - let decoded :: ([DynCollTraj (Vector Double)], CollTrajMeta) - decoded = case decode msg of - Left err -> error err - Right t -> t - writeChan decoded - -main :: IO () -main = do - args <- CA.cmdArgs (myargs &= CA.program "dynoplot") - let ip' = ip args - channel' = channel args - putStrLn $ "using ip \""++ip'++"\"" - putStrLn $ "using channel \""++channel'++"\"" - - (c0, writeMe) <- newChannel channel' - - listenerTid0 <- CC.forkIO (sub ip' writeMe channel') - runPlotter c0 [listenerTid0] - -data VisArgs = VisArgs { ip :: String - , channel :: String - } deriving (Show, Data, Typeable) - -myargs :: VisArgs -myargs = VisArgs { ip = gliderUrl &= CA.help "an IP address" &= CA.typ "ADDRESS" - , channel = gliderChannelName &= CA.help "zmq channel name" - } &= CA.summary "plotter for dynobud OCPs" diff --git a/examples/Rocket.hs b/examples/Rocket.hs deleted file mode 100644 --- a/examples/Rocket.hs +++ /dev/null @@ -1,91 +0,0 @@ -{-# OPTIONS_GHC -Wall #-} - -module Main ( main ) where - -import Control.Monad ( void ) ---import Control.Concurrent ( threadDelay ) - -import Dyno.OcpMonad -import Dyno.Solvers - -import ServerSender -import GliderShared - -myDae :: SXElement -> DaeMonad () -myDae time = do - (p,p') <- diffState "p" - (v,v') <- diffState "v" - (m,m') <- diffState "m" - (u,u') <- diffState "u" - u'' <- control "u'" - - let g = 9.8 - force = u - m*g - - output "force" force - - p' === v - v' === force/m - m' === -1e-2*u**2 - u'' === u' - -boundaryConditions :: (String -> BCMonad SXElement) -> (String -> BCMonad SXElement) -> BCMonad () -boundaryConditions get0 getF = do - -- initial - p0 <- get0 "p" - v0 <- get0 "v" - m0 <- get0 "m" - - p0 === 1 - v0 === 0 - m0 === 10 - - -- final - pF <- getF "p" - vF <- getF "v" - - pF === 0 - vF === 0 - -mayer :: (Floating a, Monad m) => a -> (String -> m a) -> (String -> m a) -> m a -mayer endTime get0 getF = do - m <- getF "m" - - return (-m) -- endTime -- (p**2 + v**2) - -myOcp :: SXElement -> (String -> OcpMonad SXElement) -> OcpMonad () -myOcp time get = do - p <- get "p" - v <- get "v" - m <- get "m" - u <- get "u" - u' <- get "u'" - - -200 <== u - u <== 200 - - -100 <== u' - u' <== 100 - - 0.01 <== m - - 0 <== p - - -10 <== v - v <== 0.0 - - lagrangeTerm (1e-4*u'*u') - --lagrangeTerm (1e-8*u*u + 1e-9*p*p + 1e-9*v*v + 1e-9*m*m) - --lagrangeTerm (1e-6*u*u + 1e-6*p*p + 1e-6*v*v + 1e-6*m*m) - -main :: IO () -main = void $ withCallback gliderUrl gliderChannelName go - where - n = 100 - deg = 3 - tbnds = (Just 0.2, Just 6) - --tbnds = (Just 1.5, Just 1.5) - go cb = solveStaticOcp ipoptSolver myDae mayer boundaryConditions myOcp tbnds n deg (Just cb') - where - cb' meta x = cb (x, meta) - --cb' meta x = threadDelay 200000 >> cb (x, meta) diff --git a/examples/Sailboat.hs b/examples/Sailboat.hs --- a/examples/Sailboat.hs +++ b/examples/Sailboat.hs @@ -7,14 +7,28 @@ {-# Language FlexibleInstances #-} {-# Language DeriveFunctor #-} {-# Language DeriveGeneric #-} +{-# Language DataKinds #-} -module Main ( main ) where +module Main ( main + , SbX(..) -- to suppress warnings about unused record names + , SbU(..) -- to suppress warnings about unused record names + ) where +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 as BS +import qualified Data.ByteString.Char8 as BS8 +import qualified Data.Serialize as Ser +import Text.Printf ( printf ) import Dyno.Vectorize -import Dyno.View -import Dyno.Nats +import Dyno.View.View ( View(..), J ) +import Dyno.View.JV ( splitJV ) import Dyno.Solvers import Dyno.NlpSolver import Dyno.Server.Accessors @@ -25,13 +39,6 @@ import Dyno.DirectCollocation.Formulate ( makeGuess ) import Dyno.DirectCollocation.Dynamic -import qualified System.ZMQ4 as ZMQ -import Linear -- ( V2(..) ) -import qualified Data.List.NonEmpty as NE -import qualified Data.ByteString as BS -import qualified Data.ByteString.Char8 as BS8 -import qualified Data.Serialize as Ser - data SbX a = SbX { xGamma :: a , xP :: V2 a , xV :: V2 a @@ -82,8 +89,8 @@ sbDae :: forall a . Floating a => SbX a -> SbX a -> SbZ a -> SbU a -> SbP a -> a -> (SbR a, SbO a) sbDae - (SbX gamma' p'@(V2 px' pz') v'@(V2 vx' vz')) - (SbX gamma p@(V2 px pz) v@(V2 vx vz)) + (SbX gamma' p' v') + (SbX gamma _ v@(V2 vx vz)) _ (SbU omega alpha) _ @@ -151,8 +158,8 @@ deriving (Functor, Generic, Generic1, Show) bc :: Num a => SbX a -> SbX a -> SbBc a bc - (SbX gamma0 p0@(V2 px0 pz0) (V2 vx0 vz0)) - (SbX gammaF (V2 pxF pzF) (V2 vxF vzF)) + (SbX gamma0 p0@(V2 _ pz0) (V2 vx0 vz0)) + (SbX gammaF (V2 _ pzF) (V2 vxF vzF)) = SbBc { bcPeriodicGamma = gamma0 + gammaF , bcPeriodicPz = pz0 - pzF @@ -242,8 +249,8 @@ where w = pi/tf -type NCollStages = D200 -type CollDeg = D2 +type NCollStages = 200 +type CollDeg = 2 solver :: NlpSolverStuff solver = ipoptSolver @@ -253,7 +260,6 @@ main = do cp <- makeCollProblem ocp let nlp = cpNlp cp - toDyn = cpCallback cp ZMQ.withContext $ \context -> withPublisher context urlDynoPlot $ \sendDynoPlotMsg -> do -- withPublisher context urlOptTelem $ \sendOptTelemMsg -> do @@ -265,10 +271,11 @@ callback :: J (CollTraj SbX SbZ SbU SbP NCollStages CollDeg) (Vector Double) -> IO Bool callback traj = do - (dyn,_) <- toDyn traj + plotPoints <- cpPlotPoints cp traj -- dynoplot - let dynoPlotMsg = encodeSerial ([dyn], meta) + let dynoPlotMsg = encodeSerial (plotPoints, meta) sendDynoPlotMsg "glider" dynoPlotMsg + -- -- 3d vis -- let CollTraj tf' _ _ stages' xf = split traj -- stages :: [(CollStage (JV SbX) (JV None) (JV SbU) CollDeg) (Vector Double)] @@ -308,4 +315,7 @@ (msg0,opt0') <- solveNlp' solver (nlp { nlpX0' = guess }) (Just callback) opt0 <- case msg0 of Left msg' -> error msg' Right _ -> return opt0' - return () + let CollTraj endTime' _ _ xf = split (xOpt' opt0) + endTime = unId $ splitJV endTime' + V2 pxF _ = xP $ splitJV xf + printf "optimal velocity: %.2f m/s\n" (pxF / endTime) diff --git a/examples/Sofa.hs b/examples/Sofa.hs deleted file mode 100644 --- a/examples/Sofa.hs +++ /dev/null @@ -1,274 +0,0 @@ --- | How big of a sofa can we get around a corner? - -{-# OPTIONS_GHC -Wall #-} -{-# Language DeriveFunctor #-} -{-# Language DeriveGeneric #-} -{-# Language ScopedTypeVariables #-} - -module Main where - -import Data.IORef ( newIORef, readIORef, writeIORef ) -import qualified Data.Foldable as F -import Data.Serialize -import qualified System.ZMQ4 as ZMQ -import Data.ByteString.Char8 ( pack ) - -import Dyno.Vectorize -import Dyno.Nlp -import Dyno.NlpSolver -import Dyno.TypeVecs ( Vec ) -import qualified Dyno.TypeVecs as TV -import Dyno.Solvers -import Dyno.Nats - -import SofaShared - -type NPoints = D81 -type NSteps = D61 - -data X a = - X - { xR :: a - , xPoints :: Vec NPoints (Point a) - , xStages :: Vec NSteps (Stage a) - } deriving (Functor, Generic1, Show) - -data G a = - G - { gMin90 :: Vec NPoints a - , gEqualR :: Vec NPoints a - , g360s :: Vec NPoints a - , gMean0 :: Point a - , gStages :: Vec NSteps (StageCon a) - , gCloseMean :: Vec NSteps (Point a) - , gCloseTheta :: Vec NSteps a - } deriving (Functor, Generic1, Show) - -data Stage a = - Stage - { sTheta :: a - , sMean :: Point a - , sPhis :: Vec NPoints a - } deriving (Functor, Generic1, Show) - -data StageCon a = - StageCon - { scOuters :: Vec NPoints (Point a) - , scInners :: Vec NPoints a - } deriving (Functor, Generic1, Show) - -instance Vectorize X -instance Vectorize G -instance Vectorize Stage -instance Vectorize StageCon - -npoints :: Int -npoints = vlength (Proxy :: Proxy (Vec NPoints)) - -nsteps :: Int -nsteps = vlength (Proxy :: Proxy (Vec NSteps)) - -linspace :: Fractional a => a -> a -> Int -> [a] -linspace x0 xf n = - fmap - (\x -> x0 + (xf - x0)*(fromIntegral x / fromIntegral (n-1))) - $ take n [(0::Int)..] - -radius0 :: Fractional a => a -radius0 = 0.3 - -segment0 :: Floating a => a -segment0 = 2 * radius0 * sin(pi/fromIntegral npoints) - -points0 :: Vec NPoints (Point Double) -points0 = TV.mkVec' $ map (\q -> Point (radius0*cos(q)) (radius0*sin(q))) $ take npoints $ linspace 0 (2*pi) (npoints + 1) - -atan2' :: RealFloat a => Point a -> a -atan2' (Point x y) = atan2 y x - ---data G a = --- G --- { gMin90 :: Vec NPoints a --- , gEqualR :: Vec NPoints a --- , gMean0 :: Point a --- , gStages :: Vec NSteps (StageCon a) --- , gCloseMean :: Vec NSteps (Point a) --- , gCloseTheta :: Vec NSteps a --- } deriving (Functor, Generic1, Show) - ---(f0,g0) = fg guess undefined - -----worst :: Vectorize f => f Double -> Double -----worst = V.toList (fmap abs) --- ---blah :: IO () ---blah = do ----- putStrLn $ "gmin90: " ++ show (minimum $ F.toList $ gMin90 g0) ----- putStrLn $ "gmin90: " ++ show (maximum $ F.toList $ gMin90 g0) --- print $ gMean0 g0 --- print $ g360 g0 - - -guess :: X Double -guess = - X - { xR = segment0 - , xPoints = points0 - , xStages = TV.tvzipWith (\mean theta -> - Stage { sTheta = theta - , sMean = mean - , sPhis = fill $ min 0 (max (pi/2) (atan2' mean)) - }) means0 thetas0 - } - where - thetas0 :: Vec NSteps Double - thetas0 = TV.mkVec' $ linspace 0 0 nsteps - - means0 :: Vec NSteps (Point Double) - means0 = TV.mkVec' $ map f (linspace (-pi/4) (3*pi/4) nsteps) --- means0 = TV.mkVec' $ map f (linspace 0 (pi/2) npoints) - where - f :: Double -> Point Double - f q - | q <= pi/4 = fmap (/ (2*px)) p0 - | otherwise = fmap (/ (2*py)) p0 - where - p0 = Point px py - px = cos q - py = sin q - - -myNlp :: Nlp X None G SXElement -myNlp = Nlp { nlpFG = fg - , nlpBX = bx - , nlpBG = bg - , nlpX0 = guess - , nlpP = None - , nlpLamX0 = Nothing - , nlpLamG0 = Nothing - , nlpScaleF = Nothing - , nlpScaleX = Nothing - , nlpScaleG = Nothing - } - where - - bx :: X Bounds - bx = X - { xR = (Just (segment0/2), Nothing) - , xPoints = fill $ Point (Just (-5), Just 5) (Just (-5), Just 5) - , xStages = TV.mkVec' $ stage0 : replicate (nsteps-1) otherStages - } - where - stage0 = - Stage - { sTheta = (Just 0, Just 0) - , sMean = Point (Just (-3), Just 3) (Just (-3), Just 3) - , sPhis = fill (Just 0, Just (pi/2)) - } - otherStages = - Stage - { sTheta = (Just (-4*pi), Just (4*pi)) - , sMean = Point (Just (-3), Just 3) (Just (-3), Just 3) - , sPhis = fill (Just 0, Just (pi/2)) - } - - - bg :: G Bounds - bg = G - { gMin90 = fill (Just 0.8, Nothing) - , gEqualR = fill (Just 0, Just 0) - , gMean0 = fill (Just 0, Just 0) - , g360s = TV.mkVec' $ map (\q -> (Just (q - pi), Just (q + pi))) - $ linspace 0 (2*pi) npoints - , gStages = TV.mkVec' $ stage0 : replicate (nsteps-2) midStages ++ [stageF] - , gCloseMean = TV.mkVec' $ replicate (nsteps - 1) (fill (Just (-deltaMean), Just deltaMean)) ++ [fill (Nothing, Nothing)] - , gCloseTheta = TV.mkVec' $ replicate (nsteps - 1) (Just (-deltaTheta), Just deltaTheta) ++ [(Nothing, Nothing)] - } - where - deltaTheta = pi / fromIntegral nsteps - deltaMean = 4 / fromIntegral nsteps - stage0 = StageCon - { scOuters = fill $ Point (Nothing, Just 1) (Nothing, Just 0) - , scInners = fill (Just 0, Nothing) - } - stageF = StageCon - { scOuters = fill $ Point (Nothing, Just 0) (Nothing, Just 1) - , scInners = fill (Just 0, Nothing) - } - midStages = StageCon - { scOuters = fill $ Point (Nothing, Just 1) (Nothing, Just 1) - , scInners = fill (Just 0, Nothing) - } - -dot :: Num a => Point a -> Point a -> a -dot (Point x0 y0) (Point x1 y1) = x0*x1 + y0*y1 - -fg :: forall a . Floating a => X a -> None a -> (a, G a) -fg (X r points stages) _ = (f, g) - where - ds :: Vec NPoints (Point a) - ds = zipWithNext (\x0 x1 -> x1 - x0) points - - curvatureRegularization = (F.sum (zipWithNext (\x0 x1 -> dot x0 x1) ds)) / (fromIntegral npoints) - - f = 1*curvatureRegularization - 0.5 * (F.sum $ zipWithNext cross points) - g = G - { gMin90 = zipWithNext (\x0 x1 -> dot x0 x1 / ((norm2 x0) * (norm2 x1))) ds - , gEqualR = fmap (\(Point x y) -> x*x + y*y - r*r) ds - , gMean0 = F.sum points / (fromIntegral npoints) - , g360s = TV.mkVec' $ - drop 1 $ scanl (+) 0 $ - F.toList $ - zipWithNext - (\d0 d1 -> asin ((d0 `cross` d1) / ((1e-9 + norm2 d0) * (1e-9 + norm2 d1)))) - ds - , gStages = fmap stageCon stages - , gCloseMean = zipWithNext (\(Stage _ mean1 _) (Stage _ mean0 _) -> mean1 - mean0) stages - , gCloseTheta = zipWithNext (\(Stage theta1 _ _) (Stage theta0 _ _) -> theta1 - theta0) stages - } - - stageCon :: Stage a -> StageCon a - stageCon (Stage theta mean phis) = StageCon { scOuters = points' - , scInners = TV.tvzipWith inner points' phis - } - where - rot :: Point a -> Point a - rot (Point x y) = mean + Point (x*cos(theta) + y*sin(theta)) (-x*sin(theta) + y*cos(theta)) - - points' :: Vec NPoints (Point a) - points' = fmap rot points - - inner (Point xij' yij') phiij = xij'*cos(phiij) + yij'*sin(phiij) - -solver :: NlpSolverStuff -solver = ipoptSolver { options = [("ma86_order", Opt "metis"), ("max_iter", Opt (1000 :: Int))]} ---solver = snoptSolver { options = [ ("detect_linear", Opt False) ] } - -send :: Serialize a => ZMQ.Socket ZMQ.Pub -> String -> a -> IO () -send publisher chanName stuff = do - let bs = encode stuff - ZMQ.send publisher [ZMQ.SendMore] (pack chanName) - ZMQ.send publisher [] bs - -main :: IO () -main = - ZMQ.withContext $ \context -> - ZMQ.withSocket context ZMQ.Pub $ \publisher -> do - ZMQ.bind publisher url - putStrLn $ "# design vars: " ++ show (vlength (Proxy :: Proxy X)) - putStrLn $ "# constraints: " ++ show (vlength (Proxy :: Proxy G)) - iters <- newIORef 0 - _ <- solveNlp solver myNlp $ Just $ \x -> do - k <- readIORef iters - writeIORef iters (k + 1) - let msg = SofaMessage - { smSegmentLength = xR x - , smIters = k - , smPoints = F.toList (xPoints x) - , smMeanThetas = map (\stg -> (sMean stg, sTheta stg)) $ F.toList (xStages x) - } - --mapM_ (\stg -> print (sMean stg, sTheta stg)) $ F.toList (xStages x) - send publisher sofaChannel msg - return True - return () - diff --git a/examples/Sofa/Common.hs b/examples/Sofa/Common.hs new file mode 100644 --- /dev/null +++ b/examples/Sofa/Common.hs @@ -0,0 +1,87 @@ +{-# OPTIONS_GHC -Wall #-} +{-# Language DeriveFunctor #-} +{-# Language DeriveGeneric #-} +{-# Language PolyKinds #-} + +module Sofa.Common + ( SofaMessage(..) + , Point(..) + , url + , sofaChannel + , zipWithNext + , zipWithNext' + , cross + , norm2 + ) where + +import GHC.Generics ( Generic, Generic1 ) + +import qualified Data.Foldable as F +import Data.Serialize + +import Dyno.TypeVecs ( Vec, Dim ) +import qualified Dyno.TypeVecs as TV +import Dyno.Vectorize + + +data Point a = Point a a deriving (Functor, Generic, Generic1, Show) + +instance Num a => Num (Point a) where + Point x0 y0 + Point x1 y1 = Point (x0 + x1) (y0 + y1) + Point x0 y0 - Point x1 y1 = Point (x0 - x1) (y0 - y1) + Point x0 y0 * Point x1 y1 = Point (x0 * x1) (y0 * y1) + abs = fmap abs + signum = fmap signum + fromInteger k' = Point k k + where + k = fromInteger k' + +instance Fractional a => Fractional (Point a) where + Point x0 y0 / Point x1 y1 = Point (x0 / x1) (y0 / y1) + fromRational k' = Point k k + where + k = fromRational k' + +instance Vectorize Point + +data SofaMessage = + SofaMessage + { smIters :: Int + , smSegmentLength :: Double + , smPoints :: [Point Double] + , smMeanThetas :: [(Point Double, Double)] + } deriving Generic + +instance Serialize SofaMessage +instance Serialize a => Serialize (Point a) + +url :: String +url = "tcp://127.0.0.1:5563" + +sofaChannel :: String +sofaChannel = "sofa_telemetry" + + +cross :: Num a => Point a -> Point a -> a +cross (Point x0 y0) (Point x1 y1) = x0*y1 - x1*y0 + +norm2s :: Num a => Point a -> a +norm2s (Point x y) = x*x + y*y + +norm2 :: Floating a => Point a -> a +norm2 = sqrt . norm2s + +zipWithNext :: Dim n => (a -> a -> b) -> Vec n a -> Vec n b +zipWithNext f v = TV.mkVec' $ diff' (v' ++ [v0]) + where + diff' (x0:theRest@(x1:_)) = f x0 x1 : diff' theRest + diff' _ = [] + + v'@(v0:_) = F.toList v + +zipWithNext' :: (a -> a -> b) -> [a] -> [b] +zipWithNext' f v'@(v0:_) = diff' (v' ++ [v0]) + where + diff' (x0:theRest@(x1:_)) = f x0 x1 : diff' theRest + diff' _ = [] +zipWithNext' _ [] = [] diff --git a/examples/SofaExpando.hs b/examples/SofaExpando.hs new file mode 100644 --- /dev/null +++ b/examples/SofaExpando.hs @@ -0,0 +1,277 @@ +-- | How big of a sofa can we get around a corner? + +{-# OPTIONS_GHC -Wall #-} +{-# Language DeriveFunctor #-} +{-# Language DeriveGeneric #-} +{-# Language ScopedTypeVariables #-} +{-# Language DataKinds #-} + +module Main where + +import GHC.Generics ( Generic1 ) + +import Data.Proxy ( Proxy(..) ) +import Data.IORef ( newIORef, readIORef, writeIORef ) +import qualified Data.Foldable as F +import Data.Serialize +import qualified System.ZMQ4 as ZMQ +import Data.ByteString.Char8 ( pack ) + +import Dyno.Vectorize +import Dyno.Nlp +import Dyno.NlpSolver +import Dyno.TypeVecs ( Vec ) +import qualified Dyno.TypeVecs as TV +import Dyno.Solvers + +import Sofa.Common + +type NPoints = 81 +type NSteps = 61 + +data X a = + X + { xR :: a + , xPoints :: Vec NPoints (Point a) + , xStages :: Vec NSteps (Stage a) + } deriving (Functor, Generic1, Show) + +data G a = + G + { gMin90 :: Vec NPoints a + , gEqualR :: Vec NPoints a + , g360s :: Vec NPoints a + , gMean0 :: Point a + , gStages :: Vec NSteps (StageCon a) + , gCloseMean :: Vec NSteps (Point a) + , gCloseTheta :: Vec NSteps a + } deriving (Functor, Generic1, Show) + +data Stage a = + Stage + { sTheta :: a + , sMean :: Point a + , sPhis :: Vec NPoints a + } deriving (Functor, Generic1, Show) + +data StageCon a = + StageCon + { scOuters :: Vec NPoints (Point a) + , scInners :: Vec NPoints a + } deriving (Functor, Generic1, Show) + +instance Vectorize X +instance Vectorize G +instance Vectorize Stage +instance Vectorize StageCon + +npoints :: Int +npoints = vlength (Proxy :: Proxy (Vec NPoints)) + +nsteps :: Int +nsteps = vlength (Proxy :: Proxy (Vec NSteps)) + +linspace :: Fractional a => a -> a -> Int -> [a] +linspace x0 xf n = + fmap + (\x -> x0 + (xf - x0)*(fromIntegral x / fromIntegral (n-1))) + $ take n [(0::Int)..] + +radius0 :: Fractional a => a +radius0 = 0.3 + +segment0 :: Floating a => a +segment0 = 2 * radius0 * sin(pi/fromIntegral npoints) + +points0 :: Vec NPoints (Point Double) +points0 = TV.mkVec' $ map (\q -> Point (radius0*cos(q)) (radius0*sin(q))) $ take npoints $ linspace 0 (2*pi) (npoints + 1) + +atan2' :: RealFloat a => Point a -> a +atan2' (Point x y) = atan2 y x + +--data G a = +-- G +-- { gMin90 :: Vec NPoints a +-- , gEqualR :: Vec NPoints a +-- , gMean0 :: Point a +-- , gStages :: Vec NSteps (StageCon a) +-- , gCloseMean :: Vec NSteps (Point a) +-- , gCloseTheta :: Vec NSteps a +-- } deriving (Functor, Generic1, Show) + +--(f0,g0) = fg guess undefined + +----worst :: Vectorize f => f Double -> Double +----worst = V.toList (fmap abs) +-- +--blah :: IO () +--blah = do +---- putStrLn $ "gmin90: " ++ show (minimum $ F.toList $ gMin90 g0) +---- putStrLn $ "gmin90: " ++ show (maximum $ F.toList $ gMin90 g0) +-- print $ gMean0 g0 +-- print $ g360 g0 + + +guess :: X Double +guess = + X + { xR = segment0 + , xPoints = points0 + , xStages = TV.tvzipWith (\mean theta -> + Stage { sTheta = theta + , sMean = mean + , sPhis = fill $ min 0 (max (pi/2) (atan2' mean)) + }) means0 thetas0 + } + where + thetas0 :: Vec NSteps Double + thetas0 = TV.mkVec' $ linspace 0 0 nsteps + + means0 :: Vec NSteps (Point Double) + means0 = TV.mkVec' $ map f (linspace (-pi/4) (3*pi/4) nsteps) +-- means0 = TV.mkVec' $ map f (linspace 0 (pi/2) npoints) + where + f :: Double -> Point Double + f q + | q <= pi/4 = fmap (/ (2*px)) p0 + | otherwise = fmap (/ (2*py)) p0 + where + p0 = Point px py + px = cos q + py = sin q + + +myNlp :: Nlp X None G SXElement +myNlp = Nlp { nlpFG = fg + , nlpBX = bx + , nlpBG = bg + , nlpX0 = guess + , nlpP = None + , nlpLamX0 = Nothing + , nlpLamG0 = Nothing + , nlpScaleF = Nothing + , nlpScaleX = Nothing + , nlpScaleG = Nothing + } + where + + bx :: X Bounds + bx = X + { xR = (Just (segment0/2), Nothing) + , xPoints = fill $ Point (Just (-5), Just 5) (Just (-5), Just 5) + , xStages = TV.mkVec' $ stage0 : replicate (nsteps-1) otherStages + } + where + stage0 = + Stage + { sTheta = (Just 0, Just 0) + , sMean = Point (Just (-3), Just 3) (Just (-3), Just 3) + , sPhis = fill (Just 0, Just (pi/2)) + } + otherStages = + Stage + { sTheta = (Just (-4*pi), Just (4*pi)) + , sMean = Point (Just (-3), Just 3) (Just (-3), Just 3) + , sPhis = fill (Just 0, Just (pi/2)) + } + + + bg :: G Bounds + bg = G + { gMin90 = fill (Just 0.8, Nothing) + , gEqualR = fill (Just 0, Just 0) + , gMean0 = fill (Just 0, Just 0) + , g360s = TV.mkVec' $ map (\q -> (Just (q - pi), Just (q + pi))) + $ linspace 0 (2*pi) npoints + , gStages = TV.mkVec' $ stage0 : replicate (nsteps-2) midStages ++ [stageF] + , gCloseMean = TV.mkVec' $ replicate (nsteps - 1) (fill (Just (-deltaMean), Just deltaMean)) ++ [fill (Nothing, Nothing)] + , gCloseTheta = TV.mkVec' $ replicate (nsteps - 1) (Just (-deltaTheta), Just deltaTheta) ++ [(Nothing, Nothing)] + } + where + deltaTheta = pi / fromIntegral nsteps + deltaMean = 4 / fromIntegral nsteps + stage0 = StageCon + { scOuters = fill $ Point (Nothing, Just 1) (Nothing, Just 0) + , scInners = fill (Just 0, Nothing) + } + stageF = StageCon + { scOuters = fill $ Point (Nothing, Just 0) (Nothing, Just 1) + , scInners = fill (Just 0, Nothing) + } + midStages = StageCon + { scOuters = fill $ Point (Nothing, Just 1) (Nothing, Just 1) + , scInners = fill (Just 0, Nothing) + } + +dot :: Num a => Point a -> Point a -> a +dot (Point x0 y0) (Point x1 y1) = x0*x1 + y0*y1 + +fg :: forall a . Floating a => X a -> None a -> (a, G a) +fg (X r points stages) _ = (f, g) + where + ds :: Vec NPoints (Point a) + ds = zipWithNext (\x0 x1 -> x1 - x0) points + + curvatureRegularization = (F.sum (zipWithNext (\x0 x1 -> dot x0 x1) ds)) / (fromIntegral npoints) + + f = 1*curvatureRegularization - 0.5 * (F.sum $ zipWithNext cross points) + g = G + { gMin90 = zipWithNext (\x0 x1 -> dot x0 x1 / ((norm2 x0) * (norm2 x1))) ds + , gEqualR = fmap (\(Point x y) -> x*x + y*y - r*r) ds + , gMean0 = F.sum points / (fromIntegral npoints) + , g360s = TV.mkVec' $ + drop 1 $ scanl (+) 0 $ + F.toList $ + zipWithNext + (\d0 d1 -> asin ((d0 `cross` d1) / ((1e-9 + norm2 d0) * (1e-9 + norm2 d1)))) + ds + , gStages = fmap stageCon stages + , gCloseMean = zipWithNext (\(Stage _ mean1 _) (Stage _ mean0 _) -> mean1 - mean0) stages + , gCloseTheta = zipWithNext (\(Stage theta1 _ _) (Stage theta0 _ _) -> theta1 - theta0) stages + } + + stageCon :: Stage a -> StageCon a + stageCon (Stage theta mean phis) = StageCon { scOuters = points' + , scInners = TV.tvzipWith inner points' phis + } + where + rot :: Point a -> Point a + rot (Point x y) = mean + Point (x*cos(theta) + y*sin(theta)) (-x*sin(theta) + y*cos(theta)) + + points' :: Vec NPoints (Point a) + points' = fmap rot points + + inner (Point xij' yij') phiij = xij'*cos(phiij) + yij'*sin(phiij) + +solver :: NlpSolverStuff +solver = ipoptSolver { options = [("ma86_order", Opt "metis"), ("max_iter", Opt (1000 :: Int))]} +--solver = snoptSolver { options = [ ("detect_linear", Opt False) ] } + +send :: Serialize a => ZMQ.Socket ZMQ.Pub -> String -> a -> IO () +send publisher chanName stuff = do + let bs = encode stuff + ZMQ.send publisher [ZMQ.SendMore] (pack chanName) + ZMQ.send publisher [] bs + +main :: IO () +main = + ZMQ.withContext $ \context -> + ZMQ.withSocket context ZMQ.Pub $ \publisher -> do + ZMQ.bind publisher url + putStrLn $ "# design vars: " ++ show (vlength (Proxy :: Proxy X)) + putStrLn $ "# constraints: " ++ show (vlength (Proxy :: Proxy G)) + iters <- newIORef 0 + _ <- solveNlp solver myNlp $ Just $ \x -> do + k <- readIORef iters + writeIORef iters (k + 1) + let msg = SofaMessage + { smSegmentLength = xR x + , smIters = k + , smPoints = F.toList (xPoints x) + , smMeanThetas = map (\stg -> (sMean stg, sTheta stg)) $ F.toList (xStages x) + } + --mapM_ (\stg -> print (sMean stg, sTheta stg)) $ F.toList (xStages x) + send publisher sofaChannel msg + return True + return () + diff --git a/examples/SofaVisualizer.hs b/examples/SofaVisualizer.hs new file mode 100644 --- /dev/null +++ b/examples/SofaVisualizer.hs @@ -0,0 +1,171 @@ +{-# OPTIONS_GHC -Wall #-} +{-# Language CPP #-} + +module Main ( main ) where + +import qualified Data.Foldable as F +import Linear.V3 ( V3(..) ) +import Linear.Quaternion ( Quaternion(..) ) +import Control.Monad ( when, forever ) +import Data.ByteString.Char8 ( pack ) +import Data.Serialize +import qualified System.ZMQ4 as ZMQ +import qualified Control.Concurrent.STM as STM +import qualified Control.Concurrent as CC +import Text.Printf + +import Vis + +import Sofa.Common + +--type M22 = ((Double,Double),(Double,Double)) + + +--sub :: ((DynCollTraj (Vector Double), CollTrajMeta, [M22], M22) -> IO ()) -> IO () +--sub writeChan = ZMQ.withContext $ \context -> +-- ZMQ.withSocket context ZMQ.Sub $ \subscriber -> do +-- ZMQ.connect subscriber url +-- ZMQ.subscribe subscriber (pack channelName) +-- forever $ do +-- _ <- ZMQ.receive subscriber +-- mre <- ZMQ.moreToReceive subscriber +-- when mre $ do + +sub :: (SofaMessage -> IO ()) -> IO () +sub writeChan = ZMQ.withContext $ \context -> + ZMQ.withSocket context ZMQ.Sub $ \subscriber -> do + ZMQ.connect subscriber url + ZMQ.subscribe subscriber (pack sofaChannel) + forever $ do + _ <- ZMQ.receive subscriber + mre <- ZMQ.moreToReceive subscriber + when mre $ do + msg <- ZMQ.receive subscriber + let decoded :: SofaMessage + decoded = case decode msg of + Left err -> error err + Right t -> t + writeChan decoded + +main :: IO () +main = do + -- keep reading from tcp and storing results in the queue + trajChan <- STM.atomically STM.newTQueue + _ <- CC.forkIO (sub (STM.atomically . (STM.writeTQueue trajChan))) + + -- keep parsing results from the queue into nice form + trajMVar <- CC.newMVar (VisObjects [], [], []) + + let getLastValue = do + val <- STM.atomically (STM.readTQueue trajChan) + empty' <- STM.atomically (STM.isEmptyTQueue trajChan) + if empty' then return val else getLastValue + + parserThread = do + sofaX <- getLastValue + CC.modifyMVar_ trajMVar $ \(_,_,xs) -> do + let (mainvis, stages) = toVisObjects sofaX + return (mainvis, stages, xs) + parserThread + + _ <- CC.forkIO parserThread + + animateIO (defaultOpts { optWindowName = "sofa lol" }) (animateFun trajMVar) + +multiplyList :: Int -> Int -> [a] -> [a] +multiplyList _ _ [] = [] +multiplyList k 0 (_:xs) = multiplyList k k xs +multiplyList k j xs@(x0:_) = x0 : multiplyList k (j-1) xs + +animateFun :: CC.MVar (VisObject Double, [VisObject Double], [VisObject Double]) + -> Float -> IO (VisObject Double) +animateFun mv = const $ do + (mainvis, stages, plotstages) <- CC.takeMVar mv + case plotstages of + (s0:ss) -> do + CC.putMVar mv (mainvis, stages, ss) + return $ VisObjects [mainvis, s0] + []-> do + let n = max 1 $ 400 `div` length stages + CC.putMVar mv (mainvis, stages, multiplyList n n stages) + return mainvis + +linspace :: Fractional a => a -> a -> Int -> [a] +linspace x0 xf n = xs + where + h = (xf-x0)/(fromIntegral n - 1) + xs = map (\k -> x0 + h*(fromIntegral k)) (take n [(0::Int)..]) + +qy :: Quaternion Double +qy = Quaternion 0 $ V3 1 0 0 + +toVisObjects :: SofaMessage -> (VisObject Double, [VisObject Double]) +toVisObjects (SofaMessage iters r points stages) = + ( RotQuat qy $ VisObjects [walls, txt, shape0 + , VisObjects (allPoints (-1)) + , Trans (V3 (-2) (-2) 0) axes + ] + , map (RotQuat qy . Trans (V3 1 1 0)) (allPoints 0) + ) + where + walls = VisObjects + [ Line [ V3 (-4) 1 0 + , V3 1 1 0 + , V3 1 (-4) 0 + ] (makeColor 1 1 1 1) + , Line [ V3 (-4) 2 0 + , V3 2 2 0 + , V3 2 (-4) 0 + ] (makeColor 1 1 1 1) + , Line [ V3 (-4) 0 0 + , V3 0 0 0 + , V3 0 (-4) 0 + ] (makeColor 1 1 1 1) + ] + + axes = Axes (0.5, 15) + npoints = length points + nsteps = length stages + shape0 = Line' $ + zipWith (\(Point x y) c -> ((V3 x y 0) - (V3 2 2 0), c)) + (points ++ [head points]) + (colors (npoints + 1)) + drawOne :: [Point Double] -> Double -> Color -> VisObject Double + drawOne ps@(p0:_) z = + Line + (map (\(Point x y) -> (V3 x y z)) (ps ++ [p0])) + drawOne _ _ = const (VisObjects []) + + area = 0.5 * (F.sum $ zipWithNext' cross points) + + allPoints :: Double -> [VisObject Double] + allPoints maxheight = zipWith3 (\c so z -> drawOne (stagePoints so) z c) + (colors (nsteps + 1)) + stages + (linspace 0 maxheight (nsteps + 1)) + + colors :: Int -> [Color] + colors k = fmap (\gamma -> makeColor 0 gamma (1 - gamma) 1) (gammas k) + + gammas :: Int -> [Float] + gammas k = linspace 0 1 k + + stagePoints :: (Point Double, Double) -> [Point Double] + stagePoints (mean, theta) = fmap rot points + where + rot :: Point Double -> Point Double + rot (Point x y) = mean + Point (x*cos(theta) + y*sin(theta)) (-x*sin(theta) + y*cos(theta)) + + + messages = [ show npoints ++ " segments" + , show nsteps ++ " stages" + , printf "segment length: %.4f" r + , printf "area: %.4f" area + , "iteration: " ++ show iters + ] + txt = VisObjects $ + zipWith (\s k -> Text2d s (30,fromIntegral $ 30*k) TimesRoman24 (makeColor 1 1 1 1)) + messages (reverse [1..length messages]) +-- trajLine = Line (zipWith (\x y -> V3 x y 0) (concat xs0) (concat ys0)) (makeColor 1 0 0 0.4) +-- trajDots = Points (zipWith (\x y -> V3 x y 0) xsCollPts ysCollPts) (Just 1) red +-- trajDots' = Points (zipWith (\x y -> V3 x y 0) xsBigPts ysBigPts) (Just 2) red diff --git a/examples/StaticExample.hs b/examples/StaticExample.hs deleted file mode 100644 --- a/examples/StaticExample.hs +++ /dev/null @@ -1,35 +0,0 @@ -{-# OPTIONS_GHC -Wall #-} - -module Main where - -import Dyno.NlpMonad -import Dyno.Solvers - -rosen :: NlpMonad () -rosen = do - x1 <- designVar "x1" - x2 <- designVar "x2" - x3 <- designVar "x3" - x4 <- designVar "x4" - - 0 + x1**2 + x2**2 + x3 === 2 - 0 + x2**4 + x4 === 4 - 0 + 2*x1 + 4*x2 >== 0.0 - x3 >== 0 - x4 >== 0 - - minimize $ (x1 + x2 + x3)**2 + 3*x3 + 5*x4 - - -main :: IO () -main = do - let guess = [ ("x1", 0.1) - , ("x2", 0.125) - , ("x3", 0.666666) - , ("x4", 0.142857) - ] - - (status, fopt, xopt) <- solveStaticNlp ipoptSolver rosen guess Nothing - print status - putStrLn $ "value: " ++ show fopt - mapM_ (\(n,v) -> putStrLn $ n ++ ": " ++ show v) xopt diff --git a/examples/Vec.hs b/examples/Vec.hs --- a/examples/Vec.hs +++ b/examples/Vec.hs @@ -1,18 +1,22 @@ --- | How to use type-indexed Vectors +-- | How to use type-indexed Vectors. +-- Don't forget to import DataKinds/PolyKinds ! {-# OPTIONS_GHC -Wall #-} {-# Language ScopedTypeVariables #-} {-# Language DeriveFunctor #-} {-# Language DeriveGeneric #-} +{-# Language DataKinds #-} +{-# Language PolyKinds #-} module Main where +import GHC.Generics ( Generic1 ) + import qualified Data.Foldable as F import qualified Data.Vector as V import Dyno.Vectorize import Dyno.TypeVecs -import Dyno.Nats data Params a = Params a a deriving (Functor, Generic1, Show) data X n a = X (Vec n (Params a)) a deriving (Functor, Generic1, Show) @@ -35,7 +39,7 @@ unknownLength = V.fromList [Params 1 2, Params 3 4, Params 5 6, Params 7 8] -- you do know the length at compile time -knownLength :: (Num a, Show a) => Vec D4 (Params a) +knownLength :: (Num a, Show a) => Vec 4 (Params a) knownLength = mkVec unknownLength -- do something on type-safe vec data @@ -49,6 +53,6 @@ main :: IO () main = do print (unknownLength :: V.Vector (Params Double)) - print (knownLength :: Vec D4 (Params Double)) + print (knownLength :: Vec 4 (Params Double)) print (doSomething knownLength :: Double) print (doSomethingAtRuntime unknownLength :: Double) diff --git a/src/Dyno/Cov.hs b/src/Dyno/Cov.hs deleted file mode 100644 --- a/src/Dyno/Cov.hs +++ /dev/null @@ -1,110 +0,0 @@ -{-# OPTIONS_GHC -Wall -fno-cse #-} -{-# Language ScopedTypeVariables #-} -{-# Language KindSignatures #-} ---{-# Language DeriveGeneric #-} - -module Dyno.Cov - ( Cov(..) - , toMat - , fromMat - , toMatrix - , toHMatrix - , toHMatrix' - , fromMatrix - , diag - , diag' - , nOfVecLen - ) where - ---import GHC.Generics ( Generic ) -import Data.Vector ( Vector ) -import qualified Data.Sequence as Seq -import System.IO.Unsafe ( unsafePerformIO ) -import qualified Data.Packed.Matrix as Mat - -import qualified Casadi.Sparsity as Sparsity -import Casadi.Slice ( slice' ) -import Casadi.DMatrix ( DMatrix ) -import qualified Casadi.DMatrix as DMatrix - -import Dyno.Vectorize ( Vectorize(..), Proxy(..) ) -import Dyno.View.View ( View(..), J, unJ, mkJ ) -import Dyno.View.CasadiMat ( CasadiMat ) -import qualified Dyno.View.CasadiMat as CM -import Dyno.View.JV ( JV ) -import Dyno.View.Viewable ( Viewable(..) ) -import Dyno.View.M ( M(..), mkM, toHMat ) - -newtype Cov (f :: * -> *) a = Cov a -instance View f => View (Cov f) where - cat (Cov x) = mkJ x - split x = Cov (unJ x) - size = const $ (n*n + n) `div` 2 - where - n = size (Proxy :: Proxy f) - sizes k0 = const (Seq.singleton (k0 + n)) - where - n = size (Proxy :: Proxy f) - -nOfVecLen :: Int -> Int -nOfVecLen m - | (n*n + n) `div` 2 == m = n - | otherwise = error $ "nOfVecLen fail: " ++ show m - where - m' = fromIntegral m :: Double - n = round $ sqrt (2*m' + 1/4) - 1/2 - ----- THIS SKIPS THE DEVECTORIZE LENGTH CHECK!! ---instance (Serialize a) => Serialize (Cov f a) where --- put = put . V.toList . unCov --- get = fmap (Cov . V.fromList) get - -toMat :: (View f, CasadiMat a, Viewable a) => J (Cov f) a -> M f f a -toMat c = mkM (toMatrix c) -{-# NOINLINE toMat #-} - -toMatrix :: forall f a . (View f, CasadiMat a, Viewable a) => J (Cov f) a -> a -toMatrix c = unsafePerformIO $ do - let n = size (Proxy :: Proxy f) - m <- CM.copy (CM.zerosSp (Sparsity.upper n)) - --CM.setNZ m (CM.dense (unJ c)) slice' - CM.setNZ m (unJ c) slice' -- Joel says that "dense" isn't required here - return (CM.triu2symm m) -{-# NOINLINE toMatrix #-} - -toHMatrix :: forall f . View f => J (Cov f) DMatrix -> Mat.Matrix Double -toHMatrix m = toHMat (toMat m) - -toHMatrix' :: forall f . View f => J (Cov f) (Vector Double) -> Mat.Matrix Double -toHMatrix' v = toHMatrix $ (mkJ (DMatrix.dvector (unJ v)) :: J (Cov f) DMatrix) - -diag :: (View f, CasadiMat a, Viewable a) => J f a -> J (Cov f) a -diag = fromMatrix . CM.diag . unJ - -diag' :: forall f . Vectorize f => f Double -> J (Cov (JV f)) (Vector Double) -diag' x = mkJ $ DMatrix.ddata $ DMatrix.ddense $ unJ y - where - y :: J (Cov (JV f)) DMatrix - y = diag $ mkJ $ DMatrix.dvector $ vectorize x - ---data X a = X (J S a) (J S a) deriving (Generic, Show) ---instance View X ---xx = X (mkJ 1) (mkJ 2) :: X DMatrix ---xx' = cat xx --- ---dd :: J (Cov X) DMatrix ---dd = diag xx' --- ---sp :: DMatrix ---sp = toMatrix dd --- ---dd2 :: J (Cov X) DMatrix ---dd2 = fromMatrix sp - --- todo CasadiMat class -fromMat :: (View f, CasadiMat a, Viewable a) => M f f a -> J (Cov f) a -fromMat (UnsafeM c) = fromMatrix c - -fromMatrix :: (View f, CasadiMat a, Viewable a) => a -> J (Cov f) a -fromMatrix x = mkJ $ CM.getNZ (CM.triu (CM.dense x)) slice' ---fromMatrix x = mkJ $ CM.getNZ (CM.triu x) slice' diff --git a/src/Dyno/Dae.hs b/src/Dyno/Dae.hs deleted file mode 100644 --- a/src/Dyno/Dae.hs +++ /dev/null @@ -1,41 +0,0 @@ -{-# OPTIONS_GHC -Wall #-} - -module Dyno.Dae - ( Dae - , ExplicitOde - , ImplicitOde - , SemiExplicitDae - , Integrator - , forwardEuler - , rk4 - ) where - -import Dyno.Vectorize - ---type Dae x z u p r a = x a -> x a -> z a -> u a -> p a -> a -> r a -type Dae x z u p r a = x a -> z a -> u a -> p a -> r a - -type ExplicitOde x u p a = Dae x None u p x a -type ImplicitOde x u p r a = Dae x None u p r a -type SemiExplicitDae x z u p r a = Dae x z u p (Tuple x r) a - -type Integrator x z u p r a = Dae x z u p r a -> a -> x a -> u a -> p a -> x a - --- x0 + dx - xf == 0 -forwardEuler :: (Vectorize x, Num a) => Integrator x None u p x a -forwardEuler f ts x0 u p = vzipWith (+) x0 deltaX - where - deltaX = fmap (*ts) xdot - xdot = f x0 None u p - -rk4 :: (Vectorize x, Fractional a) => Integrator x None u p x a -rk4 f h x0 u p = vzipWith (+) x0 deltaX - where - deltaX = mul (h/6) (k1 `add` mul 2 k2 `add` mul 2 k3 `add` k4) - k1 = f x0 None u p - k2 = f (x0 `add` mul (h/2) k1) None u p - k3 = f (x0 `add` mul (h/2) k2) None u p - k4 = f (x0 `add` mul h k3) None u p - - mul y = fmap (y*) - add = vzipWith (+) diff --git a/src/Dyno/DirectCollocation.hs b/src/Dyno/DirectCollocation.hs --- a/src/Dyno/DirectCollocation.hs +++ b/src/Dyno/DirectCollocation.hs @@ -11,21 +11,21 @@ import Data.Proxy import Data.Vector ( Vector ) -import Dyno.View ( J, jfill ) +import Dyno.View.View ( J, jfill ) import Dyno.Vectorize ( Vectorize ) import Dyno.Ocp ( OcpPhase ) import Dyno.NlpSolver ( NlpSolverStuff, solveNlp' ) import Dyno.Nlp ( Nlp'(..) ) import Dyno.DirectCollocation.Formulate ( CollProblem(..), makeCollProblem ) import Dyno.DirectCollocation.Types ( CollTraj(..) ) -import Dyno.DirectCollocation.Dynamic ( DynCollTraj ) +import Dyno.DirectCollocation.Dynamic ( DynPlotPoints ) import qualified Dyno.TypeVecs as TV solveOcp :: forall x z u p r o c h . (Vectorize x, Vectorize z, Vectorize u, Vectorize p, Vectorize r, Vectorize o, Vectorize c, Vectorize h) - => NlpSolverStuff -> Int -> Int -> Maybe ([DynCollTraj (Vector Double)] -> IO Bool) + => NlpSolverStuff -> Int -> Int -> Maybe (DynPlotPoints Double -> IO Bool) -> OcpPhase x z u p r o c h -> IO (Either String String) solveOcp solverStuff n deg cb0 ocp = @@ -35,12 +35,11 @@ guess = jfill 1 cp <- makeCollProblem ocp let nlp = cpNlp cp - toDynamic = cpCallback cp + toPlotPoints = cpPlotPoints cp --_ <- solveNlp' solverStuff (nlp {nlpX0' = guess}) (fmap (. ctToDynamic) cb) let cb = case cb0 of Nothing -> Nothing - Just cb' -> Just $ \x -> do - (dyn,_) <- toDynamic x - cb' [dyn] + Just cb' -> Just $ \x -> toPlotPoints x >>= cb' + (res, _) <- solveNlp' solverStuff (nlp {nlpX0' = guess}) cb return res diff --git a/src/Dyno/DirectCollocation/Dynamic.hs b/src/Dyno/DirectCollocation/Dynamic.hs --- a/src/Dyno/DirectCollocation/Dynamic.hs +++ b/src/Dyno/DirectCollocation/Dynamic.hs @@ -1,22 +1,24 @@ {-# OPTIONS_GHC -Wall -fno-warn-orphans #-} {-# Language ScopedTypeVariables #-} {-# Language DeriveGeneric #-} +{-# Language PolyKinds #-} module Dyno.DirectCollocation.Dynamic - ( DynCollTraj(..) - , DynPlotPoints + ( DynPlotPoints , CollTrajMeta(..) , MetaTree , forestFromMeta , toMeta , toMetaCov - , ctToDynamic , dynPlotPoints , catDynPlotPoints -- , toPlotTree , NameTree(..) ) where +import GHC.Generics ( Generic ) + +import Data.Proxy ( Proxy(..) ) import Data.List ( mapAccumL, unzip5 ) import Data.Tree ( Tree(..) ) import Data.Vector ( Vector ) @@ -24,19 +26,20 @@ import qualified Data.Foldable as F import qualified Data.Tree as Tree import Data.Serialize ( Serialize(..) ) -import GHC.Generics ( Generic ) import Linear.V +import Dyno.View.Unsafe.View ( unJ, unJ' ) + import Dyno.Server.Accessors ( AccessorTree(..), Lookup(..), accessors ) -import Dyno.Vectorize +import Dyno.Vectorize ( Vectorize, Id(..) ) import Dyno.View.JV import Dyno.View.View +import Dyno.View.JVec ( JVec(..) ) import qualified Dyno.TypeVecs as TV import Dyno.TypeVecs ( Vec ) import Dyno.DirectCollocation.Types -import Dyno.DirectCollocation.Quadratures ( QuadratureRoots, mkTaus, interpolate ) -import Dyno.DirectCollocation.Reify ( reifyCollTraj ) +import Dyno.DirectCollocation.Quadratures ( QuadratureRoots, mkTaus ) data DynPlotPoints a = DynPlotPoints @@ -45,7 +48,11 @@ [[(a, Vector a)]] [[(a, Vector a)]] [[(a, Vector a)]] - deriving Show + deriving (Show, Generic) +instance Serialize a => Serialize (DynPlotPoints a) +instance Serialize a => Serialize (Vector a) where + get = fmap V.fromList get + put = put . V.toList catDynPlotPoints :: [DynPlotPoints a] -> DynPlotPoints a catDynPlotPoints pps = @@ -56,52 +63,20 @@ (concatMap (\(DynPlotPoints _ _ _ x _) -> x) pps) (concatMap (\(DynPlotPoints _ _ _ _ x) -> x) pps) -data D a -data DynCollTraj a = DynCollTraj (J (CollTraj D D D D () ()) a) (Vec () (Vec () (J D a, J D a))) - deriving (Generic, Show) -instance Serialize a => Serialize (DynCollTraj a) -instance Serialize a => Serialize (V.Vector a) where - put = put . V.toList - get = fmap V.fromList get - dynPlotPoints :: - forall a . - (Real a, Fractional a, Show a) - => DynCollTraj (Vector a) -> CollTrajMeta -> DynPlotPoints a -dynPlotPoints (DynCollTraj traj outputs) meta = - reifyCollTraj (nx,nz,nu,np,no,n,deg) traj outputs foo - where - quadratureRoots = ctmQuadRoots meta - nx = ctmNx meta - nz = ctmNz meta - nu = ctmNu meta - np = ctmNp meta - no = ctmNo meta - n = ctmN meta - deg = ctmDeg meta - - foo :: (Vectorize x, Vectorize z, Vectorize u, Vectorize p, Vectorize o, Dim deg, Dim n) - => J (CollTraj x z u p n deg) (Vector a) - -> Vec n (Vec deg (J (JV o) (Vector a), J (JV x) (Vector a))) - -> DynPlotPoints a - foo ct outs = plotPoints quadratureRoots (split ct) outs - - --- a safe, point maker which is difficult to work with --- first stage in making a list -plotPoints :: forall x z u p o n deg a . (Dim n, Dim deg, Real a, Fractional a, Show a, Vectorize x, Vectorize z, Vectorize u, Vectorize o, Vectorize p) - => QuadratureRoots -> CollTraj x z u p n deg (Vector a) - -> Vec n (Vec deg (J (JV o) (Vector a), J (JV x) (Vector a))) + => 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)) -> DynPlotPoints a -plotPoints quadratureRoots (CollTraj (UnsafeJ tf') _ stages' xf) outputs = +dynPlotPoints quadratureRoots (CollTraj tf' _ stages' xf) outputs = DynPlotPoints (xss++[[(tf,unJ xf)]]) zss uss oss xdss where - nStages = size (Proxy :: Proxy (JVec n S)) + nStages = size (Proxy :: Proxy (JVec n (JV Id))) tf,h :: a - tf = V.head tf' + Id tf = splitJV tf' h = tf / fromIntegral nStages taus :: Vec deg a @@ -115,7 +90,7 @@ -- todo: check this final time against expected tf f :: a -> [( CollStage (JV x) (JV z) (JV u) deg (Vector a) - , Vec deg (J (JV o) (Vector a), J (JV x) (Vector a)) + , (Vec deg (J (JV o) (Vector a), J (JV x) (Vector a)), J (JV x) (Vector a)) )] -> [( [(a,Vector a)] , [(a,Vector a)] @@ -124,24 +99,24 @@ , [(a,Vector a)] )] f _ [] = [] - f t0 ((CollStage x0 xzus', xdos') : css) = (xs,zs,us,os,xds) : f tnext css + f t0 ((CollStage x0 xzus', (xdos, xnext)) : css) = (xs,zs,us,os,xds) : f tnext css where tnext = t0 + h xzus0 = fmap split (unJVec (split xzus')) :: Vec deg (CollPoint (JV x) (JV z) (JV u) (Vector a)) - xnext = interpolate taus x0 (fmap getX xzus0) - getX (CollPoint x _ _) = x - xs :: [(a,Vector a)] xs = (t0,unJ x0):xs'++[(tnext,unJ xnext)] xs',zs,us,os,xds :: [(a,Vector a)] - (xs',zs,us,os,xds) = unzip5 $ F.toList $ TV.tvzipWith3 g xzus0 xdos' taus + (xs',zs,us,os,xds) = unzip5 $ F.toList $ TV.tvzipWith3 g xzus0 xdos taus g (CollPoint x z u) (o,x') tau = ( (t,unJ' "x" x), (t,unJ' "z" z), (t,unJ' "u" u), (t,unJ' "o" o), (t,unJ' "x'" x') ) where t = t0 + h*tau + + + --toPlotTree :: forall x z u . -- (Lookup (x Double), Lookup (z Double), Lookup (u Double), -- Vectorize x, Vectorize z, Vectorize u) => @@ -247,14 +222,3 @@ toMetaCov roots _ _ = meta0 { ctmNsx = size (Proxy :: Proxy (JV sx)) } where meta0 = toMeta roots (Proxy :: Proxy o) (Proxy :: Proxy (CollTraj x z u p n deg)) - -ctToDynamic :: forall x z u p o n deg a . - (Vectorize x, Vectorize z, Vectorize u, Vectorize p) => - J (CollTraj x z u p n deg) a -> Vec n (Vec deg (J (JV o) a, J (JV x) a)) -> DynCollTraj a -ctToDynamic (UnsafeJ x) os = DynCollTraj (UnsafeJ x) (castO os) -- this should be totally safe - where - castO :: Vec n (Vec deg (J (JV o) a, J (JV x) a)) -> Vec () (Vec () (J D a, J D a)) - castO = TV.mkUnit . fmap (TV.mkUnit . fmap cast) - - cast :: (J (JV o) a, J (JV x) a) -> (J D a, J D a) - cast (UnsafeJ o, UnsafeJ x') = (UnsafeJ o, UnsafeJ x') diff --git a/src/Dyno/DirectCollocation/Export.hs b/src/Dyno/DirectCollocation/Export.hs --- a/src/Dyno/DirectCollocation/Export.hs +++ b/src/Dyno/DirectCollocation/Export.hs @@ -1,19 +1,25 @@ {-# OPTIONS_GHC -Wall #-} {-# Language ScopedTypeVariables #-} +{-# Language PolyKinds #-} module Dyno.DirectCollocation.Export ( toMatlab ) where +import Data.Proxy ( Proxy(..) ) import Linear.V ( Dim(..) ) import Data.Vector ( Vector ) import qualified Data.Vector as V import qualified Data.Foldable as F +import Dyno.View.Unsafe.View ( unJ ) + import Dyno.Server.Accessors ( Lookup, flatten, accessors ) import Dyno.TypeVecs ( Vec ) -import Dyno.Vectorize ( Vectorize, Proxy(..), fill ) -import Dyno.View ( View(..), JV, JVec(..), unJ, splitJV ) +import Dyno.Vectorize ( Vectorize, fill ) +import Dyno.View.View ( View(..) ) +import Dyno.View.JV ( JV, splitJV ) +import Dyno.View.JVec ( JVec(..) ) import Dyno.DirectCollocation.Formulate ( CollProblem(..) ) import Dyno.DirectCollocation.Types ( CollTraj(..), CollStage(..), CollPoint(..) ) import Dyno.DirectCollocation.Quadratures ( timesFromTaus ) @@ -32,7 +38,7 @@ -> CollTraj x z u p n deg (Vector Double) -> IO String toMatlab cp ct@(CollTraj tf' p' stages' xf) = do - outs <- fmap snd $ cpCallback cp (cat ct) + outs <- cpOutputs cp (cat ct) let taus :: Vec deg Double taus = cpTaus cp @@ -62,7 +68,7 @@ os :: [o Double] xdots :: [x Double] - (os, xdots) = unzip $ F.concatMap F.toList outs + (os, xdots) = unzip $ 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))) diff --git a/src/Dyno/DirectCollocation/Formulate.hs b/src/Dyno/DirectCollocation/Formulate.hs --- a/src/Dyno/DirectCollocation/Formulate.hs +++ b/src/Dyno/DirectCollocation/Formulate.hs @@ -4,6 +4,7 @@ {-# Language ScopedTypeVariables #-} {-# Language TypeOperators #-} {-# Language FlexibleContexts #-} +{-# Language PolyKinds #-} module Dyno.DirectCollocation.Formulate ( CovTraj(..) @@ -12,12 +13,12 @@ , makeCollProblem , makeCollCovProblem , mkTaus - , interpolate , makeGuess , makeGuessSim ) where import GHC.Generics ( Generic ) + import Data.Maybe ( fromMaybe ) import Data.Proxy ( Proxy(..) ) import Data.Vector ( Vector ) @@ -29,18 +30,20 @@ import Linear.Matrix hiding ( trace ) import Linear.V -import Casadi.DMatrix ( dvector, ddata, ddense ) +import Casadi.DMatrix ( DMatrix ) +import Casadi.MX ( MX ) -import Dyno.SXElement ( sxToSXElement, sxElementToSX ) -import Dyno.View.CasadiMat hiding ( solve ) -import Dyno.Cov -import Dyno.View.View -import Dyno.View.JV ( JV, sxCatJV, sxSplitJV, catJV, catJV' ) +import Dyno.SXElement ( sxCatJV, sxSplitJV ) +import Dyno.View.View ( View(..), J, jfill, JTuple(..), JNone(..), v2d, d2v ) +import qualified Dyno.View.M as M +import Dyno.View.Cov ( Cov ) +import Dyno.View.JV ( JV, splitJV, catJV, catJV' ) import Dyno.View.HList ( (:*:)(..) ) import Dyno.View.Fun +import Dyno.View.JVec( JVec(..), jreplicate ) import Dyno.View.Viewable ( Viewable ) import Dyno.View.Scheme ( Scheme ) -import Dyno.Vectorize ( Vectorize(..), fill, vlength, vzipWith ) +import Dyno.Vectorize ( Vectorize(..), Id(..), fill, vlength, vzipWith ) import Dyno.TypeVecs ( Vec ) import qualified Dyno.TypeVecs as TV import Dyno.LagrangePolynomials ( lagrangeDerivCoeffs ) @@ -48,7 +51,7 @@ import Dyno.Ocp ( OcpPhase(..), OcpPhaseWithCov(..) ) import Dyno.DirectCollocation.Types -import Dyno.DirectCollocation.Dynamic ( DynCollTraj, ctToDynamic ) +import Dyno.DirectCollocation.Dynamic ( DynPlotPoints, dynPlotPoints ) import Dyno.DirectCollocation.Quadratures ( QuadratureRoots(..), mkTaus, interpolate, timesFromTaus ) import Dyno.DirectCollocation.Robust @@ -56,8 +59,9 @@ CollProblem { cpNlp :: Nlp' (CollTraj x z u p n deg) JNone (CollOcpConstraints n deg x r c h) MX , cpOcp :: OcpPhase x z u p r o c h - , cpCallback :: J (CollTraj x z u p n deg) (Vector Double) - -> IO (DynCollTraj (Vector Double), Vec n (Vec deg (o Double, x Double))) + , cpPlotPoints :: J (CollTraj x z u p n deg) (Vector Double) -> IO (DynPlotPoints Double) + , cpOutputs :: J (CollTraj x z u p n deg) (Vector Double) + -> IO (Vec n (Vec deg (o Double, x Double), x Double)) , cpTaus :: Vec deg Double , cpRoots :: QuadratureRoots } @@ -82,84 +86,101 @@ cijs :: Vec (TV.Succ deg) (Vec (TV.Succ deg) Double) cijs = lagrangeDerivCoeffs (0 TV.<| taus) + interpolate' :: (J (JV x) :*: J (JVec deg (JV x))) MX -> J (JV x) MX + interpolate' (x0 :*: xs) = interpolate taus x0 (unJVec (split xs)) + interpolateScalar' :: (J (JV Id) :*: J (JVec deg (JV Id))) MX -> J (JV Id) MX + interpolateScalar' (x0 :*: xs) = interpolate taus x0 (unJVec (split xs)) + + interpolateFun <- toMXFun "interpolate (JV x)" interpolate' >>= expandMXFun + interpolateScalarFun <- toMXFun "interpolate (JV Id)" interpolateScalar' >>= expandMXFun + let callInterpolateScalar :: J (JV Id) MX -> Vec deg (J (JV Id) MX) -> J (JV Id) MX + callInterpolateScalar x0 xs = call interpolateScalarFun (x0 :*: cat (JVec xs)) + + callInterpolate :: J (JV x) MX -> Vec deg (J (JV x) MX) -> J (JV x) MX + callInterpolate x0 xs = call interpolateFun (x0 :*: cat (JVec xs)) + bcFun <- toSXFun "bc" $ \(x0:*:x1) -> sxCatJV $ ocpBc ocp (sxSplitJV x0) (sxSplitJV x1) mayerFun <- toSXFun "mayer" $ \(x0:*:x1:*:x2) -> - mkJ $ sxElementToSX $ ocpMayer ocp (sxToSXElement (unJ x0)) (sxSplitJV x1) (sxSplitJV x2) + sxCatJV $ Id $ ocpMayer ocp (unId (sxSplitJV x0)) (sxSplitJV x1) (sxSplitJV x2) lagrangeFun <- toSXFun "lagrange" $ \(x0:*:x1:*:x2:*:x3:*:x4:*:x5:*:x6) -> - mkJ $ sxElementToSX $ ocpLagrange ocp (sxSplitJV x0) (sxSplitJV x1) (sxSplitJV x2) (sxSplitJV x3) (sxSplitJV x4) (sxToSXElement (unJ x5)) (sxToSXElement (unJ x6)) - quadFun <- toMXFun "quadratures" $ evaluateQuadraturesFunction lagrangeFun cijs taus n + sxCatJV $ Id $ ocpLagrange ocp (sxSplitJV x0) (sxSplitJV x1) (sxSplitJV x2) (sxSplitJV x3) (sxSplitJV x4) (unId (sxSplitJV x5)) (unId (sxSplitJV x6)) + quadFun <- toMXFun "quadratures" $ evaluateQuadraturesFunction lagrangeFun callInterpolateScalar cijs n -- let callQuadFun = call quadFun callQuadFun <- fmap call (expandMXFun quadFun) dynFun <- toSXFun "dynamics" $ dynamicsFunction $ \x0 x1 x2 x3 x4 x5 -> - let (r,o) = ocpDae ocp (sxSplitJV x0) (sxSplitJV x1) (sxSplitJV x2) (sxSplitJV x3) (sxSplitJV x4) (sxToSXElement (unJ x5)) + let (r,o) = ocpDae ocp (sxSplitJV x0) (sxSplitJV x1) (sxSplitJV x2) (sxSplitJV x3) (sxSplitJV x4) (unId (sxSplitJV x5)) in (sxCatJV r, sxCatJV o) pathConFun <- toSXFun "pathConstraints" $ pathConFunction $ - \x0 x1 x2 x3 x4 x5 -> sxCatJV $ ocpPathC ocp (sxSplitJV x0) (sxSplitJV x1) (sxSplitJV x2) (sxSplitJV x3) (sxSplitJV x4) (sxToSXElement (unJ x5)) + \x0 x1 x2 x3 x4 x5 -> sxCatJV $ ocpPathC ocp (sxSplitJV x0) (sxSplitJV x1) (sxSplitJV x2) (sxSplitJV x3) (sxSplitJV x4) (unId (sxSplitJV x5)) pathStageConFun <- toMXFun "pathStageCon" (pathStageConstraints pathConFun) - dynStageConFun <- toMXFun "dynamicsStageCon" (dynStageConstraints cijs taus dynFun) + dynStageConFun <- toMXFun "dynamicsStageCon" (dynStageConstraints callInterpolate cijs dynFun) stageFun <- toMXFun "stageFunction" $ stageFunction pathStageConFun (call dynStageConFun) -- let callStageFun = call stageFun callStageFun <- fmap call (expandMXFun stageFun) - outputFun <- toMXFun "stageOutputs" $ outputFunction cijs taus dynFun + outputFun <- toMXFun "stageOutputs" $ outputFunction callInterpolate cijs taus dynFun -- prepare callbacks let nlpX0 = jfill 0 :: J (CollTraj x z u p n deg) (Vector Double) f :: J (JV o) DMatrix -> J (JV x) DMatrix -> (J (JV o) (Vector Double), J (JV x) (Vector Double)) - f o' x' = (mkJ (ddata (ddense (unJ o'))), mkJ (ddata (ddense (unJ x')))) - - dmToDv :: J a (Vector Double) -> J a DMatrix - dmToDv (UnsafeJ v) = UnsafeJ (dvector v) + f o' x' = (d2v o', d2v x') callOutputFun :: J (JV p) (Vector Double) - -> J S (Vector Double) + -> J (JV Id) (Vector Double) -> J (CollStage (JV x) (JV z) (JV u) deg) (Vector Double) - -> J S (Vector Double) - -> IO (Vec deg (J (JV o) (Vector Double), J (JV x) (Vector Double))) + -> J (JV Id) (Vector Double) + -> IO (Vec deg (J (JV o) (Vector Double), J (JV x) (Vector Double)), J (JV x) (Vector Double)) callOutputFun p h stage k = do - (_ :*: xdot :*: out) <- eval outputFun $ - (dmToDv stage) :*: (dmToDv p) :*: (dmToDv h) :*: (dmToDv k) + (_ :*: xdot :*: out :*: xnext) <- + eval outputFun $ (v2d stage) :*: (v2d p) :*: (v2d h) :*: (v2d k) let outs0 = unJVec (split out) :: Vec deg (J (JV o) DMatrix) xdots0 = unJVec (split xdot) :: Vec deg (J (JV x) DMatrix) - return (TV.tvzipWith f outs0 xdots0) + return (TV.tvzipWith f outs0 xdots0, 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)))) + -> IO (Vec n (Vec deg (J (JV o) (Vector Double), J (JV x) (Vector Double)), J (JV x) (Vector Double))) mapOutputFun ct = do let CollTraj tf p stages _ = split ct - h = tf / fromIntegral n + h = catJV $ Id (tf' / fromIntegral n) + where + Id tf' = splitJV tf vstages = unJVec (split stages) :: Vec n (J (CollStage (JV x) (JV z) (JV u) deg) (Vector Double)) - ks :: Vec n (J S (Vector Double)) - ks = TV.mkVec' $ map (mkJ . V.singleton . realToFrac) (take n [(0::Int)..]) + ks :: Vec n (J (JV Id) (Vector Double)) + ks = TV.mkVec' $ map (catJV . Id . realToFrac) (take n [(0::Int)..]) T.sequence $ TV.tvzipWith (callOutputFun p h) vstages ks - callback :: J (CollTraj x z u p n deg) (Vector Double) - -> IO (DynCollTraj (Vector Double), Vec n (Vec deg (o Double, x Double))) - callback traj = do + getPlotPoints :: J (CollTraj x z u p n deg) (Vector Double) -> IO (DynPlotPoints Double) + getPlotPoints traj = do outputs <- mapOutputFun traj - let -- devectorize outputs - devec :: (J (JV o) (Vector Double), J (JV x) (Vector Double)) -> (o Double, x Double) - devec (UnsafeJ os, UnsafeJ xds) = (devectorize os, devectorize xds) - return (ctToDynamic traj outputs, fmap (fmap devec) outputs) + return (dynPlotPoints roots (split traj) outputs) + getOutputs :: J (CollTraj x z u p n deg) (Vector Double) + -> IO (Vec n (Vec deg (o Double, x Double), x Double)) + getOutputs traj = do + 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)) + return $ fmap (\(x,y) -> (devec x, splitJV y)) outputs + let nlp = Nlp' { nlpFG' = getFg taus (bcFun :: SXFun (J (JV x) :*: J (JV x)) (J (JV c))) - (mayerFun :: SXFun (J S :*: (J (JV x) :*: (J (JV x)))) (J S)) - (callQuadFun :: (J (JV p) :*: J (JVec deg (CollPoint (JV x) (JV z) (JV u))) :*: J (JVec deg (JV o)) :*: J S :*: J (JVec deg S)) MX - -> J S MX) - (callStageFun :: (J S :*: J (JV p) :*: J (JVec deg S) :*: J (JV x) :*: J (JVec deg (JTuple (JV x) (JV z))) :*: J (JVec deg (JV u))) MX + (mayerFun :: SXFun (J (JV Id) :*: (J (JV x) :*: (J (JV x)))) (J (JV Id))) + (callQuadFun :: (J (JV p) :*: J (JVec deg (CollPoint (JV x) (JV z) (JV u))) :*: J (JVec deg (JV o)) :*: J (JV Id) :*: J (JVec deg (JV Id))) MX + -> J (JV Id) MX) + (callStageFun :: (J (JV Id) :*: J (JV p) :*: J (JVec deg (JV Id)) :*: J (JV x) :*: J (JVec deg (JTuple (JV x) (JV z))) :*: J (JVec deg (JV u))) MX -> (J (JVec deg (JV r)) :*: J (JVec deg (JV o)) :*: J (JVec deg (JV h)) :*: J (JV x)) MX) , nlpBX' = cat $ fillCollTraj (ocpXbnd ocp) @@ -188,7 +209,8 @@ } return $ CollProblem { cpNlp = nlp , cpOcp = ocp - , cpCallback = callback + , cpPlotPoints = getPlotPoints + , cpOutputs = getOutputs , cpTaus = taus , cpRoots = roots } @@ -200,10 +222,12 @@ (CollTrajCov sx x z u p n deg) JNone (CollOcpCovConstraints n deg x r c h sh shr sc) MX - , ccpCallback :: + , ccpPlotPoints :: J (CollTrajCov sx x z u p n deg) (Vector Double) -> IO (DynPlotPoints Double) + , ccpOutputs :: J (CollTrajCov sx x z u p n deg) (Vector Double) - -> IO ( DynCollTraj (Vector Double), Vec n (Vec deg (o Double, x Double)) - , Vec n (J (Cov (JV sx)) (Vector Double)), J (Cov (JV sx)) (Vector Double) + -> IO ( Vec n (Vec deg (o Double, x Double), x Double) + , Vec n (J (Cov (JV sx)) (Vector Double)) + , J (Cov (JV sx)) (Vector Double) ) , ccpSensitivities :: MXFun (J (CollTraj x z u p n deg)) @@ -236,16 +260,15 @@ sbcFun <- toSXFun "sbc" $ \(x0:*:x1) -> ocpCovSbc ocpCov x0 x1 shFun <- toSXFun "sh" $ \(x0:*:x1) -> ocpCovSh ocpCov (sxSplitJV x0) x1 mayerFun <- toSXFun "cov mayer" $ \(x0:*:x1:*:x2:*:x3:*:x4) -> - mkJ $ sxElementToSX $ ocpCovMayer ocpCov (sxToSXElement (unJ x0)) (sxSplitJV x1) (sxSplitJV x2) x3 x4 + sxCatJV $ Id $ ocpCovMayer ocpCov (unId (sxSplitJV x0)) (sxSplitJV x1) (sxSplitJV x2) x3 x4 lagrangeFun <- toSXFun "cov lagrange" $ \(x0:*:x1:*:x2:*:x3) -> - mkJ $ sxElementToSX $ ocpCovLagrange ocpCov (sxToSXElement (unJ x0)) (sxSplitJV x1) x2 (sxToSXElement (unJ x3)) + sxCatJV $ Id $ ocpCovLagrange ocpCov (unId (sxSplitJV x0)) (sxSplitJV x1) x2 (unId (sxSplitJV x3)) cp0 <- makeCollProblem ocp robustify <- mkRobustifyFunction (ocpCovProjection ocpCov) (ocpCovRobustifyPathC ocpCov) let nlp0 = cpNlp cp0 - callback0 = cpCallback cp0 gammas' = ocpCovGammas ocpCov :: shr Double gammas :: J (JV shr) MX @@ -260,35 +283,37 @@ -- the NLP fg :: J (CollTrajCov sx x z u p n deg) MX -> J JNone MX - -> (J S MX, J (CollOcpCovConstraints n deg x r c h sh shr sc) MX) + -> (J (JV Id) MX, J (CollOcpCovConstraints n deg x r c h sh shr sc) MX) fg = getFgCov taus computeCovariances gammas (robustify :: (J (JV shr) MX -> J (JV p) MX -> J (JV x) MX -> J (Cov (JV sx)) MX -> J (JV shr) MX)) (sbcFun :: SXFun (J (Cov (JV sx)) :*: J (Cov (JV sx))) (J sc)) (shFun :: SXFun (J (JV x) :*: J (Cov (JV sx))) (J sh)) - (lagrangeFun :: SXFun (J S :*: J (JV x) :*: J (Cov (JV sx)) :*: J S) (J S)) - (mayerFun :: SXFun (J S :*: (J (JV x) :*: (J (JV x) :*: (J (Cov (JV sx)) :*: J (Cov (JV sx)))))) (J S)) + (lagrangeFun :: SXFun (J (JV Id) :*: J (JV x) :*: J (Cov (JV sx)) :*: J (JV Id)) (J (JV Id))) + (mayerFun :: SXFun (J (JV Id) :*: (J (JV x) :*: (J (JV x) :*: (J (Cov (JV sx)) :*: J (Cov (JV sx)))))) (J (JV Id))) (nlpFG' nlp0) computeCovariancesFun' <- toMXFun "compute covariances" computeCovariances -- callbacks - let dmToDv :: J a (Vector Double) -> J a DMatrix - dmToDv (UnsafeJ v) = UnsafeJ (dvector v) - - --dvToDm :: View a => J a DMatrix -> J a (Vector Double) - --dvToDm v = mkJ (ddata (ddense (unJ v))) - dvToDm :: J a DMatrix -> J a (Vector Double) - dvToDm (UnsafeJ v) = UnsafeJ (ddata (ddense v)) + let getPlotPoints :: J (CollTrajCov sx x z u p n deg) (Vector Double) -> IO (DynPlotPoints Double) + getPlotPoints collTrajCov = do + let CollTrajCov _ collTraj = split collTrajCov + cpPlotPoints cp0 collTraj - callback collTrajCov = do + getOutputs :: J (CollTrajCov sx x z u p n deg) (Vector Double) + -> IO ( Vec n (Vec deg (o Double, x Double), x Double) + , Vec n (J (Cov (JV sx)) (Vector Double)) + , J (Cov (JV sx)) (Vector Double) + ) + getOutputs collTrajCov = do let CollTrajCov _ collTraj = split collTrajCov - (dynCollTraj, outputs) <- callback0 collTraj - covTraj <- fmap split $ eval computeCovariancesFun' (dmToDv collTrajCov) + outputs <- (cpOutputs cp0) collTraj + covTraj <- fmap split $ eval computeCovariancesFun' (v2d collTrajCov) let covs' = ctAllButLast covTraj pF = ctLast covTraj let covs = unJVec (split covs') :: Vec n (J (Cov (JV sx)) DMatrix) - return (dynCollTraj, outputs, fmap dvToDm covs, dvToDm pF) + return (outputs, fmap d2v covs, d2v pF) nlp = Nlp' @@ -329,7 +354,8 @@ } computeSensitivitiesFun' <- toMXFun "compute sensitivities" computeSensitivities return $ CollCovProblem { ccpNlp = nlp - , ccpCallback = callback + , ccpPlotPoints = getPlotPoints + , ccpOutputs = getOutputs , ccpSensitivities = computeSensitivitiesFun' , ccpCovariances = computeCovariancesFun' , ccpRoots = roots @@ -342,13 +368,13 @@ => Vec deg Double -> SXFun (J (JV x) :*: J (JV x)) (J (JV c)) -> SXFun - (J S :*: J (JV x) :*: J (JV x)) (J S) - -> ((J (JV p) :*: J (JVec deg (CollPoint (JV x) (JV z) (JV u))) :*: J (JVec deg (JV o)) :*: J S :*: J (JVec deg S)) MX -> - (J S) MX) - -> ((J S :*: J (JV p) :*: J (JVec deg S) :*: J (JV x) :*: J (JVec deg (JTuple (JV x) (JV z))) :*: J (JVec deg (JV u))) MX -> (J (JVec deg (JV r)) :*: J (JVec deg (JV o)) :*: J (JVec deg (JV h)) :*: J (JV x)) MX) + (J (JV Id) :*: J (JV x) :*: J (JV x)) (J (JV Id)) + -> ((J (JV p) :*: J (JVec deg (CollPoint (JV x) (JV z) (JV u))) :*: J (JVec deg (JV o)) :*: J (JV Id) :*: J (JVec deg (JV Id))) MX -> + (J (JV Id)) MX) + -> ((J (JV Id) :*: J (JV p) :*: J (JVec deg (JV Id)) :*: J (JV x) :*: J (JVec deg (JTuple (JV x) (JV z))) :*: J (JVec deg (JV u))) MX -> (J (JVec deg (JV r)) :*: J (JVec deg (JV o)) :*: J (JVec deg (JV h)) :*: J (JV x)) MX) -> J (CollTraj x z u p n deg) MX -> J JNone MX - -> (J S MX, J (CollOcpConstraints n deg x r c h) MX) + -> (J (JV Id) MX, J (CollOcpConstraints n deg x r c h) MX) getFg taus bcFun mayerFun quadFun stageFun collTraj _ = (obj, cat g) where -- split up the design vars @@ -363,10 +389,10 @@ objMayer = call mayerFun (tf :*: x0 :*: xf) - objLagrange :: J S MX + objLagrange :: J (JV Id) MX objLagrange = F.sum $ TV.tvzipWith3 oneStage spstagesPoints outputs times' - oneStage :: J (JVec deg (CollPoint (JV x) (JV z) (JV u))) MX -> J (JVec deg (JV o)) MX -> J (JVec deg S) MX - -> J S MX + oneStage :: J (JVec deg (CollPoint (JV x) (JV z) (JV u))) MX -> J (JVec deg (JV o)) MX -> J (JVec deg (JV Id)) MX + -> J (JV Id) MX oneStage stagePoints stageOutputs stageTimes = quadFun (parm :*: stagePoints :*: stageOutputs :*: dt :*: stageTimes) @@ -375,10 +401,10 @@ n = reflectDim (Proxy :: Proxy n) -- times at each collocation point - times :: Vec n (Vec deg (J S MX)) + times :: Vec n (Vec deg (J (JV Id) MX)) times = fmap snd $ timesFromTaus 0 (fmap realToFrac taus) dt - times' :: Vec n (J (JVec deg S) MX) + times' :: Vec n (J (JVec deg (JV Id)) MX) times' = fmap (cat . JVec) times -- initial point at each stage @@ -405,7 +431,7 @@ hs :: Vec n (J (JVec deg (JV h)) MX) interpolatedXs :: Vec n (J (JV x) MX) (dcs, outputs, hs, interpolatedXs) = TV.tvunzip4 $ fmap fff $ TV.tvzip spstages times' - fff :: (CollStage (JV x) (JV z) (JV u) deg MX, J (JVec deg S) MX) -> + fff :: (CollStage (JV x) (JV z) (JV u) deg MX, J (JVec deg (JV Id)) MX) -> (J (JVec deg (JV r)) MX, J (JVec deg (JV o)) MX, J (JVec deg (JV h)) MX, J (JV x) MX) fff (CollStage x0' xzus, stageTimes) = (dc, output, stageHs, interpolatedX') where @@ -438,15 +464,15 @@ -> SXFun (J (JV x) :*: J (Cov (JV sx))) (J sh) -- lagrangeFun -> SXFun - (J S :*: J (JV x) :*: J (Cov (JV sx)) :*: J S) (J S) + (J (JV Id) :*: J (JV x) :*: J (Cov (JV sx)) :*: J (JV Id)) (J (JV Id)) -- mayerFun -> SXFun - (J S :*: J (JV x) :*: J (JV x) :*: J (Cov (JV sx)) :*: J (Cov (JV sx))) (J S) - -> (J (CollTraj x z u p n deg) MX -> J JNone MX -> (J S MX, J (CollOcpConstraints n deg x r c h) MX) + (J (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 (CollTrajCov sx x z u p n deg) MX -> J JNone MX - -> (J S MX, J (CollOcpCovConstraints n deg x r c h sh shr sc) MX) + -> (J (JV Id) MX, J (CollOcpCovConstraints n deg x r c h sh shr sc) MX) getFgCov taus computeCovariances gammas robustify sbcFun shFun lagrangeFun mayerFun @@ -474,7 +500,7 @@ n = reflectDim (Proxy :: Proxy n) -- times at each collocation point - t0s :: Vec n (J S MX) + t0s :: Vec n (J (JV Id) MX) (t0s, _) = TV.tvunzip $ timesFromTaus 0 (fmap realToFrac taus) dt -- initial point at each stage @@ -519,21 +545,21 @@ { coCollPoints = jreplicate (jfill (Just 0, Just 0)) -- dae residual constraint , coContinuity = jreplicate (jfill (Just 0, Just 0)) -- continuity constraint , coPathC = jreplicate (jreplicate hbnds) - , coBc = mkJ $ vectorize $ ocpBcBnds ocp + , coBc = catJV (ocpBcBnds ocp) } where - hbnds = mkJ $ vectorize $ ocpPathCBnds ocp + hbnds = catJV (ocpPathCBnds ocp) evaluateQuadraturesFunction :: forall x z u p o deg . (Dim deg, View x, View z, View u, View o, View p) - => SXFun (J x :*: J z :*: J u :*: J p :*: J o :*: J S :*: J S) (J S) + => SXFun (J x :*: J z :*: J u :*: J p :*: J o :*: J (JV Id) :*: J (JV Id)) (J (JV Id)) + -> (J (JV Id) MX -> Vec deg (J (JV Id) MX) -> J (JV Id) MX) -> Vec (TV.Succ deg) (Vec (TV.Succ deg) Double) - -> Vec deg Double -> Int - -> (J p :*: J (JVec deg (CollPoint x z u)) :*: J (JVec deg o) :*: J S :*: J (JVec deg S)) MX - -> J S MX -evaluateQuadraturesFunction f cijs' taus n (p :*: stage' :*: outputs' :*: dt :*: stageTimes') = + -> (J p :*: J (JVec deg (CollPoint x z u)) :*: J (JVec deg o) :*: J (JV Id) :*: J (JVec deg (JV Id))) MX + -> J (JV Id) MX +evaluateQuadraturesFunction f interpolate' cijs' n (p :*: stage' :*: outputs' :*: dt :*: stageTimes') = dt * qnext where tf = dt * fromIntegral n @@ -544,13 +570,13 @@ outputs :: Vec deg (J o MX) outputs = unJVec (split outputs') - stageTimes :: Vec deg (J S MX) + stageTimes :: Vec deg (J (JV Id) MX) stageTimes = unJVec (split stageTimes') - qnext :: J S MX - qnext = interpolate taus 0 qs + qnext :: J (JV Id) MX + qnext = interpolate' 0 qs - qdots :: Vec deg (J S MX) + qdots :: Vec deg (J (JV Id) MX) qdots = TV.tvzipWith3 (\(CollPoint x z u) o t -> call f (x:*:z:*:u:*:p:*:o:*:t:*:tf)) stage outputs stageTimes qs = cijInvFr !* qdots @@ -567,11 +593,12 @@ cijInv :: Vec deg (Vec deg Double) cijInv = TV.mkVec' (map TV.mkVec' (Mat.toLists cijInv')) - cijInvFr :: Vec deg (Vec deg (J S MX)) + cijInvFr :: Vec deg (Vec deg (J (JV Id) MX)) cijInvFr = fmap (fmap realToFrac) cijInv -dot :: forall x deg a b. (Fractional (J x a), Real b) => Vec deg b -> Vec deg (J x a) -> J x a -dot cks xs = F.sum $ TV.unSeq elemwise +-- todo: code duplication +dot :: forall x deg a b. (Fractional (J x a), Real b, Dim deg) => Vec deg b -> Vec deg (J x a) -> J x a +dot cks xs = F.sum $ TV.unVec elemwise where elemwise :: Vec deg (J x a) elemwise = TV.tvzipWith smul cks xs @@ -580,7 +607,8 @@ smul x y = realToFrac x * y -interpolateXDots' :: (Real b, Fractional (J x a)) => Vec deg (Vec deg b) -> Vec deg (J x a) -> Vec deg (J x a) +-- todo: code duplication +interpolateXDots' :: (Real b, Fractional (J x a), Dim deg) => Vec deg (Vec deg b) -> Vec deg (J x a) -> Vec deg (J x a) interpolateXDots' cjks xs = fmap (`dot` xs) cjks interpolateXDots :: @@ -594,8 +622,8 @@ -- dynamics residual and outputs dynamicsFunction :: forall x z u p r o a . (View x, View z, View u, View r, View o, Viewable a) - => (J x a -> J x a -> J z a -> J u a -> J p a -> J S a -> (J r a, J o a)) - -> (J S :*: J p :*: J x :*: J (CollPoint x z u)) a + => (J x a -> J x a -> J z a -> J u a -> J p a -> J (JV Id) a -> (J r a, J o a)) + -> (J (JV Id) :*: J p :*: J x :*: J (CollPoint x z u)) a -> (J r :*: J o) a dynamicsFunction dae (t :*: parm :*: x' :*: collPoint) = r :*: o @@ -606,8 +634,8 @@ -- path constraints pathConFunction :: forall x z u p o h a . (View x, View z, View u, View o, View h, Viewable a) - => (J x a -> J z a -> J u a -> J p a -> J o a -> J S a -> J h a) - -> (J S :*: J p :*: J o :*: J (CollPoint x z u)) a + => (J x a -> J z a -> J u a -> J p a -> J o a -> J (JV Id) a -> J h a) + -> (J (JV Id) :*: J p :*: J o :*: J (CollPoint x z u)) a -> J h a pathConFunction pathC (t :*: parm :*: o :*: collPoint) = pathC x z u parm o t @@ -617,12 +645,13 @@ -- return dynamics constraints, outputs, and interpolated state dynStageConstraints :: forall x z u p r o deg . (Dim deg, View x, View z, View u, View p, View r, View o) - => Vec (TV.Succ deg) (Vec (TV.Succ deg) Double) -> Vec deg Double - -> SXFun (J S :*: J p :*: J x :*: J (CollPoint x z u)) + => (J x MX -> Vec deg (J x MX) -> J x MX) + -> Vec (TV.Succ deg) (Vec (TV.Succ deg) Double) + -> SXFun (J (JV Id) :*: J p :*: J x :*: J (CollPoint x z u)) (J r :*: J o) - -> (J x :*: J (JVec deg (JTuple x z)) :*: J (JVec deg u) :*: J S :*: J p :*: J (JVec deg S)) MX + -> (J x :*: J (JVec deg (JTuple x z)) :*: J (JVec deg u) :*: J (JV Id) :*: J p :*: J (JVec deg (JV Id))) MX -> (J (JVec deg r) :*: J x :*: J (JVec deg o)) MX -dynStageConstraints cijs taus dynFun (x0 :*: xzs' :*: us' :*: UnsafeJ h :*: p :*: stageTimes') = +dynStageConstraints interpolate' cijs dynFun (x0 :*: xzs' :*: us' :*: h :*: p :*: stageTimes') = cat (JVec dynConstrs) :*: xnext :*: cat (JVec outputs) where xzs = fmap split (unJVec (split xzs')) :: Vec deg (JTuple x z MX) @@ -630,7 +659,7 @@ -- interpolated final state xnext :: J x MX - xnext = interpolate taus x0 xs + xnext = interpolate' x0 xs stageTimes = unJVec $ split stageTimes' @@ -639,7 +668,7 @@ outputs :: Vec deg (J o MX) (dynConstrs, outputs) = TV.tvunzip $ TV.tvzipWith4 applyDae xdots xzs us stageTimes - applyDae :: J x MX -> JTuple x z MX -> J u MX -> J S MX -> (J r MX, J o MX) + applyDae :: J x MX -> JTuple x z MX -> J u MX -> J (JV Id) MX -> (J r MX, J o MX) applyDae x' (JTuple x z) u t = (r, o) where r :*: o = call dynFun (t :*: p :*: x' :*: collPoint) @@ -647,14 +676,14 @@ -- state derivatives, maybe these could be useful as outputs xdots :: Vec deg (J x MX) - xdots = fmap (/ UnsafeJ h) $ interpolateXDots cijs (x0 TV.<| xs) + xdots = fmap (`M.vs` (1/h)) $ interpolateXDots cijs (x0 TV.<| xs) xs :: Vec deg (J x MX) xs = fmap (\(JTuple x _) -> x) xzs data ErrorIn0 x z u p deg a = - ErrorIn0 (J x a) (J (JVec deg (CollPoint x z u)) a) (J S a) (J p a) (J (JVec deg S) a) + ErrorIn0 (J x a) (J (JVec deg (CollPoint x z u)) a) (J (JV Id) a) (J p a) (J (JVec deg (JV Id)) a) deriving Generic data ErrorInD sx sw sz deg a = ErrorInD (J sx a) (J sw a) (J (JVec deg (JTuple sx sz)) a) @@ -672,47 +701,49 @@ -- outputs outputFunction :: forall x z u p r o deg . (Dim deg, View x, View z, View u, View p, View r, View o) - => Vec (TV.Succ deg) (Vec (TV.Succ deg) Double) -> Vec deg Double - -> SXFun (J S :*: J p :*: J x :*: J (CollPoint x z u)) + => (J x MX -> Vec deg (J x MX) -> J x MX) + -> Vec (TV.Succ deg) (Vec (TV.Succ deg) Double) -> Vec deg Double + -> SXFun (J (JV Id) :*: J p :*: J x :*: J (CollPoint x z u)) (J r :*: J o) - -> (J (CollStage x z u deg) :*: J p :*: J S :*: J S) MX - -> (J (JVec deg r) :*: J (JVec deg x) :*: J (JVec deg o)) MX -outputFunction cijs taus dynFun (collStage :*: p :*: h'@(UnsafeJ h) :*: k) = - cat (JVec dynConstrs) :*: cat (JVec xdots) :*: cat (JVec outputs) + -> (J (CollStage x z u deg) :*: J p :*: J (JV Id) :*: J (JV Id)) MX + -> (J (JVec deg r) :*: J (JVec deg x) :*: J (JVec deg o) :*: J x) MX +outputFunction callInterpolate cijs taus dynFun (collStage :*: p :*: h :*: k) = + cat (JVec dynConstrs) :*: cat (JVec xdots) :*: cat (JVec outputs) :*: xnext where xzus = unJVec (split xzus') :: Vec deg (J (CollPoint x z u) MX) CollStage x0 xzus' = split collStage -- times at each collocation point - stageTimes :: Vec deg (J S MX) - stageTimes = fmap (\tau -> t0 + realToFrac tau * h') taus - t0 = k*h' + stageTimes :: Vec deg (J (JV Id) MX) + stageTimes = fmap (\tau -> t0 + realToFrac tau * h) taus + t0 = k*h + xnext = callInterpolate x0 xs + -- dae constraints (dynamics) dynConstrs :: Vec deg (J r MX) outputs :: Vec deg (J o MX) (dynConstrs, outputs) = TV.tvunzip $ TV.tvzipWith3 applyDae xdots xzus stageTimes - applyDae :: J x MX -> J (CollPoint x z u) MX -> J S MX -> (J r MX, J o MX) + applyDae :: J x MX -> J (CollPoint x z u) MX -> J (JV Id) MX -> (J r MX, J o MX) applyDae x' xzu t = (r, o) where r :*: o = call dynFun (t :*: p :*: x' :*: xzu) -- state derivatives, maybe these could be useful as outputs xdots :: Vec deg (J x MX) - xdots = fmap (/ UnsafeJ h) $ interpolateXDots cijs (x0 TV.<| xs) + xdots = fmap (`M.vs` (1/h)) $ interpolateXDots cijs (x0 TV.<| xs) xs :: Vec deg (J x MX) xs = fmap ((\(CollPoint x _ _) -> x) . split) xzus - -- return dynamics constraints, outputs, and interpolated state pathStageConstraints :: forall x z u p o h deg . (Dim deg, View x, View z, View u, View p, View o, View h) - => SXFun (J S :*: J p :*: J o :*: J (CollPoint x z u)) + => SXFun (J (JV Id) :*: J p :*: J o :*: J (CollPoint x z u)) (J h) - -> (J p :*: J (JVec deg S) :*: J (JVec deg o) :*: J (JVec deg (CollPoint x z u))) MX + -> (J p :*: J (JVec deg (JV Id)) :*: J (JVec deg o) :*: J (JVec deg (CollPoint x z u))) MX -> J (JVec deg h) MX pathStageConstraints pathCFun (p :*: stageTimes' :*: outputs :*: collPoints) = @@ -725,7 +756,7 @@ hs :: Vec deg (J h MX) hs = TV.tvzipWith3 applyH cps stageTimes (unJVec (split outputs)) - applyH :: CollPoint x z u MX -> J S MX -> J o MX -> J h MX + applyH :: CollPoint x z u MX -> J (JV Id) MX -> J o MX -> J h MX applyH (CollPoint x z u) t o = pathc' where pathc' = call pathCFun (t :*: p :*: o :*: collPoint) @@ -734,11 +765,11 @@ stageFunction :: forall x z u p o r h deg . (Dim deg, View x, View z, View u, View p, View r, View o, View h) - => MXFun (J p :*: J (JVec deg S) :*: J (JVec deg o) :*: J (JVec deg (CollPoint x z u))) + => MXFun (J p :*: J (JVec deg (JV Id)) :*: J (JVec deg o) :*: J (JVec deg (CollPoint x z u))) (J (JVec deg h)) - -> ((J x :*: J (JVec deg (JTuple x z)) :*: J (JVec deg u) :*: J S :*: J p :*: J (JVec deg S)) MX + -> ((J x :*: J (JVec deg (JTuple x z)) :*: J (JVec deg u) :*: J (JV Id) :*: J p :*: J (JVec deg (JV Id))) MX -> (J (JVec deg r) :*: J x :*: J (JVec deg o)) MX) - -> (J S :*: J p :*: J (JVec deg S) :*: J x :*: J (JVec deg (JTuple x z)) :*: J (JVec deg u)) MX + -> (J (JV Id) :*: J p :*: J (JVec deg (JV Id)) :*: J x :*: J (JVec deg (JTuple x z)) :*: J (JVec deg u)) MX -> (J (JVec deg r) :*: J (JVec deg o) :*: J (JVec deg h) :*: J x) MX stageFunction pathConStageFun dynStageCon (dt :*: parm :*: stageTimes :*: x0' :*: xzs' :*: us) = @@ -770,7 +801,7 @@ -> p Double -> CollTraj x z u p n deg (Vector Double) makeGuess quadratureRoots tf guessX guessZ guessU parm = - CollTraj (jfill tf) (v2j parm) guesses (v2j (guessX tf)) + CollTraj (jfill tf) (catJV parm) guesses (catJV (guessX tf)) where -- timestep dt = tf / fromIntegral n @@ -786,8 +817,8 @@ mkGuess' :: (Double, Vec deg Double) -> CollStage (JV x) (JV z) (JV u) deg (Vector Double) mkGuess' (t,ts) = - CollStage (v2j (guessX t)) $ - cat $ JVec $ fmap (\t' -> cat (CollPoint (v2j (guessX t')) (v2j (guessZ t')) (v2j (guessU t')))) ts + CollStage (catJV (guessX t)) $ + cat $ JVec $ fmap (\t' -> cat (CollPoint (catJV (guessX t')) (catJV (guessZ t')) (catJV (guessU t')))) ts guesses :: J (JVec n (CollStage (JV x) (JV z) (JV u) deg)) (Vector Double) guesses = cat $ JVec $ fmap (cat . mkGuess') times @@ -797,10 +828,6 @@ taus = mkTaus quadratureRoots - v2j :: Vectorize v => v Double -> J (JV v) (Vector Double) - v2j = mkJ . vectorize - - -- | make an initial guess makeGuessSim :: forall x z u p deg n . @@ -813,7 +840,7 @@ -> p Double -> CollTraj x z u p n deg (Vector Double) makeGuessSim quadratureRoots tf x00 ode guessU p = - CollTraj (jfill tf) (v2j p) (cat (JVec stages)) (v2j xf) + CollTraj (jfill tf) (catJV p) (cat (JVec stages)) (catJV xf) where -- timestep dt = tf / fromIntegral n @@ -829,20 +856,17 @@ stageGuess :: x Double -> Double -> (x Double, J (CollStage (JV x) (JV z) (JV u) deg) (Vector Double)) - stageGuess x0 t0 = (integrate 1, cat (CollStage (v2j x0) points)) + stageGuess x0 t0 = (integrate 1, cat (CollStage (catJV x0) points)) where points = cat $ JVec $ fmap (toCollPoint . integrate) taus u = guessU x0 t0 f x = ode x u - toCollPoint x = cat $ CollPoint (v2j x) (v2j (fill 0 :: z Double)) (v2j u) + toCollPoint x = cat $ CollPoint (catJV x) (catJV (fill 0 :: z Double)) (catJV u) integrate localTau = rk4 f (localTau * dt) x0 -- the collocation points taus :: Vec deg Double taus = mkTaus quadratureRoots - - v2j :: Vectorize v => v Double -> J (JV v) (Vector Double) - v2j = mkJ . vectorize rk4 :: (x Double -> x Double) -> Double -> x Double -> x Double rk4 f h x0 = x0 ^+^ ((k1 ^+^ (2 *^ k2) ^+^ (2 *^ k3) ^+^ k4) ^/ 6) diff --git a/src/Dyno/DirectCollocation/Integrate.hs b/src/Dyno/DirectCollocation/Integrate.hs --- a/src/Dyno/DirectCollocation/Integrate.hs +++ b/src/Dyno/DirectCollocation/Integrate.hs @@ -8,18 +8,26 @@ ( withIntegrator ) where +import GHC.Generics ( Generic ) + import qualified Control.Concurrent as CC import Control.Monad ( void, forever ) -import GHC.Generics ( Generic ) import Data.Proxy ( Proxy(..) ) import Data.Vector ( Vector ) -import qualified Data.Vector as V import qualified Data.Foldable as F import Linear.V -import Dyno.SXElement ( SXElement, sxToSXElement ) -import Dyno.View -import Dyno.Vectorize ( Vectorize(..), vzipWith ) +import Casadi.MX ( MX ) + +import Dyno.SXElement ( SXElement, sxSplitJV, sxCatJV ) +import Dyno.View.JV ( JV, splitJV, catJV ) +import Dyno.View.Viewable ( Viewable ) +import Dyno.View.View ( View(..), J, JNone, JTuple(..), jfill ) +import Dyno.View.Fun ( SXFun, call, toSXFun, toMXFun, expandMXFun ) +import Dyno.View.JVec ( JVec(..), jreplicate ) +import Dyno.View.HList ( (:*:)(..) ) +import qualified Dyno.View.M as M +import Dyno.Vectorize ( Vectorize(..), Id(..), vzipWith ) import Dyno.TypeVecs ( Vec ) import qualified Dyno.TypeVecs as TV import Dyno.LagrangePolynomials ( lagrangeDerivCoeffs ) @@ -37,7 +45,7 @@ } deriving (Generic) data IntegratorP u p n deg a = IntegratorP - { ipTf :: J S a + { ipTf :: J (JV Id) a , ipParm :: J (JV p) a , ipU :: J (JVec n (JVec deg (JV u))) a } deriving (Generic) @@ -56,8 +64,9 @@ => View (IntegratorG x r n deg) -dot :: forall x deg a b. (Fractional (J x a), Real b) => Vec deg b -> Vec deg (J x a) -> J x a -dot cks xs = F.sum $ TV.unSeq elemwise +-- todo: code duplication +dot :: forall x deg a b. (Fractional (J x a), Real b, Dim deg) => Vec deg b -> Vec deg (J x a) -> J x a +dot cks xs = F.sum $ TV.unVec elemwise where elemwise :: Vec deg (J x a) elemwise = TV.tvzipWith smul cks xs @@ -66,7 +75,8 @@ smul x y = realToFrac x * y -interpolateXDots' :: (Real b, Fractional (J x a)) => Vec deg (Vec deg b) -> Vec deg (J x a) -> Vec deg (J x a) +-- todo: code duplication +interpolateXDots' :: (Real b, Fractional (J x a), Dim deg) => Vec deg (Vec deg b) -> Vec deg (J x a) -> Vec deg (J x a) interpolateXDots' cjks xs = fmap (`dot` xs) cjks interpolateXDots :: @@ -81,10 +91,10 @@ dynStageConstraints' :: forall x z u p r deg . (Dim deg, View x, View z, View u, View p, View r) => Vec (TV.Succ deg) (Vec (TV.Succ deg) Double) -> Vec deg Double - -> SXFun (J S :*: J p :*: J x :*: J (CollPoint x z u)) (J r) - -> (J x :*: J (JVec deg (JTuple x z)) :*: J (JVec deg u) :*: J S :*: J p :*: J (JVec deg S)) MX + -> SXFun (J (JV Id) :*: J p :*: J x :*: J (CollPoint x z u)) (J r) + -> (J x :*: J (JVec deg (JTuple x z)) :*: J (JVec deg u) :*: J (JV Id) :*: J p :*: J (JVec deg (JV Id))) MX -> (J (JVec deg r) :*: J x) MX -dynStageConstraints' cijs taus dynFun (x0 :*: xzs' :*: us' :*: UnsafeJ h :*: p :*: stageTimes') = +dynStageConstraints' cijs taus dynFun (x0 :*: xzs' :*: us' :*: h :*: p :*: stageTimes') = cat (JVec dynConstrs) :*: xnext where xzs = fmap split (unJVec (split xzs')) :: Vec deg (JTuple x z MX) @@ -100,7 +110,7 @@ dynConstrs :: Vec deg (J r MX) dynConstrs = TV.tvzipWith4 applyDae xdots xzs us stageTimes - applyDae :: J x MX -> JTuple x z MX -> J u MX -> J S MX -> J r MX + applyDae :: J x MX -> JTuple x z MX -> J u MX -> J (JV Id) MX -> J r MX applyDae x' (JTuple x z) u t = r where r = call dynFun (t :*: p :*: x' :*: collPoint) @@ -108,7 +118,7 @@ -- state derivatives, maybe these could be useful as outputs xdots :: Vec deg (J x MX) - xdots = fmap (/ UnsafeJ h) $ interpolateXDots cijs (x0 TV.<| xs) + xdots = fmap (`M.vs` (1/h)) $ interpolateXDots cijs (x0 TV.<| xs) xs :: Vec deg (J x MX) xs = fmap (\(JTuple x _) -> x) xzs @@ -117,8 +127,8 @@ -- dynamics residual and outputs dynamicsFunction' :: forall x z u p r a . (View x, View z, View u, View r, Viewable a) - => (J x a -> J x a -> J z a -> J u a -> J p a -> J S a -> J r a) - -> (J S :*: J p :*: J x :*: J (CollPoint x z u)) a + => (J x a -> J x a -> J z a -> J u a -> J p a -> J (JV Id) a -> J r a) + -> (J (JV Id) :*: J p :*: J x :*: J (CollPoint x z u)) a -> J r a dynamicsFunction' dae (t :*: parm :*: x' :*: collPoint) = dae x' x z u parm t where @@ -153,7 +163,7 @@ dynFun <- toSXFun "dynamics" $ dynamicsFunction' $ \x0 x1 x2 x3 x4 x5 -> let r = dae (sxSplitJV x0) (sxSplitJV x1) (sxSplitJV x2) (sxSplitJV x3) - (sxSplitJV x4) (sxToSXElement (unJ x5)) + (sxSplitJV x4) (unId (sxSplitJV x5)) in sxCatJV r dynStageConFun <- toMXFun "dynamicsStageCon" (dynStageConstraints' cijs taus dynFun) @@ -162,7 +172,7 @@ let fg :: J (IntegratorX x z n deg) MX -> J (IntegratorP u p n deg) MX - -> (J S MX, J (IntegratorG x r n deg) MX) + -> (J (JV Id) MX, J (IntegratorG x r n deg) MX) fg = getFgIntegrator taus callDynStageConFun scaleX = Nothing @@ -188,7 +198,7 @@ toParams us p tf = cat $ IntegratorP - { ipTf = mkJ (V.singleton tf) + { ipTf = catJV (Id tf) , ipParm = catJV p , ipU = case us of Left u -> jreplicate (jreplicate (catJV u)) @@ -251,10 +261,10 @@ forall x z u p r n deg . (Dim deg, Dim n, Vectorize x, Vectorize z, Vectorize u, Vectorize p, Vectorize r) => Vec deg Double - -> ((J (JV x) :*: J (JVec deg (JTuple (JV x) (JV z))) :*: J (JVec deg (JV u)) :*: J S :*: J (JV p) :*: J (JVec deg S)) MX -> (J (JVec deg (JV r)) :*: J (JV x)) MX) + -> ((J (JV x) :*: J (JVec deg (JTuple (JV x) (JV z))) :*: J (JVec deg (JV u)) :*: J (JV Id) :*: J (JV p) :*: J (JVec deg (JV Id))) MX -> (J (JVec deg (JV r)) :*: J (JV x)) MX) -> J (IntegratorX x z n deg) MX -> J (IntegratorP u p n deg) MX - -> (J S MX, J (IntegratorG x r n deg) MX) + -> (J (JV Id) MX, J (IntegratorG x r n deg) MX) getFgIntegrator taus stageFun ix' ip' = (0, cat g) where ix = split ix' @@ -276,10 +286,10 @@ n = reflectDim (Proxy :: Proxy n) -- times at each collocation point - times :: Vec n (Vec deg (J S MX)) + times :: Vec n (Vec deg (J (JV Id) MX)) times = fmap snd $ timesFromTaus 0 (fmap realToFrac taus) dt - times' :: Vec n (J (JVec deg S) MX) + times' :: Vec n (J (JVec deg (JV Id)) MX) times' = fmap (cat . JVec) times -- initial point at each stage @@ -294,7 +304,7 @@ { igCollPoints = cat $ JVec dcs , igContinuity = cat $ JVec integratorMatchingConstraints } - integratorMatchingConstraints :: Vec n (J (JV x) MX) -- THIS SHOULD BE A NONLINEAR FUNCTION + integratorMatchingConstraints :: Vec n (J (JV x) MX) -- todo: THIS SHOULD BE A NONLINEAR FUNCTION integratorMatchingConstraints = vzipWith (-) interpolatedXs xfs dcs :: Vec n (J (JVec deg (JV r)) MX) @@ -302,7 +312,7 @@ (dcs, interpolatedXs) = TV.tvunzip $ TV.tvzipWith3 fff spstages us times' fff :: CollStage (JV x) (JV z) JNone deg MX -> J (JVec deg (JV u)) MX - -> J (JVec deg S) MX + -> J (JVec deg (JV Id)) MX -> (J (JVec deg (JV r)) MX, J (JV x) MX) fff (CollStage x0' xzs') us' stageTimes = (dc, interpolatedX') where diff --git a/src/Dyno/DirectCollocation/Profile.hs b/src/Dyno/DirectCollocation/Profile.hs --- a/src/Dyno/DirectCollocation/Profile.hs +++ b/src/Dyno/DirectCollocation/Profile.hs @@ -7,11 +7,12 @@ , profile ) where +import Data.Proxy ( Proxy(..) ) import Data.Vector ( Vector ) import Linear.V ( Dim(..) ) import Dyno.View.View ( J ) -import Dyno.Vectorize ( Vectorize, Proxy(..) ) +import Dyno.Vectorize ( Vectorize ) import Dyno.Ocp ( OcpPhase ) import Dyno.Solvers ( NlpSolverStuff ) import Dyno.DirectCollocation.Types ( CollTraj, CollOcpConstraints ) diff --git a/src/Dyno/DirectCollocation/Quadratures.hs b/src/Dyno/DirectCollocation/Quadratures.hs --- a/src/Dyno/DirectCollocation/Quadratures.hs +++ b/src/Dyno/DirectCollocation/Quadratures.hs @@ -2,6 +2,7 @@ {-# Language ScopedTypeVariables #-} {-# Language FlexibleContexts #-} {-# Language DeriveGeneric #-} +{-# Language PolyKinds #-} module Dyno.DirectCollocation.Quadratures ( QuadratureRoots(..) @@ -12,6 +13,7 @@ ) where import GHC.Generics ( Generic ) + import Data.Proxy ( Proxy(..) ) import qualified Data.Vector as V import qualified Data.Foldable as F @@ -20,7 +22,7 @@ import JacobiRoots ( shiftedLegendreRoots ) --, shiftedRadauRoots ) -import Dyno.View +import Dyno.View.View ( View, J ) import Dyno.TypeVecs ( Vec ) import qualified Dyno.TypeVecs as TV import Dyno.LagrangePolynomials ( lagrangeXis ) @@ -42,8 +44,9 @@ Radau -> error "radau not yet supported" -- shiftedRadauRoots (deg-1) ++ [1.0] -dot :: forall x deg a b. (Fractional (J x a), Real b) => Vec deg b -> Vec deg (J x a) -> J x a -dot cks xs = F.sum $ TV.unSeq elemwise +-- todo: code duplication +dot :: forall x deg a b. (Fractional (J x a), Real b, Dim deg) => Vec deg b -> Vec deg (J x a) -> J x a +dot cks xs = F.sum $ TV.unVec elemwise where elemwise :: Vec deg (J x a) elemwise = TV.tvzipWith smul cks xs @@ -52,6 +55,7 @@ smul x y = realToFrac x * y +-- todo: code duplication interpolate :: (Dim deg, Real b, Fractional b, Fractional (J x a), View x) => Vec deg b -> J x a -> Vec deg (J x a) -> J x a interpolate taus x0 xs = dot (TV.mkVec' xis) (x0 TV.<| xs) diff --git a/src/Dyno/DirectCollocation/Reify.hs b/src/Dyno/DirectCollocation/Reify.hs deleted file mode 100644 --- a/src/Dyno/DirectCollocation/Reify.hs +++ /dev/null @@ -1,104 +0,0 @@ -{-# OPTIONS_GHC -Wall #-} -{-# Language RankNTypes #-} -{-# Language ScopedTypeVariables #-} - -module Dyno.DirectCollocation.Reify - ( reifyCollTraj - , reifyCollTrajCov - ) where - -import Linear.V ( Dim ) - -import Dyno.Vectorize -import Dyno.TypeVecs ( Vec ) -import Dyno.View.JV -import Dyno.View.View -import Dyno.View.Viewable -import qualified Dyno.TypeVecs as TV -import Dyno.DirectCollocation.Types - --- TODO: re-enable the check on output dimension -reifyCollTraj - :: forall a r x' z' u' p' o' . - Viewable a - => (Int,Int,Int,Int,Int,Int,Int) - -> J (CollTraj x' z' u' p' () ()) a - -> Vec () (Vec () (J o' a, J x' a)) - -> (forall x z u p o n deg . - (Vectorize x, Vectorize z, Vectorize u, Vectorize p, Vectorize o, Dim n, Dim deg) - => J (CollTraj x z u p n deg) a -> Vec n (Vec deg (J (JV o) a, J (JV x) a)) -> r) - -> r -reifyCollTraj (nx,nz,nu,np,no,n,deg) (UnsafeJ x) outputs f - | ntotal /= ntotal' = - error $ "reifyCollTraj stages dimension mismatch, " ++ - "expected: " ++ show ntotal ++ - "actual : " ++ show ntotal' --- | nOutsTotal /= nOutsTotal' = --- error $ "reifyCollTraj outputs dimension mismatch, " ++ --- "expected: " ++ show nOutsTotal ++ --- "actual : " ++ show nOutsTotal' - | otherwise = - TV.reifyDim nx $ \(Proxy :: Proxy nx) -> - TV.reifyDim nz $ \(Proxy :: Proxy nz) -> - TV.reifyDim nu $ \(Proxy :: Proxy nu) -> - TV.reifyDim np $ \(Proxy :: Proxy np) -> - TV.reifyDim no $ \(Proxy :: Proxy no) -> - TV.reifyDim n $ \(Proxy :: Proxy n) -> - TV.reifyDim deg $ \(Proxy :: Proxy deg) -> - f - (mkJ x :: J (CollTraj (Vec nx) (Vec nz) (Vec nu) (Vec np) n deg) a) - (unsafeCastDim (fmap (unsafeCastDim . fmap (\(o,x') -> (unsafeToVec o, unsafeToVec x'))) outputs) - :: Vec n (Vec deg (J (JV (Vec no)) a, J (JV (Vec nx)) a))) - where - ntotal = 1 + np + n*(nx + deg*(nx + nz + nu)) + nx - ntotal' = vsize1 x - --- nOutsTotal = n*deg*no :: Int --- nOutsTotal' = :: Int --vsize1 outs - --- TODO: re-enable the check on output dimension -reifyCollTrajCov - :: forall a r x' z' u' p' o' sx' . - Viewable a - => (Int,Int,Int,Int,Int,Int,Int,Int) - -> J (CollTrajCov sx' x' z' u' p' () ()) a - -> Vec () (Vec () (J o' a, J x' a)) - -> (forall x z u p o sx n deg . - (Vectorize x, Vectorize z, Vectorize u, Vectorize p, Vectorize o, Vectorize sx, Dim n, Dim deg) - => J (CollTrajCov sx x z u p n deg) a -> Vec n (Vec deg (J (JV o) a, J (JV x) a)) -> r) - -> r -reifyCollTrajCov (nsx,nx,nz,nu,np,no,n,deg) (UnsafeJ x) outputs f - | ntotal /= ntotal' = - error $ "reifyCollTraj stages dimension mismatch, " ++ - "expected: " ++ show ntotal ++ - "actual : " ++ show ntotal' --- | nOutsTotal /= nOutsTotal' = --- error $ "reifyCollTraj outputs dimension mismatch, " ++ --- "expected: " ++ show nOutsTotal ++ --- "actual : " ++ show nOutsTotal' - | otherwise = - TV.reifyDim nx $ \(Proxy :: Proxy nx) -> - TV.reifyDim nz $ \(Proxy :: Proxy nz) -> - TV.reifyDim nu $ \(Proxy :: Proxy nu) -> - TV.reifyDim np $ \(Proxy :: Proxy np) -> - TV.reifyDim no $ \(Proxy :: Proxy no) -> - TV.reifyDim nsx $ \(Proxy :: Proxy nsx) -> - TV.reifyDim n $ \(Proxy :: Proxy n) -> - TV.reifyDim deg $ \(Proxy :: Proxy deg) -> - f - (mkJ x :: J (CollTrajCov (Vec nsx) (Vec nx) (Vec nz) (Vec nu) (Vec np) n deg) a) - (unsafeCastDim (fmap (unsafeCastDim . fmap (\(o,x') -> (unsafeToVec o, unsafeToVec x'))) outputs) - :: Vec n (Vec deg (J (JV (Vec no)) a, J (JV (Vec nx)) a))) - where - ncov = (nsx*nsx + nsx) `div` 2 - ntotal = 1 + ncov + np + n*(nx + deg*(nx + nz + nu)) + nx - ntotal' = vsize1 x - --- nOutsTotal = n*deg*no :: Int --- nOutsTotal' = :: Int --vsize1 outs - -unsafeToVec :: (Viewable a, Dim no) => J dummy a -> J (JV (Vec no)) a -unsafeToVec (UnsafeJ x) = mkJ x - -unsafeCastDim :: Dim no => Vec () a -> Vec no a -unsafeCastDim = TV.mkSeq . TV.unSeq diff --git a/src/Dyno/DirectCollocation/Robust.hs b/src/Dyno/DirectCollocation/Robust.hs --- a/src/Dyno/DirectCollocation/Robust.hs +++ b/src/Dyno/DirectCollocation/Robust.hs @@ -3,6 +3,7 @@ {-# Language TypeOperators #-} {-# Language DeriveGeneric #-} {-# Language FlexibleContexts #-} +{-# LANGUAGE PolyKinds #-} module Dyno.DirectCollocation.Robust ( CovarianceSensitivities(..) @@ -19,21 +20,25 @@ import qualified Data.Traversable as T import Linear.V -import Casadi.MX ( d2m ) +import Casadi.MX ( MX ) +import Casadi.SX ( SX ) +import Casadi.DMatrix ( DMatrix ) -import Dyno.SXElement ( SXElement, sxToSXElement ) -import Dyno.View.CasadiMat as CM -import Dyno.Cov -import Dyno.View.View -import Dyno.View.JV ( JV(..), sxSplitJV, sxCatJV ) +import qualified Dyno.View.Unsafe.M as M ( mkM, blockSplit ) + +import Dyno.SXElement ( SXElement, sxSplitJV, sxCatJV ) +import Dyno.View.View ( View(..), J, JNone(..), JTuple(..), fromDMatrix ) +import Dyno.View.JV ( JV, catJV', splitJV' ) import Dyno.View.HList ( (:*:)(..) ) +import Dyno.View.Cov ( Cov, toMat, fromMat ) import Dyno.View.Fun import Dyno.View.Viewable ( Viewable ) import qualified Dyno.View.M as M import Dyno.View.M ( M ) +import Dyno.View.JVec ( JVec(..) ) import Dyno.View.FunJac -import Dyno.View.Scheme ( Scheme, blockSplit ) -import Dyno.Vectorize ( Vectorize(..), Id, vzipWith4 ) +import Dyno.View.Scheme ( Scheme ) +import Dyno.Vectorize ( Vectorize(..), Id(..), vzipWith4 ) import Dyno.TypeVecs ( Vec ) import qualified Dyno.TypeVecs as TV import Dyno.LagrangePolynomials ( lagrangeDerivCoeffs ) @@ -80,7 +85,7 @@ \x0 x1 x2 x3 x4 x5 x6 x7 x8 x9 -> let r = covDae (sxSplitJV x0) (sxSplitJV x1) (sxSplitJV x2) (sxSplitJV x3) (sxSplitJV x4) - (sxToSXElement (unJ x5)) (sxSplitJV x6) (sxSplitJV x7) (sxSplitJV x8) (sxSplitJV x9) + (unId (sxSplitJV x5)) (sxSplitJV x6) (sxSplitJV x7) (sxSplitJV x8) (sxSplitJV x9) in sxCatJV r edscf <- toMXFun "errorDynamicsStageCon" (errorDynStageConstraints cijs taus errorDynFun) @@ -89,9 +94,9 @@ sensitivityStageFun' <- toMXFun "sensitivity stage function" $ sensitivityStageFunction (call errorDynStageConFunJac) sensitivityStageFun <- expandMXFun sensitivityStageFun' - let sens :: J S MX + let sens :: J (JV Id) MX -> J (JV p) MX - -> J (JVec deg S) MX + -> J (JVec deg (JV Id)) MX -> J (JV x) MX -> J (JVec deg (CollPoint (JV x) (JV z) (JV u))) MX -> (M (JV sx) (JV sx) MX, M (JV sx) (JV sw) MX) @@ -113,14 +118,14 @@ n = reflectDim (Proxy :: Proxy n) -- initial time at each collocation stage - t0s :: Vec n (J S MX) + t0s :: Vec n (J (JV Id) MX) t0s = TV.mkVec' $ take n [dt * fromIntegral k | k <- [(0::Int)..]] -- times at each collocation point - times :: Vec n (Vec deg (J S MX)) + times :: Vec n (Vec deg (J (JV Id) MX)) times = fmap (\t0 -> fmap (\tau -> t0 + realToFrac tau * dt) taus) t0s - times' :: Vec n (J (JVec deg S) MX) + times' :: Vec n (J (JVec deg (JV Id)) MX) times' = fmap (cat . JVec) times fs :: Vec n (M (JV sx) (JV sx) MX) @@ -137,7 +142,7 @@ forall z x u p sx sw n deg . (Dim deg, Dim n, Vectorize x, Vectorize z, Vectorize u, Vectorize p, Vectorize sx, Vectorize sw) - => (M (JV sx) (JV sx) MX -> M (JV sx) (JV sw) MX -> J (Cov (JV sw)) MX -> J S MX + => (M (JV sx) (JV sx) MX -> 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 @@ -163,7 +168,8 @@ (pF, covs) = T.mapAccumL ffs p0 $ TV.tvzip (M.vsplit' (csFs sensitivities)) (M.vsplit' (csWs sensitivities)) - qc = mkJ (d2m (unJ qc')) + qc :: J (Cov (JV sw)) MX + qc = fromDMatrix qc' ffs :: J (Cov (JV sx)) MX -> (M (JV sx) (JV sx) MX, M (JV sx) (JV sw) MX) @@ -182,8 +188,9 @@ return computeCovs -- toMXFun "compute all covariances" computeCovs -dot :: forall x deg a b. (Fractional (J x a), Real b) => Vec deg b -> Vec deg (J x a) -> J x a -dot cks xs = F.sum $ TV.unSeq elemwise +-- todo: code duplication +dot :: forall x deg a b. (Fractional (J x a), Real b, Dim deg) => Vec deg b -> Vec deg (J x a) -> J x a +dot cks xs = F.sum $ TV.unVec elemwise where elemwise :: Vec deg (J x a) elemwise = TV.tvzipWith smul cks xs @@ -191,8 +198,8 @@ smul :: b -> J x a -> J x a smul x y = realToFrac x * y - -interpolateXDots' :: (Real b, Fractional (J x a)) => Vec deg (Vec deg b) -> Vec deg (J x a) -> Vec deg (J x a) +-- todo: code duplication +interpolateXDots' :: (Real b, Fractional (J x a), Dim deg) => Vec deg (Vec deg b) -> Vec deg (J x a) -> Vec deg (J x a) interpolateXDots' cjks xs = fmap (`dot` xs) cjks interpolateXDots :: @@ -207,9 +214,9 @@ errorDynamicsFunction :: forall x z u p r sx sz sw a . (View x, View z, View u, View r, View sx, View sz, View sw, Viewable a) - => (J x a -> J x a -> J z a -> J u a -> J p a -> J S a + => (J x a -> J x a -> J z a -> J u a -> J p a -> J (JV Id) a -> J sx a -> J sx a -> J sz a -> J sw a -> J r a) - -> (J S :*: J p :*: J x :*: J (CollPoint x z u) :*: J sx :*: J sx :*: J sz :*: J sw) a + -> (J (JV Id) :*: J p :*: J x :*: J (CollPoint x z u) :*: J sx :*: J sx :*: J sz :*: J sw) a -> J r a errorDynamicsFunction dae (t :*: parm :*: x' :*: collPoint :*: sx' :*: sx :*: sz :*: sw) = r @@ -219,7 +226,7 @@ data ErrorIn0 x z u p deg a = - ErrorIn0 (J x a) (J (JVec deg (CollPoint x z u)) a) (J S a) (J p a) (J (JVec deg S) a) + ErrorIn0 (J x a) (J (JVec deg (CollPoint x z u)) a) (J (JV Id) a) (J p a) (J (JVec deg (JV Id)) a) deriving Generic data ErrorInD sx sw sz deg a = ErrorInD (J sx a) (J sw a) (J (JVec deg (JTuple sx sz)) a) @@ -239,12 +246,12 @@ View sr, View sw, View sz, View sx) => Vec (TV.Succ deg) (Vec (TV.Succ deg) Double) -> Vec deg Double - -> SXFun (J S :*: J p :*: J x :*: J (CollPoint x z u) :*: J sx :*: J sx :*: J sz :*: J sw) + -> SXFun (J (JV Id) :*: J p :*: J x :*: J (CollPoint x z u) :*: J sx :*: J sx :*: J sz :*: J sw) (J sr) -> JacIn (ErrorInD sx sw sz deg) (ErrorIn0 x z u p deg) MX -> JacOut (ErrorOut sr sx deg) (J JNone) MX errorDynStageConstraints cijs taus dynFun - (JacIn errorInD (ErrorIn0 x0 xzus' (UnsafeJ h) p stageTimes')) + (JacIn errorInD (ErrorIn0 x0 xzus' h p stageTimes')) = JacOut (cat (ErrorOut (cat (JVec dynConstrs)) sxnext)) (cat JNone) where ErrorInD sx0 sw0 sxzs' = split errorInD @@ -255,7 +262,7 @@ xs = fmap ((\(CollPoint x _ _) -> x) . split) xzus xdots :: Vec deg (J x MX) - xdots = fmap (/ UnsafeJ h) $ interpolateXDots cijs (x0 TV.<| xs) + xdots = fmap (`M.vs` (1 / h)) $ interpolateXDots cijs (x0 TV.<| xs) -- -- interpolated final state -- xnext :: J x MX @@ -273,7 +280,7 @@ applyDae :: J sx MX -> J sx MX -> J sz MX - -> J x MX -> J (CollPoint x z u) MX -> J S MX + -> J x MX -> J (CollPoint x z u) MX -> J (JV Id) MX -> J sr MX applyDae sx' sx sz x' xzu t = call dynFun @@ -281,7 +288,7 @@ -- error state derivatives sxdots :: Vec deg (J sx MX) - sxdots = fmap (/ UnsafeJ h) $ interpolateXDots cijs (sx0 TV.<| sxs) + sxdots = fmap (`M.vs` (1/h)) $ interpolateXDots cijs (sx0 TV.<| sxs) sxs :: Vec deg (J sx MX) szs :: Vec deg (J sz MX) @@ -291,7 +298,7 @@ continuousToDiscreetNoiseApprox :: (View sx, View sw) - => M sx sx MX -> M sx sw MX -> J (Cov sw) MX -> J S MX -> M sx sx MX + => M sx sx MX -> M sx sw MX -> J (Cov sw) MX -> J (JV Id) MX -> M sx sx MX continuousToDiscreetNoiseApprox _dsx1_dsx0 dsx1_dsw0 qs h = qd where -- Qs' = G * Qs * G.T @@ -305,8 +312,8 @@ propOneCov :: forall sx sw . (View sx, View sw) - => (M sx sx MX -> M sx sw MX -> J (Cov sw) MX -> J S MX -> M sx sx MX) - -> (M sx sx :*: M sx sw :*: J (Cov sx) :*: J (Cov sw) :*: J S) MX + => (M sx sx MX -> M sx sw MX -> J (Cov sw) MX -> J (JV Id) MX -> M sx sx MX) + -> (M sx sx :*: M sx sw :*: J (Cov sx) :*: J (Cov sw) :*: J (JV Id)) MX -> J (Cov sx) MX propOneCov c2d (dsx1_dsx0 :*: dsx1_dsw0 :*: p0 :*: qs :*: h) = fromMat p1 where @@ -321,7 +328,7 @@ . (Dim deg, View x, View z, View u, View p, View sx, View sz, View sw, View sr) => (JacIn (ErrorInD sx sw sz deg) (ErrorIn0 x z u p deg) MX -> Jac (ErrorInD sx sw sz deg) (ErrorOut sr sx deg) (J JNone) MX) - -> (J S :*: J p :*: J (JVec deg S) :*: J x :*: J (JVec deg (CollPoint x z u))) MX + -> (J (JV Id) :*: J p :*: J (JVec deg (JV Id)) :*: J x :*: J (JVec deg (CollPoint x z u))) MX -> (M sx sx :*: M sx sw) MX sensitivityStageFunction dynStageConJac (dt :*: parm :*: stageTimes :*: x0' :*: xzus') = dsx1_dsx0 :*: dsx1_dsw0 @@ -345,7 +352,7 @@ dg_dsw0 :: M sx sw MX dg_dsxz :: M sx (JVec deg (JTuple sx sz)) MX ((df_dsx0, df_dsw0, df_dsxz), (dg_dsx0, dg_dsw0, dg_dsxz)) = - case fmap F.toList (F.toList (blockSplit mat)) of + case fmap F.toList (F.toList (M.blockSplit mat)) of [[x00,x01,x02],[x10,x11,x12]] -> ((M.mkM x00, M.mkM x01, M.mkM x02), (M.mkM x10, M.mkM x11, M.mkM x12)) _ -> error "stageFunction: got wrong number of elements in jacobian" @@ -416,7 +423,7 @@ let gogo :: J (JV shr) MX -> J (JV p) MX -> J (JV x) MX -> J (Cov (JV sx)) MX -> J (JV shr) MX gogo gammas' theta x pe' = rcs' where - gammas = fmap mkJ (unJV (split gammas')) :: shr (J (JV Id) MX) + gammas = splitJV' gammas' :: shr (J (JV Id) MX) jHx :: M (JV shr) (JV x) MX jHe :: M (JV shr) (JV sx) MX @@ -444,10 +451,10 @@ jHes :: shr (M.M (JV Id) (JV sx) MX) jHes = M.vsplit jHe - shr' = fmap mkJ (unJV (split h0vec)) :: shr (J (JV Id) MX) + shr' = splitJV' h0vec :: shr (J (JV Id) MX) rcs' :: J (JV shr) MX - rcs' = cat $ JV $ fmap unsafeUnJ rcs + rcs' = catJV' rcs rcs :: shr (J (JV Id) MX) rcs = vzipWith4 robustify gammas shr' jHxs jHes @@ -457,12 +464,10 @@ -> M.M (JV Id) (JV x) MX -> M.M (JV Id) (JV sx) MX -> J (JV Id) MX - robustify gamma h0 gHx gHe = h0 + gamma * sqrt sigma2 + robustify gamma h0 gHx gHe = h0 + gamma * sqrt (M.uncol sigma2) where - sigma2 :: J (JV Id) MX - sigma2 = mkJ sigma2' - - M.UnsafeM sigma2' = + sigma2 :: M.M (JV Id) (JV Id) MX + sigma2 = gHx `M.mm` fpef `M.mm` (M.trans gHx) + 2 * gHx `M.mm` fpe `M.mm` (M.trans gHe) + gHe `M.mm` pe `M.mm` (M.trans gHe) diff --git a/src/Dyno/DirectCollocation/Types.hs b/src/Dyno/DirectCollocation/Types.hs --- a/src/Dyno/DirectCollocation/Types.hs +++ b/src/Dyno/DirectCollocation/Types.hs @@ -1,7 +1,7 @@ {-# OPTIONS_GHC -Wall #-} {-# Language ScopedTypeVariables #-} -{-# Language FlexibleContexts #-} {-# Language DeriveGeneric #-} +{-# Language PolyKinds #-} module Dyno.DirectCollocation.Types ( CollTraj(..) @@ -19,21 +19,22 @@ , getXzus ) where -import qualified Data.Foldable as F -import Data.Serialize ( Serialize ) import GHC.Generics ( Generic ) + +import qualified Data.Foldable as F import Linear.V ( Dim(..) ) import Data.Vector ( Vector ) -import Dyno.View ( View(..), J, JVec(..), S, mkJ, unJ, jfill, jreplicate ) -import Dyno.View.JV ( JV, splitJV ) -import Dyno.Vectorize ( Vectorize(..) ) -import Dyno.Cov ( Cov ) +import Dyno.View.View ( View(..), J, jfill ) +import Dyno.View.JVec ( JVec(..), jreplicate ) +import Dyno.View.Cov ( Cov ) +import Dyno.View.JV ( JV, splitJV, catJV ) +import Dyno.Vectorize ( Vectorize(..), Id ) -- design variables data CollTraj x z u p n deg a = - CollTraj (J S a) (J (JV p) a) (J (JVec n (CollStage (JV x) (JV z) (JV u) deg)) a) (J (JV x) a) + 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 @@ -70,12 +71,6 @@ , cocSbc :: J sc a } deriving (Eq, Generic, Show) --- serialize instances -instance Serialize a => Serialize (CollPoint x z u a) -instance Serialize a => Serialize (CollStage x z u deg a) -instance Serialize a => Serialize (CollTraj x z u p n deg a) -instance Serialize a => Serialize (CollTrajCov sx x z u p n deg a) - -- View instances instance (View x, View z, View u) => View (CollPoint x z u) instance (View x, View z, View u, Dim deg) => View (CollStage x z u deg) @@ -113,10 +108,10 @@ => x a -> r a -> c a -> h a -> CollOcpConstraints n deg x r c h (Vector a) fillCollConstraints x r c h = CollOcpConstraints - { coCollPoints = jreplicate $ jreplicate $ mkJ (vectorize r) - , coContinuity = jreplicate $ mkJ (vectorize x) - , coPathC = jreplicate $ jreplicate $ mkJ (vectorize h) - , coBc = mkJ (vectorize c) + { coCollPoints = jreplicate $ jreplicate $ catJV r + , coContinuity = jreplicate $ catJV x + , coPathC = jreplicate $ jreplicate $ catJV h + , coBc = catJV c } @@ -151,14 +146,14 @@ -> CollTraj x2 z2 u2 p2 n deg (Vector b) fmapCollTraj fx fz fu fp ft (CollTraj tf1 p stages1 xf) = CollTraj tf2 (fj fp p) stages2 (fj fx xf) where - tf2 :: J S (Vector b) - tf2 = mkJ $ fmap ft (unJ tf1) + tf2 :: J (JV Id) (Vector b) + tf2 = catJV $ fmap ft (splitJV tf1) stages2 = cat $ fmapJVec (fmapStage fx fz fu) (split stages1) fj :: (Vectorize f1, Vectorize f2) => (f1 a -> f2 b) -> J (JV f1) (Vector a) -> J (JV f2) (Vector b) - fj f = mkJ . vectorize . f . devectorize . unJ + 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) @@ -183,7 +178,7 @@ => (f1 a -> f2 b) -> J (JV f1) (Vector a) -> J (JV f2) (Vector b) - fj f = mkJ . vectorize . f . devectorize . unJ + fj f = catJV . f . splitJV fmapCollPoint :: forall x1 x2 z1 z2 u1 u2 a b . ( Vectorize x1, Vectorize x2 @@ -201,4 +196,4 @@ => (f1 a -> f2 b) -> J (JV f1) (Vector a) -> J (JV f2) (Vector b) - fj f = mkJ . vectorize . f . devectorize . unJ + fj f = catJV . f . splitJV diff --git a/src/Dyno/Interface/LogsAndErrors.hs b/src/Dyno/Interface/LogsAndErrors.hs deleted file mode 100644 --- a/src/Dyno/Interface/LogsAndErrors.hs +++ /dev/null @@ -1,58 +0,0 @@ -{-# OPTIONS_GHC -Wall #-} -{-# Language PackageImports #-} -{-# Language FlexibleContexts #-} - -module Dyno.Interface.LogsAndErrors - ( ErrorMessage (..) - , LogMessage (..) - , countLogs - , debug - , warn - , err - , impossible - ) where - -import "mtl" Control.Monad.Except ( MonadError, throwError ) -import "mtl" Control.Monad.Writer ( MonadWriter, tell ) - -data LogMessage = Debug String - | Warning String - | Error String - | Impossible String - -instance Show LogMessage where - show (Debug x) = "Debug: " ++ x - show (Warning x) = "Warning: " ++ x - show (Error x) = "Error: " ++ x - show (Impossible x) = "\"Impossible\" Error: " ++ x - -countLogs' :: (Int,Int,Int,Int) -> [LogMessage] -> (Int,Int,Int,Int) -countLogs' x [] = x -countLogs' (a,b,c,d) (Debug _:xs) = countLogs' (a+1, b, c, d) xs -countLogs' (a,b,c,d) (Warning _:xs) = countLogs' ( a, b+1, c, d) xs -countLogs' (a,b,c,d) (Error _:xs) = countLogs' ( a, b, c+1, d) xs -countLogs' (a,b,c,d) (Impossible _:xs) = countLogs' ( a, b, c, d+1) xs - -countLogs :: [LogMessage] -> (Int,Int,Int,Int) -countLogs = countLogs' (0,0,0,0) - -newtype ErrorMessage = ErrorMessage String -- deriving Error -instance Show ErrorMessage where - show (ErrorMessage msg) = msg - -logMessage :: MonadWriter [t] m => t -> m () -logMessage x = tell [x] - -debug :: MonadWriter [LogMessage] m => String -> m () -debug = logMessage . Debug - -warn :: MonadWriter [LogMessage] m => String -> m () -warn = logMessage . Warning - -err :: (MonadError ErrorMessage m, MonadWriter [LogMessage] m) => - String -> m a -err x = logMessage (Error x) >> throwError (ErrorMessage x) - -impossible :: (MonadError ErrorMessage m, MonadWriter [LogMessage] m) => - String -> m b -impossible x = logMessage (Impossible x) >> throwError (ErrorMessage ("\"impossible error\": " ++ x)) diff --git a/src/Dyno/Interface/Types.hs b/src/Dyno/Interface/Types.hs deleted file mode 100644 --- a/src/Dyno/Interface/Types.hs +++ /dev/null @@ -1,86 +0,0 @@ -{-# OPTIONS_GHC -Wall -ddump-splices #-} - -module Dyno.Interface.Types - ( Constraint(..) - , Objective(..) - , HomotopyParam(..) - , NlpMonadState(..) - , OcpState(..) - , DaeState(..) - , daeX - , daeXDot - , daeZ - , daeU - , daeP - , daeO - ) where - -import qualified Data.HashSet as HS -import qualified Data.Sequence as S -import qualified Data.Map as M -import Control.Lens -import Data.Functor ( (<$>) ) - -import Dyno.SXElement ( SXElement ) - -data Constraint a = Eq2 a a - | Ineq2 a a - | Ineq3 a (Double, Double) - -data Objective a = ObjectiveUnset | Objective a -data HomotopyParam a = HomotopyParamUnset | HomotopyParam a - -data NlpMonadState = - NlpMonadState - { nlpX :: S.Seq (String, SXElement) - , nlpXSet :: HS.HashSet String - , nlpConstraints :: S.Seq (Constraint SXElement) - , nlpObj :: Objective SXElement - , nlpHomoParam :: HomotopyParam SXElement - } - -data OcpState = OcpState { ocpPathConstraints :: S.Seq (Constraint SXElement) - , ocpLagrangeObj :: Objective SXElement - , ocpHomoParam :: HomotopyParam SXElement - } - -data DaeState = DaeState { _daeXDot :: S.Seq (String, SXElement) - , _daeX :: S.Seq (String, SXElement) - , _daeZ :: S.Seq (String, SXElement) - , _daeU :: S.Seq (String, SXElement) - , _daeP :: S.Seq (String, SXElement) - , _daeO :: M.Map String SXElement - , daeNameSet :: HS.HashSet String - , daeConstraints :: S.Seq (SXElement, SXElement) - } - ---makeLenses ''DaeState -daeXDot :: Lens' DaeState (S.Seq (String, SXElement)) -daeXDot f (DaeState xdot' x z u p o ss c) = - (\xdot -> DaeState xdot x z u p o ss c) <$> f xdot' -{-# INLINE daeXDot #-} - -daeX :: Lens' DaeState (S.Seq (String, SXElement)) -daeX f (DaeState xdot x' z u p o ss c) = - (\x -> DaeState xdot x z u p o ss c) <$> f x' -{-# INLINE daeX #-} - -daeZ :: Lens' DaeState (S.Seq (String, SXElement)) -daeZ f (DaeState xdot x z' u p o ss c) = - (\z -> DaeState xdot x z u p o ss c) <$> f z' -{-# INLINE daeZ #-} - -daeU :: Lens' DaeState (S.Seq (String, SXElement)) -daeU f (DaeState xdot x z u' p o ss c) = - (\u -> DaeState xdot x z u p o ss c) <$> f u' -{-# INLINE daeU #-} - -daeP :: Lens' DaeState (S.Seq (String, SXElement)) -daeP f (DaeState xdot x z u p' o ss c) = - (\p -> DaeState xdot x z u p o ss c) <$> f p' -{-# INLINE daeP #-} - -daeO :: Lens' DaeState (M.Map String SXElement) -daeO f (DaeState xdot x z u p o' ss c) = - (\o -> DaeState xdot x z u p o ss c) <$> f o' -{-# INLINE daeO #-} diff --git a/src/Dyno/LagrangePolynomials.lhs b/src/Dyno/LagrangePolynomials.lhs --- a/src/Dyno/LagrangePolynomials.lhs +++ b/src/Dyno/LagrangePolynomials.lhs @@ -113,7 +113,8 @@ import Casadi.Function ( evalDMatrix ) import Casadi.SharedObject ( soInit ) import Casadi.SX ( SX, ssym, sgradient ) -import Casadi.DMatrix ( DMatrix, ddata, ddense ) +import Casadi.DMatrix ( DMatrix, ddata ) +import Casadi.CMatrix ( dense ) import Dyno.TypeVecs @@ -280,7 +281,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 (ddense x)) of + d2d x = case V.toList (ddata (dense x)) of [y] -> y ys -> error $ "d2d: need length 1, got length " ++ show (length ys) diff --git a/src/Dyno/Models/AeroCoeffs.hs b/src/Dyno/Models/AeroCoeffs.hs deleted file mode 100644 --- a/src/Dyno/Models/AeroCoeffs.hs +++ /dev/null @@ -1,271 +0,0 @@ -{-# OPTIONS_GHC -Wall #-} -{-# Language DeriveFunctor #-} -{-# Language DeriveFoldable #-} -{-# Language DeriveGeneric #-} - -module Dyno.Models.AeroCoeffs where - -import Data.Foldable ( Foldable ) -import GHC.Generics -import Linear - -import Dyno.Server.Accessors ( Lookup(..) ) - -import Dyno.Vectorize - -atan2' :: Floating a => a -> a -> a -atan2' y x = 2 * atan (y / (sqrt(x*x + y*y + 1e-15) + x) ) - -data ControlSurfaces a = - ControlSurfaces { csElev :: a - , csRudder :: a - , csAil :: a - , csFlaps :: a - } deriving (Eq, Functor, Foldable, Generic, Generic1, Show) -instance Vectorize ControlSurfaces -instance (Lookup a, Generic a) => Lookup (ControlSurfaces a) - -data AeroForceCoeffs a = - AeroForceCoeffs { af_cL0 :: a - , af_cL_A :: a - , af_cL_elev :: a - , af_cL_flaps :: a - - , af_cD0 :: a - , af_cD_A :: a - , af_cD_A2 :: a - , af_cD_B2 :: a - - , af_cD_elev :: a - , af_cD_elev2 :: a - , af_cD_A_elev :: a - - , af_cD_flaps :: a - , af_cD_flaps2 :: a - , af_cD_A_flaps :: a - - , af_cD_rudder :: a - , af_cD_rudder2 :: a - , af_cD_B_rudder :: a - - , af_cD_ail :: a - , af_cD_ail2 :: a - , af_cD_B_ail :: a - - , af_cY_B :: a - , af_cY_rudder :: a - } deriving (Functor, Generic, Generic1, Show) -instance Vectorize AeroForceCoeffs - -data AeroMomentCoeffs a = - AeroMomentCoeffs { am_cm0 :: a - - , am_cl_p :: a - , am_cl_q :: a - , am_cl_r :: a - - , am_cm_p :: a - , am_cm_q :: a - , am_cm_r :: a - - , am_cn_p :: a - , am_cn_q :: a - , am_cn_r :: a - - , am_cl_B :: a - , am_cl_AB :: a - , am_cm_A :: a - , am_cn_B :: a - , am_cn_AB :: a - - , am_cl_ail :: a - , am_cm_elev - , am_cm_flaps :: a - , am_cn_rudder :: a - } deriving (Functor, Generic, Generic1, Show) -instance Vectorize AeroMomentCoeffs - -data AeroRefs a = - AeroRefs { ar_sref :: a - , ar_bref :: a - , ar_cref :: a - } deriving (Functor, Generic, Generic1, Show) -instance Vectorize AeroRefs - - --- | Compute aerodynamic forces/moments in the body frame. --- Parameters: --- dcm_n2b: rotation matrix rotating vectors expressed in NED to vectors expressed in body --- v_bw_b: body velocity in the wind frame, expressed in the body frame --- w_bn_b: body angular velocity w.r.t. NED -aeroForcesMoments :: Floating a => AeroForceCoeffs a -> AeroMomentCoeffs a -> AeroRefs a -> - V3 a -> V3 a -> ControlSurfaces a -> (V3 a, V3 a) -aeroForcesMoments forceCoeffs momentCoeffs refs v_bw_b w_bn_b controlSurfaces = (forces, moments) - where - V3 cL cD cY = aeroForceCoeffs alpha beta controlSurfaces forceCoeffs - c_lmn = aeroMomentCoeffs alpha beta airspeed w_bn_b controlSurfaces momentCoeffs refs - - -- alpha/beta - alpha = atan2' v_bw_b_z v_bw_b_x - beta = asin (v_bw_b_y / airspeed) - V3 v_bw_b_x v_bw_b_y v_bw_b_z = v_bw_b - - airspeedSquared = quadrance v_bw_b - airspeed = sqrt airspeedSquared - - moments = rho_sref_v2*^(V3 bref cref bref)*c_lmn - forces = dragForce + liftForce + sideForce - - dragForce = (-rho_sref_v*cD) *^ v_bw_b - liftForce = rho_sref_v*cL *^ e_b2L_b_v - sideForce = rho_sref*cY *^ e_b2Y_b_v2 - - -- y axis of aircraft expressed in body frame - e_b2y_b = V3 0 1 0 - - -- lift axis normalized to airspeed - e_b2L_b_v = cross e_b2y_b v_bw_b - - -- sideforces axis normalized to airspeed^2 - e_b2Y_b_v2 = cross e_b2L_b_v (-v_bw_b) - - rho_sref = 0.5*rho*sref - rho_sref_v2 = rho_sref*airspeedSquared - rho_sref_v = rho_sref*airspeed - - sref = ar_sref refs - bref = ar_bref refs - cref = ar_cref refs - - rho = 1.23 - -aeroForceCoeffs :: Num a => a -> a -> ControlSurfaces a -> AeroForceCoeffs a -> V3 a -aeroForceCoeffs alpha beta controlSurfaces coeffs = V3 cL cD cY - where - cL_wing = cL_A'*alpha + cL0' - cD_wing = cD_A'*alpha + cD_A2'*alpha*alpha + cD_B2'*beta*beta + cD0' - cY_wing = cY_B'*beta - - cL_elev = cL_elev' * elev - cD_elev = cD_elev2' * elev * elev + cD_A_elev' * elev * alpha + cD_elev' * elev - - cD_ail = cD_ail2'*ail*ail + cD_B_ail'*beta*ail + cD_ail'*ail - - cL_flaps = cL_flaps'*flaps - cD_flaps = cD_flaps2'*flaps*flaps + cD_A_flaps'*alpha*flaps + cD_flaps'*flaps - - cY_rudder = cY_rudder'*rudder - cD_rudder = cD_rudder2'*rudder*rudder + cD_B_rudder'*beta*rudder + cD_rudder'*rudder - - cL = cL_wing + cL_elev + cL_flaps - cD = cD_wing + cD_elev + cD_ail + cD_flaps + cD_rudder - cY = cY_wing + cY_rudder - - -- inputs - elev = csElev controlSurfaces - rudder = csRudder controlSurfaces - ail = csAil controlSurfaces - flaps = csFlaps controlSurfaces - - -- unpack aero coeffs - cL_A' = af_cL_A coeffs - cL0' = af_cL0 coeffs - cD_A' = af_cD_A coeffs - cD_A2' = af_cD_A2 coeffs - cD_B2' = af_cD_B2 coeffs - cD0' = af_cD0 coeffs - cY_rudder' = af_cY_rudder coeffs - cD_rudder2' = af_cD_rudder2 coeffs - cD_flaps2' = af_cD_flaps2 coeffs - cD_elev2' = af_cD_elev2 coeffs - cD_flaps' = af_cD_flaps coeffs - cD_A_flaps' = af_cD_A_flaps coeffs - cD_A_elev' = af_cD_A_elev coeffs - cD_elev' = af_cD_elev coeffs - cD_ail2' = af_cD_ail2 coeffs - cD_ail' = af_cD_ail coeffs - cD_B_ail' = af_cD_B_ail coeffs - cD_B_rudder' = af_cD_B_rudder coeffs - cD_rudder' = af_cD_rudder coeffs - cL_elev' = af_cL_elev coeffs - cL_flaps' = af_cL_flaps coeffs - cY_B' = af_cY_B coeffs - - -aeroMomentCoeffs :: Fractional a => a -> a -> a -> V3 a -> ControlSurfaces a -> AeroMomentCoeffs a -> AeroRefs a -> V3 a -aeroMomentCoeffs alpha beta airspeed w_bn_b controlSurfaces coeffs refs = - momentCoeffs0 + momentCoeffs_pqr + momentCoeffs_AB + momentCoeffs_surf - where - elev = csElev controlSurfaces - rudder = csRudder controlSurfaces - ail = csAil controlSurfaces - flaps = csFlaps controlSurfaces - - w_bn_b_hat = (V3 bref cref bref) * w_bn_b ^* (0.5/airspeed) - - momentCoeffs0 = V3 cm0 0 0 - - momentCoeffs_pqr = - (V3 - (V3 cl_p cl_q cl_r) - (V3 cm_p cm_q cm_r) - (V3 cn_p cn_q cn_r)) !* w_bn_b_hat - - momentCoeffs_AB = - (V3 - (V3 0 cl_B cl_AB) - (V3 cm_A 0 0) - (V3 0 cn_B cn_AB)) !* (V3 alpha beta (alpha*beta)) - - momentCoeffs_surf = - V3 - (cl_ail * ail) - (cm_elev * elev + cm_flaps * flaps) - (cn_rudder * rudder) - - -- unpack aero coeffs - cm0 = am_cm0 coeffs - - cl_p = am_cl_p coeffs - cl_q = am_cl_q coeffs - cl_r = am_cl_r coeffs - - cm_p = am_cm_p coeffs - cm_q = am_cm_q coeffs - cm_r = am_cm_r coeffs - - cn_p = am_cn_p coeffs - cn_q = am_cn_q coeffs - cn_r = am_cn_r coeffs - - cl_B = am_cl_B coeffs - cl_AB = am_cl_AB coeffs - cm_A = am_cm_A coeffs - cn_B = am_cn_B coeffs - cn_AB = am_cn_AB coeffs - - cl_ail = am_cl_ail coeffs - cm_elev = am_cm_elev coeffs - cm_flaps = am_cm_flaps coeffs - cn_rudder = am_cn_rudder coeffs - - bref = ar_bref refs - cref = ar_cref refs - -trans :: V3 (V3 a) -> V3 (V3 a) -trans (V3 - (V3 e11 e12 e13) - (V3 e21 e22 e23) - (V3 e31 e32 e33)) - = - V3 - (V3 e11 e21 e31) - (V3 e12 e22 e32) - (V3 e13 e23 e33) - -skew :: Num a => V3 a -> V3 (V3 a) -skew (V3 x y z) = - V3 - (V3 0 (-z) y ) - (V3 z 0 (-x)) - (V3 (-y) x 0 ) diff --git a/src/Dyno/Models/Aircraft.hs b/src/Dyno/Models/Aircraft.hs deleted file mode 100644 --- a/src/Dyno/Models/Aircraft.hs +++ /dev/null @@ -1,65 +0,0 @@ -{-# OPTIONS_GHC -Wall -fno-warn-orphans #-} -{-# Language ScopedTypeVariables #-} -{-# Language DeriveFunctor #-} -{-# Language DeriveGeneric #-} - -module Dyno.Models.Aircraft ( AcX(..), AcU(..), aircraftDae ) where - -import GHC.Generics -import Linear - -import Dyno.Vectorize -import Dyno.Server.Accessors ( Lookup(..) ) -import Dyno.Models.AeroCoeffs - -data AcX a = AcX { ac_r_n2b_n :: V3 a - , ac_v_bn_b :: V3 a - , ac_R_n2b :: M33 a - , ac_w_bn_b :: V3 a - , ac_u :: AcU a - } deriving (Eq, Functor, Generic, Generic1, Show) -data AcU a = AcU { acSurfaces :: ControlSurfaces a - } deriving (Eq, Functor, Generic, Generic1, Show) -newtype AcZ a = AcZ (None a) deriving (Eq, Functor, Generic, Generic1, Show) -newtype AcR a = AcR (AcX a) deriving (Eq, Functor, Generic, Generic1, Show) -newtype AcP a = AcP (None a) deriving (Eq, Functor, Generic, Generic1, Show) - -instance Vectorize AcX -instance Vectorize AcZ -instance Vectorize AcU -instance Vectorize AcP -instance Vectorize AcR - -instance (Lookup a, Generic a) => Lookup (AcX a) -instance (Lookup a, Generic a) => Lookup (AcZ a) -instance (Lookup a, Generic a) => Lookup (AcU a) -instance (Lookup a, Generic a) => Lookup (AcP a) -instance (Lookup a, Generic a) => Lookup (AcR a) - -subCs :: Num a => ControlSurfaces a -> ControlSurfaces a -> ControlSurfaces a -subCs (ControlSurfaces x0 x1 x2 x3) (ControlSurfaces y0 y1 y2 y3) = - ControlSurfaces (x0-y0) (x1-y1) (x2-y2) (x3-y3) - -aircraftDae :: forall a. Floating a => - (a, M33 a) -> AeroForceCoeffs a -> AeroMomentCoeffs a -> AeroRefs a -> - AcX a -> AcX a -> AcU a -> AcX a -aircraftDae - (mass, inertia) - forceCoeffs - momentCoeffs - refs - (AcX r_n2b_n' v_bn_b' dcm_n2b' w_bn_b' (AcU controlSurfaces')) - (AcX _ v_bn_b dcm_n2b w_bn_b (AcU controlSurfaces)) - (AcU controlSurfaces'') = daeResidual - where - v_bw_b = v_bn_b -- no relative wind - (aero_forces_body, moments_body) = aeroForcesMoments forceCoeffs momentCoeffs refs v_bw_b w_bn_b controlSurfaces - forces_body = aero_forces_body + dcm_n2b !* (V3 0 0 (9.81*mass)) - - daeResidual = - AcX { ac_r_n2b_n = (trans dcm_n2b) !* v_bn_b - r_n2b_n' - , ac_v_bn_b = v_bn_b' + cross w_bn_b v_bn_b - forces_body ^/ mass - , ac_R_n2b = (trans (skew w_bn_b)) !*! dcm_n2b - dcm_n2b' - , ac_w_bn_b = inertia !* w_bn_b' + cross w_bn_b (inertia !* w_bn_b) - moments_body - , ac_u = AcU $ subCs controlSurfaces'' controlSurfaces' - } diff --git a/src/Dyno/Models/Betty.hs b/src/Dyno/Models/Betty.hs deleted file mode 100644 --- a/src/Dyno/Models/Betty.hs +++ /dev/null @@ -1,72 +0,0 @@ -{-# OPTIONS_GHC -Wall #-} - -module Dyno.Models.Betty - ( bettyFc - , bettyMc - , bettyRefs - , bettyInertia - , bettyMass - ) where - -import Linear - -import Dyno.Models.AeroCoeffs - -bettyFc :: Floating a => AeroForceCoeffs a -bettyFc = AeroForceCoeffs - { af_cL0 = 0.203530 - , af_cL_A = 5.786876 - - , af_cD_A = 0.018751 - , af_cD_A2 = 1.529989 - , af_cD_B2 = -0.16247 - , af_cD0 = 0.008767 - - , af_cY_B = -0.239789 - - -- control surface forces - , af_cL_elev = -0.0105*180/pi - , af_cL_flaps = 0.0184*180/pi - , af_cY_rudder = 0.0035*180/pi - , af_cD_flaps2 = 3.03874e-05, af_cD_A_flaps = 0.000101404, af_cD_flaps = 0.000208995 - , af_cD_elev2 = 4.19816e-05, af_cD_A_elev = -9.79647e-05, af_cD_elev = 4.52856e-05 - , af_cD_ail2 = 5.60583e-05, af_cD_B_ail = -6.73139e-06, af_cD_ail = 0 - , af_cD_rudder2 = 2.03105e-05, af_cD_B_rudder = 5.55453e-05, af_cD_rudder = 0 - } - -bettyMc :: Floating a => AeroMomentCoeffs a -bettyMc = AeroMomentCoeffs - { am_cl_p = -0.576, am_cl_q = 0.0, am_cl_r = 0.0707 - , am_cm_p = 0.0, am_cm_q = -15.5, am_cm_r = 0.0 - , am_cn_p = -0.036, am_cn_q = 0.0, am_cn_r = -0.0667 - - , am_cl_B = -0.051808 - , am_cl_AB = -0.208344 - , am_cm_A = -0.450643 - -- cm0 valid for CG/bridle location 0.1 meters behind main wing leading edge - , am_cm0 = 0.028980 - , am_cn_B = 0.037183 - , am_cn_AB = -0.028933 - - -- control surface moments - , am_cl_ail = 0.0073*180/pi - , am_cm_elev = 0.0352*180/pi - , am_cm_flaps = 0.0026*180/pi - , am_cn_rudder = 0.001176*180/pi - } - -bettyRefs :: Fractional a => AeroRefs a -bettyRefs = AeroRefs { ar_sref = 0.684 - , ar_bref = 2.904 -- sqrt(sref*AR), - , ar_cref = 0.2512 -- sqrt(sref/AR), - } - -bettyInertia :: Fractional a => M33 a -bettyInertia = - V3 - (V3 0.565 0 0) - (V3 0 0.161 0) - (V3 0 0 0.723) - -bettyMass :: Fractional a => a -bettyMass = 7.5 diff --git a/src/Dyno/MultipleShooting.hs b/src/Dyno/MultipleShooting.hs --- a/src/Dyno/MultipleShooting.hs +++ b/src/Dyno/MultipleShooting.hs @@ -1,6 +1,7 @@ {-# OPTIONS_GHC -Wall #-} {-# Language ScopedTypeVariables #-} {-# Language DeriveGeneric #-} +{-# Language PolyKinds #-} module Dyno.MultipleShooting ( MsOcp(..) @@ -9,18 +10,26 @@ , makeMsNlp ) where -import GHC.Generics ( Generic ) +import GHC.Generics ( Generic, Generic1 ) + +import Data.Proxy ( Proxy(..) ) import Data.Vector ( Vector ) import Data.Maybe ( fromMaybe ) import qualified Data.Vector as V import Linear import qualified Data.Foldable as F +import Casadi.MX ( MX ) + import Dyno.TypeVecs -import Dyno.View -import Dyno.View.Scheme -import Dyno.Vectorize -import Dyno.Nlp +import Dyno.View.View ( View(..) ) +import Dyno.View.View ( J, JNone(..), JTuple(..), jfill ) +import Dyno.View.JV ( JV, catJV, catJV', splitJV' ) +import Dyno.View.JVec ( JVec(..) ) +import Dyno.View.Fun ( MXFun, toMXFun, call ) +import Dyno.View.Scheme ( Scheme ) +import Dyno.Vectorize ( Vectorize, Id ) +import Dyno.Nlp ( Bounds, Nlp'(..) ) data IntegratorIn x u p a = IntegratorIn (J (JV x) a) (J (JV u) a) (J (JV p) a) @@ -137,7 +146,7 @@ bg :: J (MsConstraints x n) (Vector Bounds) bg = cat MsConstraints { gContinuity = jfill (Just 0, Just 0) } - fg :: J (MsDvs x u p n) MX -> J JNone MX -> (J S MX, J (MsConstraints x n) MX) + fg :: J (MsDvs x u p n) MX -> J JNone MX -> (J (JV Id) MX, J (MsConstraints x n) MX) fg dvs _ = (f, cat g) where MsDvs xus xf p = split dvs @@ -155,8 +164,8 @@ mayer = msMayer msOcp (splitJV' xf) - f :: J S MX - f = mkJ $ unJ $ mayer + lagrangeSum + f :: J (JV Id) MX + f = mayer + lagrangeSum x0s' = fmap (extractx . split) $ unJVec $ split xus :: Vec n (J (JV x) MX) diff --git a/src/Dyno/Nats.hs b/src/Dyno/Nats.hs deleted file mode 100644 --- a/src/Dyno/Nats.hs +++ /dev/null @@ -1,625 +0,0 @@ -{-# OPTIONS_GHC -Wall #-} - -module Dyno.Nats where - -import qualified Data.Reflection -import Data.Reflection ( reflect ) -import Linear.V ( Dim(..) ) -import Data.Proxy - -data D0 -instance Dim D0 where - reflectDim _ = reflect (Proxy :: Proxy Data.Reflection.Z) -data D1 -instance Dim D1 where - reflectDim _ = reflect (Proxy :: Proxy (Data.Reflection.SD Data.Reflection.Z)) -data D2 -instance Dim D2 where - reflectDim _ = reflect (Proxy :: Proxy (Data.Reflection.D (Data.Reflection.SD Data.Reflection.Z))) -data D3 -instance Dim D3 where - reflectDim _ = reflect (Proxy :: Proxy (Data.Reflection.SD (Data.Reflection.SD Data.Reflection.Z))) -data D4 -instance Dim D4 where - reflectDim _ = reflect (Proxy :: Proxy (Data.Reflection.D (Data.Reflection.D (Data.Reflection.SD Data.Reflection.Z)))) -data D5 -instance Dim D5 where - reflectDim _ = reflect (Proxy :: Proxy (Data.Reflection.SD (Data.Reflection.D (Data.Reflection.SD Data.Reflection.Z)))) -data D6 -instance Dim D6 where - reflectDim _ = reflect (Proxy :: Proxy (Data.Reflection.D (Data.Reflection.SD (Data.Reflection.SD Data.Reflection.Z)))) -data D7 -instance Dim D7 where - reflectDim _ = reflect (Proxy :: Proxy (Data.Reflection.SD (Data.Reflection.SD (Data.Reflection.SD Data.Reflection.Z)))) -data D8 -instance Dim D8 where - reflectDim _ = reflect (Proxy :: Proxy (Data.Reflection.D (Data.Reflection.D (Data.Reflection.D (Data.Reflection.SD Data.Reflection.Z))))) -data D9 -instance Dim D9 where - reflectDim _ = reflect (Proxy :: Proxy (Data.Reflection.SD (Data.Reflection.D (Data.Reflection.D (Data.Reflection.SD Data.Reflection.Z))))) -data D10 -instance Dim D10 where - reflectDim _ = reflect (Proxy :: Proxy (Data.Reflection.D (Data.Reflection.SD (Data.Reflection.D (Data.Reflection.SD Data.Reflection.Z))))) -data D11 -instance Dim D11 where - reflectDim _ = reflect (Proxy :: Proxy (Data.Reflection.SD (Data.Reflection.SD (Data.Reflection.D (Data.Reflection.SD Data.Reflection.Z))))) -data D12 -instance Dim D12 where - reflectDim _ = reflect (Proxy :: Proxy (Data.Reflection.D (Data.Reflection.D (Data.Reflection.SD (Data.Reflection.SD Data.Reflection.Z))))) -data D13 -instance Dim D13 where - reflectDim _ = reflect (Proxy :: Proxy (Data.Reflection.SD (Data.Reflection.D (Data.Reflection.SD (Data.Reflection.SD Data.Reflection.Z))))) -data D14 -instance Dim D14 where - reflectDim _ = reflect (Proxy :: Proxy (Data.Reflection.D (Data.Reflection.SD (Data.Reflection.SD (Data.Reflection.SD Data.Reflection.Z))))) -data D15 -instance Dim D15 where - reflectDim _ = reflect (Proxy :: Proxy (Data.Reflection.SD (Data.Reflection.SD (Data.Reflection.SD (Data.Reflection.SD Data.Reflection.Z))))) -data D16 -instance Dim D16 where - reflectDim _ = reflect (Proxy :: Proxy (Data.Reflection.D (Data.Reflection.D (Data.Reflection.D (Data.Reflection.D (Data.Reflection.SD Data.Reflection.Z)))))) -data D17 -instance Dim D17 where - reflectDim _ = reflect (Proxy :: Proxy (Data.Reflection.SD (Data.Reflection.D (Data.Reflection.D (Data.Reflection.D (Data.Reflection.SD Data.Reflection.Z)))))) -data D18 -instance Dim D18 where - reflectDim _ = reflect (Proxy :: Proxy (Data.Reflection.D (Data.Reflection.SD (Data.Reflection.D (Data.Reflection.D (Data.Reflection.SD Data.Reflection.Z)))))) -data D19 -instance Dim D19 where - reflectDim _ = reflect (Proxy :: Proxy (Data.Reflection.SD (Data.Reflection.SD (Data.Reflection.D (Data.Reflection.D (Data.Reflection.SD Data.Reflection.Z)))))) -data D20 -instance Dim D20 where - reflectDim _ = reflect (Proxy :: Proxy (Data.Reflection.D (Data.Reflection.D (Data.Reflection.SD (Data.Reflection.D (Data.Reflection.SD Data.Reflection.Z)))))) -data D21 -instance Dim D21 where - reflectDim _ = reflect (Proxy :: Proxy (Data.Reflection.SD (Data.Reflection.D (Data.Reflection.SD (Data.Reflection.D (Data.Reflection.SD Data.Reflection.Z)))))) -data D22 -instance Dim D22 where - reflectDim _ = reflect (Proxy :: Proxy (Data.Reflection.D (Data.Reflection.SD (Data.Reflection.SD (Data.Reflection.D (Data.Reflection.SD Data.Reflection.Z)))))) -data D23 -instance Dim D23 where - reflectDim _ = reflect (Proxy :: Proxy (Data.Reflection.SD (Data.Reflection.SD (Data.Reflection.SD (Data.Reflection.D (Data.Reflection.SD Data.Reflection.Z)))))) -data D24 -instance Dim D24 where - reflectDim _ = reflect (Proxy :: Proxy (Data.Reflection.D (Data.Reflection.D (Data.Reflection.D (Data.Reflection.SD (Data.Reflection.SD Data.Reflection.Z)))))) -data D25 -instance Dim D25 where - reflectDim _ = reflect (Proxy :: Proxy (Data.Reflection.SD (Data.Reflection.D (Data.Reflection.D (Data.Reflection.SD (Data.Reflection.SD Data.Reflection.Z)))))) -data D26 -instance Dim D26 where - reflectDim _ = reflect (Proxy :: Proxy (Data.Reflection.D (Data.Reflection.SD (Data.Reflection.D (Data.Reflection.SD (Data.Reflection.SD Data.Reflection.Z)))))) -data D27 -instance Dim D27 where - reflectDim _ = reflect (Proxy :: Proxy (Data.Reflection.SD (Data.Reflection.SD (Data.Reflection.D (Data.Reflection.SD (Data.Reflection.SD Data.Reflection.Z)))))) -data D28 -instance Dim D28 where - reflectDim _ = reflect (Proxy :: Proxy (Data.Reflection.D (Data.Reflection.D (Data.Reflection.SD (Data.Reflection.SD (Data.Reflection.SD Data.Reflection.Z)))))) -data D29 -instance Dim D29 where - reflectDim _ = reflect (Proxy :: Proxy (Data.Reflection.SD (Data.Reflection.D (Data.Reflection.SD (Data.Reflection.SD (Data.Reflection.SD Data.Reflection.Z)))))) -data D30 -instance Dim D30 where - reflectDim _ = reflect (Proxy :: Proxy (Data.Reflection.D (Data.Reflection.SD (Data.Reflection.SD (Data.Reflection.SD (Data.Reflection.SD Data.Reflection.Z)))))) -data D31 -instance Dim D31 where - reflectDim _ = reflect (Proxy :: Proxy (Data.Reflection.SD (Data.Reflection.SD (Data.Reflection.SD (Data.Reflection.SD (Data.Reflection.SD Data.Reflection.Z)))))) -data D32 -instance Dim D32 where - reflectDim _ = reflect (Proxy :: Proxy (Data.Reflection.D (Data.Reflection.D (Data.Reflection.D (Data.Reflection.D (Data.Reflection.D (Data.Reflection.SD Data.Reflection.Z))))))) -data D33 -instance Dim D33 where - reflectDim _ = reflect (Proxy :: Proxy (Data.Reflection.SD (Data.Reflection.D (Data.Reflection.D (Data.Reflection.D (Data.Reflection.D (Data.Reflection.SD Data.Reflection.Z))))))) -data D34 -instance Dim D34 where - reflectDim _ = reflect (Proxy :: Proxy (Data.Reflection.D (Data.Reflection.SD (Data.Reflection.D (Data.Reflection.D (Data.Reflection.D (Data.Reflection.SD Data.Reflection.Z))))))) -data D35 -instance Dim D35 where - reflectDim _ = reflect (Proxy :: Proxy (Data.Reflection.SD (Data.Reflection.SD (Data.Reflection.D (Data.Reflection.D (Data.Reflection.D (Data.Reflection.SD Data.Reflection.Z))))))) -data D36 -instance Dim D36 where - reflectDim _ = reflect (Proxy :: Proxy (Data.Reflection.D (Data.Reflection.D (Data.Reflection.SD (Data.Reflection.D (Data.Reflection.D (Data.Reflection.SD Data.Reflection.Z))))))) -data D37 -instance Dim D37 where - reflectDim _ = reflect (Proxy :: Proxy (Data.Reflection.SD (Data.Reflection.D (Data.Reflection.SD (Data.Reflection.D (Data.Reflection.D (Data.Reflection.SD Data.Reflection.Z))))))) -data D38 -instance Dim D38 where - reflectDim _ = reflect (Proxy :: Proxy (Data.Reflection.D (Data.Reflection.SD (Data.Reflection.SD (Data.Reflection.D (Data.Reflection.D (Data.Reflection.SD Data.Reflection.Z))))))) -data D39 -instance Dim D39 where - reflectDim _ = reflect (Proxy :: Proxy (Data.Reflection.SD (Data.Reflection.SD (Data.Reflection.SD (Data.Reflection.D (Data.Reflection.D (Data.Reflection.SD Data.Reflection.Z))))))) -data D40 -instance Dim D40 where - reflectDim _ = reflect (Proxy :: Proxy (Data.Reflection.D (Data.Reflection.D (Data.Reflection.D (Data.Reflection.SD (Data.Reflection.D (Data.Reflection.SD Data.Reflection.Z))))))) -data D41 -instance Dim D41 where - reflectDim _ = reflect (Proxy :: Proxy (Data.Reflection.SD (Data.Reflection.D (Data.Reflection.D (Data.Reflection.SD (Data.Reflection.D (Data.Reflection.SD Data.Reflection.Z))))))) -data D42 -instance Dim D42 where - reflectDim _ = reflect (Proxy :: Proxy (Data.Reflection.D (Data.Reflection.SD (Data.Reflection.D (Data.Reflection.SD (Data.Reflection.D (Data.Reflection.SD Data.Reflection.Z))))))) -data D43 -instance Dim D43 where - reflectDim _ = reflect (Proxy :: Proxy (Data.Reflection.SD (Data.Reflection.SD (Data.Reflection.D (Data.Reflection.SD (Data.Reflection.D (Data.Reflection.SD Data.Reflection.Z))))))) -data D44 -instance Dim D44 where - reflectDim _ = reflect (Proxy :: Proxy (Data.Reflection.D (Data.Reflection.D (Data.Reflection.SD (Data.Reflection.SD (Data.Reflection.D (Data.Reflection.SD Data.Reflection.Z))))))) -data D45 -instance Dim D45 where - reflectDim _ = reflect (Proxy :: Proxy (Data.Reflection.SD (Data.Reflection.D (Data.Reflection.SD (Data.Reflection.SD (Data.Reflection.D (Data.Reflection.SD Data.Reflection.Z))))))) -data D46 -instance Dim D46 where - reflectDim _ = reflect (Proxy :: Proxy (Data.Reflection.D (Data.Reflection.SD (Data.Reflection.SD (Data.Reflection.SD (Data.Reflection.D (Data.Reflection.SD Data.Reflection.Z))))))) -data D47 -instance Dim D47 where - reflectDim _ = reflect (Proxy :: Proxy (Data.Reflection.SD (Data.Reflection.SD (Data.Reflection.SD (Data.Reflection.SD (Data.Reflection.D (Data.Reflection.SD Data.Reflection.Z))))))) -data D48 -instance Dim D48 where - reflectDim _ = reflect (Proxy :: Proxy (Data.Reflection.D (Data.Reflection.D (Data.Reflection.D (Data.Reflection.D (Data.Reflection.SD (Data.Reflection.SD Data.Reflection.Z))))))) -data D49 -instance Dim D49 where - reflectDim _ = reflect (Proxy :: Proxy (Data.Reflection.SD (Data.Reflection.D (Data.Reflection.D (Data.Reflection.D (Data.Reflection.SD (Data.Reflection.SD Data.Reflection.Z))))))) -data D50 -instance Dim D50 where - reflectDim _ = reflect (Proxy :: Proxy (Data.Reflection.D (Data.Reflection.SD (Data.Reflection.D (Data.Reflection.D (Data.Reflection.SD (Data.Reflection.SD Data.Reflection.Z))))))) -data D51 -instance Dim D51 where - reflectDim _ = reflect (Proxy :: Proxy (Data.Reflection.SD (Data.Reflection.SD (Data.Reflection.D (Data.Reflection.D (Data.Reflection.SD (Data.Reflection.SD Data.Reflection.Z))))))) -data D52 -instance Dim D52 where - reflectDim _ = reflect (Proxy :: Proxy (Data.Reflection.D (Data.Reflection.D (Data.Reflection.SD (Data.Reflection.D (Data.Reflection.SD (Data.Reflection.SD Data.Reflection.Z))))))) -data D53 -instance Dim D53 where - reflectDim _ = reflect (Proxy :: Proxy (Data.Reflection.SD (Data.Reflection.D (Data.Reflection.SD (Data.Reflection.D (Data.Reflection.SD (Data.Reflection.SD Data.Reflection.Z))))))) -data D54 -instance Dim D54 where - reflectDim _ = reflect (Proxy :: Proxy (Data.Reflection.D (Data.Reflection.SD (Data.Reflection.SD (Data.Reflection.D (Data.Reflection.SD (Data.Reflection.SD Data.Reflection.Z))))))) -data D55 -instance Dim D55 where - reflectDim _ = reflect (Proxy :: Proxy (Data.Reflection.SD (Data.Reflection.SD (Data.Reflection.SD (Data.Reflection.D (Data.Reflection.SD (Data.Reflection.SD Data.Reflection.Z))))))) -data D56 -instance Dim D56 where - reflectDim _ = reflect (Proxy :: Proxy (Data.Reflection.D (Data.Reflection.D (Data.Reflection.D (Data.Reflection.SD (Data.Reflection.SD (Data.Reflection.SD Data.Reflection.Z))))))) -data D57 -instance Dim D57 where - reflectDim _ = reflect (Proxy :: Proxy (Data.Reflection.SD (Data.Reflection.D (Data.Reflection.D (Data.Reflection.SD (Data.Reflection.SD (Data.Reflection.SD Data.Reflection.Z))))))) -data D58 -instance Dim D58 where - reflectDim _ = reflect (Proxy :: Proxy (Data.Reflection.D (Data.Reflection.SD (Data.Reflection.D (Data.Reflection.SD (Data.Reflection.SD (Data.Reflection.SD Data.Reflection.Z))))))) -data D59 -instance Dim D59 where - reflectDim _ = reflect (Proxy :: Proxy (Data.Reflection.SD (Data.Reflection.SD (Data.Reflection.D (Data.Reflection.SD (Data.Reflection.SD (Data.Reflection.SD Data.Reflection.Z))))))) -data D60 -instance Dim D60 where - reflectDim _ = reflect (Proxy :: Proxy (Data.Reflection.D (Data.Reflection.D (Data.Reflection.SD (Data.Reflection.SD (Data.Reflection.SD (Data.Reflection.SD Data.Reflection.Z))))))) -data D61 -instance Dim D61 where - reflectDim _ = reflect (Proxy :: Proxy (Data.Reflection.SD (Data.Reflection.D (Data.Reflection.SD (Data.Reflection.SD (Data.Reflection.SD (Data.Reflection.SD Data.Reflection.Z))))))) -data D62 -instance Dim D62 where - reflectDim _ = reflect (Proxy :: Proxy (Data.Reflection.D (Data.Reflection.SD (Data.Reflection.SD (Data.Reflection.SD (Data.Reflection.SD (Data.Reflection.SD Data.Reflection.Z))))))) -data D63 -instance Dim D63 where - reflectDim _ = reflect (Proxy :: Proxy (Data.Reflection.SD (Data.Reflection.SD (Data.Reflection.SD (Data.Reflection.SD (Data.Reflection.SD (Data.Reflection.SD Data.Reflection.Z))))))) -data D64 -instance Dim D64 where - reflectDim _ = reflect (Proxy :: Proxy (Data.Reflection.D (Data.Reflection.D (Data.Reflection.D (Data.Reflection.D (Data.Reflection.D (Data.Reflection.D (Data.Reflection.SD Data.Reflection.Z)))))))) -data D65 -instance Dim D65 where - reflectDim _ = reflect (Proxy :: Proxy (Data.Reflection.SD (Data.Reflection.D (Data.Reflection.D (Data.Reflection.D (Data.Reflection.D (Data.Reflection.D (Data.Reflection.SD Data.Reflection.Z)))))))) -data D66 -instance Dim D66 where - reflectDim _ = reflect (Proxy :: Proxy (Data.Reflection.D (Data.Reflection.SD (Data.Reflection.D (Data.Reflection.D (Data.Reflection.D (Data.Reflection.D (Data.Reflection.SD Data.Reflection.Z)))))))) -data D67 -instance Dim D67 where - reflectDim _ = reflect (Proxy :: Proxy (Data.Reflection.SD (Data.Reflection.SD (Data.Reflection.D (Data.Reflection.D (Data.Reflection.D (Data.Reflection.D (Data.Reflection.SD Data.Reflection.Z)))))))) -data D68 -instance Dim D68 where - reflectDim _ = reflect (Proxy :: Proxy (Data.Reflection.D (Data.Reflection.D (Data.Reflection.SD (Data.Reflection.D (Data.Reflection.D (Data.Reflection.D (Data.Reflection.SD Data.Reflection.Z)))))))) -data D69 -instance Dim D69 where - reflectDim _ = reflect (Proxy :: Proxy (Data.Reflection.SD (Data.Reflection.D (Data.Reflection.SD (Data.Reflection.D (Data.Reflection.D (Data.Reflection.D (Data.Reflection.SD Data.Reflection.Z)))))))) -data D70 -instance Dim D70 where - reflectDim _ = reflect (Proxy :: Proxy (Data.Reflection.D (Data.Reflection.SD (Data.Reflection.SD (Data.Reflection.D (Data.Reflection.D (Data.Reflection.D (Data.Reflection.SD Data.Reflection.Z)))))))) -data D71 -instance Dim D71 where - reflectDim _ = reflect (Proxy :: Proxy (Data.Reflection.SD (Data.Reflection.SD (Data.Reflection.SD (Data.Reflection.D (Data.Reflection.D (Data.Reflection.D (Data.Reflection.SD Data.Reflection.Z)))))))) -data D72 -instance Dim D72 where - reflectDim _ = reflect (Proxy :: Proxy (Data.Reflection.D (Data.Reflection.D (Data.Reflection.D (Data.Reflection.SD (Data.Reflection.D (Data.Reflection.D (Data.Reflection.SD Data.Reflection.Z)))))))) -data D73 -instance Dim D73 where - reflectDim _ = reflect (Proxy :: Proxy (Data.Reflection.SD (Data.Reflection.D (Data.Reflection.D (Data.Reflection.SD (Data.Reflection.D (Data.Reflection.D (Data.Reflection.SD Data.Reflection.Z)))))))) -data D74 -instance Dim D74 where - reflectDim _ = reflect (Proxy :: Proxy (Data.Reflection.D (Data.Reflection.SD (Data.Reflection.D (Data.Reflection.SD (Data.Reflection.D (Data.Reflection.D (Data.Reflection.SD Data.Reflection.Z)))))))) -data D75 -instance Dim D75 where - reflectDim _ = reflect (Proxy :: Proxy (Data.Reflection.SD (Data.Reflection.SD (Data.Reflection.D (Data.Reflection.SD (Data.Reflection.D (Data.Reflection.D (Data.Reflection.SD Data.Reflection.Z)))))))) -data D76 -instance Dim D76 where - reflectDim _ = reflect (Proxy :: Proxy (Data.Reflection.D (Data.Reflection.D (Data.Reflection.SD (Data.Reflection.SD (Data.Reflection.D (Data.Reflection.D (Data.Reflection.SD Data.Reflection.Z)))))))) -data D77 -instance Dim D77 where - reflectDim _ = reflect (Proxy :: Proxy (Data.Reflection.SD (Data.Reflection.D (Data.Reflection.SD (Data.Reflection.SD (Data.Reflection.D (Data.Reflection.D (Data.Reflection.SD Data.Reflection.Z)))))))) -data D78 -instance Dim D78 where - reflectDim _ = reflect (Proxy :: Proxy (Data.Reflection.D (Data.Reflection.SD (Data.Reflection.SD (Data.Reflection.SD (Data.Reflection.D (Data.Reflection.D (Data.Reflection.SD Data.Reflection.Z)))))))) -data D79 -instance Dim D79 where - reflectDim _ = reflect (Proxy :: Proxy (Data.Reflection.SD (Data.Reflection.SD (Data.Reflection.SD (Data.Reflection.SD (Data.Reflection.D (Data.Reflection.D (Data.Reflection.SD Data.Reflection.Z)))))))) -data D80 -instance Dim D80 where - reflectDim _ = reflect (Proxy :: Proxy (Data.Reflection.D (Data.Reflection.D (Data.Reflection.D (Data.Reflection.D (Data.Reflection.SD (Data.Reflection.D (Data.Reflection.SD Data.Reflection.Z)))))))) -data D81 -instance Dim D81 where - reflectDim _ = reflect (Proxy :: Proxy (Data.Reflection.SD (Data.Reflection.D (Data.Reflection.D (Data.Reflection.D (Data.Reflection.SD (Data.Reflection.D (Data.Reflection.SD Data.Reflection.Z)))))))) -data D82 -instance Dim D82 where - reflectDim _ = reflect (Proxy :: Proxy (Data.Reflection.D (Data.Reflection.SD (Data.Reflection.D (Data.Reflection.D (Data.Reflection.SD (Data.Reflection.D (Data.Reflection.SD Data.Reflection.Z)))))))) -data D83 -instance Dim D83 where - reflectDim _ = reflect (Proxy :: Proxy (Data.Reflection.SD (Data.Reflection.SD (Data.Reflection.D (Data.Reflection.D (Data.Reflection.SD (Data.Reflection.D (Data.Reflection.SD Data.Reflection.Z)))))))) -data D84 -instance Dim D84 where - reflectDim _ = reflect (Proxy :: Proxy (Data.Reflection.D (Data.Reflection.D (Data.Reflection.SD (Data.Reflection.D (Data.Reflection.SD (Data.Reflection.D (Data.Reflection.SD Data.Reflection.Z)))))))) -data D85 -instance Dim D85 where - reflectDim _ = reflect (Proxy :: Proxy (Data.Reflection.SD (Data.Reflection.D (Data.Reflection.SD (Data.Reflection.D (Data.Reflection.SD (Data.Reflection.D (Data.Reflection.SD Data.Reflection.Z)))))))) -data D86 -instance Dim D86 where - reflectDim _ = reflect (Proxy :: Proxy (Data.Reflection.D (Data.Reflection.SD (Data.Reflection.SD (Data.Reflection.D (Data.Reflection.SD (Data.Reflection.D (Data.Reflection.SD Data.Reflection.Z)))))))) -data D87 -instance Dim D87 where - reflectDim _ = reflect (Proxy :: Proxy (Data.Reflection.SD (Data.Reflection.SD (Data.Reflection.SD (Data.Reflection.D (Data.Reflection.SD (Data.Reflection.D (Data.Reflection.SD Data.Reflection.Z)))))))) -data D88 -instance Dim D88 where - reflectDim _ = reflect (Proxy :: Proxy (Data.Reflection.D (Data.Reflection.D (Data.Reflection.D (Data.Reflection.SD (Data.Reflection.SD (Data.Reflection.D (Data.Reflection.SD Data.Reflection.Z)))))))) -data D89 -instance Dim D89 where - reflectDim _ = reflect (Proxy :: Proxy (Data.Reflection.SD (Data.Reflection.D (Data.Reflection.D (Data.Reflection.SD (Data.Reflection.SD (Data.Reflection.D (Data.Reflection.SD Data.Reflection.Z)))))))) -data D90 -instance Dim D90 where - reflectDim _ = reflect (Proxy :: Proxy (Data.Reflection.D (Data.Reflection.SD (Data.Reflection.D (Data.Reflection.SD (Data.Reflection.SD (Data.Reflection.D (Data.Reflection.SD Data.Reflection.Z)))))))) -data D91 -instance Dim D91 where - reflectDim _ = reflect (Proxy :: Proxy (Data.Reflection.SD (Data.Reflection.SD (Data.Reflection.D (Data.Reflection.SD (Data.Reflection.SD (Data.Reflection.D (Data.Reflection.SD Data.Reflection.Z)))))))) -data D92 -instance Dim D92 where - reflectDim _ = reflect (Proxy :: Proxy (Data.Reflection.D (Data.Reflection.D (Data.Reflection.SD (Data.Reflection.SD (Data.Reflection.SD (Data.Reflection.D (Data.Reflection.SD Data.Reflection.Z)))))))) -data D93 -instance Dim D93 where - reflectDim _ = reflect (Proxy :: Proxy (Data.Reflection.SD (Data.Reflection.D (Data.Reflection.SD (Data.Reflection.SD (Data.Reflection.SD (Data.Reflection.D (Data.Reflection.SD Data.Reflection.Z)))))))) -data D94 -instance Dim D94 where - reflectDim _ = reflect (Proxy :: Proxy (Data.Reflection.D (Data.Reflection.SD (Data.Reflection.SD (Data.Reflection.SD (Data.Reflection.SD (Data.Reflection.D (Data.Reflection.SD Data.Reflection.Z)))))))) -data D95 -instance Dim D95 where - reflectDim _ = reflect (Proxy :: Proxy (Data.Reflection.SD (Data.Reflection.SD (Data.Reflection.SD (Data.Reflection.SD (Data.Reflection.SD (Data.Reflection.D (Data.Reflection.SD Data.Reflection.Z)))))))) -data D96 -instance Dim D96 where - reflectDim _ = reflect (Proxy :: Proxy (Data.Reflection.D (Data.Reflection.D (Data.Reflection.D (Data.Reflection.D (Data.Reflection.D (Data.Reflection.SD (Data.Reflection.SD Data.Reflection.Z)))))))) -data D97 -instance Dim D97 where - reflectDim _ = reflect (Proxy :: Proxy (Data.Reflection.SD (Data.Reflection.D (Data.Reflection.D (Data.Reflection.D (Data.Reflection.D (Data.Reflection.SD (Data.Reflection.SD Data.Reflection.Z)))))))) -data D98 -instance Dim D98 where - reflectDim _ = reflect (Proxy :: Proxy (Data.Reflection.D (Data.Reflection.SD (Data.Reflection.D (Data.Reflection.D (Data.Reflection.D (Data.Reflection.SD (Data.Reflection.SD Data.Reflection.Z)))))))) -data D99 -instance Dim D99 where - reflectDim _ = reflect (Proxy :: Proxy (Data.Reflection.SD (Data.Reflection.SD (Data.Reflection.D (Data.Reflection.D (Data.Reflection.D (Data.Reflection.SD (Data.Reflection.SD Data.Reflection.Z)))))))) -data D100 -instance Dim D100 where - reflectDim _ = reflect (Proxy :: Proxy (Data.Reflection.D (Data.Reflection.D (Data.Reflection.SD (Data.Reflection.D (Data.Reflection.D (Data.Reflection.SD (Data.Reflection.SD Data.Reflection.Z)))))))) -data D101 -instance Dim D101 where - reflectDim _ = reflect (Proxy :: Proxy (Data.Reflection.SD (Data.Reflection.D (Data.Reflection.SD (Data.Reflection.D (Data.Reflection.D (Data.Reflection.SD (Data.Reflection.SD Data.Reflection.Z)))))))) -data D102 -instance Dim D102 where - reflectDim _ = reflect (Proxy :: Proxy (Data.Reflection.D (Data.Reflection.SD (Data.Reflection.SD (Data.Reflection.D (Data.Reflection.D (Data.Reflection.SD (Data.Reflection.SD Data.Reflection.Z)))))))) -data D103 -instance Dim D103 where - reflectDim _ = reflect (Proxy :: Proxy (Data.Reflection.SD (Data.Reflection.SD (Data.Reflection.SD (Data.Reflection.D (Data.Reflection.D (Data.Reflection.SD (Data.Reflection.SD Data.Reflection.Z)))))))) -data D104 -instance Dim D104 where - reflectDim _ = reflect (Proxy :: Proxy (Data.Reflection.D (Data.Reflection.D (Data.Reflection.D (Data.Reflection.SD (Data.Reflection.D (Data.Reflection.SD (Data.Reflection.SD Data.Reflection.Z)))))))) -data D105 -instance Dim D105 where - reflectDim _ = reflect (Proxy :: Proxy (Data.Reflection.SD (Data.Reflection.D (Data.Reflection.D (Data.Reflection.SD (Data.Reflection.D (Data.Reflection.SD (Data.Reflection.SD Data.Reflection.Z)))))))) -data D106 -instance Dim D106 where - reflectDim _ = reflect (Proxy :: Proxy (Data.Reflection.D (Data.Reflection.SD (Data.Reflection.D (Data.Reflection.SD (Data.Reflection.D (Data.Reflection.SD (Data.Reflection.SD Data.Reflection.Z)))))))) -data D107 -instance Dim D107 where - reflectDim _ = reflect (Proxy :: Proxy (Data.Reflection.SD (Data.Reflection.SD (Data.Reflection.D (Data.Reflection.SD (Data.Reflection.D (Data.Reflection.SD (Data.Reflection.SD Data.Reflection.Z)))))))) -data D108 -instance Dim D108 where - reflectDim _ = reflect (Proxy :: Proxy (Data.Reflection.D (Data.Reflection.D (Data.Reflection.SD (Data.Reflection.SD (Data.Reflection.D (Data.Reflection.SD (Data.Reflection.SD Data.Reflection.Z)))))))) -data D109 -instance Dim D109 where - reflectDim _ = reflect (Proxy :: Proxy (Data.Reflection.SD (Data.Reflection.D (Data.Reflection.SD (Data.Reflection.SD (Data.Reflection.D (Data.Reflection.SD (Data.Reflection.SD Data.Reflection.Z)))))))) -data D110 -instance Dim D110 where - reflectDim _ = reflect (Proxy :: Proxy (Data.Reflection.D (Data.Reflection.SD (Data.Reflection.SD (Data.Reflection.SD (Data.Reflection.D (Data.Reflection.SD (Data.Reflection.SD Data.Reflection.Z)))))))) -data D111 -instance Dim D111 where - reflectDim _ = reflect (Proxy :: Proxy (Data.Reflection.SD (Data.Reflection.SD (Data.Reflection.SD (Data.Reflection.SD (Data.Reflection.D (Data.Reflection.SD (Data.Reflection.SD Data.Reflection.Z)))))))) -data D112 -instance Dim D112 where - reflectDim _ = reflect (Proxy :: Proxy (Data.Reflection.D (Data.Reflection.D (Data.Reflection.D (Data.Reflection.D (Data.Reflection.SD (Data.Reflection.SD (Data.Reflection.SD Data.Reflection.Z)))))))) -data D113 -instance Dim D113 where - reflectDim _ = reflect (Proxy :: Proxy (Data.Reflection.SD (Data.Reflection.D (Data.Reflection.D (Data.Reflection.D (Data.Reflection.SD (Data.Reflection.SD (Data.Reflection.SD Data.Reflection.Z)))))))) -data D114 -instance Dim D114 where - reflectDim _ = reflect (Proxy :: Proxy (Data.Reflection.D (Data.Reflection.SD (Data.Reflection.D (Data.Reflection.D (Data.Reflection.SD (Data.Reflection.SD (Data.Reflection.SD Data.Reflection.Z)))))))) -data D115 -instance Dim D115 where - reflectDim _ = reflect (Proxy :: Proxy (Data.Reflection.SD (Data.Reflection.SD (Data.Reflection.D (Data.Reflection.D (Data.Reflection.SD (Data.Reflection.SD (Data.Reflection.SD Data.Reflection.Z)))))))) -data D116 -instance Dim D116 where - reflectDim _ = reflect (Proxy :: Proxy (Data.Reflection.D (Data.Reflection.D (Data.Reflection.SD (Data.Reflection.D (Data.Reflection.SD (Data.Reflection.SD (Data.Reflection.SD Data.Reflection.Z)))))))) -data D117 -instance Dim D117 where - reflectDim _ = reflect (Proxy :: Proxy (Data.Reflection.SD (Data.Reflection.D (Data.Reflection.SD (Data.Reflection.D (Data.Reflection.SD (Data.Reflection.SD (Data.Reflection.SD Data.Reflection.Z)))))))) -data D118 -instance Dim D118 where - reflectDim _ = reflect (Proxy :: Proxy (Data.Reflection.D (Data.Reflection.SD (Data.Reflection.SD (Data.Reflection.D (Data.Reflection.SD (Data.Reflection.SD (Data.Reflection.SD Data.Reflection.Z)))))))) -data D119 -instance Dim D119 where - reflectDim _ = reflect (Proxy :: Proxy (Data.Reflection.SD (Data.Reflection.SD (Data.Reflection.SD (Data.Reflection.D (Data.Reflection.SD (Data.Reflection.SD (Data.Reflection.SD Data.Reflection.Z)))))))) -data D120 -instance Dim D120 where - reflectDim _ = reflect (Proxy :: Proxy (Data.Reflection.D (Data.Reflection.D (Data.Reflection.D (Data.Reflection.SD (Data.Reflection.SD (Data.Reflection.SD (Data.Reflection.SD Data.Reflection.Z)))))))) -data D121 -instance Dim D121 where - reflectDim _ = reflect (Proxy :: Proxy (Data.Reflection.SD (Data.Reflection.D (Data.Reflection.D (Data.Reflection.SD (Data.Reflection.SD (Data.Reflection.SD (Data.Reflection.SD Data.Reflection.Z)))))))) -data D122 -instance Dim D122 where - reflectDim _ = reflect (Proxy :: Proxy (Data.Reflection.D (Data.Reflection.SD (Data.Reflection.D (Data.Reflection.SD (Data.Reflection.SD (Data.Reflection.SD (Data.Reflection.SD Data.Reflection.Z)))))))) -data D123 -instance Dim D123 where - reflectDim _ = reflect (Proxy :: Proxy (Data.Reflection.SD (Data.Reflection.SD (Data.Reflection.D (Data.Reflection.SD (Data.Reflection.SD (Data.Reflection.SD (Data.Reflection.SD Data.Reflection.Z)))))))) -data D124 -instance Dim D124 where - reflectDim _ = reflect (Proxy :: Proxy (Data.Reflection.D (Data.Reflection.D (Data.Reflection.SD (Data.Reflection.SD (Data.Reflection.SD (Data.Reflection.SD (Data.Reflection.SD Data.Reflection.Z)))))))) -data D125 -instance Dim D125 where - reflectDim _ = reflect (Proxy :: Proxy (Data.Reflection.SD (Data.Reflection.D (Data.Reflection.SD (Data.Reflection.SD (Data.Reflection.SD (Data.Reflection.SD (Data.Reflection.SD Data.Reflection.Z)))))))) -data D126 -instance Dim D126 where - reflectDim _ = reflect (Proxy :: Proxy (Data.Reflection.D (Data.Reflection.SD (Data.Reflection.SD (Data.Reflection.SD (Data.Reflection.SD (Data.Reflection.SD (Data.Reflection.SD Data.Reflection.Z)))))))) -data D127 -instance Dim D127 where - reflectDim _ = reflect (Proxy :: Proxy (Data.Reflection.SD (Data.Reflection.SD (Data.Reflection.SD (Data.Reflection.SD (Data.Reflection.SD (Data.Reflection.SD (Data.Reflection.SD Data.Reflection.Z)))))))) -data D128 -instance Dim D128 where - reflectDim _ = reflect (Proxy :: Proxy (Data.Reflection.D (Data.Reflection.D (Data.Reflection.D (Data.Reflection.D (Data.Reflection.D (Data.Reflection.D (Data.Reflection.D (Data.Reflection.SD Data.Reflection.Z))))))))) -data D129 -instance Dim D129 where - reflectDim _ = reflect (Proxy :: Proxy (Data.Reflection.SD (Data.Reflection.D (Data.Reflection.D (Data.Reflection.D (Data.Reflection.D (Data.Reflection.D (Data.Reflection.D (Data.Reflection.SD Data.Reflection.Z))))))))) -data D130 -instance Dim D130 where - reflectDim _ = reflect (Proxy :: Proxy (Data.Reflection.D (Data.Reflection.SD (Data.Reflection.D (Data.Reflection.D (Data.Reflection.D (Data.Reflection.D (Data.Reflection.D (Data.Reflection.SD Data.Reflection.Z))))))))) -data D131 -instance Dim D131 where - reflectDim _ = reflect (Proxy :: Proxy (Data.Reflection.SD (Data.Reflection.SD (Data.Reflection.D (Data.Reflection.D (Data.Reflection.D (Data.Reflection.D (Data.Reflection.D (Data.Reflection.SD Data.Reflection.Z))))))))) -data D132 -instance Dim D132 where - reflectDim _ = reflect (Proxy :: Proxy (Data.Reflection.D (Data.Reflection.D (Data.Reflection.SD (Data.Reflection.D (Data.Reflection.D (Data.Reflection.D (Data.Reflection.D (Data.Reflection.SD Data.Reflection.Z))))))))) -data D133 -instance Dim D133 where - reflectDim _ = reflect (Proxy :: Proxy (Data.Reflection.SD (Data.Reflection.D (Data.Reflection.SD (Data.Reflection.D (Data.Reflection.D (Data.Reflection.D (Data.Reflection.D (Data.Reflection.SD Data.Reflection.Z))))))))) -data D134 -instance Dim D134 where - reflectDim _ = reflect (Proxy :: Proxy (Data.Reflection.D (Data.Reflection.SD (Data.Reflection.SD (Data.Reflection.D (Data.Reflection.D (Data.Reflection.D (Data.Reflection.D (Data.Reflection.SD Data.Reflection.Z))))))))) -data D135 -instance Dim D135 where - reflectDim _ = reflect (Proxy :: Proxy (Data.Reflection.SD (Data.Reflection.SD (Data.Reflection.SD (Data.Reflection.D (Data.Reflection.D (Data.Reflection.D (Data.Reflection.D (Data.Reflection.SD Data.Reflection.Z))))))))) -data D136 -instance Dim D136 where - reflectDim _ = reflect (Proxy :: Proxy (Data.Reflection.D (Data.Reflection.D (Data.Reflection.D (Data.Reflection.SD (Data.Reflection.D (Data.Reflection.D (Data.Reflection.D (Data.Reflection.SD Data.Reflection.Z))))))))) -data D137 -instance Dim D137 where - reflectDim _ = reflect (Proxy :: Proxy (Data.Reflection.SD (Data.Reflection.D (Data.Reflection.D (Data.Reflection.SD (Data.Reflection.D (Data.Reflection.D (Data.Reflection.D (Data.Reflection.SD Data.Reflection.Z))))))))) -data D138 -instance Dim D138 where - reflectDim _ = reflect (Proxy :: Proxy (Data.Reflection.D (Data.Reflection.SD (Data.Reflection.D (Data.Reflection.SD (Data.Reflection.D (Data.Reflection.D (Data.Reflection.D (Data.Reflection.SD Data.Reflection.Z))))))))) -data D139 -instance Dim D139 where - reflectDim _ = reflect (Proxy :: Proxy (Data.Reflection.SD (Data.Reflection.SD (Data.Reflection.D (Data.Reflection.SD (Data.Reflection.D (Data.Reflection.D (Data.Reflection.D (Data.Reflection.SD Data.Reflection.Z))))))))) -data D140 -instance Dim D140 where - reflectDim _ = reflect (Proxy :: Proxy (Data.Reflection.D (Data.Reflection.D (Data.Reflection.SD (Data.Reflection.SD (Data.Reflection.D (Data.Reflection.D (Data.Reflection.D (Data.Reflection.SD Data.Reflection.Z))))))))) -data D141 -instance Dim D141 where - reflectDim _ = reflect (Proxy :: Proxy (Data.Reflection.SD (Data.Reflection.D (Data.Reflection.SD (Data.Reflection.SD (Data.Reflection.D (Data.Reflection.D (Data.Reflection.D (Data.Reflection.SD Data.Reflection.Z))))))))) -data D142 -instance Dim D142 where - reflectDim _ = reflect (Proxy :: Proxy (Data.Reflection.D (Data.Reflection.SD (Data.Reflection.SD (Data.Reflection.SD (Data.Reflection.D (Data.Reflection.D (Data.Reflection.D (Data.Reflection.SD Data.Reflection.Z))))))))) -data D143 -instance Dim D143 where - reflectDim _ = reflect (Proxy :: Proxy (Data.Reflection.SD (Data.Reflection.SD (Data.Reflection.SD (Data.Reflection.SD (Data.Reflection.D (Data.Reflection.D (Data.Reflection.D (Data.Reflection.SD Data.Reflection.Z))))))))) -data D144 -instance Dim D144 where - reflectDim _ = reflect (Proxy :: Proxy (Data.Reflection.D (Data.Reflection.D (Data.Reflection.D (Data.Reflection.D (Data.Reflection.SD (Data.Reflection.D (Data.Reflection.D (Data.Reflection.SD Data.Reflection.Z))))))))) -data D145 -instance Dim D145 where - reflectDim _ = reflect (Proxy :: Proxy (Data.Reflection.SD (Data.Reflection.D (Data.Reflection.D (Data.Reflection.D (Data.Reflection.SD (Data.Reflection.D (Data.Reflection.D (Data.Reflection.SD Data.Reflection.Z))))))))) -data D146 -instance Dim D146 where - reflectDim _ = reflect (Proxy :: Proxy (Data.Reflection.D (Data.Reflection.SD (Data.Reflection.D (Data.Reflection.D (Data.Reflection.SD (Data.Reflection.D (Data.Reflection.D (Data.Reflection.SD Data.Reflection.Z))))))))) -data D147 -instance Dim D147 where - reflectDim _ = reflect (Proxy :: Proxy (Data.Reflection.SD (Data.Reflection.SD (Data.Reflection.D (Data.Reflection.D (Data.Reflection.SD (Data.Reflection.D (Data.Reflection.D (Data.Reflection.SD Data.Reflection.Z))))))))) -data D148 -instance Dim D148 where - reflectDim _ = reflect (Proxy :: Proxy (Data.Reflection.D (Data.Reflection.D (Data.Reflection.SD (Data.Reflection.D (Data.Reflection.SD (Data.Reflection.D (Data.Reflection.D (Data.Reflection.SD Data.Reflection.Z))))))))) -data D149 -instance Dim D149 where - reflectDim _ = reflect (Proxy :: Proxy (Data.Reflection.SD (Data.Reflection.D (Data.Reflection.SD (Data.Reflection.D (Data.Reflection.SD (Data.Reflection.D (Data.Reflection.D (Data.Reflection.SD Data.Reflection.Z))))))))) -data D150 -instance Dim D150 where - reflectDim _ = reflect (Proxy :: Proxy (Data.Reflection.D (Data.Reflection.SD (Data.Reflection.SD (Data.Reflection.D (Data.Reflection.SD (Data.Reflection.D (Data.Reflection.D (Data.Reflection.SD Data.Reflection.Z))))))))) -data D151 -instance Dim D151 where - reflectDim _ = reflect (Proxy :: Proxy (Data.Reflection.SD (Data.Reflection.SD (Data.Reflection.SD (Data.Reflection.D (Data.Reflection.SD (Data.Reflection.D (Data.Reflection.D (Data.Reflection.SD Data.Reflection.Z))))))))) -data D152 -instance Dim D152 where - reflectDim _ = reflect (Proxy :: Proxy (Data.Reflection.D (Data.Reflection.D (Data.Reflection.D (Data.Reflection.SD (Data.Reflection.SD (Data.Reflection.D (Data.Reflection.D (Data.Reflection.SD Data.Reflection.Z))))))))) -data D153 -instance Dim D153 where - reflectDim _ = reflect (Proxy :: Proxy (Data.Reflection.SD (Data.Reflection.D (Data.Reflection.D (Data.Reflection.SD (Data.Reflection.SD (Data.Reflection.D (Data.Reflection.D (Data.Reflection.SD Data.Reflection.Z))))))))) -data D154 -instance Dim D154 where - reflectDim _ = reflect (Proxy :: Proxy (Data.Reflection.D (Data.Reflection.SD (Data.Reflection.D (Data.Reflection.SD (Data.Reflection.SD (Data.Reflection.D (Data.Reflection.D (Data.Reflection.SD Data.Reflection.Z))))))))) -data D155 -instance Dim D155 where - reflectDim _ = reflect (Proxy :: Proxy (Data.Reflection.SD (Data.Reflection.SD (Data.Reflection.D (Data.Reflection.SD (Data.Reflection.SD (Data.Reflection.D (Data.Reflection.D (Data.Reflection.SD Data.Reflection.Z))))))))) -data D156 -instance Dim D156 where - reflectDim _ = reflect (Proxy :: Proxy (Data.Reflection.D (Data.Reflection.D (Data.Reflection.SD (Data.Reflection.SD (Data.Reflection.SD (Data.Reflection.D (Data.Reflection.D (Data.Reflection.SD Data.Reflection.Z))))))))) -data D157 -instance Dim D157 where - reflectDim _ = reflect (Proxy :: Proxy (Data.Reflection.SD (Data.Reflection.D (Data.Reflection.SD (Data.Reflection.SD (Data.Reflection.SD (Data.Reflection.D (Data.Reflection.D (Data.Reflection.SD Data.Reflection.Z))))))))) -data D158 -instance Dim D158 where - reflectDim _ = reflect (Proxy :: Proxy (Data.Reflection.D (Data.Reflection.SD (Data.Reflection.SD (Data.Reflection.SD (Data.Reflection.SD (Data.Reflection.D (Data.Reflection.D (Data.Reflection.SD Data.Reflection.Z))))))))) -data D159 -instance Dim D159 where - reflectDim _ = reflect (Proxy :: Proxy (Data.Reflection.SD (Data.Reflection.SD (Data.Reflection.SD (Data.Reflection.SD (Data.Reflection.SD (Data.Reflection.D (Data.Reflection.D (Data.Reflection.SD Data.Reflection.Z))))))))) -data D160 -instance Dim D160 where - reflectDim _ = reflect (Proxy :: Proxy (Data.Reflection.D (Data.Reflection.D (Data.Reflection.D (Data.Reflection.D (Data.Reflection.D (Data.Reflection.SD (Data.Reflection.D (Data.Reflection.SD Data.Reflection.Z))))))))) -data D161 -instance Dim D161 where - reflectDim _ = reflect (Proxy :: Proxy (Data.Reflection.SD (Data.Reflection.D (Data.Reflection.D (Data.Reflection.D (Data.Reflection.D (Data.Reflection.SD (Data.Reflection.D (Data.Reflection.SD Data.Reflection.Z))))))))) -data D162 -instance Dim D162 where - reflectDim _ = reflect (Proxy :: Proxy (Data.Reflection.D (Data.Reflection.SD (Data.Reflection.D (Data.Reflection.D (Data.Reflection.D (Data.Reflection.SD (Data.Reflection.D (Data.Reflection.SD Data.Reflection.Z))))))))) -data D163 -instance Dim D163 where - reflectDim _ = reflect (Proxy :: Proxy (Data.Reflection.SD (Data.Reflection.SD (Data.Reflection.D (Data.Reflection.D (Data.Reflection.D (Data.Reflection.SD (Data.Reflection.D (Data.Reflection.SD Data.Reflection.Z))))))))) -data D164 -instance Dim D164 where - reflectDim _ = reflect (Proxy :: Proxy (Data.Reflection.D (Data.Reflection.D (Data.Reflection.SD (Data.Reflection.D (Data.Reflection.D (Data.Reflection.SD (Data.Reflection.D (Data.Reflection.SD Data.Reflection.Z))))))))) -data D165 -instance Dim D165 where - reflectDim _ = reflect (Proxy :: Proxy (Data.Reflection.SD (Data.Reflection.D (Data.Reflection.SD (Data.Reflection.D (Data.Reflection.D (Data.Reflection.SD (Data.Reflection.D (Data.Reflection.SD Data.Reflection.Z))))))))) -data D166 -instance Dim D166 where - reflectDim _ = reflect (Proxy :: Proxy (Data.Reflection.D (Data.Reflection.SD (Data.Reflection.SD (Data.Reflection.D (Data.Reflection.D (Data.Reflection.SD (Data.Reflection.D (Data.Reflection.SD Data.Reflection.Z))))))))) -data D167 -instance Dim D167 where - reflectDim _ = reflect (Proxy :: Proxy (Data.Reflection.SD (Data.Reflection.SD (Data.Reflection.SD (Data.Reflection.D (Data.Reflection.D (Data.Reflection.SD (Data.Reflection.D (Data.Reflection.SD Data.Reflection.Z))))))))) -data D168 -instance Dim D168 where - reflectDim _ = reflect (Proxy :: Proxy (Data.Reflection.D (Data.Reflection.D (Data.Reflection.D (Data.Reflection.SD (Data.Reflection.D (Data.Reflection.SD (Data.Reflection.D (Data.Reflection.SD Data.Reflection.Z))))))))) -data D169 -instance Dim D169 where - reflectDim _ = reflect (Proxy :: Proxy (Data.Reflection.SD (Data.Reflection.D (Data.Reflection.D (Data.Reflection.SD (Data.Reflection.D (Data.Reflection.SD (Data.Reflection.D (Data.Reflection.SD Data.Reflection.Z))))))))) -data D170 -instance Dim D170 where - reflectDim _ = reflect (Proxy :: Proxy (Data.Reflection.D (Data.Reflection.SD (Data.Reflection.D (Data.Reflection.SD (Data.Reflection.D (Data.Reflection.SD (Data.Reflection.D (Data.Reflection.SD Data.Reflection.Z))))))))) -data D171 -instance Dim D171 where - reflectDim _ = reflect (Proxy :: Proxy (Data.Reflection.SD (Data.Reflection.SD (Data.Reflection.D (Data.Reflection.SD (Data.Reflection.D (Data.Reflection.SD (Data.Reflection.D (Data.Reflection.SD Data.Reflection.Z))))))))) -data D172 -instance Dim D172 where - reflectDim _ = reflect (Proxy :: Proxy (Data.Reflection.D (Data.Reflection.D (Data.Reflection.SD (Data.Reflection.SD (Data.Reflection.D (Data.Reflection.SD (Data.Reflection.D (Data.Reflection.SD Data.Reflection.Z))))))))) -data D173 -instance Dim D173 where - reflectDim _ = reflect (Proxy :: Proxy (Data.Reflection.SD (Data.Reflection.D (Data.Reflection.SD (Data.Reflection.SD (Data.Reflection.D (Data.Reflection.SD (Data.Reflection.D (Data.Reflection.SD Data.Reflection.Z))))))))) -data D174 -instance Dim D174 where - reflectDim _ = reflect (Proxy :: Proxy (Data.Reflection.D (Data.Reflection.SD (Data.Reflection.SD (Data.Reflection.SD (Data.Reflection.D (Data.Reflection.SD (Data.Reflection.D (Data.Reflection.SD Data.Reflection.Z))))))))) -data D175 -instance Dim D175 where - reflectDim _ = reflect (Proxy :: Proxy (Data.Reflection.SD (Data.Reflection.SD (Data.Reflection.SD (Data.Reflection.SD (Data.Reflection.D (Data.Reflection.SD (Data.Reflection.D (Data.Reflection.SD Data.Reflection.Z))))))))) -data D176 -instance Dim D176 where - reflectDim _ = reflect (Proxy :: Proxy (Data.Reflection.D (Data.Reflection.D (Data.Reflection.D (Data.Reflection.D (Data.Reflection.SD (Data.Reflection.SD (Data.Reflection.D (Data.Reflection.SD Data.Reflection.Z))))))))) -data D177 -instance Dim D177 where - reflectDim _ = reflect (Proxy :: Proxy (Data.Reflection.SD (Data.Reflection.D (Data.Reflection.D (Data.Reflection.D (Data.Reflection.SD (Data.Reflection.SD (Data.Reflection.D (Data.Reflection.SD Data.Reflection.Z))))))))) -data D178 -instance Dim D178 where - reflectDim _ = reflect (Proxy :: Proxy (Data.Reflection.D (Data.Reflection.SD (Data.Reflection.D (Data.Reflection.D (Data.Reflection.SD (Data.Reflection.SD (Data.Reflection.D (Data.Reflection.SD Data.Reflection.Z))))))))) -data D179 -instance Dim D179 where - reflectDim _ = reflect (Proxy :: Proxy (Data.Reflection.SD (Data.Reflection.SD (Data.Reflection.D (Data.Reflection.D (Data.Reflection.SD (Data.Reflection.SD (Data.Reflection.D (Data.Reflection.SD Data.Reflection.Z))))))))) -data D180 -instance Dim D180 where - reflectDim _ = reflect (Proxy :: Proxy (Data.Reflection.D (Data.Reflection.D (Data.Reflection.SD (Data.Reflection.D (Data.Reflection.SD (Data.Reflection.SD (Data.Reflection.D (Data.Reflection.SD Data.Reflection.Z))))))))) -data D181 -instance Dim D181 where - reflectDim _ = reflect (Proxy :: Proxy (Data.Reflection.SD (Data.Reflection.D (Data.Reflection.SD (Data.Reflection.D (Data.Reflection.SD (Data.Reflection.SD (Data.Reflection.D (Data.Reflection.SD Data.Reflection.Z))))))))) -data D182 -instance Dim D182 where - reflectDim _ = reflect (Proxy :: Proxy (Data.Reflection.D (Data.Reflection.SD (Data.Reflection.SD (Data.Reflection.D (Data.Reflection.SD (Data.Reflection.SD (Data.Reflection.D (Data.Reflection.SD Data.Reflection.Z))))))))) -data D183 -instance Dim D183 where - reflectDim _ = reflect (Proxy :: Proxy (Data.Reflection.SD (Data.Reflection.SD (Data.Reflection.SD (Data.Reflection.D (Data.Reflection.SD (Data.Reflection.SD (Data.Reflection.D (Data.Reflection.SD Data.Reflection.Z))))))))) -data D184 -instance Dim D184 where - reflectDim _ = reflect (Proxy :: Proxy (Data.Reflection.D (Data.Reflection.D (Data.Reflection.D (Data.Reflection.SD (Data.Reflection.SD (Data.Reflection.SD (Data.Reflection.D (Data.Reflection.SD Data.Reflection.Z))))))))) -data D185 -instance Dim D185 where - reflectDim _ = reflect (Proxy :: Proxy (Data.Reflection.SD (Data.Reflection.D (Data.Reflection.D (Data.Reflection.SD (Data.Reflection.SD (Data.Reflection.SD (Data.Reflection.D (Data.Reflection.SD Data.Reflection.Z))))))))) -data D186 -instance Dim D186 where - reflectDim _ = reflect (Proxy :: Proxy (Data.Reflection.D (Data.Reflection.SD (Data.Reflection.D (Data.Reflection.SD (Data.Reflection.SD (Data.Reflection.SD (Data.Reflection.D (Data.Reflection.SD Data.Reflection.Z))))))))) -data D187 -instance Dim D187 where - reflectDim _ = reflect (Proxy :: Proxy (Data.Reflection.SD (Data.Reflection.SD (Data.Reflection.D (Data.Reflection.SD (Data.Reflection.SD (Data.Reflection.SD (Data.Reflection.D (Data.Reflection.SD Data.Reflection.Z))))))))) -data D188 -instance Dim D188 where - reflectDim _ = reflect (Proxy :: Proxy (Data.Reflection.D (Data.Reflection.D (Data.Reflection.SD (Data.Reflection.SD (Data.Reflection.SD (Data.Reflection.SD (Data.Reflection.D (Data.Reflection.SD Data.Reflection.Z))))))))) -data D189 -instance Dim D189 where - reflectDim _ = reflect (Proxy :: Proxy (Data.Reflection.SD (Data.Reflection.D (Data.Reflection.SD (Data.Reflection.SD (Data.Reflection.SD (Data.Reflection.SD (Data.Reflection.D (Data.Reflection.SD Data.Reflection.Z))))))))) -data D190 -instance Dim D190 where - reflectDim _ = reflect (Proxy :: Proxy (Data.Reflection.D (Data.Reflection.SD (Data.Reflection.SD (Data.Reflection.SD (Data.Reflection.SD (Data.Reflection.SD (Data.Reflection.D (Data.Reflection.SD Data.Reflection.Z))))))))) -data D191 -instance Dim D191 where - reflectDim _ = reflect (Proxy :: Proxy (Data.Reflection.SD (Data.Reflection.SD (Data.Reflection.SD (Data.Reflection.SD (Data.Reflection.SD (Data.Reflection.SD (Data.Reflection.D (Data.Reflection.SD Data.Reflection.Z))))))))) -data D192 -instance Dim D192 where - reflectDim _ = reflect (Proxy :: Proxy (Data.Reflection.D (Data.Reflection.D (Data.Reflection.D (Data.Reflection.D (Data.Reflection.D (Data.Reflection.D (Data.Reflection.SD (Data.Reflection.SD Data.Reflection.Z))))))))) -data D193 -instance Dim D193 where - reflectDim _ = reflect (Proxy :: Proxy (Data.Reflection.SD (Data.Reflection.D (Data.Reflection.D (Data.Reflection.D (Data.Reflection.D (Data.Reflection.D (Data.Reflection.SD (Data.Reflection.SD Data.Reflection.Z))))))))) -data D194 -instance Dim D194 where - reflectDim _ = reflect (Proxy :: Proxy (Data.Reflection.D (Data.Reflection.SD (Data.Reflection.D (Data.Reflection.D (Data.Reflection.D (Data.Reflection.D (Data.Reflection.SD (Data.Reflection.SD Data.Reflection.Z))))))))) -data D195 -instance Dim D195 where - reflectDim _ = reflect (Proxy :: Proxy (Data.Reflection.SD (Data.Reflection.SD (Data.Reflection.D (Data.Reflection.D (Data.Reflection.D (Data.Reflection.D (Data.Reflection.SD (Data.Reflection.SD Data.Reflection.Z))))))))) -data D196 -instance Dim D196 where - reflectDim _ = reflect (Proxy :: Proxy (Data.Reflection.D (Data.Reflection.D (Data.Reflection.SD (Data.Reflection.D (Data.Reflection.D (Data.Reflection.D (Data.Reflection.SD (Data.Reflection.SD Data.Reflection.Z))))))))) -data D197 -instance Dim D197 where - reflectDim _ = reflect (Proxy :: Proxy (Data.Reflection.SD (Data.Reflection.D (Data.Reflection.SD (Data.Reflection.D (Data.Reflection.D (Data.Reflection.D (Data.Reflection.SD (Data.Reflection.SD Data.Reflection.Z))))))))) -data D198 -instance Dim D198 where - reflectDim _ = reflect (Proxy :: Proxy (Data.Reflection.D (Data.Reflection.SD (Data.Reflection.SD (Data.Reflection.D (Data.Reflection.D (Data.Reflection.D (Data.Reflection.SD (Data.Reflection.SD Data.Reflection.Z))))))))) -data D199 -instance Dim D199 where - reflectDim _ = reflect (Proxy :: Proxy (Data.Reflection.SD (Data.Reflection.SD (Data.Reflection.SD (Data.Reflection.D (Data.Reflection.D (Data.Reflection.D (Data.Reflection.SD (Data.Reflection.SD Data.Reflection.Z))))))))) -data D200 -instance Dim D200 where - reflectDim _ = reflect (Proxy :: Proxy (Data.Reflection.D (Data.Reflection.D (Data.Reflection.D (Data.Reflection.SD (Data.Reflection.D (Data.Reflection.D (Data.Reflection.SD (Data.Reflection.SD Data.Reflection.Z))))))))) -data D500 -instance Dim D500 where - reflectDim _ = reflect (Proxy :: Proxy (Data.Reflection.D (Data.Reflection.D (Data.Reflection.SD (Data.Reflection.D (Data.Reflection.SD (Data.Reflection.SD (Data.Reflection.SD (Data.Reflection.SD (Data.Reflection.SD Data.Reflection.Z)))))))))) -data D1000 -instance Dim D1000 where - reflectDim _ = reflect (Proxy :: Proxy (Data.Reflection.D (Data.Reflection.D (Data.Reflection.D (Data.Reflection.SD (Data.Reflection.D (Data.Reflection.SD (Data.Reflection.SD (Data.Reflection.SD (Data.Reflection.SD (Data.Reflection.SD Data.Reflection.Z))))))))))) -data D1500 -instance Dim D1500 where - reflectDim _ = reflect (Proxy :: Proxy (Data.Reflection.D (Data.Reflection.D (Data.Reflection.SD (Data.Reflection.SD (Data.Reflection.SD (Data.Reflection.D (Data.Reflection.SD (Data.Reflection.SD (Data.Reflection.SD (Data.Reflection.D (Data.Reflection.SD Data.Reflection.Z)))))))))))) -data D2000 -instance Dim D2000 where - reflectDim _ = reflect (Proxy :: Proxy (Data.Reflection.D (Data.Reflection.D (Data.Reflection.D (Data.Reflection.D (Data.Reflection.SD (Data.Reflection.D (Data.Reflection.SD (Data.Reflection.SD (Data.Reflection.SD (Data.Reflection.SD (Data.Reflection.SD Data.Reflection.Z)))))))))))) - diff --git a/src/Dyno/Nlp.hs b/src/Dyno/Nlp.hs --- a/src/Dyno/Nlp.hs +++ b/src/Dyno/Nlp.hs @@ -10,11 +10,13 @@ ) where import GHC.Generics ( Generic, Generic1 ) + import qualified Data.Vector as V import Data.Serialize ( Serialize(..) ) -import Dyno.Vectorize ( Vectorize(..) ) -import Dyno.View.View ( View(..), J, S, unJ, mkJ ) +import Dyno.Vectorize ( Vectorize(..), Id ) +import Dyno.View.View ( View(..), J ) +import Dyno.View.JV ( JV ) type Bounds = (Maybe Double, Maybe Double) @@ -58,7 +60,7 @@ -- | NLP using Views data NlpOut' x g a = NlpOut' - { fOpt' :: J S a + { fOpt' :: J (JV Id) a , xOpt' :: J x a , gOpt' :: J g a , lambdaXOpt' :: J x a @@ -66,13 +68,13 @@ } deriving (Eq, Show, Generic) instance (View x, View g) => View (NlpOut' x g) instance (View x, View g, Serialize a) => Serialize (NlpOut' x g (V.Vector a)) where - put = put . V.toList . unJ . cat - get = fmap (split . mkJ . V.fromList) get + put = put . cat + get = fmap split get data Nlp' x p g a = Nlp' - { nlpFG' :: J x a -> J p a -> (J S a, J g a) + { nlpFG' :: J x a -> J p a -> (J (JV Id) a, J g a) , nlpBX' :: J x (V.Vector Bounds) , nlpBG' :: J g (V.Vector Bounds) , nlpX0' :: J x (V.Vector Double) diff --git a/src/Dyno/NlpMonad.hs b/src/Dyno/NlpMonad.hs deleted file mode 100644 --- a/src/Dyno/NlpMonad.hs +++ /dev/null @@ -1,231 +0,0 @@ -{-# OPTIONS_GHC -Wall #-} -{-# Language ScopedTypeVariables #-} -{-# Language PackageImports #-} -{-# Language GeneralizedNewtypeDeriving #-} -{-# Language RankNTypes #-} - -module Dyno.NlpMonad - ( NlpMonad - , (===) - , (<==) - , (>==) - , bound - , minimize - , designVar - , solveStaticNlp - ) where - -import Control.Applicative ( Applicative ) -import Control.Monad ( when ) -import "mtl" Control.Monad.Reader ( MonadIO(..) ) -import "mtl" Control.Monad.Except ( ExceptT, MonadError, runExceptT ) -import "mtl" Control.Monad.State ( StateT, MonadState, runStateT, get, put ) -import "mtl" Control.Monad.Writer ( WriterT, MonadWriter, runWriterT ) -import qualified Data.Foldable as F -import qualified Data.HashSet as HS -import qualified Data.Sequence as S -import qualified Data.Map.Strict as M -import Data.Sequence ( (|>) ) -import Data.Vector ( Vector ) -import qualified Data.Vector as V -import Linear.V ( Dim(..) ) -import Data.Proxy - -import Casadi.SharedObject ( soInit ) -import Casadi.MX ( MX ) -import Casadi.SXFunction -import Casadi.Function - -import Dyno.View.CasadiMat ( veccat ) -import Dyno.SXElement ( SXElement, sxElementSym, sxElementToSX ) -import Dyno.Vectorize -import Dyno.TypeVecs ( Vec ) -import Dyno.View.View -import qualified Dyno.TypeVecs as TV -import Dyno.Interface.LogsAndErrors -import Dyno.Interface.Types -import Dyno.NlpSolver ( NlpSolverStuff, solveNlp' ) -import Dyno.Nlp ( Nlp'(..), NlpOut'(..), Bounds) - ---withEllipse :: Int -> String -> String ---withEllipse n blah --- | length blah <= n = blah --- | otherwise = take n blah ++ "..." - -newtype NlpMonad a = - NlpMonad - { runNlp :: ExceptT ErrorMessage (WriterT [LogMessage] (StateT NlpMonadState IO)) a - } deriving ( Functor - , Applicative - , Monad - , MonadError ErrorMessage - , MonadState NlpMonadState - , MonadWriter [LogMessage] - , MonadIO - ) - -emptySymbolicNlp :: NlpMonadState -emptySymbolicNlp = NlpMonadState S.empty HS.empty S.empty ObjectiveUnset HomotopyParamUnset - -build :: NlpMonad a -> IO (Either ErrorMessage a, [LogMessage], NlpMonadState) -build = build' emptySymbolicNlp - where - build' :: NlpMonadState -> NlpMonad a -> IO (Either ErrorMessage a, [LogMessage], NlpMonadState) - build' nlp0 builder = do - ((result,logs),state) <- flip runStateT nlp0 . runWriterT . runExceptT . runNlp $ builder - return (result, logs, state) - -designVar :: String -> NlpMonad SXElement -designVar name = do - debug $ "adding design variable \""++name++"\"" - state0 <- get - let map0 = nlpXSet state0 - sym <- liftIO (sxElementSym name) - when (HS.member name map0) $ err $ name ++ " already in symbol map" - let state1 = state0 { nlpX = nlpX state0 |> (name, sym) - , nlpXSet = HS.insert name map0 - } - put state1 - return sym - -infix 4 === -(===) :: SXElement -> SXElement -> NlpMonad () -(===) lhs rhs = do - debug $ "adding equality constraint: " --- ++ withEllipse 30 (show lhs) ++ " == " ++ withEllipse 30 (show rhs) - state0 <- get - put $ state0 { nlpConstraints = nlpConstraints state0 |> Eq2 lhs rhs } - -infix 4 <== -(<==) :: SXElement -> SXElement -> NlpMonad () -(<==) lhs rhs = do - debug $ "adding inequality constraint: " --- ++ withEllipse 30 (show lhs) ++ " <= " ++ withEllipse 30 (show rhs) - state0 <- get - put $ state0 { nlpConstraints = nlpConstraints state0 |> Ineq2 lhs rhs } - -infix 4 >== -(>==) :: SXElement -> SXElement -> NlpMonad () -(>==) lhs rhs = do - debug $ "adding inequality constraint: " --- ++ withEllipse 30 (show lhs) ++ " >= " ++ withEllipse 30 (show rhs) - state0 <- get - put $ state0 { nlpConstraints = nlpConstraints state0 |> Ineq2 rhs lhs } - -bound :: SXElement -> (Double,Double) -> NlpMonad () -bound mid (lhs, rhs) = do - debug $ "adding inequality bound: " -- ++ --- withEllipse 30 (show lhs) ++ " <= " ++ --- withEllipse 30 (show mid) ++ " <= " ++ --- withEllipse 30 (show rhs) - state0 <- get - put $ state0 { nlpConstraints = nlpConstraints state0 |> Ineq3 mid (lhs, rhs) } - -minimize :: SXElement -> NlpMonad () -minimize obj = do - debug $ "setting objective function: " -- ++ withEllipse 30 (show obj) - state0 <- get - case nlpObj state0 of - Objective _x -> err $ init $ unlines - [ "you set the objective function twice" --- , " old val: " ++ show x --- , " new val: " ++ show obj - ] - ObjectiveUnset -> put $ state0 { nlpObj = Objective obj } - - -constr :: Constraint SXElement -> (SXElement, Bounds) -constr (Eq2 lhs rhs) = (lhs - rhs, (Just 0, Just 0)) -constr (Ineq2 lhs rhs) = (lhs - rhs, (Nothing, Just 0)) -constr (Ineq3 x (lhs,rhs)) = (x, (Just lhs, Just rhs)) - - -toG :: Dim ng => S.Seq (Constraint SXElement) -> Vec ng (SXElement, Bounds) -toG nlpConstraints' = TV.mkSeq $ fmap constr nlpConstraints' - -buildNlp :: forall nx ng . - (Dim nx, Dim ng) => NlpMonadState -> IO (Nlp' (JVec nx S) JNone (JVec ng S) MX) -buildNlp state = do - obj <- case nlpObj state of - Objective obj' -> return obj' - ObjectiveUnset -> error "solveNlp: objective unset" - - let inputs :: Vector SXElement - inputs = V.fromList $ map snd $ F.toList (nlpX state) - - g :: Vec ng SXElement - gbnd :: Vec ng Bounds - (g, gbnd) = TV.tvunzip $ toG (nlpConstraints state) - - xbnd :: Vec nx Bounds - xbnd = fill (Nothing, Nothing) - - svector = veccat . fmap sxElementToSX - - sxfun <- sxFunction (V.fromList [svector inputs]) (V.fromList [svector (V.singleton obj), svector (TV.unVec g)]) - soInit sxfun - let fg :: J (JVec nx S) MX -> J JNone MX -> (J S MX, J (JVec ng S) MX) - fg x _ = (mkJ (ret V.! 0), mkJ (ret V.! 1)) - where - ret = callMX sxfun (V.singleton (unJ x)) - - return Nlp' { nlpFG' = fg - , nlpBX' = mkJ (TV.unVec xbnd) - , nlpBG' = mkJ (TV.unVec gbnd) - , nlpX0' = jfill 0 - , nlpP' = cat JNone - , nlpScaleF' = Nothing - , nlpScaleX' = Nothing - , nlpScaleG' = Nothing - , nlpLamX0' = Nothing - , nlpLamG0' = Nothing - } - - -reifyNlp :: - forall r . - NlpMonad () -> Maybe (Vector Double -> IO Bool) -> M.Map String Double - -> (forall x g . (View x, View g) - => Nlp' x JNone g MX -> Maybe (J x (Vector Double) -> IO Bool) -> NlpMonadState -> IO r) - -> IO r -reifyNlp nlpmonad cb x0map f = do - (ret,logs,state) <- build nlpmonad - case ret of - Right _ -> return () - Left err' -> error $ unlines $ map show logs ++ [show err'] - - let nx = S.length (nlpX state) - ng = S.length (nlpConstraints state) - - lookupGuess = flip (M.findWithDefault 0) x0map - x0 = V.fromList $ map (lookupGuess . fst) $ F.toList (nlpX state) - - TV.reifyDim nx $ \(Proxy :: Proxy nx) -> --- TV.reifyDim np $ \(Proxy :: Proxy np) -> - TV.reifyDim ng $ \(Proxy :: Proxy ng) -> do - nlp0 <- buildNlp state :: IO (Nlp' (JVec nx S) JNone (JVec ng S) MX) - let nlp = nlp0 { nlpX0' = mkJ x0 } - f nlp (fmap (. unJ) cb) state - - -solveStaticNlp :: - NlpSolverStuff - -> NlpMonad () -> [(String,Double)] -> Maybe (Vector Double -> IO Bool) - -> IO (Either String String, Double, [(String,Double)]) -solveStaticNlp solverStuff nlp x0' callback = reifyNlp nlp callback x0 foo - where - x0 = M.fromListWithKey errlol x0' - errlol name xx yy = - error $ "solveStaticNlp: initial guess has variable \"" ++ name ++ "\" more than once: " ++ - show (xx,yy) - - foo :: - (View x, View p, View g) => - Nlp' x p g MX -> Maybe (J x (Vector Double) -> IO Bool) -> NlpMonadState -> - IO (Either String String, Double, [(String,Double)]) - foo nlp' cb' state = do - (ret,nlpOut) <- solveNlp' solverStuff nlp' cb' - let fopt = V.head (unJ (fOpt' nlpOut)) :: Double - xopt = F.toList $ unJ (xOpt' nlpOut) :: [Double] - xnames = map fst (F.toList (nlpX state)) :: [String] - return (ret, fopt, zip xnames xopt) diff --git a/src/Dyno/NlpScaling.hs b/src/Dyno/NlpScaling.hs --- a/src/Dyno/NlpScaling.hs +++ b/src/Dyno/NlpScaling.hs @@ -10,14 +10,19 @@ import Data.Maybe ( fromMaybe ) import qualified Data.Vector as V -import Dyno.View.View +import Casadi.CMatrix ( CMatrix, fromDVector ) + +import Dyno.View.Unsafe.View ( unJ, mkJ ) + +import Dyno.Vectorize ( Id ) +import Dyno.View.View ( View, J ) +import Dyno.View.JV ( JV ) import Dyno.View.Viewable ( Viewable ) -import Dyno.View.CasadiMat ( CasadiMat(..) ) data ScaleFuns x g a = ScaleFuns - { fToFBar :: J S a -> J S a - , fbarToF :: J S a -> J S a + { fToFBar :: J (JV Id) a -> J (JV Id) a + , fbarToF :: J (JV Id) a -> J (JV Id) a , xToXBar :: J x a -> J x a , xbarToX :: J x a -> J x a , gToGBar :: J g a -> J g a @@ -30,12 +35,12 @@ scaledFG :: forall x p g a . - (View x, View g, CasadiMat a, Viewable a) + (View x, View g, CMatrix a, Viewable a) => ScaleFuns x g a - -> (J x a -> J p a -> (J S a, J g a)) + -> (J x a -> J p a -> (J (JV Id) a, J g a)) -> J x a -> J p a - -> (J S a, J g a) + -> (J (JV Id) a, J g a) scaledFG scaleFuns fg x p = (fToFBar scaleFuns f, gToGBar scaleFuns g) where (f, g) = fg (xbarToX scaleFuns x) p @@ -45,7 +50,7 @@ mkScaleFuns :: forall x g a . - (View x, View g, CasadiMat a, Viewable a) + (View x, View g, CMatrix a, Viewable a) => Maybe (J x (V.Vector Double)) -> Maybe (J g (V.Vector Double)) -> Maybe Double @@ -91,8 +96,8 @@ divByXScale :: J x a -> J x a (mulByXScale, divByXScale) = case mx of Nothing -> (id, id) - Just xscl -> ( \(UnsafeJ x') -> mkJ (x' * s) - , \(UnsafeJ x') -> mkJ (x' / s) + Just xscl -> ( mkJ . (* s) . unJ + , mkJ . (/ s) . unJ ) where s :: a @@ -102,19 +107,19 @@ divByGScale :: J g a -> J g a (mulByGScale, divByGScale) = case mg of Nothing -> (id, id) - Just gscl -> ( \(UnsafeJ g') -> mkJ (g' * s) - , \(UnsafeJ g') -> mkJ (g' / s) + Just gscl -> ( mkJ . (* s) . unJ + , mkJ . (/ s) . unJ ) where s :: a s = fromDVector (unJ gscl) - mulByFScale :: J S a -> J S a - divByFScale :: J S a -> J S a + mulByFScale :: J (JV Id) a -> J (JV Id) a + divByFScale :: J (JV Id) a -> J (JV Id) a (mulByFScale, divByFScale) = case mf of Nothing -> (id, id) - Just fscl -> ( \(UnsafeJ f') -> mkJ (f' * s) - , \(UnsafeJ f') -> mkJ (f' / s) + Just fscl -> ( mkJ . (* s) . unJ + , mkJ . (/ s) . unJ ) where s :: a diff --git a/src/Dyno/NlpSolver.hs b/src/Dyno/NlpSolver.hs --- a/src/Dyno/NlpSolver.hs +++ b/src/Dyno/NlpSolver.hs @@ -9,6 +9,7 @@ ( NlpSolver , SXElement , runNlpSolver + , runNlp -- * solve , solveNlp , solveNlp' @@ -43,6 +44,7 @@ , Op.Opt(..) , setOption , reinit + , MonadIO , liftIO , generateAndCompile ) where @@ -67,21 +69,23 @@ import qualified Casadi.Core.Classes.IOInterfaceFunction as C import Casadi.Callback ( makeCallback ) -import Casadi.DMatrix -import Casadi.SX +import Casadi.DMatrix ( DMatrix, ddata ) +import Casadi.SX ( SX ) import Casadi.Function ( Function, externalFunction ) import qualified Casadi.Option as Op import qualified Casadi.GenericC as Gen import Casadi.SharedObject ( soInit ) +import Casadi.CMatrix ( CMatrix ) +import qualified Casadi.CMatrix as CM -import Dyno.SXElement ( SXElement, sxElementToSX ) -import Dyno.Vectorize ( Vectorize(..) ) -import Dyno.View.JV -import Dyno.View.View -import Dyno.View.Symbolic +import Dyno.View.Unsafe.View ( unJ, mkJ ) + +import Dyno.SXElement ( SXElement, sxSplitJV, sxCatJV ) +import Dyno.Vectorize ( Vectorize(..), Id(..) ) +import Dyno.View.JV ( JV ) +import Dyno.View.View ( View(..), J, JNone(..), JTuple(..), jfill, unzipJ, fmapJ ) +import Dyno.View.Symbolic ( Symbolic, sym, mkScheme, mkFunction ) import Dyno.View.Viewable ( Viewable ) -import Dyno.View.CasadiMat ( CasadiMat ) -import qualified Dyno.View.CasadiMat as CM import Dyno.Nlp ( Nlp(..), NlpOut(..), Nlp'(..), NlpOut'(..), Bounds ) import Dyno.NlpScaling ( ScaleFuns(..), scaledFG, mkScaleFuns ) import Data.Proxy @@ -115,7 +119,7 @@ setInput scaleFun getLen name x0 = do nlpState <- ask let x = unJ $ scaleFun (isScale nlpState) $ mkJ $ CM.fromDVector (unJ x0) - let nActual = (dsize1 x, dsize2 x) + let nActual = (CM.size1 x, CM.size2 x) nTypeLevel = (getLen nlpState, 1) when (nTypeLevel /= nActual) $ error $ name ++ " dimension mismatch, " ++ show nTypeLevel ++ @@ -130,10 +134,10 @@ inf = read "Infinity" toLb :: View x => J x (Vector (Maybe Double)) -> J x (Vector Double) -toLb = mkJ . V.map (fromMaybe (-inf)) . unJ +toLb = fmapJ (fromMaybe (-inf)) toUb :: View x => J x (Vector (Maybe Double)) -> J x (Vector Double) -toUb = mkJ . V.map (fromMaybe inf ) . unJ +toUb = fmapJ (fromMaybe inf ) setLbx :: View x => VMD x -> NlpSolver x p g () setLbx = setInput xToXBar isNx "lbx" . toLb @@ -200,7 +204,7 @@ let scale = scaleFun (isScale nlpState) return (mkJ $ ddata $ unJ $ scale (mkJ dmat)) -getF :: NlpSolver x p g (VD S) +getF :: NlpSolver x p g (VD (JV Id)) getF = getOutput fbarToF "f" getX :: View x => NlpSolver x p g (VD x) @@ -318,19 +322,18 @@ forall x p g a s . (View x, View p, View g, Symbolic s) => NlpSolverStuff - -> (J x s -> J p s -> (J S s, J g s)) + -> (J x s -> J p s -> (J (JV Id) s, J g s)) -> Maybe (J x (Vector Double)) -> Maybe (J g (Vector Double)) -> Maybe Double -> Maybe (J x (Vector Double) -> IO Bool) -> NlpSolver x p g a -> IO a ---runNlpSolver solverStuff nlpFun nlpX0' callback' (NlpSolver nlpMonad) = do runNlpSolver solverStuff nlpFun scaleX scaleG scaleF callback' (NlpSolver nlpMonad) = do inputsX <- sym "x" inputsP <- sym "p" - let scale :: forall sfa . (CasadiMat sfa, Viewable sfa) => ScaleFuns x g sfa + let scale :: forall sfa . (CMatrix sfa, Viewable sfa) => ScaleFuns x g sfa scale = mkScaleFuns scaleX scaleG scaleF let (obj, g) = scaledFG scale nlpFun inputsX inputsP @@ -373,7 +376,7 @@ callbackRet <- case callback' of Nothing -> return True Just callback -> do - xval <- fmap (mkJ . ddata . unJ . xbarToX scale . mkJ . ddense) $ + xval <- fmap (mkJ . ddata . unJ . xbarToX scale . mkJ . CM.dense) $ C.ioInterfaceFunction_output__2 function' 0 callback xval interrupt <- readIORef intref @@ -428,7 +431,7 @@ nlp' = Nlp' { nlpFG' = \x' p' -> let x = sxSplitJV x' :: x SXElement p = sxSplitJV p' :: p SXElement (obj,g) = nlpFG nlp x p :: (SXElement, g SXElement) - obj' = mkJ (sxElementToSX obj) :: J S SX + obj' = sxCatJV (Id obj) :: J (JV Id) SX g' = sxCatJV g :: J (JV g) SX in (obj',g') , nlpBX' = mkJ $ vectorize (nlpBX nlp) :: J (JV x) (V.Vector Bounds) @@ -462,14 +465,6 @@ return (r0, r1) -fmapJ :: View x => (a -> b) -> J x (Vector a) -> J x (Vector b) -fmapJ f (UnsafeJ v) = mkJ (V.map f v) - -junzip :: View x => J x (Vector (a,b)) -> (J x (Vector a), J x (Vector b)) -junzip (UnsafeJ v) = (mkJ x, mkJ y) - where - (x,y) = V.unzip v - -- | convenience function to solve a pure Nlp' solveNlp' :: (View x, View p, View g, Symbolic a) @@ -477,10 +472,20 @@ -> Nlp' x p g a -> Maybe (J x (Vector Double) -> IO Bool) -> IO (Either String String, NlpOut' x g (Vector Double)) solveNlp' solverStuff nlp callback = --- runNlpSolver solverStuff (nlpFG' nlp) (nlpX0' nlp) callback $ do + runNlp solverStuff nlp callback solve' + + +-- | set all inputs, handle scaling, and let the user run a NlpMonad +runNlp :: + (View x, View p, View g, Symbolic a) + => NlpSolverStuff + -> Nlp' x p g a -> Maybe (J x (Vector Double) -> IO Bool) + -> NlpSolver x p g b + -> IO b +runNlp solverStuff nlp callback runMe = runNlpSolver solverStuff (nlpFG' nlp) (nlpScaleX' nlp) (nlpScaleG' nlp) (nlpScaleF' nlp) callback $ do - let (lbx,ubx) = junzip (nlpBX' nlp) - (lbg,ubg) = junzip (nlpBG' nlp) + let (lbx,ubx) = unzipJ (nlpBX' nlp) + (lbg,ubg) = unzipJ (nlpBG' nlp) setX0 (nlpX0' nlp) setP (nlpP' nlp) @@ -494,8 +499,7 @@ case nlpLamG0' nlp of Just lam -> setLamG0 lam Nothing -> return () - - solve' + runMe -- | solve a homotopy nlp solveNlpHomotopy' :: @@ -507,21 +511,21 @@ -> Maybe (J x (Vector Double) -> J p (Vector Double) -> Double -> IO ()) -> IO (Either String String, NlpOut' (JTuple x p) g (Vector Double)) solveNlpHomotopy' userStep (reduction, increase, iterIncrease, iterDecrease) - solverStuff nlp (UnsafeJ pF) callback callbackP = do + solverStuff nlp pF callback callbackP = do when (reduction >= 1) $ error $ "homotopy reduction factor " ++ show reduction ++ " >= 1" when (increase <= 1) $ error $ "homotopy increase factor " ++ show increase ++ " <= 1" - let fg :: J (JTuple x p) a -> J JNone a -> (J S a, J g a) + let fg :: J (JTuple x p) a -> J JNone a -> (J (JV Id) a, J g a) fg xp _ = nlpFG' nlp x p where JTuple x p = split xp runNlpSolver solverStuff fg Nothing (nlpScaleG' nlp) (nlpScaleF' nlp) callback $ do - let (lbx,ubx) = junzip (nlpBX' nlp) - (lbg,ubg) = junzip (nlpBG' nlp) - UnsafeJ p0 = nlpP' nlp + let (lbx,ubx) = unzipJ (nlpBX' nlp) + (lbg,ubg) = unzipJ (nlpBG' nlp) + p0 = unJ $ nlpP' nlp setAlpha :: Double -> NlpSolver (JTuple x p) JNone g () setAlpha alpha = do - let p = mkJ $ V.zipWith (+) p0 (V.map (alpha*) (V.zipWith (-) pF p0)) + let p = mkJ $ V.zipWith (+) p0 (V.map (alpha*) (V.zipWith (-) (unJ pF) p0)) setLbx $ cat (JTuple lbx (fmapJ Just p)) setUbx $ cat (JTuple ubx (fmapJ Just p)) diff --git a/src/Dyno/Ocp.hs b/src/Dyno/Ocp.hs --- a/src/Dyno/Ocp.hs +++ b/src/Dyno/Ocp.hs @@ -12,9 +12,9 @@ import Data.Vector ( Vector ) import Dyno.Vectorize ( Vectorize, None(..), fill ) -import Dyno.View.JV -import Dyno.View.View -import Dyno.Cov +import Dyno.View.JV ( JV ) +import Dyno.View.View ( J ) +import Dyno.View.Cov ( Cov ) import Dyno.Nlp ( Bounds ) import Dyno.SXElement ( SXElement ) diff --git a/src/Dyno/OcpMonad.hs b/src/Dyno/OcpMonad.hs deleted file mode 100644 --- a/src/Dyno/OcpMonad.hs +++ /dev/null @@ -1,496 +0,0 @@ -{-# OPTIONS_GHC -Wall #-} -{-# Language ScopedTypeVariables #-} -{-# Language PackageImports #-} -{-# Language MultiParamTypeClasses #-} -{-# Language FunctionalDependencies #-} -{-# Language GeneralizedNewtypeDeriving #-} -{-# Language FlexibleContexts #-} -{-# Language RankNTypes #-} - -module Dyno.OcpMonad - ( OcpMonad - , EqMonad(..) - , LeqMonad(..) - , DaeMonad - , BCMonad - , SXElement - , diffState - , algVar - , control - , parameter - , output - , lagrangeTerm - , solveStaticOcp - ) where - -import Control.Applicative ( Applicative(..) ) -import Control.Lens ( Lens', over ) -import Control.Monad ( when ) -import qualified "mtl" Control.Monad.State as State -import "mtl" Control.Monad.Reader ( MonadIO(..) ) -import "mtl" Control.Monad.Writer ( WriterT, Writer, MonadWriter, runWriterT, runWriter ) -import "mtl" Control.Monad.State ( StateT, MonadState, runStateT ) -import "mtl" Control.Monad.Except ( ExceptT, MonadError, runExceptT ) -import qualified Data.Foldable as F -import qualified Data.HashSet as HS -import qualified Data.Sequence as S -import qualified Data.Map as M -import Data.Sequence ( (|>) ) -import Data.Vector ( Vector ) -import qualified Data.Vector as V -import Data.Proxy ( Proxy(..) ) - -import Casadi.Option ( setOption ) -import Casadi.SXFunction ( sxFunction ) -import Casadi.SX ( SX ) -import Casadi.Function ( callSX ) -import Casadi.SharedObject ( soInit ) - -import qualified Dyno.View.CasadiMat as CM -import Dyno.SXElement ( SXElement, sxElementSym, sxElementToSX, sxToSXElement ) -import Dyno.Ocp ( OcpPhase(..) ) -import Dyno.Nlp ( Bounds ) -import Dyno.Vectorize ( Vectorize(..), fill ) -import Dyno.View.View ( mkJ ) -import Dyno.View.JV ( sxSplitJV ) -import Dyno.TypeVecs ( Vec ) -import qualified Dyno.TypeVecs as TV -import Dyno.NlpSolver ( NlpSolverStuff ) -import Dyno.DirectCollocation.Quadratures ( QuadratureRoots(..) ) -import Dyno.DirectCollocation.Dynamic ( DynCollTraj, CollTrajMeta(..), NameTree(..) ) -import Dyno.DirectCollocation ( solveOcp ) - -import Dyno.Interface.LogsAndErrors -import Dyno.Interface.Types - ---withEllipse :: Int -> String -> String ---withEllipse n blah --- | length blah <= n = blah --- | otherwise = take n blah ++ "..." - -newtype OcpMonad a = - OcpMonad - { runOcp :: ExceptT ErrorMessage (WriterT [LogMessage] (StateT OcpState IO)) a - } deriving ( Functor - , Applicative - , Monad - , MonadError ErrorMessage - , MonadState OcpState - , MonadWriter [LogMessage] - , MonadIO - ) - -newtype BCMonad a = - BCMonad - { runBc :: ExceptT ErrorMessage (WriterT [LogMessage] (StateT (S.Seq (Constraint SXElement)) IO)) a - } deriving ( Functor - , Applicative - , Monad - , MonadError ErrorMessage - , MonadState (S.Seq (Constraint SXElement)) - , MonadWriter [LogMessage] - , MonadIO - ) - -newtype DaeMonad a = - DaeMonad - { runDae :: ExceptT ErrorMessage (WriterT [LogMessage] (StateT DaeState IO)) a - } deriving ( Functor - , Applicative - , Monad - , MonadError ErrorMessage - , MonadState DaeState - , MonadWriter [LogMessage] - , MonadIO - ) - -emptySymbolicDae :: DaeState -emptySymbolicDae = DaeState S.empty S.empty S.empty S.empty S.empty M.empty HS.empty S.empty - -buildDae :: DaeMonad a -> IO (Either ErrorMessage a, [LogMessage], DaeState) -buildDae = buildDae' emptySymbolicDae - where - buildDae' :: DaeState -> DaeMonad a -> IO (Either ErrorMessage a, [LogMessage], DaeState) - buildDae' nlp0 builder = do - ((result,logs),state) <- flip runStateT nlp0 . runWriterT . runExceptT . runDae $ builder - return (result, logs, state) - -newDaeVariable :: - (MonadState DaeState m, MonadError ErrorMessage m, MonadWriter [LogMessage] m, MonadIO m) - => String -> Lens' DaeState (S.Seq (String, SXElement)) -> String -> m SXElement -newDaeVariable description lens name = do - debug $ "adding " ++ description ++ " \""++name++"\"" - case name of [] -> err "name cannot be empty" - ('_':_) -> err $ "name \"" ++ name ++ - "\" cannot have leading underscore (this is reserved for internal use)" - _ -> return () - state0 <- State.get - let map0 = daeNameSet state0 - sym <- liftIO (sxElementSym name) - when (HS.member name map0) $ err $ name ++ " already in name set" - let state1 = state0 { daeNameSet = HS.insert name map0 } - state2 = over lens (|> (name, sym)) state1 - State.put state2 - return sym - -svector :: Vector SXElement -> SX -svector = CM.vertcat . fmap sxElementToSX - -diffState :: String -> DaeMonad (SXElement, SXElement) -diffState name = do - x <- newDaeVariable "differential state" daeX name - xdot <- newDaeVariable "differential state derivative" daeXDot ("ddt( " ++ name ++ " )") - return (x, xdot) - -algVar :: String -> DaeMonad SXElement -algVar = newDaeVariable "algebraic variable" daeZ - -control :: String -> DaeMonad SXElement -control = newDaeVariable "control" daeU - -parameter :: String -> DaeMonad SXElement -parameter = newDaeVariable "parameter" daeP - -output :: String -> SXElement -> DaeMonad () -output name expr = do - debug $ "adding output \""++name++"\"" --- debug $ "adding output \""++name++"\": " ++ withEllipse 30 (show expr) - state0 <- State.get - let nameSet0 = daeNameSet state0 - outputs0 = _daeO state0 - when (HS.member name nameSet0) $ err $ name ++ " already in name set" - when (M.member name outputs0) $ impossible $ name ++ " already in output map" - let state1 = state0 { daeNameSet = HS.insert name nameSet0 - , _daeO = M.insert name expr outputs0 - } - State.put state1 - -infix 4 === -class EqMonad m a | m -> a where - (===) :: a -> a -> m () - -instance EqMonad DaeMonad SXElement where - (===) lhs rhs = do - debug $ "adding equality constraint: " --- ++ withEllipse 30 (show lhs) ++ " == " ++ withEllipse 30 (show rhs) - state0 <- State.get - State.put $ state0 { daeConstraints = daeConstraints state0 |> (lhs, rhs) } - -instance EqMonad OcpMonad SXElement where - (===) lhs rhs = do - debug $ "adding equality constraint: " --- ++ withEllipse 30 (show lhs) ++ " == " ++ withEllipse 30 (show rhs) - state0 <- State.get - State.put $ state0 { ocpPathConstraints = ocpPathConstraints state0 |> Eq2 lhs rhs } - - -infix 4 <== -class LeqMonad m where - (<==) :: SXElement -> SXElement -> m () - -instance LeqMonad OcpMonad where - (<==) lhs rhs = do - debug $ "adding inequality constraint: " --- ++ withEllipse 30 (show lhs) ++ " <= " ++ withEllipse 30 (show rhs) - state0 <- State.get - State.put $ state0 { ocpPathConstraints = ocpPathConstraints state0 |> Ineq2 lhs rhs } - -instance EqMonad BCMonad SXElement where - (===) lhs rhs = do - debug $ "adding inequality constraint: " - -- ++ withEllipse 30 (show lhs) ++ " == " ++ withEllipse 30 (show rhs) - state0 <- State.get - State.put $ state0 |> Eq2 lhs rhs - -instance LeqMonad BCMonad where - (<==) lhs rhs = do - debug $ "adding inequality constraint: " --- ++ withEllipse 30 (show lhs) ++ " <= " ++ withEllipse 30 (show rhs) - state0 <- State.get - State.put $ state0 |> Ineq2 lhs rhs - - -constr :: Constraint SXElement -> (SXElement, Bounds) -constr (Eq2 lhs rhs) = (lhs - rhs, (Just 0, Just 0)) -constr (Ineq2 lhs rhs) = (lhs - rhs, (Nothing, Just 0)) -constr (Ineq3 x (lhs,rhs)) = (x, (Just lhs, Just rhs)) - - - -lagrangeTerm :: SXElement -> OcpMonad () -lagrangeTerm obj = do - debug "setting lagrange term" - --debug $ "setting lagrange term: " ++ withEllipse 30 (show obj) - state0 <- State.get - case ocpLagrangeObj state0 of - Objective _x -> err $ init $ unlines - [ "you set the lagrange objective function twice" --- , " old val: " ++ withEllipse 30 (show x) --- , " new val: " ++ withEllipse 30 (show obj) - ] - ObjectiveUnset -> State.put $ state0 { ocpLagrangeObj = Objective obj } - - - -emptySymbolicOcp :: OcpState -emptySymbolicOcp = OcpState S.empty ObjectiveUnset HomotopyParamUnset - -reifyOcpPhase :: - forall ret . - (SXElement -> DaeMonad ()) - -> (forall a m . (Floating a, Monad m) => a -> (String -> m a) -> (String -> m a) -> m a) - -> ((String -> BCMonad SXElement) -> (String -> BCMonad SXElement) -> BCMonad ()) - -> (SXElement -> (String -> OcpMonad SXElement) -> OcpMonad ()) - -> (Maybe Double, Maybe Double) - -> Int -> Int - -> (forall x z u p r o c h . - (Vectorize x, Vectorize z, Vectorize u, Vectorize p, Vectorize r, Vectorize o, Vectorize c, Vectorize h) - => OcpPhase x z u p r o c h -> CollTrajMeta -> IO ret) - -> IO ret -reifyOcpPhase daeMonad mayerMonad bcMonad ocpMonad tbnds n deg f = do - time <- sxElementSym "_t" - endT <- sxElementSym "T" - let time' = sxElementToSX time - endT' = sxElementToSX endT - dae' <- buildDae (daeMonad time) - let dae :: DaeState - dae = case dae' of - (Left errmsg, _, _) -> error $ "buildOcpPhase: buildDae failure: " ++ show errmsg - (_, _, daeState) -> daeState - - xdotnames, xnames, znames, unames, pnames :: Vector String - xdots, xs, zs, us, ps :: Vector SXElement - (xdotnames,xdots) = V.unzip $ V.fromList $ F.toList $ _daeXDot dae - (xnames,xs) = V.unzip $ V.fromList $ F.toList $ _daeX dae - (znames,zs) = V.unzip $ V.fromList $ F.toList $ _daeZ dae - (unames,us) = V.unzip $ V.fromList $ F.toList $ _daeU dae - (pnames,ps) = V.unzip $ V.fromList $ F.toList $ _daeP dae - - xdots' = svector xdots - xs' = svector xs - zs' = svector zs - us' = svector us - ps' = svector ps - - daeResidual :: Vector SXElement - daeResidual = V.map (uncurry (-)) $ V.fromList $ F.toList $ daeConstraints dae - - onames :: Vector String - osOut :: Vector SXElement - (onames, osOut) = V.unzip $ V.fromList $ M.toList $ _daeO dae - os <- V.mapM sxElementSym onames :: IO (Vector SXElement) - let os' = svector os - - lookupThingy :: String -> OcpMonad SXElement - lookupThingy name = do - debug $ "ocp monad: looking up \"" ++ name ++ "\"" - case M.lookup name varmap of - Nothing -> err $ "ocp monad: nothing named \"" ++ name ++ "\"" - Just expr -> do - debug $ "ocp monad: found \"" ++ name ++ "\"" - --debug $ "ocp monad: found \"" ++ name ++ "\": " ++ show expr - return expr - where - varmap :: M.Map String SXElement - varmap = M.fromList $ F.toList $ V.concat - [ V.zip xdotnames xdots - , V.zip xnames xs - , V.zip znames zs - , V.zip unames us - , V.zip pnames ps - , V.zip onames os - ] - - ocp' <- flip runStateT emptySymbolicOcp $ runWriterT $ runExceptT (runOcp (ocpMonad time lookupThingy)) - let ocp :: OcpState - ocp = case ocp' of - ((Left errmsg, logs),_) -> - error $ unlines $ ("" : map show logs) ++ ["","ocp monad failure: " ++ show errmsg] - ((Right _, _), ocpState) -> ocpState - - obj = case ocpLagrangeObj ocp of - ObjectiveUnset -> 0 - Objective obj' -> obj' - - lagFunSX <- sxFunction (V.fromList [xs',zs',us',ps',os',time',endT']) (V.fromList [svector (V.singleton obj)]) - setOption lagFunSX "name" "lagrange" - soInit lagFunSX - - let pathConstraints :: [SXElement] - pathConstraintBnds :: [(Maybe Double, Maybe Double)] - (pathConstraints, pathConstraintBnds) = unzip $ map constr (F.toList (ocpPathConstraints ocp)) - - pathcFunSX <- sxFunction (V.fromList [xs',zs',us',ps',os',time']) - (V.singleton (svector (V.fromList pathConstraints))) - setOption pathcFunSX "name" "pathConstraints" - soInit pathcFunSX - - - daeFunSX <- sxFunction (V.fromList [xdots', xs', zs', us', ps', time']) - (V.fromList [svector daeResidual, svector osOut]) - setOption pathcFunSX "name" "daeResidualAndOutputs" - soInit daeFunSX - - -- run the mayer function - x0s <- mapM (sxElementSym . (++ "_0")) (F.toList xnames) - xFs <- mapM (sxElementSym . (++ "_F")) (F.toList xnames) - let lookupState :: M.Map String SXElement -> String - -> ExceptT ErrorMessage (Writer [LogMessage]) SXElement - lookupState xmap name = do - debug $ "mayer monad: looking up \"" ++ name ++ "\"" - case M.lookup name xmap of - Nothing -> err $ "mayer monad: no state named \"" ++ name ++ "\"" - Just expr -> do - debug $ "mayer monad: found \"" ++ name ++ "\"" - return expr - - xmap0 :: M.Map String SXElement - xmap0 = M.fromList $ zip (F.toList xnames) x0s - - xmapF :: M.Map String SXElement - xmapF = M.fromList $ zip (F.toList xnames) xFs - - mayerObj :: SXElement - mayerObj = case runWriter (runExceptT (mayerMonad endT (lookupState xmap0) (lookupState xmapF))) of - (Left errmsg, logs) -> - error $ unlines $ ("" : map show logs) ++ ["","mayer monad failure: " ++ show errmsg] - (Right ret, _) -> ret - mayerFunSX <- sxFunction (V.fromList [svector (V.singleton endT), svector (V.fromList x0s), svector (V.fromList xFs)]) - (V.singleton (svector (V.singleton mayerObj))) - setOption mayerFunSX "name" "mayer" - soInit mayerFunSX - - - let lookupState0 :: String -> BCMonad SXElement - lookupState0 name = do - debug $ "boundary condition monad: looking up initial \"" ++ name ++ "\"" - case M.lookup name xmap0 of - Nothing -> err $ "boundary condition monad: no state named \"" ++ name ++ "\"" - Just expr -> do - debug $ "boundary condition monad: found \"" ++ name ++ "\"" - return expr - - lookupStateF :: String -> BCMonad SXElement - lookupStateF name = do - debug $ "boundary condition monad: looking up final \"" ++ name ++ "\"" - case M.lookup name xmapF of - Nothing -> err $ "boundary condition monad: no state named \"" ++ name ++ "\"" - Just expr -> do - debug $ "boundary condition monad: found \"" ++ name ++ "\"" - return expr - bcs' <- flip runStateT S.empty $ runWriterT (runExceptT (runBc $ bcMonad lookupState0 lookupStateF)) - let bcs :: Vector SXElement - bcbnds :: Vector Bounds - (bcs,bcbnds) = case bcs' of - ((Left errmsg, logs),_) -> - error $ unlines $ ("" : map show logs) ++ ["","boundary condition monad failure: " ++ show errmsg] - ((Right _,_), ret) -> V.unzip $ V.fromList $ map constr $ F.toList ret - bcFunSX <- sxFunction (V.fromList [svector (V.fromList x0s), svector (V.fromList xFs)]) - (V.singleton (svector bcs)) - setOption bcFunSX "name" "boundaryConditions" - soInit bcFunSX - - let meta = CollTrajMeta - { ctmX = NameTreeNode ("", "") (zip (F.toList xnames) (map NameTreeLeaf [0..])) - , ctmZ = NameTreeNode ("", "") (zip (F.toList znames) (map NameTreeLeaf [0..])) - , ctmU = NameTreeNode ("", "") (zip (F.toList unames) (map NameTreeLeaf [0..])) - , ctmP = NameTreeNode ("", "") (zip (F.toList pnames) (map NameTreeLeaf [0..])) - , ctmO = NameTreeNode ("", "") (zip (F.toList onames) (map NameTreeLeaf [0..])) - , ctmN = n - , ctmDeg = deg - , ctmNx = V.length xnames - , ctmNz = V.length znames - , ctmNu = V.length unames - , ctmNp = V.length pnames - , ctmNo = V.length onames - , ctmNsx = 0 - , ctmQuadRoots = Legendre -- TODO: make this an input - } - TV.reifyDim (ctmNx meta) $ \(Proxy :: Proxy nx) -> - TV.reifyDim (ctmNz meta) $ \(Proxy :: Proxy nz) -> - TV.reifyDim (ctmNu meta) $ \(Proxy :: Proxy nu) -> - TV.reifyDim (ctmNp meta) $ \(Proxy :: Proxy np) -> - TV.reifyDim (V.length daeResidual) $ \(Proxy :: Proxy nr) -> - TV.reifyDim (V.length onames) $ \(Proxy :: Proxy no) -> - TV.reifyDim (V.length bcs) $ \(Proxy :: Proxy nc) -> - TV.reifyDim (length pathConstraints) $ \(Proxy :: Proxy nh) -> do - -- TV.reifyDim ncov $ \(Proxy :: Proxy ncov) -> do - -- TV.reifyDim nsh $ \(Proxy :: Proxy nsh) -> do - -- TV.reifyDim nsc $ \(Proxy :: Proxy nsc) -> do - - let daeFun :: Vec nx SXElement -> Vec nx SXElement -> Vec nz SXElement -> Vec nu SXElement - -> Vec np SXElement -> SXElement - -> (Vec nr SXElement, Vec no SXElement) - daeFun x' x z u p t = (devec (rets V.! 0), devec (rets V.! 1)) - where - rets = callSX daeFunSX (V.fromList [vec x', vec x, vec z, vec u, vec p, sxElementToSX t]) - - lagrangeFun :: Vec nx SXElement -> Vec nz SXElement -> Vec nu SXElement -> Vec np SXElement -> Vec no SXElement -> SXElement -> SXElement -> SXElement - lagrangeFun x z u p o t tf = - sxToSXElement $ V.head $ callSX lagFunSX $ - (V.fromList [vec x, vec z, vec u, vec p, vec o, sxElementToSX t, sxElementToSX tf]) - --Left errmsg -> error $ "toOcpPhase: lagrangeFun: " ++ errmsg ++ - -- "\ninputs: " ++ show (xnames ++ znames ++ unames ++ pnames) ++ show onames ++ - -- "\nnumeric inputs x: " ++ show (V.length x) ++ - -- "\nnumeric inputs z: " ++ show (V.length z) ++ - -- "\nnumeric inputs u: " ++ show (V.length u) ++ - -- "\nnumeric inputs p: " ++ show (V.length p) ++ - -- "\nnumeric inputs o: " ++ show (V.length o) - - pathConstraintFun :: Vec nx SXElement -> Vec nz SXElement -> Vec nu SXElement - -> Vec np SXElement -> Vec no SXElement -> SXElement -> Vec nh SXElement - pathConstraintFun x z u p o t = - devec $ V.head $ callSX pathcFunSX (V.fromList [vec x, vec z, vec u, vec p, vec o, sxElementToSX t]) - - mayerFun :: SXElement -> Vec nx SXElement -> Vec nx SXElement - -> SXElement - mayerFun endT'' x0 xF = sxToSXElement $ V.head $ callSX mayerFunSX (V.fromList [sxElementToSX endT'', vec x0, vec xF]) - - bcFun :: Vec nx SXElement -> Vec nx SXElement -> Vec nc SXElement - bcFun x0 xF = devec $ V.head $ callSX bcFunSX (V.fromList [vec x0, vec xF]) - - ocpPhase = - OcpPhase { ocpMayer = mayerFun - , ocpLagrange = lagrangeFun - , ocpDae = daeFun - , ocpBc = bcFun - , ocpBcBnds = devectorize bcbnds - , ocpPathC = pathConstraintFun - , ocpPathCBnds = devectorize (V.fromList pathConstraintBnds) - , ocpXbnd = fill (Nothing, Nothing) - , ocpZbnd = fill (Nothing, Nothing) - , ocpUbnd = fill (Nothing, Nothing) - , ocpPbnd = fill (Nothing, Nothing) - , ocpTbnd = tbnds - , ocpObjScale = Nothing - , ocpTScale = Nothing - , ocpXScale = Nothing - , ocpZScale = Nothing - , ocpUScale = Nothing - , ocpPScale = Nothing - , ocpResidualScale = Nothing - , ocpBcScale = Nothing - , ocpPathCScale = Nothing - } - f ocpPhase meta - -vec :: Vectorize f => f SXElement -> SX -vec = svector . vectorize - -devec :: Vectorize f => SX -> f SXElement -devec = sxSplitJV . mkJ - -solveStaticOcp :: - NlpSolverStuff - -> (SXElement -> DaeMonad ()) - -> (forall a m . (Floating a, Monad m) => a -> (String -> m a) -> (String -> m a) -> m a) - -> ((String -> BCMonad SXElement) -> (String -> BCMonad SXElement) -> BCMonad ()) - -> (SXElement -> (String -> OcpMonad SXElement) -> OcpMonad ()) - -> (Maybe Double, Maybe Double) - -> Int -> Int - -> Maybe (CollTrajMeta -> [DynCollTraj (Vector Double)] -> IO Bool) - -> IO (Either String String) -solveStaticOcp solverStuff dae mayer bc ocp tbnds n deg cb = - reifyOcpPhase dae mayer bc ocp tbnds n deg woo - where - woo ocpphase meta = solveOcp solverStuff n deg (cb <*> pure meta) ocpphase diff --git a/src/Dyno/SXElement.hs b/src/Dyno/SXElement.hs --- a/src/Dyno/SXElement.hs +++ b/src/Dyno/SXElement.hs @@ -3,16 +3,27 @@ module Dyno.SXElement ( SXElement(..) + , sxSplitJV + , sxCatJV + -- todo: remove this completely after NlpMonad/OcpMonad are done with it , sxElementSym - , sxToSXElement + -- todo: remove the next two exports after NlpMonad/OcpMonad are done with it , sxElementToSX + , sxToSXElement ) where import Linear.Conjugate ( Conjugate(..) ) -import Casadi.SX -import Casadi.Overloading +import Casadi.SX ( SX, ssym ) +import qualified Casadi.CMatrix as CM +import Casadi.Overloading ( Fmod, ArcTan2, SymOrd ) +import Dyno.View.Unsafe.View ( mkJ, unJ ) + +import Dyno.View.JV ( JV, splitJV', catJV' ) +import Dyno.View.View ( J ) +import Dyno.Vectorize ( Vectorize, Id ) + newtype SXElement = SXElement SX deriving ( Num, Fractional, Floating @@ -20,19 +31,34 @@ , Show, Eq, Conjugate ) +-- todo: take this out after NlpMonad/OcpMonad are done with it sxElementSym :: String -> IO SXElement sxElementSym = fmap SXElement . ssym + sxToSXElement :: SX -> SXElement sxToSXElement x | (1,1) == sizes = SXElement x | otherwise = error $ "sxToSXElement: got non-scalar of size " ++ show sizes where - sizes = (ssize1 x, ssize2 x) + sizes = (CM.size1 x, CM.size2 x) sxElementToSX :: SXElement -> SX sxElementToSX (SXElement x) | (1,1) == sizes = x | otherwise = error $ "sxElementToSX: got non-scalar of size " ++ show sizes where - sizes = (ssize1 x, ssize2 x) + sizes = (CM.size1 x, CM.size2 x) + + +sxSplitJV :: Vectorize f => J (JV f) SX -> f SXElement +sxSplitJV v = fmap f (splitJV' v) + where + f :: J (JV Id) SX -> SXElement + f = sxToSXElement . unJ + +sxCatJV :: Vectorize f => f SXElement -> J (JV f) SX +sxCatJV v = catJV' (fmap f v) + where + f :: SXElement -> J (JV Id) SX + f x = mkJ (sxElementToSX x) diff --git a/src/Dyno/Server/Accessors.hs b/src/Dyno/Server/Accessors.hs --- a/src/Dyno/Server/Accessors.hs +++ b/src/Dyno/Server/Accessors.hs @@ -7,16 +7,16 @@ --{-# LANGUAGE DeriveGeneric #-} -- for example at bottom module Dyno.Server.Accessors - ( Generic - , Lookup(..) + ( Lookup(..) , AccessorTree(..) , accessors , flatten ) where +import GHC.Generics + import Data.List ( intercalate ) import qualified Linear -import GHC.Generics import SpatialMath ( Euler ) import SpatialMathT ( V3T, Rot ) diff --git a/src/Dyno/Server/Server.hs b/src/Dyno/Server/Server.hs --- a/src/Dyno/Server/Server.hs +++ b/src/Dyno/Server/Server.hs @@ -6,7 +6,6 @@ , Channel ) where -import Data.Vector ( Vector ) import qualified Control.Concurrent as CC import qualified Data.IORef as IORef import Data.Time ( getCurrentTime, diffUTCTime ) @@ -20,25 +19,23 @@ import Dyno.Server.PlotTypes ( Channel(..), Message(..) ) import Dyno.Server.GraphWidget ( newGraph ) -import Dyno.DirectCollocation.Dynamic ( DynCollTraj(..), CollTrajMeta(..) - , dynPlotPoints, catDynPlotPoints ) +import Dyno.DirectCollocation.Dynamic ( CollTrajMeta(..), DynPlotPoints ) -newChannel :: - String -> IO (Channel, ([DynCollTraj (Vector Double)], CollTrajMeta) -> IO ()) +newChannel :: String -> IO (Channel, (DynPlotPoints Double, CollTrajMeta) -> IO ()) newChannel name = do time0 <- getCurrentTime msgStore <- Gtk.listStoreNew [] counter <- IORef.newIORef 0 - let newMessage :: ([DynCollTraj (Vector Double)], CollTrajMeta) -> IO () + let newMessage :: (DynPlotPoints Double, CollTrajMeta) -> IO () newMessage (newTrajs, newMeta) = do -- grab the time and counter time <- getCurrentTime k <- IORef.readIORef counter IORef.writeIORef counter (k+1) Gtk.postGUIAsync $ do - let pps = catDynPlotPoints $ map (flip dynPlotPoints newMeta) newTrajs + let pps = newTrajs val = Message pps k (diffUTCTime time time0) newMeta size <- Gtk.listStoreGetSize msgStore if size == 0 diff --git a/src/Dyno/TypeVecs.hs b/src/Dyno/TypeVecs.hs --- a/src/Dyno/TypeVecs.hs +++ b/src/Dyno/TypeVecs.hs @@ -7,17 +7,14 @@ {-# LANGUAGE DeriveTraversable #-} {-# LANGUAGE GeneralizedNewtypeDeriving #-} {-# LANGUAGE UndecidableInstances #-} +{-# LANGUAGE PolyKinds #-} -- so that "Vec (n :: Nat) a" works module Dyno.TypeVecs ( Vec , Succ - , unSeq - , mkSeq - , mkUnit , unVec , mkVec , mkVec' - , unsafeVec , tvlength , (|>) , (<|) @@ -46,27 +43,28 @@ ) where +import GHC.Generics ( Generic, Generic1 ) + import Control.Applicative import Data.Foldable ( Foldable ) import Data.Traversable ( Traversable ) import qualified Data.Traversable as T -import qualified Data.Foldable as F -import qualified Data.Sequence as S import qualified Data.Vector as V -import Data.Serialize ( Serialize ) +import Data.Serialize ( Serialize(..) ) import Linear.Vector import Linear.V ( Dim(..) ) import Data.Proxy import Data.Reflection as R -import GHC.Generics ( Generic ) import Data.Distributive ( Distributive(..) ) import Dyno.Vectorize -- length-indexed vectors using phantom types -newtype Vec n a = MkVec {unSeq :: S.Seq a} +newtype Vec (n :: k) a = MkVec (V.Vector a) deriving (Eq, Ord, Functor, Traversable, Foldable, Generic, Generic1) -instance Serialize a => Serialize (Vec n a) +instance (Dim n, Serialize a) => Serialize (Vec n a) where + put = put . V.toList . unVec + get = fmap (mkVec . V.fromList) get instance Dim n => Distributive (Vec n) where distribute f = mkVec $ V.generate (reflectDim (Proxy :: Proxy n)) @@ -83,157 +81,133 @@ instance Dim n => Applicative (Vec n) where pure x = ret where - ret = MkVec $ S.replicate (tvlength ret) x - MkVec xs <*> MkVec ys = MkVec $ S.zipWith id xs ys + ret = MkVec $ V.replicate (tvlength ret) x + MkVec xs <*> MkVec ys = MkVec $ V.zipWith id xs ys instance Dim n => Additive (Vec n) where zero = pure 0 - MkVec xs ^+^ MkVec ys = MkVec (S.zipWith (+) xs ys) - MkVec xs ^-^ MkVec ys = MkVec (S.zipWith (-) xs ys) + MkVec xs ^+^ MkVec ys = MkVec (V.zipWith (+) xs ys) + MkVec xs ^-^ MkVec ys = MkVec (V.zipWith (-) xs ys) instance Dim n => Vectorize (Vec n) where vectorize = unVec devectorize = mkVec empty = pure () -tvtranspose :: Vec n (Vec m a) -> Vec m (Vec n a) +tvtranspose :: (Dim n, Dim m) => Vec n (Vec m a) -> Vec m (Vec n a) tvtranspose vec = mkVec $ fmap mkVec $ T.sequence (unVec (fmap unVec vec)) -unVec :: Vec n a -> V.Vector a -unVec = V.fromList . F.toList . unSeq - infixr 5 <| infixl 5 |> (<|) :: a -> Vec n a -> Vec (Succ n) a -(<|) x xs = MkVec $ x S.<| unSeq xs +(<|) x (MkVec xs) = MkVec $ V.cons x xs (|>) :: Vec n a -> a -> Vec (Succ n) a -(|>) xs x = MkVec $ unSeq xs S.|> x - --- create a Vec with a runtime check -unsafeVec :: Dim n => V.Vector a -> Vec n a -unsafeVec = unsafeSeq . S.fromList . V.toList - -unsafeSeq :: Dim n => S.Seq a -> Vec n a -unsafeSeq xs = case MkVec xs of - ret -> let staticLen = tvlength ret - dynLen = S.length xs - in if staticLen == dynLen - then ret - else error $ "unsafeVec: static/dynamic length mismatch: " ++ - "static: " ++ show staticLen ++ ", dynamic: " ++ show dynLen +(|>) (MkVec xs) x = MkVec $ V.snoc xs x -mkUnit :: Vec n a -> Vec () a -mkUnit (MkVec v) = MkVec v +unVec :: forall n a . Dim n => Vec n a -> V.Vector a +unVec (MkVec x) + | n == n' = x + | otherwise = error $ "unVec: length mismatch, " ++ show (n,n') + where + n = reflectDim (Proxy :: Proxy n) + n' = V.length x -mkVec :: V.Vector a -> Vec n a -mkVec = MkVec . S.fromList . V.toList +mkVec :: forall n a . Dim n => V.Vector a -> Vec n a +mkVec x + | n == n' = MkVec x + | otherwise = error $ "mkVec: length mismatch, " ++ show (n,n') + where + n = reflectDim (Proxy :: Proxy n) + n' = V.length x mkVec' :: Dim n => [a] -> Vec n a -mkVec' = MkVec . S.fromList - -mkSeq :: S.Seq a -> Vec n a -mkSeq = MkVec - --- --mkVec :: (IntegerT n) => V.Vector a -> Vec n a --- --mkVec = unsafeVec -- lets just run the check every time for now --- --- --mkSeq :: (IntegerT n) => S.Seq a -> Vec n a --- --mkSeq = unsafeSeq -- lets just run the check every time for now +mkVec' = mkVec . V.fromList tvlength :: forall n a. Dim n => Vec n a -> Int -tvlength _ = reflectDim (Proxy :: Proxy n) +tvlength = const $ reflectDim (Proxy :: Proxy n) -tvzip :: Vec n a -> Vec n b -> Vec n (a,b) -tvzip x y = mkSeq (S.zip (unSeq x) (unSeq y)) +tvzip :: Dim n => Vec n a -> Vec n b -> Vec n (a,b) +tvzip x y = mkVec (V.zip (unVec x) (unVec y)) -tvzip3 :: Vec n a -> Vec n b -> Vec n c -> Vec n (a,b,c) -tvzip3 x y z = mkSeq (S.zip3 (unSeq x) (unSeq y) (unSeq z)) +tvzip3 :: Dim n => Vec n a -> Vec n b -> Vec n c -> Vec n (a,b,c) +tvzip3 x y z = mkVec (V.zip3 (unVec x) (unVec y) (unVec z)) -tvzip4 :: Vec n a -> Vec n b -> Vec n c -> Vec n d -> Vec n (a,b,c,d) -tvzip4 x y z w = mkSeq (S.zip4 (unSeq x) (unSeq y) (unSeq z) (unSeq w)) +tvzip4 :: Dim n => Vec n a -> Vec n b -> Vec n c -> Vec n d -> Vec n (a,b,c,d) +tvzip4 x y z w = mkVec (V.zip4 (unVec x) (unVec y) (unVec z) (unVec w)) -tvzipWith :: (a -> b -> c) -> Vec n a -> Vec n b -> Vec n c -tvzipWith f x y = mkSeq (S.zipWith f (unSeq x) (unSeq y)) +tvzipWith :: Dim n => (a -> b -> c) -> Vec n a -> Vec n b -> Vec n c +tvzipWith f x y = mkVec (V.zipWith f (unVec x) (unVec y)) -tvzipWith3 :: (a -> b -> c -> d) -> Vec n a -> Vec n b -> Vec n c -> Vec n d -tvzipWith3 f x y z = mkSeq (S.zipWith3 f (unSeq x) (unSeq y) (unSeq z)) +tvzipWith3 :: Dim n => (a -> b -> c -> d) -> Vec n a -> Vec n b -> Vec n c -> Vec n d +tvzipWith3 f x y z = mkVec (V.zipWith3 f (unVec x) (unVec y) (unVec z)) -tvzipWith4 :: (a -> b -> c -> d -> e) -> Vec n a -> Vec n b -> Vec n c -> Vec n d -> Vec n e -tvzipWith4 f x y z u = mkSeq (S.zipWith4 f (unSeq x) (unSeq y) (unSeq z) (unSeq u)) +tvzipWith4 :: Dim n => (a -> b -> c -> d -> e) -> Vec n a -> Vec n b -> Vec n c -> Vec n d -> Vec n e +tvzipWith4 f x y z u = mkVec (V.zipWith4 f (unVec x) (unVec y) (unVec z) (unVec u)) -tvzipWith5 :: (a -> b -> c -> d -> e -> f) +tvzipWith5 :: Dim n => (a -> b -> c -> d -> e -> f) -> Vec n a -> Vec n b -> Vec n c -> Vec n d -> Vec n e -> Vec n f tvzipWith5 f x0 x1 x2 x3 x4 = - mkSeq (szipWith5 f (unSeq x0) (unSeq x1) (unSeq x2) (unSeq x3) (unSeq x4)) - where - szipWith5 :: (a -> b -> c -> d -> e -> f) - -> S.Seq a -> S.Seq b -> S.Seq c -> S.Seq d -> S.Seq e -> S.Seq f - szipWith5 f' s1 s2 s3 s4 s5 = - S.zipWith ($) (S.zipWith ($) (S.zipWith ($) (S.zipWith f' s1 s2) s3) s4) s5 + mkVec (V.zipWith5 f (unVec x0) (unVec x1) (unVec x2) (unVec x3) (unVec x4)) -tvzipWith6 :: (a -> b -> c -> d -> e -> f -> g) +tvzipWith6 :: Dim n => (a -> b -> c -> d -> e -> f -> g) -> Vec n a -> Vec n b -> Vec n c -> Vec n d -> Vec n e -> Vec n f -> Vec n g tvzipWith6 f x0 x1 x2 x3 x4 x5 = - mkSeq (szipWith6 f (unSeq x0) (unSeq x1) (unSeq x2) (unSeq x3) (unSeq x4) (unSeq x5)) - where - szipWith6 :: (a -> b -> c -> d -> e -> f -> g) - -> S.Seq a -> S.Seq b -> S.Seq c -> S.Seq d -> S.Seq e -> S.Seq f -> S.Seq g - szipWith6 f' s1 s2 s3 s4 s5 s6 = - S.zipWith ($) (S.zipWith ($) (S.zipWith ($) (S.zipWith ($) (S.zipWith f' s1 s2) s3) s4) s5) s6 - + mkVec (V.zipWith6 f (unVec x0) (unVec x1) (unVec x2) (unVec x3) (unVec x4) (unVec x5)) -tvunzip :: Vec n (a,b) -> (Vec n a, Vec n b) +tvunzip :: Dim n => Vec n (a,b) -> (Vec n a, Vec n b) tvunzip v = (mkVec v1, mkVec v2) where (v1,v2) = V.unzip (unVec v) -tvunzip3 :: Vec n (a,b,c) -> (Vec n a, Vec n b, Vec n c) +tvunzip3 :: Dim n => Vec n (a,b,c) -> (Vec n a, Vec n b, Vec n c) tvunzip3 v = (mkVec v1, mkVec v2, mkVec v3) where (v1,v2,v3) = V.unzip3 (unVec v) -tvunzip4 :: Vec n (a,b,c,d) -> (Vec n a, Vec n b, Vec n c, Vec n d) +tvunzip4 :: Dim n => Vec n (a,b,c,d) -> (Vec n a, Vec n b, Vec n c, Vec n d) tvunzip4 v = (mkVec v1, mkVec v2, mkVec v3, mkVec v4) where (v1,v2,v3,v4) = V.unzip4 (unVec v) -tvunzip5 :: Vec n (a,b,c,d,e) -> (Vec n a, Vec n b, Vec n c, Vec n d, Vec n e) +tvunzip5 :: Dim n => Vec n (a,b,c,d,e) -> (Vec n a, Vec n b, Vec n c, Vec n d, Vec n e) tvunzip5 v = (mkVec v1, mkVec v2, mkVec v3, mkVec v4, mkVec v5) where (v1,v2,v3,v4,v5) = V.unzip5 (unVec v) -tvhead :: Vec n a -> a -tvhead x = case S.viewl (unSeq x) of - y S.:< _ -> y - S.EmptyL -> error "vhead: empty" +tvhead :: Dim n => Vec n a -> a +tvhead x = case V.length v of + 0 -> error "tvhead: empty" + _ -> V.head v + where + v = unVec x tvtail :: Dim n => Vec (Succ n) a -> Vec n a -tvtail x = case S.viewl (unSeq x) of - _ S.:< ys -> mkSeq ys - S.EmptyL -> error "vtail: empty" +tvtail x = case V.length v of + 0 -> error "tvtail: empty" + _ -> mkVec $ V.tail v + where + v = unVec x -tvlast :: Vec n a -> a -tvlast x = case S.viewr (unSeq x) of - _ S.:> y -> y - S.EmptyR -> error "vlast: empty" +tvlast :: Dim n => Vec n a -> a +tvlast x = case V.length v of + 0 -> error "tvlast: empty" + _ -> V.last v + where + v = unVec x tvshiftl :: Dim n => Vec n a -> a -> Vec n a -tvshiftl xs x = case S.viewl (unSeq xs) of - _ S.:< ys -> mkSeq (ys S.|> x) - S.EmptyL -> error "tvshiftl: EmptyL" +tvshiftl xs x = mkVec $ V.tail (V.snoc (unVec xs) x) tvshiftr :: Dim n => a -> Vec n a -> Vec n a -tvshiftr x xs = case S.viewr (unSeq xs) of - ys S.:> _ -> mkSeq (x S.<| ys) - S.EmptyR -> error "tvshiftr: EmptyR" +tvshiftr x xs = mkVec $ V.init (V.cons x (unVec xs)) instance Show a => Show (Vec n a) where - showsPrec _ = showV . F.toList . unSeq + showsPrec _ (MkVec v) = showV (V.toList v) where showV [] = showString "<>" showV (x:xs) = showChar '<' . shows x . showl xs @@ -258,7 +232,7 @@ {-# INLINE reifyDim #-} reifyVector :: forall a r. V.Vector a -> (forall (n :: *). Dim n => Vec n a -> r) -> r -reifyVector v f = reifyDim (V.length v) $ \(Proxy :: Proxy n) -> f (MkVec (S.fromList (V.toList v)) :: Vec n a) +reifyVector v f = reifyDim (V.length v) $ \(Proxy :: Proxy n) -> f (mkVec v :: Vec n a) {-# INLINE reifyVector #-} tvlinspace :: forall n a . (Dim n, Fractional a) => a -> a -> Vec n a diff --git a/src/Dyno/Vectorize.hs b/src/Dyno/Vectorize.hs --- a/src/Dyno/Vectorize.hs +++ b/src/Dyno/Vectorize.hs @@ -21,12 +21,11 @@ , vzipWith4 , fill , GVectorize(..) - , Generic1 - , Proxy(..) ) where -import Control.Applicative ( Applicative(..) ) import GHC.Generics + +import Control.Applicative ( Applicative(..) ) import qualified Data.Vector as V import Data.Foldable ( Foldable ) import Data.Traversable ( Traversable ) @@ -48,7 +47,7 @@ instance Linear.Additive None where -- | a length-1 vectorizable type -newtype Id a = Id a +newtype Id a = Id { unId :: a } deriving (Eq, Ord, Generic, Generic1, Functor, Foldable, Traversable, Show) instance Vectorize Id instance Applicative Id where diff --git a/src/Dyno/View.hs b/src/Dyno/View.hs deleted file mode 100644 --- a/src/Dyno/View.hs +++ /dev/null @@ -1,12 +0,0 @@ -{-# OPTIONS_GHC -Wall #-} - -module Dyno.View ( module X - , Num() - ) where - -import Dyno.View.View as X -import Dyno.View.JV as X -import Dyno.View.Fun as X -import Dyno.View.Viewable as X -import Dyno.View.HList as X -import Dyno.View.NumInstances() diff --git a/src/Dyno/View/CasadiMat.hs b/src/Dyno/View/CasadiMat.hs deleted file mode 100644 --- a/src/Dyno/View/CasadiMat.hs +++ /dev/null @@ -1,153 +0,0 @@ -{-# OPTIONS_GHC -Wall #-} - -module Dyno.View.CasadiMat - ( CasadiMat(..), MX.MX, SX.SX, DMatrix.DMatrix - , vertslice, horzslice - ) where - -import qualified Data.Vector as V - -import System.IO.Unsafe ( unsafePerformIO ) -import Casadi.Overloading ( Fmod, ArcTan2, SymOrd, Erf ) -import Casadi.Sparsity ( Sparsity ) -import qualified Casadi.SX as SX -import qualified Casadi.MX as MX -import qualified Casadi.DMatrix as DMatrix -import Casadi.Slice ( Slice, slice ) -import Casadi.Core.Tools as C - -class (Eq a, Show a, Floating a, Fmod a, ArcTan2 a, SymOrd a, Erf a) => CasadiMat a where - vertsplit :: a -> V.Vector Int -> V.Vector a - vertcat :: V.Vector a -> a - horzsplit :: a -> V.Vector Int -> V.Vector a - horzcat :: V.Vector a -> a - veccat :: V.Vector a -> a - size1 :: a -> Int - size2 :: a -> Int - mm :: a -> a -> a - trans :: a -> a - diag :: a -> a - eye :: Int -> a - ones :: (Int,Int) -> a - zeros :: (Int,Int) -> a - zerosSp :: Sparsity -> a - fromDVector :: V.Vector Double -> a - solve :: a -> a -> a - indexed :: a -> Slice -> Slice -> a - sparsity :: a -> Sparsity - getNZ :: a -> Slice -> a - setNZ :: a -> a -> Slice -> IO () - triu :: a -> a - tril :: a -> a - triu2symm :: a -> a - tril2symm :: a -> a - copy :: a -> IO a - dense :: a -> a - -instance CasadiMat SX.SX where - veccat = SX.sveccat --- vertsplit = vertslice - vertsplit = SX.svertsplit - vertcat = SX.svertcat --- horzsplit = horzslice - horzsplit = SX.shorzsplit - horzcat = SX.shorzcat - size1 = SX.ssize1 - size2 = SX.ssize2 - mm = SX.smm - trans = SX.strans - diag = SX.sdiag - eye = SX.seye - ones = SX.sones - zeros = SX.szeros - zerosSp = SX.szerosSp - fromDVector = SX.d2s . fromDVector - solve = SX.ssolve - indexed = SX.sindexed - sparsity = SX.scrs - getNZ = SX.sgetNZ - setNZ = SX.ssetNZ - triu = SX.striu - tril = SX.stril - triu2symm = SX.striu2symm - tril2symm = SX.stril2symm - copy = SX.scopy - dense = SX.sdense - -instance CasadiMat MX.MX where - veccat = MX.veccat --- vertsplit = vertslice - vertsplit = MX.vertsplit - vertcat = MX.vertcat --- horzsplit = horzslice - horzsplit = MX.horzsplit - horzcat = MX.horzcat - size1 = MX.size1 - size2 = MX.size2 - mm = MX.mm - trans = MX.trans - diag = MX.diag - eye = MX.eye - ones = MX.ones - zeros = MX.zeros - zerosSp = MX.zerosSp - fromDVector = MX.d2m . fromDVector - solve = MX.solve - indexed = MX.indexed - sparsity = MX.crs - getNZ = MX.getNZ - setNZ = MX.setNZ - triu = MX.triu - tril = MX.tril - triu2symm = MX.triu2symm - tril2symm = MX.tril2symm - copy = MX.copy - dense = MX.dense - -instance CasadiMat DMatrix.DMatrix where - veccat = DMatrix.dveccat --- vertsplit = vertslice - vertsplit = DMatrix.dvertsplit - vertcat = DMatrix.dvertcat --- horzsplit = horzslice - horzsplit = DMatrix.dhorzsplit - horzcat = DMatrix.dhorzcat - size1 = DMatrix.dsize1 - size2 = DMatrix.dsize2 - mm = DMatrix.dmm - trans = DMatrix.dtrans - diag = DMatrix.ddiag - eye = DMatrix.deye - ones = DMatrix.dones - zeros = DMatrix.dzeros - zerosSp = DMatrix.dzerosSp - fromDVector = DMatrix.dvector - solve x y = unsafePerformIO (C.solve__3 x y) - indexed = DMatrix.dindexed - sparsity = DMatrix.dcrs - getNZ = DMatrix.dgetNZ - setNZ = DMatrix.dsetNZ - triu = DMatrix.dtriu - tril = DMatrix.dtril - triu2symm = DMatrix.dtriu2symm - tril2symm = DMatrix.dtril2symm - copy = DMatrix.dcopy - dense = DMatrix.ddense - -vertslice :: CasadiMat a => a -> V.Vector Int -> V.Vector a -vertslice x vs = V.fromList (f (V.toList vs)) - where - cols = size2 x - hslice = slice 0 cols 1 - - f (v0:v1:others) = indexed x (slice v0 v1 1) hslice : f (v1:others) - f _ = [] - -horzslice :: CasadiMat a => a -> V.Vector Int -> V.Vector a -horzslice x vs = V.fromList (f (V.toList vs)) - where - rows = size1 x - vslice = slice 0 rows 1 - - f (v0:v1:others) = indexed x vslice (slice v0 v1 1) : f (v1:others) - f _ = [] diff --git a/src/Dyno/View/Cov.hs b/src/Dyno/View/Cov.hs new file mode 100644 --- /dev/null +++ b/src/Dyno/View/Cov.hs @@ -0,0 +1,106 @@ +{-# OPTIONS_GHC -Wall -fno-cse #-} +{-# Language ScopedTypeVariables #-} +{-# Language KindSignatures #-} + +module Dyno.View.Cov + ( Cov(..) + , toMat + , fromMat + , toMatrix + , toHMatrix + , toHMatrix' + , fromMatrix + , diag + , diag' + , nOfVecLen + ) where + +import Data.Proxy ( Proxy(..) ) +import Data.Vector ( Vector ) +import qualified Data.Vector as V +import qualified Data.Sequence as Seq +import System.IO.Unsafe ( unsafePerformIO ) +import qualified Data.Packed.Matrix as Mat + +import qualified Casadi.Sparsity as Sparsity +import Casadi.Slice ( slice' ) +import Casadi.DMatrix ( DMatrix ) +import Casadi.CMatrix ( CMatrix ) +import qualified Casadi.CMatrix as CM + +import Dyno.View.Unsafe.View ( unJ, mkJ ) +import Dyno.View.Unsafe.M ( M(UnsafeM), mkM ) + +import Dyno.Vectorize ( Vectorize(..) ) +import Dyno.View.View ( View(..), J ) +import Dyno.View.JV ( JV ) +import Dyno.View.Viewable ( Viewable(..) ) +import Dyno.View.M ( toHMat ) + +newtype Cov (f :: * -> *) a = Cov a +instance View f => View (Cov f) where + cat (Cov x) = mkJ x + split x = Cov (unJ x) + size = const $ (n*n + n) `div` 2 + where + n = size (Proxy :: Proxy f) + sizes k0 = const (Seq.singleton (k0 + n)) + where + n = size (Proxy :: Proxy f) + +nOfVecLen :: Int -> Int +nOfVecLen m + | (n*n + n) `div` 2 == m = n + | otherwise = error $ "nOfVecLen fail: " ++ show m + where + m' = fromIntegral m :: Double + n = round $ sqrt (2*m' + 1/4) - 1/2 + +toMat :: (View f, CMatrix a, Viewable a) => J (Cov f) a -> M f f a +toMat c = mkM (toMatrix c) +{-# NOINLINE toMat #-} + +toMatrix :: forall f a . (View f, CMatrix a, Viewable a) => J (Cov f) a -> a +toMatrix c = unsafePerformIO $ do + let n = size (Proxy :: Proxy f) + m <- CM.copy (CM.zerosSp (Sparsity.upper n)) + --CM.setNZ m (CM.dense (unJ c)) slice' + CM.setNZ m (unJ c) slice' -- Joel says that "dense" isn't required here + return (CM.triu2symm m) +{-# NOINLINE toMatrix #-} + +toHMatrix :: forall f . View f => J (Cov f) DMatrix -> Mat.Matrix Double +toHMatrix m = toHMat (toMat m) + +toHMatrix' :: forall f . View f => J (Cov f) (Vector Double) -> Mat.Matrix Double +toHMatrix' v = toHMatrix $ (mkJ (CM.fromDVector (unJ v)) :: J (Cov f) DMatrix) + +diag :: (View f, CMatrix a, Viewable a) => J f a -> J (Cov f) a +diag = fromMatrix . CM.diag . unJ + +diag' :: Vectorize f => f a -> a -> J (Cov (JV f)) (Vector a) +diag' x offDiag = mkJ $ V.fromList $ concat $ zipWith f vx [0..] + where + f y k = replicate k offDiag ++ [y] + vx = V.toList $ vectorize x + +--data X a = X (J S a) (J S a) deriving (Generic, Show) +--instance View X +--xx = X (mkJ 1) (mkJ 2) :: X DMatrix +--xx' = cat xx +-- +--dd :: J (Cov X) DMatrix +--dd = diag xx' +-- +--sp :: DMatrix +--sp = toMatrix dd +-- +--dd2 :: J (Cov X) DMatrix +--dd2 = fromMatrix sp + +fromMat :: (View f, CMatrix a, Viewable a) => M f f a -> J (Cov f) a +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 x) slice' diff --git a/src/Dyno/View/CustomFunction.hs b/src/Dyno/View/CustomFunction.hs --- a/src/Dyno/View/CustomFunction.hs +++ b/src/Dyno/View/CustomFunction.hs @@ -17,10 +17,11 @@ 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.CasadiMat ( DMatrix, sparsity ) import Dyno.View.Scheme ( Scheme(..) ) import Dyno.View.Fun ( Fun(..) ) import Casadi.Callback ( makeCustomEvaluate, makeDerivativeGenerator ) diff --git a/src/Dyno/View/Fun.hs b/src/Dyno/View/Fun.hs --- a/src/Dyno/View/Fun.hs +++ b/src/Dyno/View/Fun.hs @@ -28,13 +28,17 @@ import qualified Casadi.SXFunction as C import Casadi.Option import Casadi.SharedObject +import Casadi.MX ( MX ) +import Casadi.SX ( SX ) +import Casadi.DMatrix ( DMatrix ) +import Casadi.CMatrix ( CMatrix ) import qualified Casadi.Core.Classes.Function as F import qualified Casadi.Core.Classes.MXFunction as M import qualified Casadi.Core.Classes.SharedObject as C import qualified Casadi.Core.Classes.OptionsFunctionality as C -import Dyno.View.CasadiMat +import Dyno.View.Viewable ( Viewable ) import Dyno.View.Scheme import Dyno.View.FunJac @@ -87,7 +91,7 @@ callSX (SXFun sxf) = fromVector . C.callSX sxf . toVector mkSym :: forall a f . - (Scheme f, CasadiMat a) + (Scheme f, CMatrix a, Viewable a) => (String -> Int -> Int -> IO a) -> String -> Proxy f -> IO (f a) mkSym mk name _ = do @@ -99,7 +103,7 @@ return $ fromVector (V.fromList ms) mkFun :: forall f g fun fun' a - . (Scheme f, Scheme g, C.SharedObjectClass fun, C.OptionsFunctionalityClass fun) + . (Scheme f, Scheme g, Viewable a, C.SharedObjectClass fun, C.OptionsFunctionalityClass fun) => (Vector a -> Vector a -> IO fun) -> (String -> Proxy f -> IO (f a)) -> (fun -> fun' f g) diff --git a/src/Dyno/View/JV.hs b/src/Dyno/View/JV.hs --- a/src/Dyno/View/JV.hs +++ b/src/Dyno/View/JV.hs @@ -7,32 +7,29 @@ {-# LANGUAGE InstanceSigs #-} module Dyno.View.JV - ( JV(..) + ( JV , splitJV , catJV , splitJV' , catJV' - , sxSplitJV - , sxCatJV ) where -import GHC.Generics hiding ( S ) +import GHC.Generics ( Generic, Generic1 ) import qualified Data.Sequence as Seq import Data.Proxy ( Proxy(..) ) import Data.Vector ( Vector ) import qualified Data.Vector as V -import Casadi.SX ( SX ) +import Dyno.View.Unsafe.View ( mkJ, unJ ) -import Dyno.SXElement +import Dyno.View.View ( View(..), J ) import Dyno.View.Viewable ( Viewable(..) ) -import Dyno.View.View import Dyno.Vectorize ( Vectorize(..), Id, vlength ) import Dyno.Server.Accessors ( Lookup(..) ) -- | views into Vectorizable things -newtype JV f a = JV { unJV :: f a } deriving (Functor, Generic) +newtype JV f a = JV { unJV :: f a } deriving (Functor, Generic, Generic1) instance Vectorize f => View (JV f) where cat :: forall a . Viewable a => JV f a -> J (JV f) a @@ -59,15 +56,3 @@ catJV' :: (Vectorize f, Viewable a) => f (J (JV Id) a) -> J (JV f) a catJV' = cat . JV . fmap unJ - -sxSplitJV :: Vectorize f => J (JV f) SX -> f SXElement -sxSplitJV v = fmap f (splitJV' v) - where - f :: J (JV Id) SX -> SXElement - f (UnsafeJ x) = sxToSXElement x - -sxCatJV :: Vectorize f => f SXElement -> J (JV f) SX -sxCatJV v = catJV' (fmap f v) - where - f :: SXElement -> J (JV Id) SX - f x = mkJ (sxElementToSX x) diff --git a/src/Dyno/View/JVec.hs b/src/Dyno/View/JVec.hs new file mode 100644 --- /dev/null +++ b/src/Dyno/View/JVec.hs @@ -0,0 +1,63 @@ +{-# OPTIONS_GHC -Wall #-} +{-# LANGUAGE ScopedTypeVariables #-} +{-# LANGUAGE KindSignatures #-} +{-# LANGUAGE FlexibleContexts #-} +{-# LANGUAGE RankNTypes #-} +{-# LANGUAGE PolyKinds #-} + +module Dyno.View.JVec + ( JVec(..) + , jreplicate, jreplicate' + , reifyJVec + ) where + +import qualified Data.Foldable as F +import qualified Data.Sequence as Seq +import Data.Proxy ( Proxy(..) ) +import Linear.V ( Dim(..) ) +import Data.Vector ( Vector ) +import qualified Data.Vector as V +import Data.Serialize ( Serialize(..) ) + +import Dyno.View.Unsafe.View ( mkJ, unJ ) + +import Dyno.TypeVecs ( Vec, unVec, mkVec, mkVec', reifyVector ) +import Dyno.View.Viewable ( Viewable(..) ) +import Dyno.View.View ( View(..), J ) + + +-- | vectors in View +newtype JVec (n :: k) f a = JVec { unJVec :: Vec n (J f a) } deriving ( Show, Eq ) +instance (Dim n, View f) => View (JVec n f) where + cat = mkJ . vveccat . fmap unJ . unVec . unJVec + split = JVec . fmap mkJ . mkVec . flip vvertsplit ks . unJ + where + ks = V.fromList (take (n+1) [0,m..]) + n = reflectDim (Proxy :: Proxy n) + m = size (Proxy :: Proxy f) + size = const (n * m) + where + n = reflectDim (Proxy :: Proxy n) + m = size (Proxy :: Proxy f) + sizes = const . Seq.iterateN n (+m) . (+ m) + where + n = reflectDim (Proxy :: Proxy n) + m = size (Proxy :: Proxy f) +instance (Dim n, Serialize (J f a)) => Serialize (JVec n f a) where + get = fmap (JVec . mkVec') get + put = put . F.toList . unJVec + +jreplicate' :: forall a n f . (Dim n, View f) => J f a -> JVec n f a +jreplicate' el = ret + where + ret = JVec (mkVec (V.replicate nvec el)) + nvec = reflectDim (Proxy :: Proxy n) + +jreplicate :: forall a n f . (Dim n, View f, Viewable a) => J f a -> J (JVec n f) a +jreplicate = cat . jreplicate' + + +reifyJVec :: forall a f r . Vector (J f a) -> (forall (n :: *). Dim n => JVec n f a -> r) -> r +reifyJVec v f = reifyVector v $ \(v' :: Vec n (J f a)) -> f (JVec v' :: JVec n f a) +{-# INLINE reifyJVec #-} + diff --git a/src/Dyno/View/M.hs b/src/Dyno/View/M.hs --- a/src/Dyno/View/M.hs +++ b/src/Dyno/View/M.hs @@ -3,13 +3,13 @@ {-# LANGUAGE KindSignatures #-} {-# LANGUAGE DeriveFunctor #-} {-# LANGUAGE DeriveGeneric #-} +{-# LANGUAGE PolyKinds #-} module Dyno.View.M - ( M(..) -- TODO: hide the unsafe constructor - , mkM - , mkM' + ( M , mm , ms + , vs , trans , zeros , eye @@ -24,6 +24,7 @@ , vcat' , hcat' , hsplitTup + , hsplitTrip , row , col , unrow @@ -34,113 +35,39 @@ , fromHMat' ) where -import qualified Data.Packed.Matrix as Mat -import Data.Proxy import qualified Data.Vector as V -import GHC.Generics ( Generic ) +import Data.Proxy ( Proxy(..) ) +import Casadi.CMatrix ( CMatrix ) +import Casadi.DMatrix ( DMatrix, ddata ) +import qualified Casadi.CMatrix as CM +import qualified Data.Packed.Matrix as Mat -import Casadi.Overloading -import Casadi.DMatrix ( ddata, ddense, dvector ) +import Dyno.View.Unsafe.View ( unJ, mkJ ) +import Dyno.View.Unsafe.M ( M(UnsafeM), mkM, mkM', unM ) -import Dyno.Vectorize -import Dyno.View.CasadiMat ( CasadiMat ) -import Dyno.View.JV +import Dyno.Vectorize ( Vectorize(..), Id, fill ) import Dyno.TypeVecs ( Vec, Dim(..) ) -import Dyno.View.View -import Dyno.View.Viewable -import qualified Dyno.View.CasadiMat as CM +import Dyno.View.View ( View(..), J, JTuple, JTriple ) +import Dyno.View.JV ( JV ) +import Dyno.View.JVec ( JVec ) +import Dyno.View.Viewable ( Viewable ) -newtype M (f :: * -> *) (g :: * -> *) (a :: *) = - UnsafeM { unM :: a } deriving (Eq, Functor, Generic) -instance Show a => Show (M f g a) where - showsPrec p (UnsafeM x) = showsPrec p x - -over :: (View f, View g, CasadiMat a) => (a -> a) -> M f g a -> M f g a -over f (UnsafeM x) = mkM (f x) - -over2 :: (View f, View g, CasadiMat a) => (a -> a -> a) -> M f g a -> M f g a -> M f g a -over2 f (UnsafeM x) (UnsafeM y)= mkM (f x y) - -instance (View f, View g, CasadiMat a) => Num (M f g a) where - (+) = over2 (+) - (-) = over2 (-) - (*) = over2 (*) - negate = over negate - abs = over abs - signum = over signum - fromInteger k = mkM $ fromInteger k * CM.ones (nx,ny) - where - nx = size (Proxy :: Proxy f) - ny = size (Proxy :: Proxy f) -instance (View f, View g, CasadiMat a) => Fractional (M f g a) where - (/) = over2 (/) - fromRational k = mkM $ fromRational k * CM.ones (nx,ny) - where - nx = size (Proxy :: Proxy f) - ny = size (Proxy :: Proxy f) -instance (View f, View g, CasadiMat a) => Floating (M f g a) where - pi = mkM $ pi * CM.ones (nx,ny) - where - nx = size (Proxy :: Proxy f) - ny = size (Proxy :: Proxy f) - (**) = over2 (**) - exp = over exp - log = over log - sin = over sin - cos = over cos - tan = over tan - asin = over asin - atan = over atan - acos = over acos - sinh = over sinh - cosh = over cosh - tanh = over tanh - asinh = over asinh - atanh = over atanh - acosh = over acosh - -instance (View f, View g, CasadiMat a) => Fmod (M f g a) where - fmod = over2 fmod - -instance (View f, View g, CasadiMat a) => ArcTan2 (M f g a) where - arctan2 = over2 arctan2 - -instance (View f, View g, CasadiMat a) => SymOrd (M f g a) where - leq = over2 leq - geq = over2 geq - eq = over2 eq - -mkM :: forall f g a . (View f, View g, CasadiMat a) => a -> M f g a -mkM x = case mkM' x of - Right x' -> x' - Left msg -> error msg - -mkM' :: forall f g a . (View f, View g, CasadiMat a) => a -> Either String (M f g a) -mkM' x - | nx == nx' && ny == ny' = Right (UnsafeM x) - | all (== 0) [nx,nx'] && ny' == 0 = Right zeros - | all (== 0) [ny,ny'] && nx' == 0 = Right zeros - | otherwise = Left $ "mkM length mismatch: typed size: " ++ show (nx,ny) ++ - ", actual size: " ++ show (nx', ny') - where - nx = size (Proxy :: Proxy f) - ny = size (Proxy :: Proxy g) - nx' = CM.size1 x - ny' = CM.size2 x - -mm :: (View f, View h, CasadiMat a) => M f g a -> M g h a -> M f h a +mm :: (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) -ms :: (View f, View h, CasadiMat a) => M f g a -> J S a -> M f h a -ms (UnsafeM m0) (UnsafeJ m1) = mkM (m0 * m1) +ms :: (View f, View h, Viewable a, CMatrix a) => M f g a -> J (JV Id) a -> M f h a +ms (UnsafeM m0) m1 = mkM (m0 * (unJ m1)) -trans :: (View f, View g, CasadiMat a) => M f g a -> M g f a +vs :: (View f, Viewable a, CMatrix a) => J f a -> J (JV Id) a -> J f a +vs m0 m1 = uncol $ ms (col m0) m1 + +trans :: (View f, View g, CMatrix a) => M f g a -> M g f a trans (UnsafeM m) = mkM (CM.trans m) vsplit :: forall f g a . - (Vectorize f, View g, CasadiMat a) + (Vectorize f, View g, CMatrix a) => M (JV f) g a -> f (M (JV Id) g a) vsplit (UnsafeM x) = fmap mkM $ devectorize $ CM.vertsplit x nrs where @@ -149,13 +76,13 @@ vcat :: forall f g a . - (Vectorize f, View g, CasadiMat a) + (Vectorize f, View g, CMatrix a) => f (M (JV Id) g a) -> M (JV f) g a vcat x = mkM $ CM.vertcat $ V.map unM (vectorize x) hsplit :: forall f g a . - (View f, Vectorize g, CasadiMat a) + (View f, Vectorize g, CMatrix a) => M f (JV g) a -> g (M f (JV Id) a) hsplit (UnsafeM x) = fmap mkM $ devectorize $ CM.horzsplit x ncs where @@ -164,7 +91,7 @@ hsplitTup :: forall f g h a . - (View f, View g, View h, CasadiMat a) + (View f, View g, View h, CMatrix a) => M f (JTuple g h) a -> (M f g a, M f h a) hsplitTup (UnsafeM x) = case V.toList (CM.horzsplit x ncs) of @@ -175,21 +102,35 @@ nh = size (Proxy :: Proxy h) ncs = V.fromList [0,ng,ng+nh] +hsplitTrip :: + forall f g h j a . + (View f, View g, View h, View j, CMatrix a) + => M f (JTriple g h j) a -> (M f g a, M f h a, M f j a) +hsplitTrip (UnsafeM x) = + case V.toList (CM.horzsplit x ncs) of + [g,h,j] -> (mkM g, mkM h, mkM j) + n -> error $ "hsplitTup made a bad split with length " ++ show (length n) + where + ng = size (Proxy :: Proxy g) + nh = size (Proxy :: Proxy h) + nj = size (Proxy :: Proxy j) + ncs = V.fromList [0,ng,ng+nh,ng+nh+nj] + hcat :: forall f g a . - (View f, Vectorize g, CasadiMat a) + (View f, Vectorize g, CMatrix a) => g (M f (JV Id) a) -> M f (JV g) a hcat x = mkM $ CM.horzcat $ V.map unM (vectorize x) vcat' :: forall f g n a . - (View f, View g, Dim n, CasadiMat a) + (View f, View g, Dim n, CMatrix a) => Vec n (M f g a) -> M (JVec n f) g a vcat' x = mkM $ CM.vertcat $ V.map unM (vectorize x) vsplit' :: forall f g n a . - (View f, View g, Dim n, CasadiMat a) + (View f, View g, Dim n, CMatrix a) => M (JVec n f) g a -> Vec n (M f g a) vsplit' (UnsafeM x) | n == 0 = fill zeros @@ -202,13 +143,13 @@ hcat' :: forall f g n a . - (View f, View g, Dim n, CasadiMat a) + (View f, View g, Dim n, CMatrix a) => Vec n (M f g a) -> M f (JVec n g) a hcat' x = mkM $ CM.horzcat $ V.map unM (vectorize x) hsplit' :: forall f g n a . - (View f, View g, Dim n, CasadiMat a) + (View f, View g, Dim n, CMatrix a) => M f (JVec n g) a -> Vec n (M f g a) hsplit' (UnsafeM x) | n == 0 = fill zeros @@ -219,20 +160,20 @@ nc = size (Proxy :: Proxy g) ncs = V.fromList [0,nc..n*nc] -zeros :: forall f g a . (View f, View g, CasadiMat a) => M f g a +zeros :: forall f g a . (View f, View g, CMatrix a) => M f g a zeros = mkM z where z = CM.zeros (rows, cols) rows = size (Proxy :: Proxy f) cols = size (Proxy :: Proxy g) -eye :: forall f a . (View f, CasadiMat a) => M f f a +eye :: forall f a . (View f, CMatrix a) => M f f a eye = mkM z where z = CM.eye n n = size (Proxy :: Proxy f) -ones :: forall f g a . (View f, View g, CasadiMat a) => M f g a +ones :: forall f g a . (View f, View g, CMatrix a) => M f g a ones = mkM z where z = CM.ones (rows, cols) @@ -240,7 +181,7 @@ cols = size (Proxy :: Proxy g) -- this is mainly for unit tests -countUp :: forall f g a . (View f, View g, CasadiMat a) => M f g a +countUp :: forall f g a . (View f, View g, CMatrix a) => M f g a countUp = mkM z where z = CM.vertcat (V.fromList [CM.horzcat (V.fromList [ fromIntegral (c + cols*r) @@ -251,19 +192,19 @@ rows = size (Proxy :: Proxy f) cols = size (Proxy :: Proxy g) -row :: (CasadiMat a, View f) => J f a -> M (JV Id) f a -row (UnsafeJ x) = mkM (CM.trans x) +row :: (CMatrix a, View f, Viewable a) => J f a -> M (JV Id) f a +row = mkM . CM.trans . unJ -col :: (CasadiMat a, View f) => J f a -> M f (JV Id) a -col (UnsafeJ x) = mkM x +col :: (CMatrix a, View f, Viewable a) => J f a -> M f (JV Id) a +col = mkM . unJ -unrow :: (Viewable a, CasadiMat a, View f) => M (JV Id) f a -> J f a +unrow :: (Viewable a, CMatrix a, View f) => M (JV Id) f a -> J f a unrow (UnsafeM x) = mkJ (CM.trans x) -uncol :: (Viewable a, CasadiMat a, View f) => M f (JV Id) a -> J f a +uncol :: (Viewable a, CMatrix a, View f) => M f (JV Id) a -> J f a uncol (UnsafeM x) = mkJ x -solve :: (View g, View h, CasadiMat a) => M f g a -> M f h a -> M g h a +solve :: (View g, View h, CMatrix a) => M f g a -> M f h a -> M g h a solve (UnsafeM x) (UnsafeM y) = mkM (CM.solve x y) toHMat :: forall n m @@ -271,7 +212,7 @@ => M n m DMatrix -> Mat.Matrix Double toHMat (UnsafeM d) = Mat.trans $ (m Mat.>< n) (V.toList v) where - v = ddata (ddense d) + v = ddata (CM.dense d) n = size (Proxy :: Proxy n) m = size (Proxy :: Proxy m) @@ -281,4 +222,5 @@ Left msg -> error msg fromHMat' :: (View g, View f) => Mat.Matrix Double -> Either String (M f g DMatrix) -fromHMat' = mkM' . CM.vertcat . V.fromList . fmap (CM.trans . dvector . V.fromList) . Mat.toLists +fromHMat' = mkM' . CM.vertcat . V.fromList . fmap (CM.trans . CM.fromDVector . V.fromList) . Mat.toLists + diff --git a/src/Dyno/View/NumInstances.hs b/src/Dyno/View/NumInstances.hs deleted file mode 100644 --- a/src/Dyno/View/NumInstances.hs +++ /dev/null @@ -1,166 +0,0 @@ -{-# OPTIONS_GHC -Wall -fno-warn-orphans #-} -{-# LANGUAGE FlexibleInstances #-} -{-# LANGUAGE ScopedTypeVariables #-} - -module Dyno.View.NumInstances - ( - ) where - -import Data.Proxy ( Proxy(..) ) -import Data.Vector ( Vector ) -import qualified Data.Vector as V - -import Dyno.View.CasadiMat ( CasadiMat(..) ) -import Casadi.MX ( MX ) -import Casadi.SX ( SX ) -import Casadi.DMatrix ( DMatrix ) - -import Dyno.View.View - ---------------------------- SX --------------------------- -instance (View f) => Num (J f SX) where - (UnsafeJ x) + (UnsafeJ y) = mkJ (x + y) - (UnsafeJ x) - (UnsafeJ y) = mkJ (x - y) - (UnsafeJ x) * (UnsafeJ y) = mkJ (x * y) - abs = fmap abs - signum = fmap signum - fromInteger k = mkJ (fromInteger k * ones (n, 1)) - where - n = size (Proxy :: Proxy f) - -instance (View f) => Fractional (J f SX) where - (UnsafeJ x) / (UnsafeJ y) = mkJ (x / y) - fromRational x = mkJ (fromRational x * ones (n, 1)) - where - n = size (Proxy :: Proxy f) - -instance (View f) => Floating (J f SX) where - pi = mkJ (pi * ones (n, 1)) - where - n = size (Proxy :: Proxy f) - (**) (UnsafeJ x) (UnsafeJ y) = mkJ (x ** y) - exp = fmap exp - log = fmap log - sin = fmap sin - cos = fmap cos - tan = fmap tan - asin = fmap asin - atan = fmap atan - acos = fmap acos - sinh = fmap sinh - cosh = fmap cosh - tanh = fmap tanh - asinh = fmap asinh - atanh = fmap atanh - acosh = fmap acosh - ---------------------------- MX --------------------------- -instance (View f) => Num (J f MX) where - (UnsafeJ x) + (UnsafeJ y) = mkJ (x + y) - (UnsafeJ x) - (UnsafeJ y) = mkJ (x - y) - (UnsafeJ x) * (UnsafeJ y) = mkJ (x * y) - abs = fmap abs - signum = fmap signum - fromInteger k = mkJ (fromInteger k * ones (n, 1)) - where - n = size (Proxy :: Proxy f) - -instance (View f) => Fractional (J f MX) where - (UnsafeJ x) / (UnsafeJ y) = mkJ (x / y) - fromRational x = mkJ (fromRational x * ones (n, 1)) - where - n = size (Proxy :: Proxy f) - -instance (View f) => Floating (J f MX) where - pi = mkJ (pi * ones (n, 1)) - where - n = size (Proxy :: Proxy f) - (**) (UnsafeJ x) (UnsafeJ y) = mkJ (x ** y) - exp = fmap exp - log = fmap log - sin = fmap sin - cos = fmap cos - tan = fmap tan - asin = fmap asin - atan = fmap atan - acos = fmap acos - sinh = fmap sinh - cosh = fmap cosh - tanh = fmap tanh - asinh = fmap asinh - atanh = fmap atanh - acosh = fmap acosh - ----------------------------- DMatrix ---------------------------------- -instance (View f) => Num (J f DMatrix) where - (UnsafeJ x) + (UnsafeJ y) = mkJ (x + y) - (UnsafeJ x) - (UnsafeJ y) = mkJ (x - y) - (UnsafeJ x) * (UnsafeJ y) = mkJ (x * y) - abs = fmap abs - signum = fmap signum - fromInteger k = mkJ (fromInteger k * ones (n, 1)) - where - n = size (Proxy :: Proxy f) - -instance (View f) => Fractional (J f DMatrix) where - (UnsafeJ x) / (UnsafeJ y) = mkJ (x / y) - fromRational x = mkJ (fromRational x * ones (n, 1)) - where - n = size (Proxy :: Proxy f) - -instance (View f) => Floating (J f DMatrix) where - pi = mkJ (pi * ones (n, 1)) - where - n = size (Proxy :: Proxy f) - (**) (UnsafeJ x) (UnsafeJ y) = mkJ (x ** y) - exp = fmap exp - log = fmap log - sin = fmap sin - cos = fmap cos - tan = fmap tan - asin = fmap asin - atan = fmap atan - acos = fmap acos - sinh = fmap sinh - cosh = fmap cosh - tanh = fmap tanh - asinh = fmap asinh - atanh = fmap atanh - acosh = fmap acosh - ----------------------- Vector a ------------------------ -instance (View f, Num a) => Num (J f (Vector a)) where - (UnsafeJ x) + (UnsafeJ y) = mkJ $ V.zipWith (+) x y - (UnsafeJ x) - (UnsafeJ y) = mkJ $ V.zipWith (-) x y - (UnsafeJ x) * (UnsafeJ y) = mkJ $ V.zipWith (*) x y - abs = fmap (fmap abs) - signum = fmap (fmap signum) - fromInteger k = mkJ (V.replicate n (fromInteger k)) - where - n = size (Proxy :: Proxy f) - -instance (View f, Fractional a) => Fractional (J f (Vector a)) where - (UnsafeJ x) / (UnsafeJ y) = mkJ $ V.zipWith (/) x y - fromRational x = mkJ (V.replicate n (fromRational x)) - where - n = size (Proxy :: Proxy f) - -instance (View f, Floating a) => Floating (J f (Vector a)) where - pi = mkJ (V.replicate n pi) - where - n = size (Proxy :: Proxy f) - (**) (UnsafeJ x) (UnsafeJ y) = mkJ $ V.zipWith (**) x y - exp = fmap (fmap exp) - log = fmap (fmap log) - sin = fmap (fmap sin) - cos = fmap (fmap cos) - tan = fmap (fmap tan) - asin = fmap (fmap asin) - atan = fmap (fmap atan) - acos = fmap (fmap acos) - sinh = fmap (fmap sinh) - cosh = fmap (fmap cosh) - tanh = fmap (fmap tanh) - asinh = fmap (fmap asinh) - atanh = fmap (fmap atanh) - acosh = fmap (fmap acosh) diff --git a/src/Dyno/View/Scheme.hs b/src/Dyno/View/Scheme.hs --- a/src/Dyno/View/Scheme.hs +++ b/src/Dyno/View/Scheme.hs @@ -12,39 +12,38 @@ module Dyno.View.Scheme ( Scheme(..) , FunctionIO(..) - , blockSplit ) where +import GHC.Generics + import Data.Proxy import qualified Data.Foldable as F import qualified Data.Sequence as Seq import qualified Data.Vector as V import Data.Vector ( Vector ) -import GHC.Generics hiding ( S ) -import Dyno.Nats -import Dyno.View.View -import Dyno.View.CasadiMat -import Dyno.View.M ( M(..) ) +import Casadi.CMatrix ( CMatrix ) +import qualified Casadi.CMatrix as CM -data MyScheme a = MyScheme (J (JVec D3 S) a) (J (JVec D2 S) a) deriving (Generic, Generic1, Show) -instance Scheme MyScheme +import Dyno.View.Unsafe.View ( unsafeUnJ, mkJ ) +import Dyno.View.Unsafe.M ( M(UnsafeM), unM ) +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 -blockSplit :: forall f g a . (View f, View g, CasadiMat a) => M f g a -> Vector (Vector a) -blockSplit (UnsafeM m) = fmap (flip horzsplit hsizes) ms - where - vsizes = V.fromList $ 0 : (F.toList (sizes 0 (Proxy :: Proxy f))) - hsizes = V.fromList $ 0 : (F.toList (sizes 0 (Proxy :: Proxy g))) - ms = vertsplit m vsizes - class FunctionIO (f :: * -> *) where - fromMat :: CasadiMat a => a -> Either String (f a) + fromMat :: (CMatrix a, Viewable a) => a -> Either String (f a) toFioMat :: f a -> a matSizes :: Proxy f -> (Int,Int) @@ -74,14 +73,14 @@ | n1 /= n1' = mismatch | n1 /= 0 && n2 /= n2' = mismatch | n1 == 0 && not (n2 `elem` [0,1]) = mismatch - | otherwise = Right (UnsafeJ x) + | 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 = size1 x - n2 = size2 x + n1 = CM.size1 x + n2 = CM.size2 x matSizes = const (size (Proxy :: Proxy f), 1) instance (View f, View g) => FunctionIO (M f g) where @@ -95,13 +94,13 @@ ", actual size: " ++ show (n1,n2) n1' = size (Proxy :: Proxy f) n2' = size (Proxy :: Proxy g) - n1 = size1 x - n2 = size2 x + n1 = CM.size1 x + n2 = CM.size2 x matSizes = const (size (Proxy :: Proxy f), size (Proxy :: Proxy g)) class Scheme (f :: * -> *) where numFields :: Proxy f -> Int - fromVector :: CasadiMat a => V.Vector a -> f a + fromVector :: (CMatrix a, Viewable a) => V.Vector a -> f a toVector :: f a -> V.Vector a sizeList :: Proxy f -> [(Int,Int)] @@ -118,7 +117,7 @@ reproxy = const Proxy default fromVector :: ( Rep (f a) aa ~ M1 t d ff aa, GFromVector (Rep (f a)) a - , Generic (f a), Datatype d, CasadiMat a ) + , Generic (f a), Datatype d, CMatrix a, Viewable a ) => Vector a -> f a fromVector vs = out' where @@ -140,7 +139,7 @@ class GSizeList f where gsizeList :: Proxy (f p) -> Seq.Seq (Int,Int) class GFromVector f a where - gfromVector :: CasadiMat a => String -> Vector a -> Proxy (f a) -> f a + gfromVector :: CMatrix a => String -> Vector a -> Proxy (f a) -> f a class GToVector f a where gtoVector :: f a -> Seq.Seq a @@ -200,7 +199,7 @@ reproxy :: Proxy (M1 i d f p) -> Proxy (f p) reproxy = const Proxy -instance FunctionIO f => GFromVector (Rec0 (f a)) a where +instance (FunctionIO f, Viewable a) => GFromVector (Rec0 (f a)) a where gfromVector name ms = const (K1 j) where j = case fromMat m of diff --git a/src/Dyno/View/Symbolic.hs b/src/Dyno/View/Symbolic.hs --- a/src/Dyno/View/Symbolic.hs +++ b/src/Dyno/View/Symbolic.hs @@ -3,10 +3,6 @@ module Dyno.View.Symbolic ( Symbolic(..) - , Matrix(..) - , MX - , SX - , DMatrix.DMatrix ) where import Data.Proxy ( Proxy(..) ) @@ -18,21 +14,18 @@ import Casadi.Core.Classes.MXFunction import Casadi.Core.Enums ( InputOutputScheme(..) ) +import Casadi.CMatrix ( CMatrix(..) ) import Casadi.SX ( SX, ssymV ) import Casadi.Option ( setOption ) import Casadi.MX ( MX, symV ) -import qualified Casadi.SX as SX -import qualified Casadi.MX as MX -import qualified Casadi.DMatrix as DMatrix import Casadi.IOSchemes +import Dyno.View.Unsafe.View ( mkJ ) -import Dyno.View.View ( View(..), J, mkJ ) +import Dyno.View.View ( View(..), J ) import Dyno.View.Viewable ( Viewable(..) ) -import Dyno.View.CasadiMat ( CasadiMat ) - -class (Viewable a, CasadiMat a) => Symbolic a where +class (Viewable a, CMatrix a) => Symbolic a where -- | creating symbolics sym :: View f => String -> IO (J f a) mkScheme :: InputOutputScheme -> [(String,a)] -> IO (Vector a) @@ -54,17 +47,6 @@ setOption f "name" name sharedObject_init__0 f return (castFunction f) - - -class Matrix a where - diag :: a -> a -instance Matrix DMatrix.DMatrix where - diag = DMatrix.ddiag -instance Matrix SX where - diag = SX.sdiag -instance Matrix MX where - diag = MX.diag - mkSym :: forall f a . (View f, Viewable a) => (String -> Int -> IO a) -> String -> IO (J f a) mkSym vsym name = ret diff --git a/src/Dyno/View/Unsafe/M.hs b/src/Dyno/View/Unsafe/M.hs new file mode 100644 --- /dev/null +++ b/src/Dyno/View/Unsafe/M.hs @@ -0,0 +1,113 @@ +{-# OPTIONS_GHC -Wall #-} +{-# LANGUAGE ScopedTypeVariables #-} +{-# LANGUAGE KindSignatures #-} +{-# LANGUAGE DeriveFunctor #-} +{-# LANGUAGE DeriveGeneric #-} + +module Dyno.View.Unsafe.M + ( M(..) + , mkM + , mkM' + , blockSplit + ) where + +import GHC.Generics ( Generic ) + +import Data.Proxy +import qualified Data.Foldable as F +import qualified Data.Vector as V +import Data.Vector ( Vector ) + +import Casadi.Overloading ( Fmod(..), ArcTan2(..), SymOrd(..) ) +import Casadi.CMatrix ( CMatrix ) +import qualified Casadi.CMatrix as CM + +import Dyno.View.View ( View(..) ) + +newtype M (f :: * -> *) (g :: * -> *) (a :: *) = + UnsafeM { unM :: a } deriving (Eq, Functor, Generic) + +instance Show a => Show (M f g a) where + showsPrec p (UnsafeM x) = showsPrec p x + +over :: (View f, View g, CMatrix a) => (a -> a) -> M f g a -> M f g a +over f (UnsafeM x) = mkM (f x) + +over2 :: (View f, View g, CMatrix a) => (a -> a -> a) -> M f g a -> M f g a -> M f g a +over2 f (UnsafeM x) (UnsafeM y)= mkM (f x y) + +instance (View f, View g, CMatrix a) => Num (M f g a) where + (+) = over2 (+) + (-) = over2 (-) + (*) = over2 (*) + negate = over negate + abs = over abs + signum = over signum + fromInteger k = mkM $ fromInteger k * CM.ones (nx,ny) + where + nx = size (Proxy :: Proxy f) + ny = size (Proxy :: Proxy g) +instance (View f, View g, CMatrix a) => Fractional (M f g a) where + (/) = over2 (/) + fromRational k = mkM $ fromRational k * CM.ones (nx,ny) + where + nx = size (Proxy :: Proxy f) + ny = size (Proxy :: Proxy g) +instance (View f, View g, CMatrix a) => Floating (M f g a) where + pi = mkM $ pi * CM.ones (nx,ny) + where + nx = size (Proxy :: Proxy f) + ny = size (Proxy :: Proxy g) + (**) = over2 (**) + exp = over exp + log = over log + sin = over sin + cos = over cos + tan = over tan + asin = over asin + atan = over atan + acos = over acos + sinh = over sinh + cosh = over cosh + tanh = over tanh + asinh = over asinh + atanh = over atanh + acosh = over acosh + +instance (View f, View g, CMatrix a) => Fmod (M f g a) where + fmod = over2 fmod + +instance (View f, View g, CMatrix a) => ArcTan2 (M f g a) where + arctan2 = over2 arctan2 + +instance (View f, View g, CMatrix a) => SymOrd (M f g a) where + leq = over2 leq + geq = over2 geq + eq = over2 eq + +mkM :: forall f g a . (View f, View g, CMatrix a) => a -> M f g a +mkM x = case mkM' x of + Right x' -> x' + Left msg -> error msg + +mkM' :: forall f g a . (View f, View g, CMatrix a) => a -> Either String (M f g a) +mkM' x + | nx == nx' && ny == ny' = Right (UnsafeM x) + | all (== 0) [nx,nx'] && ny' == 0 = Right zeros + | all (== 0) [ny,ny'] && nx' == 0 = Right zeros + | otherwise = Left $ "mkM length mismatch: typed size: " ++ show (nx,ny) ++ + ", actual size: " ++ show (nx', ny') + where + nx = size (Proxy :: Proxy f) + ny = size (Proxy :: Proxy g) + nx' = CM.size1 x + ny' = CM.size2 x + zeros = mkM (CM.zeros (nx, ny)) + + +blockSplit :: forall f g a . (View f, View g, CMatrix a) => M f g a -> Vector (Vector a) +blockSplit (UnsafeM m) = fmap (flip CM.horzsplit hsizes) ms + where + vsizes = V.fromList $ 0 : (F.toList (sizes 0 (Proxy :: Proxy f))) + hsizes = V.fromList $ 0 : (F.toList (sizes 0 (Proxy :: Proxy g))) + ms = CM.vertsplit m vsizes diff --git a/src/Dyno/View/Unsafe/View.hs b/src/Dyno/View/Unsafe/View.hs new file mode 100644 --- /dev/null +++ b/src/Dyno/View/Unsafe/View.hs @@ -0,0 +1,262 @@ +{-# OPTIONS_GHC -Wall #-} +{-# LANGUAGE ScopedTypeVariables #-} +{-# LANGUAGE KindSignatures #-} +{-# LANGUAGE DefaultSignatures #-} +{-# LANGUAGE TypeOperators #-} +{-# LANGUAGE MultiParamTypeClasses #-} +{-# LANGUAGE FlexibleContexts #-} +{-# LANGUAGE FlexibleInstances #-} +{-# LANGUAGE DeriveFunctor #-} +{-# LANGUAGE DeriveGeneric #-} +{-# LANGUAGE InstanceSigs #-} + +module Dyno.View.Unsafe.View + ( View(..), J(..) + , mkJ, mkJ', unJ, unJ' + ) where + +import GHC.Generics + +import qualified Data.Foldable as F +import qualified Data.Sequence as Seq +import Data.Proxy ( Proxy(..) ) +import Data.Vector ( Vector ) +import qualified Data.Vector as V +import Data.Serialize ( Serialize(..) ) + +import qualified Casadi.CMatrix as CM + +import Dyno.View.Viewable ( Viewable(..) ) + +newtype J (f :: * -> *) (a :: *) = UnsafeJ { unsafeUnJ :: a } deriving (Eq, Generic) + +instance (Serialize a, View f) => Serialize (J f (Vector a)) where + put = put . V.toList . unJ + get = fmap (mkJ . V.fromList) get + +instance Show a => Show (J f a) where + showsPrec p (UnsafeJ x) = showsPrec p x + +instance (View f, Viewable a, CM.CMatrix a) => Num (J f a) where + (UnsafeJ x) + (UnsafeJ y) = mkJ (x + y) + (UnsafeJ x) - (UnsafeJ y) = mkJ (x - y) + (UnsafeJ x) * (UnsafeJ y) = mkJ (x * y) + abs (UnsafeJ x) = mkJ $ abs x + signum (UnsafeJ x) = mkJ $ signum x + fromInteger k = mkJ (fromInteger k * CM.ones (n, 1)) + where + n = size (Proxy :: Proxy f) + +instance (View f, Viewable a, CM.CMatrix a) => Fractional (J f a) where + (UnsafeJ x) / (UnsafeJ y) = mkJ (x / y) + fromRational x = mkJ (fromRational x * CM.ones (n, 1)) + where + n = size (Proxy :: Proxy f) + +instance (View f, Viewable a, CM.CMatrix a) => Floating (J f a) where + pi = mkJ (pi * CM.ones (n, 1)) + where + n = size (Proxy :: Proxy f) + (**) (UnsafeJ x) (UnsafeJ y) = mkJ (x ** y) + exp (UnsafeJ x) = mkJ $ exp x + log (UnsafeJ x) = mkJ $ log x + sin (UnsafeJ x) = mkJ $ sin x + cos (UnsafeJ x) = mkJ $ cos x + tan (UnsafeJ x) = mkJ $ tan x + asin (UnsafeJ x) = mkJ $ asin x + atan (UnsafeJ x) = mkJ $ atan x + acos (UnsafeJ x) = mkJ $ acos x + sinh (UnsafeJ x) = mkJ $ sinh x + cosh (UnsafeJ x) = mkJ $ cosh x + tanh (UnsafeJ x) = mkJ $ tanh x + asinh (UnsafeJ x) = mkJ $ asinh x + atanh (UnsafeJ x) = mkJ $ atanh x + acosh (UnsafeJ x) = mkJ $ acosh x + +mkJ :: forall f a . (View f, Viewable a) => a -> J f a +mkJ x = case mkJ' x of + Right x' -> x' + Left msg -> error msg + +mkJ' :: forall f a . (View f, Viewable a) => a -> Either String (J f a) +mkJ' x + | ny' == 1 && nx == nx' = Right (UnsafeJ x) + | ny' == 0 && nx == nx' = Right (UnsafeJ (vrecoverDimension x 0)) + | otherwise = Left $ "mkJ length mismatch: typed size: " ++ show nx ++ + ", actual size: " ++ show nx' + where + nx = size (Proxy :: Proxy f) + nx' = vsize1 x + ny' = vsize2 x + +unJ :: forall f a . (View f, Viewable a) => J f a -> a +unJ (UnsafeJ x) + | nx == nx' = x + | otherwise = error $ "unJ length mismatch: typed size: " ++ show nx ++ + ", actual size: " ++ show nx' + where + nx = size (Proxy :: Proxy f) + nx' = vsize1 x + +unJ' :: forall f a . (View f, Viewable a) => String -> J f a -> a +unJ' msg (UnsafeJ x) + | nx == nx' = x + | otherwise = error $ "unJ length mismatch in \"" ++ msg ++ "\": typed size: " ++ show nx ++ + ", actual size: " ++ show nx' + where + nx = size (Proxy :: Proxy f) + nx' = vsize1 x + +-- | Type-save "views" into vectors, which can access subvectors +-- without splitting then concatenating everything. +class View f where + cat :: Viewable a => f a -> J f a + default cat :: (GCat (Rep (f a)) a, Generic (f a), Viewable a) => f a -> J f a + cat = mkJ . vveccat . V.fromList . F.toList . gcat . from + + size :: Proxy f -> Int + default size :: (GSize (Rep (f ())), Generic (f ())) => Proxy f -> Int + size = gsize . reproxy + where + reproxy :: Proxy g -> Proxy ((Rep (g ())) p) + reproxy = const Proxy + + sizes :: Int -> Proxy f -> Seq.Seq Int + default sizes :: (GSize (Rep (f ())), Generic (f ())) => Int -> Proxy f -> Seq.Seq Int + sizes k0 = gsizes k0 . reproxy + where + reproxy :: Proxy g -> Proxy ((Rep (g ())) p) + reproxy = const Proxy + + split :: Viewable a => J f a -> f a + default split :: (GBuild (Rep (f a)) a, Generic (f a), Viewable a) => J f a -> f a + split x' + | null leftovers = to ret + | otherwise = error $ unlines + [ "split got " ++ show (length leftovers) ++ " leftover fields" + , "ns: " ++ show ns ++ "\n" ++ show (map vsize1 leftovers) + --, "x: " ++ show x' + , "size1(x): " ++ show (vsize1 (unJ x')) + --, "leftovers: " ++ show leftovers + , "errors: " ++ show (reverse errors) + ] + where + x = unJ x' + (ret,leftovers,errors) = gbuild [] xs + xs = V.toList $ vvertsplit x (V.fromList ns) + ns :: [Int] + ns = (0 :) $ F.toList $ sizes 0 (Proxy :: Proxy f) + +------------------------------------ SIZE ------------------------------ +class GSize f where + gsize :: Proxy (f p) -> Int + gsizes :: Int -> Proxy (f p) -> Seq.Seq Int + +instance (GSize f, GSize g) => GSize (f :*: g) where + gsize pxy = gsize px + gsize py + where + reproxy :: Proxy ((x :*: y) p) -> (Proxy (x p), Proxy (y p)) + reproxy = const (Proxy,Proxy) + (px, py) = reproxy pxy + gsizes k0 pxy = xs Seq.>< ys + where + xs = gsizes k0 px + ys = gsizes k1 py + k1 = case Seq.viewr xs of + Seq.EmptyR -> k0 + _ Seq.:> k1' -> k1' + + reproxy :: Proxy ((x :*: y) p) -> (Proxy (x p), Proxy (y p)) + reproxy = const (Proxy,Proxy) + (px, py) = reproxy pxy +instance GSize f => GSize (M1 i d f) where + gsize = gsize . reproxy + where + reproxy :: Proxy (M1 i d f p) -> Proxy (f p) + reproxy _ = Proxy + gsizes k0 = gsizes k0 . reproxy + where + reproxy :: Proxy (M1 i d f p) -> Proxy (f p) + reproxy _ = Proxy + +instance View f => GSize (Rec0 (J f a)) where + gsize = size . reproxy + where + reproxy :: Proxy (Rec0 (J f a) p) -> Proxy f + reproxy _ = Proxy + gsizes k0 = Seq.singleton . (k0 +) . size . reproxy + where + reproxy :: Proxy (Rec0 (J f a) p) -> Proxy f + reproxy _ = Proxy + +instance GSize U1 where + gsize = const 0 + gsizes = const . Seq.singleton + +----------------------------- CAT ------------------------------- +class GCat f a where + gcat :: f p -> Seq.Seq a + +-- concatenate fields recursively +instance (GCat f a, GCat g a) => GCat (f :*: g) a where + gcat (x :*: y) = x' Seq.>< y' + where + x' = gcat x + y' = gcat y +-- discard the metadata +instance GCat f a => GCat (M1 i d f) a where + gcat = gcat . unM1 + +-- any field should just hold a view, no recursion here +instance (View f, Viewable a) => GCat (Rec0 (J f a)) a where + gcat (K1 x) = Seq.singleton (unJ x) + +instance GCat U1 a where + gcat U1 = Seq.empty + +------------------------- +class GBuild f a where + gbuild :: [String] -> [a] -> (f p, [a], [String]) + +-- split fields recursively +instance (GBuild f a, GBuild g a, GSize f, GSize g) => GBuild (f :*: g) a where + gbuild errs0 xs0 = (x :*: y, xs2, errs2) + where + (x,xs1,errs1) = gbuild errs0 xs0 + (y,xs2,errs2) = gbuild errs1 xs1 + +instance (GBuild f a, Datatype d) => GBuild (D1 d f) a where + gbuild :: forall p . [String] -> [a] -> (D1 d f p, [a], [String]) + gbuild errs0 xs0 = (ret, xs1, errs1) + where + err = moduleName ret ++ "." ++ datatypeName ret :: String + ret = M1 x :: D1 d f p + (x,xs1,errs1) = gbuild (err:errs0) xs0 + +instance (GBuild f a, Constructor c) => GBuild (C1 c f) a where + gbuild :: forall p . [String] -> [a] -> (C1 c f p, [a], [String]) + gbuild errs0 xs0 = (ret, xs1, errs1) + where + err = conName ret :: String + ret = M1 x :: C1 c f p + (x,xs1,errs1) = gbuild (err:errs0) xs0 + +instance (GBuild f a, Selector s) => GBuild (S1 s f) a where + gbuild :: forall p . [String] -> [a] -> (S1 s f p, [a], [String]) + gbuild errs0 xs0 = (ret, xs1, errs1) + where + err = selName ret :: String + ret = M1 x :: S1 s f p + (x,xs1,errs1) = gbuild (err:errs0) xs0 + +-- any field should just hold a view, no recursion here +instance (View f, Viewable a) => GBuild (Rec0 (J f a)) a where + gbuild errs (x:xs) = (K1 (mkJ x), xs, errs) + gbuild errs [] = error $ "GBuild (Rec0 (J f a)) a: empty list" ++ show (reverse errs) + +instance Viewable a => GBuild U1 a where + gbuild errs (x:xs) + | vsize1 x /= 0 = error $ "GBuild U1: got non-empty element: " ++ + show (vsize1 x) ++ "\n" ++ show (reverse errs) + | otherwise = (U1, xs, errs) + gbuild errs [] = error $ "GBuild U1: got empty" ++ show (reverse errs) diff --git a/src/Dyno/View/View.hs b/src/Dyno/View/View.hs --- a/src/Dyno/View/View.hs +++ b/src/Dyno/View/View.hs @@ -1,302 +1,64 @@ {-# OPTIONS_GHC -Wall #-} {-# LANGUAGE ScopedTypeVariables #-} -{-# LANGUAGE KindSignatures #-} -{-# LANGUAGE DefaultSignatures #-} -{-# LANGUAGE TypeOperators #-} -{-# LANGUAGE MultiParamTypeClasses #-} -{-# LANGUAGE FlexibleContexts #-} -{-# LANGUAGE FlexibleInstances #-} {-# LANGUAGE DeriveGeneric #-} {-# LANGUAGE DeriveFunctor #-} {-# LANGUAGE DeriveFoldable #-} {-# LANGUAGE DeriveTraversable #-} -{-# LANGUAGE GeneralizedNewtypeDeriving #-} -{-# LANGUAGE RankNTypes #-} -{-# LANGUAGE InstanceSigs #-} module Dyno.View.View - ( J(..), mkJ, mkJ', unJ, unJ', View(..), JVec(..), JNone(..), S(..) - , JTuple(..) - , JTriple(..) - , jreplicate, jreplicate' - , reifyJVec, jfill + ( View(..) + , J + , JNone(..), JTuple(..), JTriple(..) + , jfill + , v2d, d2v + , fmapJ, unzipJ + , fromDMatrix ) where -import GHC.Generics hiding ( S ) +import GHC.Generics ( Generic, Generic1 ) import Data.Foldable ( Foldable ) -import qualified Data.Foldable as F -import qualified Data.Sequence as Seq import Data.Traversable ( Traversable ) import Data.Proxy ( Proxy(..) ) -import Linear.V ( Dim(..) ) import Data.Vector ( Vector ) import qualified Data.Vector as V -import Data.Serialize ( Serialize(..) ) -import Dyno.TypeVecs ( Vec(..), unVec, mkVec, mkVec', reifyVector ) +import qualified Casadi.DMatrix as DMatrix +import qualified Casadi.CMatrix as CM + import Dyno.View.Viewable ( Viewable(..) ) import Dyno.Vectorize ( Vectorize(..) ) -import Dyno.Server.Accessors ( Lookup(..), AccessorTree ) + +import Dyno.View.Unsafe.View + +-- some helper types +data JNone a = JNone deriving ( Eq, Generic, Generic1, Show, Functor, Foldable, Traversable ) data JTuple f g a = JTuple (J f a) (J g a) deriving ( Generic, Show ) -instance (View f, View g) => View (JTuple f g) data JTriple f g h a = JTriple (J f a) (J g a) (J h a) deriving ( Generic, Show ) +instance Vectorize JNone where +instance View JNone where +instance (View f, View g) => View (JTuple f g) instance (View f, View g, View h) => View (JTriple f g h) ---instance View Id ---instance View Xy ---instance View Xyz ---instance View f => View (Fctr f) -newtype J (f :: * -> *) (a :: *) = UnsafeJ { unsafeUnJ :: a } deriving (Eq, Functor, Generic) - - -mkJ :: forall f a . (View f, Viewable a) => a -> J f a -mkJ x = case mkJ' x of - Right x' -> x' - Left msg -> error msg - -mkJ' :: forall f a . (View f, Viewable a) => a -> Either String (J f a) -mkJ' x - | ny' == 1 && nx == nx' = Right (UnsafeJ x) - | ny' == 0 && nx == nx' = Right (UnsafeJ (vrecoverDimension x 0)) - | otherwise = Left $ "mkJ length mismatch: typed size: " ++ show nx ++ - ", actual size: " ++ show nx' - where - nx = size (Proxy :: Proxy f) - nx' = vsize1 x - ny' = vsize2 x - -unJ :: forall f a . (View f, Viewable a) => J f a -> a -unJ (UnsafeJ x) - | nx == nx' = x - | otherwise = error $ "unJ length mismatch: typed size: " ++ show nx ++ - ", actual size: " ++ show nx' - where - nx = size (Proxy :: Proxy f) - nx' = vsize1 x - -unJ' :: forall f a . (View f, Viewable a) => String -> J f a -> a -unJ' msg (UnsafeJ x) - | nx == nx' = x - | otherwise = error $ "unJ length mismatch in \"" ++ msg ++ "\": typed size: " ++ show nx ++ - ", actual size: " ++ show nx' - where - nx = size (Proxy :: Proxy f) - nx' = vsize1 x - -instance Serialize a => Serialize (J f a) -instance Show a => Show (J f a) where - showsPrec p (UnsafeJ x) = showsPrec p x -instance Lookup a => Lookup (J S (Vector a)) where - toAccessorTree :: J S (Vector a) -> (b -> J S (Vector a)) -> AccessorTree b - toAccessorTree (UnsafeJ x) f = - toAccessorTree (V.head x) (V.head . unJ . f) - --- | vectors in View -newtype JVec n f a = JVec { unJVec :: Vec n (J f a) } deriving ( Show, Eq ) -instance (Dim n, View f) => View (JVec n f) where - cat = mkJ . vveccat . fmap unJ . unVec . unJVec - split = JVec . fmap mkJ . mkVec . flip vvertsplit ks . unJ - where - ks = V.fromList (take (n+1) [0,m..]) - n = reflectDim (Proxy :: Proxy n) - m = size (Proxy :: Proxy f) - size = const (n * m) - where - n = reflectDim (Proxy :: Proxy n) - m = size (Proxy :: Proxy f) - sizes = const . Seq.iterateN n (+m) . (+ m) - where - n = reflectDim (Proxy :: Proxy n) - m = size (Proxy :: Proxy f) -instance (Dim n, Serialize (J f a)) => Serialize (JVec n f a) where - get = fmap (JVec . mkVec') get - put = put . F.toList . unJVec - -jreplicate' :: forall a n f . (Dim n, View f) => J f a -> JVec n f a -jreplicate' el = ret - where - ret = JVec (mkVec (V.replicate nvec el)) - nvec = reflectDim (Proxy :: Proxy n) - -jreplicate :: forall a n f . (Dim n, View f, Viewable a) => J f a -> J (JVec n f) a -jreplicate = cat . jreplicate' - jfill :: forall a f . View f => a -> J f (Vector a) jfill x = mkJ (V.replicate n x) where n = size (Proxy :: Proxy f) -reifyJVec :: forall a f r . Vector (J f a) -> (forall (n :: *). Dim n => JVec n f a -> r) -> r -reifyJVec v f = reifyVector v $ \(v' :: Vec n (J f a)) -> f (JVec v' :: JVec n f a) -{-# INLINE reifyJVec #-} - --- | view into a None, for convenience -data JNone a = JNone deriving ( Eq, Generic, Generic1, Show, Functor, Foldable, Traversable ) -instance Vectorize JNone where -instance View JNone where - --- | view into a scalar, for convenience -newtype S a = S { unS :: a } deriving ( Eq, Num, Fractional, Floating, Generic, Generic1, Show, Functor, Foldable, Traversable ) -instance View S where - cat :: forall a . Viewable a => S a -> J S a - cat (S x) = mkJ x - size = const 1 - sizes = const . Seq.singleton . (1 +) - split :: forall a . Viewable a => J S a -> S a - split = S . unJ - --- | Type-save "views" into vectors, which can access subvectors --- without splitting then concatenating everything. -class View f where - cat :: Viewable a => f a -> J f a - default cat :: (GCat (Rep (f a)) a, Generic (f a), Viewable a) => f a -> J f a - cat = mkJ . vveccat . V.fromList . F.toList . gcat . from - - size :: Proxy f -> Int - default size :: (GSize (Rep (f ())), Generic (f ())) => Proxy f -> Int - size = gsize . reproxy - where - reproxy :: Proxy g -> Proxy ((Rep (g ())) p) - reproxy = const Proxy - - sizes :: Int -> Proxy f -> Seq.Seq Int - default sizes :: (GSize (Rep (f ())), Generic (f ())) => Int -> Proxy f -> Seq.Seq Int - sizes k0 = gsizes k0 . reproxy - where - reproxy :: Proxy g -> Proxy ((Rep (g ())) p) - reproxy = const Proxy - - split :: Viewable a => J f a -> f a - default split :: (GBuild (Rep (f a)) a, Generic (f a), Viewable a) => J f a -> f a - split x' - | null leftovers = to ret - | otherwise = error $ unlines - [ "split got " ++ show (length leftovers) ++ " leftover fields" - , "ns: " ++ show ns ++ "\n" ++ show (map vsize1 leftovers) - --, "x: " ++ show x' - , "size1(x): " ++ show (vsize1 (unJ x')) - --, "leftovers: " ++ show leftovers - , "errors: " ++ show (reverse errors) - ] - where - x = unJ x' - (ret,leftovers,errors) = gbuild [] xs - xs = V.toList $ vvertsplit x (V.fromList ns) - ns :: [Int] - ns = (0 :) $ F.toList $ sizes 0 (Proxy :: Proxy f) - ------------------------------------- SIZE ------------------------------ -class GSize f where - gsize :: Proxy (f p) -> Int - gsizes :: Int -> Proxy (f p) -> Seq.Seq Int - -instance (GSize f, GSize g) => GSize (f :*: g) where - gsize pxy = gsize px + gsize py - where - reproxy :: Proxy ((x :*: y) p) -> (Proxy (x p), Proxy (y p)) - reproxy = const (Proxy,Proxy) - (px, py) = reproxy pxy - gsizes k0 pxy = xs Seq.>< ys - where - xs = gsizes k0 px - ys = gsizes k1 py - k1 = case Seq.viewr xs of - Seq.EmptyR -> k0 - _ Seq.:> k1' -> k1' - - reproxy :: Proxy ((x :*: y) p) -> (Proxy (x p), Proxy (y p)) - reproxy = const (Proxy,Proxy) - (px, py) = reproxy pxy -instance GSize f => GSize (M1 i d f) where - gsize = gsize . reproxy - where - reproxy :: Proxy (M1 i d f p) -> Proxy (f p) - reproxy _ = Proxy - gsizes k0 = gsizes k0 . reproxy - where - reproxy :: Proxy (M1 i d f p) -> Proxy (f p) - reproxy _ = Proxy - -instance View f => GSize (Rec0 (J f a)) where - gsize = size . reproxy - where - reproxy :: Proxy (Rec0 (J f a) p) -> Proxy f - reproxy _ = Proxy - gsizes k0 = Seq.singleton . (k0 +) . size . reproxy - where - reproxy :: Proxy (Rec0 (J f a) p) -> Proxy f - reproxy _ = Proxy - -instance GSize U1 where - gsize = const 0 - gsizes = const . Seq.singleton - ------------------------------ CAT ------------------------------- -class GCat f a where - gcat :: f p -> Seq.Seq a - --- concatenate fields recursively -instance (GCat f a, GCat g a) => GCat (f :*: g) a where - gcat (x :*: y) = x' Seq.>< y' - where - x' = gcat x - y' = gcat y --- discard the metadata -instance GCat f a => GCat (M1 i d f) a where - gcat = gcat . unM1 - --- any field should just hold a view, no recursion here -instance (View f, Viewable a) => GCat (Rec0 (J f a)) a where - gcat (K1 x) = Seq.singleton (unJ x) - -instance GCat U1 a where - gcat U1 = Seq.empty - -------------------------- -class GBuild f a where - gbuild :: [String] -> [a] -> (f p, [a], [String]) - --- split fields recursively -instance (GBuild f a, GBuild g a, GSize f, GSize g) => GBuild (f :*: g) a where - gbuild errs0 xs0 = (x :*: y, xs2, errs2) - where - (x,xs1,errs1) = gbuild errs0 xs0 - (y,xs2,errs2) = gbuild errs1 xs1 - -instance (GBuild f a, Datatype d) => GBuild (D1 d f) a where - gbuild :: forall p . [String] -> [a] -> (D1 d f p, [a], [String]) - gbuild errs0 xs0 = (ret, xs1, errs1) - where - err = moduleName ret ++ "." ++ datatypeName ret :: String - ret = M1 x :: D1 d f p - (x,xs1,errs1) = gbuild (err:errs0) xs0 +fromDMatrix :: (CM.CMatrix a, Viewable a, View f) => J f DMatrix.DMatrix -> J f a +fromDMatrix = mkJ . CM.fromDMatrix . unJ -instance (GBuild f a, Constructor c) => GBuild (C1 c f) a where - gbuild :: forall p . [String] -> [a] -> (C1 c f p, [a], [String]) - gbuild errs0 xs0 = (ret, xs1, errs1) - where - err = conName ret :: String - ret = M1 x :: C1 c f p - (x,xs1,errs1) = gbuild (err:errs0) xs0 +v2d :: View f => J f (V.Vector Double) -> J f DMatrix.DMatrix +v2d = mkJ . CM.fromDVector . unJ -instance (GBuild f a, Selector s) => GBuild (S1 s f) a where - gbuild :: forall p . [String] -> [a] -> (S1 s f p, [a], [String]) - gbuild errs0 xs0 = (ret, xs1, errs1) - where - err = selName ret :: String - ret = M1 x :: S1 s f p - (x,xs1,errs1) = gbuild (err:errs0) xs0 +d2v :: View f => J f DMatrix.DMatrix -> J f (V.Vector Double) +d2v = mkJ . DMatrix.ddata . CM.dense . unJ --- any field should just hold a view, no recursion here -instance (View f, Viewable a) => GBuild (Rec0 (J f a)) a where - gbuild errs (x:xs) = (K1 (mkJ x), xs, errs) - gbuild errs [] = error $ "GBuild (Rec0 (J f a)) a: empty list" ++ show (reverse errs) +fmapJ :: View f => (a -> b) -> J f (Vector a) -> J f (Vector b) +fmapJ f = mkJ . V.map f . unJ -instance Viewable a => GBuild U1 a where - gbuild errs (x:xs) - | vsize1 x /= 0 = error $ "GBuild U1: got non-empty element: " ++ - show (vsize1 x) ++ "\n" ++ show (reverse errs) - | otherwise = (U1, xs, errs) - gbuild errs [] = error $ "GBuild U1: got empty" ++ show (reverse errs) +unzipJ :: View f => J f (Vector (a,b)) -> (J f (Vector a), J f (Vector b)) +unzipJ v = (mkJ x, mkJ y) + where + (x,y) = V.unzip (unJ v) diff --git a/src/Dyno/View/Viewable.hs b/src/Dyno/View/Viewable.hs --- a/src/Dyno/View/Viewable.hs +++ b/src/Dyno/View/Viewable.hs @@ -2,7 +2,7 @@ {-# Language TypeFamilies #-} module Dyno.View.Viewable - ( Viewable(..), MX.MX, SX.SX, DMatrix.DMatrix + ( Viewable(..) ) where import qualified Data.Vector as V @@ -10,7 +10,7 @@ import qualified Casadi.SX as SX import qualified Casadi.MX as MX import qualified Casadi.DMatrix as DMatrix -import qualified Dyno.View.CasadiMat as CM +import qualified Casadi.CMatrix as CM class Viewable a where vvertsplit :: a -> V.Vector Int -> V.Vector a @@ -21,7 +21,7 @@ vrecoverDimension :: a -> Int -> a instance Viewable SX.SX where - vveccat = SX.sveccat + vveccat = CM.veccat vvertsplit = CM.vertsplit vhorzsplit = CM.horzsplit vsize1 = CM.size1 @@ -29,7 +29,7 @@ vrecoverDimension _ k = CM.zeros (k,1) instance Viewable MX.MX where - vveccat = MX.veccat + vveccat = CM.veccat vvertsplit = CM.vertsplit vhorzsplit = CM.horzsplit vsize1 = CM.size1 @@ -37,7 +37,7 @@ vrecoverDimension _ k = CM.zeros (k,1) instance Viewable DMatrix.DMatrix where - vveccat = DMatrix.dveccat + vveccat = CM.veccat vvertsplit = CM.vertsplit vhorzsplit = CM.horzsplit vsize1 = CM.size1 diff --git a/tests/VectorizeTests.hs b/tests/VectorizeTests.hs --- a/tests/VectorizeTests.hs +++ b/tests/VectorizeTests.hs @@ -3,6 +3,8 @@ {-# Language GADTs #-} {-# Language DeriveFunctor #-} {-# Language DeriveGeneric #-} +{-# Language DataKinds #-} +{-# Language PolyKinds #-} module VectorizeTests ( Vectorizes(..) @@ -10,7 +12,9 @@ , vectorizeTests ) where -import GHC.Generics ( Generic ) +import GHC.Generics ( Generic, Generic1 ) + +import Data.Proxy ( Proxy(..) ) import qualified Data.Vector as V import Linear import Linear.V @@ -20,7 +24,6 @@ import Test.Framework.Providers.QuickCheck2 ( testProperty ) import Dyno.Vectorize -import Dyno.Nats import qualified Dyno.TypeVecs as TV import Utils @@ -43,7 +46,7 @@ data Dims where Dims :: Dim n => { dShrinks :: [Dims] - , dProxy :: Proxy n + , dProxy :: Proxy (n :: k) } -> Dims instance Show Dims where show (Dims _ p) = "D" ++ show (reflectDim p) @@ -51,13 +54,13 @@ instance Arbitrary Dims where arbitrary = elements [ d0, d1, d2, d3, d4, d10, d100 ] where - d0 = Dims [] (Proxy :: Proxy D0) - d1 = Dims [d0] (Proxy :: Proxy D1) - d2 = Dims [d0,d1] (Proxy :: Proxy D2) - d3 = Dims [d0,d1,d2] (Proxy :: Proxy D3) - d4 = Dims [d0,d1,d2,d3] (Proxy :: Proxy D4) - d10 = Dims [d0,d1,d2,d3,d4] (Proxy :: Proxy D10) - d100 = Dims [d0,d1,d2,d3,d4,d10] (Proxy :: Proxy D100) + d0 = Dims [] (Proxy :: Proxy 0) + d1 = Dims [d0] (Proxy :: Proxy 1) + d2 = Dims [d0,d1] (Proxy :: Proxy 2) + d3 = Dims [d0,d1,d2] (Proxy :: Proxy 3) + d4 = Dims [d0,d1,d2,d3] (Proxy :: Proxy 4) + d10 = Dims [d0,d1,d2,d3,d4] (Proxy :: Proxy 10) + d100 = Dims [d0,d1,d2,d3,d4,d10] (Proxy :: Proxy 100) shrink = dShrinks instance Show Vectorizes where diff --git a/tests/ViewTests.hs b/tests/ViewTests.hs --- a/tests/ViewTests.hs +++ b/tests/ViewTests.hs @@ -1,15 +1,20 @@ -{-# OPTIONS_GHC -Wall #-} +{-# OPTIONS_GHC -Wall -fno-warn-orphans #-} {-# Language ScopedTypeVariables #-} {-# Language GADTs #-} {-# Language DeriveGeneric #-} {-# Language FlexibleInstances #-} +{-# Language PolyKinds #-} module ViewTests ( Views(..) - , CasadiMats(..) + , CMatrices(..) , viewTests ) where +import GHC.Generics ( Generic1 ) + +import Data.Proxy ( Proxy(..) ) +import qualified Data.Traversable as T import qualified Data.Packed.Matrix as Mat import qualified Numeric.LinearAlgebra ( ) -- for Eq Matrix import qualified Data.Vector as V @@ -22,13 +27,21 @@ import Casadi.Function ( evalDMatrix ) import Casadi.MXFunction ( mxFunction ) import Casadi.SharedObject ( soInit ) +import Casadi.CMatrix ( CMatrix ) +import Casadi.DMatrix ( DMatrix ) +import Casadi.MX ( MX ) +import Casadi.SX ( SX ) +import Dyno.View.Unsafe.View ( J(UnsafeJ), mkJ ) +import Dyno.View.Unsafe.M ( M(UnsafeM) ) + import Dyno.TypeVecs ( Vec, Dim ) -import Dyno.Vectorize -import Dyno.View +import Dyno.Vectorize ( Vectorize(..), Id, fill ) +import Dyno.View.View ( View(..), JNone, JTuple, JTriple ) +import Dyno.View.JV ( JV ) +import Dyno.View.Viewable ( Viewable ) import Dyno.View.M -import Dyno.View.CasadiMat ( CasadiMat ) -import Dyno.Cov +import Dyno.View.Cov ( Cov, fromMat, toMat ) import Utils import VectorizeTests ( Vectorizes(..), Dims(..) ) @@ -42,21 +55,53 @@ instance Show Views where show = vwName -data CasadiMats where - CasadiMats :: (Viewable f, CasadiMat f, MyEq f) => - { cmName :: String - , cmProxy :: Proxy f - } -> CasadiMats -instance Show CasadiMats where +data CMatrices where + CMatrices :: (Viewable f, CMatrix f, MyEq f) => + { cmName :: String + , cmProxy :: Proxy f + } -> CMatrices +instance Show CMatrices where show = cmName -- MX is less frequent because evalMX takes a while -instance Arbitrary CasadiMats where - arbitrary = frequency [ (1, return (CasadiMats "MX" (Proxy :: Proxy MX))) - , (5, return (CasadiMats "SX" (Proxy :: Proxy SX))) - , (5, return (CasadiMats "DMatrix" (Proxy :: Proxy DMatrix))) +instance Arbitrary CMatrices where + arbitrary = frequency [ (1, return (CMatrices "MX" (Proxy :: Proxy MX))) + , (5, return (CMatrices "SX" (Proxy :: Proxy SX))) + , (5, return (CMatrices "DMatrix" (Proxy :: Proxy DMatrix))) ] +instance (View f, View g, CMatrix a) => Arbitrary (M f g a) where + arbitrary = do + let prim :: Gen (M f g a) + prim = oneof + [ return $ zeros + , return $ countUp + , return $ fromInteger 0 + , return $ fromRational 0 + , fmap fromInteger arbitrary + , fmap fromRational arbitrary + ] + positive :: Gen (M f g a) + positive = elements + [ ones + , 1 + countUp + , pi + ] + x <- prim + y <- prim + z <- positive + oneof [ return $ x + , return $ x * y + , return $ x + y + , return $ x - y + , return $ x / z + , fmap trans (arbitrary :: Gen (M g f a)) + ] +instance (View f, CMatrix a, Viewable a) => Arbitrary (J f a) where + arbitrary = fmap uncol arbitrary +instance (Arbitrary a, Dim n) => Arbitrary (Vec n a) where + arbitrary = T.sequence (fill arbitrary) + evalMX :: MX -> DMatrix evalMX x = unsafePerformIO $ do f <- mxFunction V.empty (V.singleton x) @@ -116,14 +161,34 @@ compound genIt = do vc'@(Vectorizes _ mz pz) <- arbitrary let vc = mkJV vc' - vw0@(Views _ mv0 pv0) <- genIt - vw1@(Views _ mv1 pv1) <- genIt + vw0@(Views {vwName = mv0, vwProxy = pv0}) <- genIt + vw1@(Views {vwName = mv1, vwProxy = pv1}) <- genIt + vw2@(Views {vwName = mv2, vwProxy = pv2}) <- genIt elements - [ Views [vc] ("JX0 (" ++ mz ++ ")") (reproxy (Proxy :: Proxy JX0) pz) - , Views [vc,vw0] ("JX1 (" ++ mz ++ ") (" ++ mv0 ++ ")") (reproxy2 (Proxy :: Proxy JX1) pz pv0) - , Views [vc, vw0, vw1] ("JX2 (" ++ mv0 ++ ") (" ++ mv1 ++ ") (" ++ mz ++ ")") - (reproxy3 (Proxy :: Proxy JX2) pv0 pv1 pz) - , Views [vw0] ("Cov (" ++ mv0 ++ ")") (reproxy (Proxy :: Proxy Cov) pv0) + [ Views { vwShrinks = [vc] + , vwName = "JX0 (" ++ mz ++ ")" + , vwProxy = reproxy (Proxy :: Proxy JX0) pz + } + , Views { vwShrinks = [vc,vw0] + , vwName = "JX1 (" ++ mz ++ ") (" ++ mv0 ++ ")" + , vwProxy = reproxy2 (Proxy :: Proxy JX1) pz pv0 + } + , Views { vwShrinks = [vc, vw0, vw1] + , vwName = "JX2 (" ++ mv0 ++ ") (" ++ mv1 ++ ") (" ++ mz ++ ")" + , vwProxy = reproxy3 (Proxy :: Proxy JX2) pv0 pv1 pz + } + , Views { vwShrinks = [vw0] + , vwName = "Cov (" ++ mv0 ++ ")" + , vwProxy = reproxy (Proxy :: Proxy Cov) pv0 + } + , Views { vwShrinks = [vw0,vw1] + , vwName = "JTuple (" ++ mv0 ++ ") (" ++ mv1 ++ ")" + , vwProxy = reproxy2 (Proxy :: Proxy JTuple) pv0 pv1 + } + , Views { vwShrinks = [vw0,vw1,vw2] + , vwName = "JTriple (" ++ mv0 ++ ") (" ++ mv1 ++ ") (" ++ mv2 ++ ")" + , vwProxy = reproxy3 (Proxy :: Proxy JTriple) pv0 pv1 pv2 + } ] viewSize :: Views -> Int @@ -133,7 +198,10 @@ mkJV = mkJV' True where mkJV' :: Bool -> Vectorizes -> Views - mkJV' sh v@(Vectorizes _ m p) = Views shrinks ("JV (" ++ m ++ ")") (reproxyJV p) + mkJV' sh v@(Vectorizes _ m p) = Views { vwShrinks = shrinks + , vwName = "JV (" ++ m ++ ")" + , vwProxy = reproxyJV p + } where shrinks :: [Views] shrinks = if sh then map (mkJV' False) (shrink v) else [] @@ -145,8 +213,7 @@ primitives = do v <- arbitrary elements - [ Views [] "JNone" (Proxy :: Proxy JNone) - , Views [] "S" (Proxy :: Proxy S) + [ Views {vwShrinks = [], vwName = "JNone", vwProxy = Proxy :: Proxy JNone} , mkJV v ] @@ -169,123 +236,129 @@ prop_VSplitVCat :: Test prop_VSplitVCat = testProperty "vcat . vsplit" $ - \(Vectorizes _ _ p1) (Views _ _ p2) (CasadiMats {cmProxy = pm}) -> test p1 p2 pm + \(Vectorizes _ _ p1) (Views {vwProxy = p2}) (CMatrices {cmProxy = pm}) -> test p1 p2 pm where test :: forall f g a - . (Vectorize f, View g, CasadiMat a, MyEq a) - => Proxy f -> Proxy g -> Proxy a -> Property - test _ _ _ = beEqual x0 x1 - where - x0 :: M (JV f) g a - x0 = countUp - - x1 :: M (JV f) g a - x1 = vcat (vsplit x0) + . (Vectorize f, View g, CMatrix a, MyEq a) + => Proxy f -> Proxy g -> Proxy a -> Gen Property + test _ _ _ = do + x0 <- arbitrary :: Gen (M (JV f) g a) + let x1 = vcat (vsplit x0) :: M (JV f) g a + return $ beEqual x0 x1 prop_HSplitHCat :: Test prop_HSplitHCat = testProperty "hcat . hsplit" $ - \(Views _ _ p1) (Vectorizes _ _ p2) (CasadiMats {cmProxy = pm}) -> test p1 p2 pm + \(Views {vwProxy = p1}) (Vectorizes _ _ p2) (CMatrices {cmProxy = pm}) -> test p1 p2 pm where test :: forall f g a - . (View f, Vectorize g, CasadiMat a, MyEq a) - => Proxy f -> Proxy g -> Proxy a -> Property - test _ _ _ = beEqual x0 x1 - where - x0 :: M f (JV g) a - x0 = countUp - - x1 :: M f (JV g) a - x1 = hcat (hsplit x0) + . (View f, Vectorize g, CMatrix a, MyEq a) + => Proxy f -> Proxy g -> Proxy a -> Gen Property + test _ _ _ = do + x0 <- arbitrary :: Gen (M f (JV g) a) + let x1 = hcat (hsplit x0) :: M f (JV g) a + return $ beEqual x0 x1 prop_VSplitVCat' :: Test prop_VSplitVCat' = testProperty "vsplit' . vcat'" $ - \(Dims _ pd) (Views _ _ p1) (Views _ _ p2) (CasadiMats {cmProxy = pm}) -> test pd p1 p2 pm + \(Dims _ pd) (Views {vwProxy = p1}) (Views {vwProxy = p2}) (CMatrices {cmProxy = pm}) -> + test pd p1 p2 pm where test :: forall f g n a - . (View f, View g, Dim n, CasadiMat a, MyEq a) - => Proxy n -> Proxy f -> Proxy g -> Proxy a -> Property - test _ _ _ _ = beEqual x0 x1 - where - x0 :: Vec n (M f g a) - x0 = fill countUp - - x1 :: Vec n (M f g a) - x1 = vsplit' (vcat' x0) - + . (View f, View g, Dim n, CMatrix a, MyEq a) + => Proxy n -> Proxy f -> Proxy g -> Proxy a -> Gen Property + test _ _ _ _ = do + x0 <- arbitrary :: Gen (Vec n (M f g a)) + let x1 = vsplit' (vcat' x0) :: Vec n (M f g a) + return $ beEqual x0 x1 prop_HSplitHCat' :: Test prop_HSplitHCat' = testProperty "hsplit' . hcat'" $ - \(Dims _ pd) (Views _ _ p1) (Views _ _ p2) (CasadiMats {cmProxy = pm}) -> test pd p1 p2 pm + \(Dims _ pd) (Views {vwProxy = p1}) (Views {vwProxy = p2}) (CMatrices {cmProxy = pm}) -> + test pd p1 p2 pm where test :: forall f g n a - . (View f, View g, Dim n, CasadiMat a, MyEq a) - => Proxy n -> Proxy f -> Proxy g -> Proxy a -> Property - test _ _ _ _ = beEqual x0 x1 - where - x0 :: Vec n (M f g a) - x0 = fill countUp - - x1 :: Vec n (M f g a) - x1 = hsplit' (hcat' x0) + . (View f, View g, Dim n, CMatrix a, MyEq a) + => Proxy n -> Proxy f -> Proxy g -> Proxy a -> Gen Property + test _ _ _ _ = do + x0 <- arbitrary :: Gen (Vec n (M f g a)) + let x1 = hsplit' (hcat' x0) :: Vec n (M f g a) + return $ beEqual x0 x1 prop_testSplitJ :: Test prop_testSplitJ = testProperty "split . cat J" $ - \(Vectorizes _ _ p) (CasadiMats {cmProxy = pm}) -> test p pm + \(Vectorizes _ _ p) (CMatrices {cmProxy = pm}) -> test p pm where test :: forall f a - . (Vectorize f, CasadiMat a, Viewable a, MyEq a) - => Proxy f -> Proxy a -> Property - test _ _ = beEqual xj0 xj2 - where - UnsafeM xm0 = countUp :: M (JV f) (JV Id) a - - xj0 :: J (JV f) a - xj0 = mkJ xm0 - - xj1 :: JV f a - xj1 = split xj0 - - xj2 :: J (JV f) a - xj2 = cat xj1 + . (Vectorize f, CMatrix a, Viewable a, MyEq a) + => Proxy f -> Proxy a -> Gen Property + test _ _ = do + UnsafeM xm0 <- arbitrary :: Gen (M (JV f) (JV Id) a) + let xj0 = mkJ xm0 :: J (JV f) a + xj1 = split xj0 :: JV f a + xj2 = cat xj1 :: J (JV f) a + return $ beEqual xj0 xj2 prop_toFromHMat :: Test prop_toFromHMat = testProperty "fromHMat . toHMat" $ - \(Views _ _ p1) (Views _ _ p2) -> test p1 p2 + \(Views {vwProxy = p1}) (Views {vwProxy = p2}) -> test p1 p2 where test :: forall f g . (View f, View g) - => Proxy f -> Proxy g -> Property - test _ _ = beEqual m0 m2 - where - m0 = countUp :: M f g DMatrix - - m1 = toHMat m0 :: Mat.Matrix Double - - m2 = fromHMat m1 :: M f g DMatrix + => Proxy f -> Proxy g -> Gen Property + test _ _ = do + m0 <- arbitrary :: Gen (M f g DMatrix) + let m1 = toHMat m0 :: Mat.Matrix Double + m2 = fromHMat m1 :: M f g DMatrix + return $ beEqual m0 m2 prop_fromToHMat :: Test prop_fromToHMat = testProperty "toHMat . fromHMat" $ - \(Views _ _ p1) (Views _ _ p2) -> test p1 p2 + \(Views {vwProxy = p1}) (Views {vwProxy = p2}) -> test p1 p2 where test :: forall f g . (View f, View g) - => Proxy f -> Proxy g -> Property - test _ _ = beEqual m1 m3 - where - m0 = countUp :: M f g DMatrix - - m1 = toHMat m0 :: Mat.Matrix Double - - m2 = fromHMat m1 :: M f g DMatrix + => Proxy f -> Proxy g -> Gen Property + test _ _ = do + m0 <- arbitrary :: Gen (M f g DMatrix) + let m1 = toHMat m0 :: Mat.Matrix Double + m2 = fromHMat m1 :: M f g DMatrix + m3 = toHMat m2 :: Mat.Matrix Double + return $ beEqual m1 m3 - m3 = toHMat m2 :: Mat.Matrix Double +prop_covToFromMat :: Test +prop_covToFromMat = + testProperty "fromMat . toMat" $ + \(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 (J (Cov f) DMatrix) + let m1 = toMat m0 :: M f f DMatrix + m2 = fromMat m1 :: J (Cov f) DMatrix + return $ beEqual m0 m2 +prop_covFromToMat :: Test +prop_covFromToMat = + testProperty "toMat . fromMat" $ + \(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 f DMatrix) + let m0 = 0.5 * (m0' + trans m0') -- make it symmetric + m1 = fromMat m0 :: J (Cov f) DMatrix + m2 = toMat m1 :: M f f DMatrix + return $ beEqual m0 m2 viewTests :: Test viewTests = @@ -297,4 +370,6 @@ , prop_testSplitJ , prop_toFromHMat , prop_fromToHMat + , prop_covFromToMat + , prop_covToFromMat ]