diff --git a/dynobud.cabal b/dynobud.cabal --- a/dynobud.cabal +++ b/dynobud.cabal @@ -1,5 +1,5 @@ name: dynobud -version: 1.8.0.0 +version: 1.9.0.0 synopsis: your dynamic optimization buddy description: See readme at license: LGPL-3 @@ -26,7 +26,9 @@ Dyno.SimpleOcp Dyno.Ocp Dyno.OcpHomotopy + Dyno.DirectCollocation Dyno.DirectCollocation.ActiveConstraints + Dyno.DirectCollocation.CheckAccuracy Dyno.DirectCollocation.Dynamic Dyno.DirectCollocation.Export Dyno.DirectCollocation.Formulate @@ -35,22 +37,26 @@ Dyno.DirectCollocation.Interpolate Dyno.DirectCollocation.Quadratures Dyno.DirectCollocation.Robust + Dyno.DirectCollocation.ScaleFactors Dyno.DirectCollocation.Types + Dyno.ExportCStruct + Dyno.Fitting + Dyno.GoldenSectionSearch Dyno.Integrate + Dyno.Linearize + Dyno.Random + Dyno.View 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.MapFun Dyno.View.Scheme - Dyno.View.Unsafe.View - Dyno.View.Unsafe.M + Dyno.View.Unsafe Dyno.View.View - Dyno.View.Viewable Dyno.Vectorize Dyno.Nlp Dyno.NlpScaling @@ -65,7 +71,7 @@ build-depends: base >=4.6 && < 5, casadi-bindings-core >= 2.4.1.0, - casadi-bindings >= 2.4.1.0, + casadi-bindings >= 2.4.1.4, -- casadi-bindings-internal, data-default-class, jacobi-roots >=0.2 && <0.3, @@ -76,6 +82,7 @@ containers >=0.5, hmatrix >= 0.17.0.1, hmatrix-gsl >= 0.17.0.0, + lens, linear >= 1.3.1.1, reflection >= 1.3.2, aeson, @@ -83,10 +90,11 @@ cereal, distributive, process, - Plot-ho-matic >= 0.5.0.2, - generic-accessors >= 0.4.2.0, + Plot-ho-matic >= 0.6.0.0, + generic-accessors >= 0.5.0.0, time, - directory + directory, + mwc-random -- cplex hs-source-dirs: src default-language: Haskell2010 @@ -225,6 +233,7 @@ default-language: Haskell2010 build-depends: dynobud, containers, + linear, casadi-bindings, time, base >=4.6 && < 5 @@ -471,7 +480,9 @@ type: exitcode-stdio-1.0 hs-source-dirs: tests main-is: NewUnitTests.hs - other-modules: QuadratureTests + other-modules: FittingTests + MapTests + QuadratureTests IntegrationTests VectorizeTests ViewTests diff --git a/examples/BasicNlp.hs b/examples/BasicNlp.hs --- a/examples/BasicNlp.hs +++ b/examples/BasicNlp.hs @@ -17,7 +17,7 @@ import Casadi.MX ( MX ) import Dyno.View.View -import Dyno.View.JV ( JV, catJV, catJV', splitJV' ) +import Dyno.View.M ( vcat, vsplit ) import Dyno.Vectorize import Dyno.Nlp import Dyno.NlpUtils @@ -54,13 +54,13 @@ 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) + fg :: J (JV X) MX -> J JNone MX -> (S MX, J (JV G) MX) + fg xy _ = (f, vcat g) where f = (1-x)**2 + 100*(y - x**2)**2 g = G x - X x y = splitJV' xy + X x y = vsplit xy main :: IO () main = do diff --git a/examples/DaePendulum.hs b/examples/DaePendulum.hs --- a/examples/DaePendulum.hs +++ b/examples/DaePendulum.hs @@ -15,8 +15,7 @@ import Accessors import Dyno.Vectorize -import Dyno.View.View ( View(..), J ) -import Dyno.View.JV ( catJV ) +import Dyno.View.View ( View(..), J, catJV ) import Dyno.Solvers import Dyno.Nlp import Dyno.NlpUtils diff --git a/examples/ExampleDsl/NlpMonad.hs b/examples/ExampleDsl/NlpMonad.hs --- a/examples/ExampleDsl/NlpMonad.hs +++ b/examples/ExampleDsl/NlpMonad.hs @@ -38,12 +38,10 @@ import Casadi.CMatrix ( veccat ) import qualified Casadi.CMatrix as CM -import Dyno.View.Unsafe.View ( J(..), mkJ, unJ ) - +import Dyno.View.Unsafe ( M(UnsafeM), mkM, unM ) import Dyno.Vectorize ( Id, devectorize, fill ) import Dyno.TypeVecs ( Vec ) -import Dyno.View.View ( View(..), JNone(..), jfill ) -import Dyno.View.JV ( JV ) +import Dyno.View.View ( View(..), JNone(..), J, S, JV, jfill ) import Dyno.View.JVec ( JVec ) import qualified Dyno.TypeVecs as TV import Dyno.Solvers ( Solver ) @@ -53,13 +51,13 @@ import ExampleDsl.LogsAndErrors import ExampleDsl.Types -type MXElement = J (JV Id) MX +type MXElement = S MX mxElementSym :: String -> IO MXElement -mxElementSym name = mkJ <$> sym name +mxElementSym name = mkM <$> sym name mxElementToMX :: MXElement -> MX -mxElementToMX (UnsafeJ x) +mxElementToMX (UnsafeM x) | (1,1) == sizes' = x | otherwise = error $ "mxElementToMX: got non-scalar of size " ++ show sizes' where @@ -181,15 +179,15 @@ svector = veccat . fmap mxElementToMX mxfun <- mxFunction "nlp" (V.fromList [svector inputs]) (V.fromList [svector (V.singleton obj), svector (TV.unVec g)]) LM.empty - 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)) + let fg :: J (JVec nx (JV Id)) MX -> J JNone MX -> (S MX, J (JVec ng (JV Id)) MX) + fg x _ = (mkM (ret V.! 0), mkM (ret V.! 1)) where - ret = callMX mxfun (V.singleton (unJ x)) + ret = callMX mxfun (V.singleton (unM x)) (AlwaysInline False) (NeverInline False) return Nlp { nlpFG = fg - , nlpBX = mkJ (TV.unVec xbnd) - , nlpBG = mkJ (TV.unVec gbnd) + , nlpBX = mkM (TV.unVec xbnd) + , nlpBG = mkM (TV.unVec gbnd) , nlpX0 = jfill 0 , nlpP = cat JNone , nlpScaleF = Nothing @@ -222,10 +220,10 @@ -- 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 } + let nlp = nlp0 { nlpX0 = mkM x0 } cb = case cb0 of Nothing -> Nothing - Just cb' -> Just $ \x _ -> cb' (unJ x) + Just cb' -> Just $ \x _ -> cb' (unM x) f nlp cb state @@ -247,7 +245,7 @@ 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] + let fopt = V.head (unM (fOpt nlpOut)) :: Double + xopt = F.toList $ unM (xOpt nlpOut) :: [Double] xnames = map fst (F.toList (nlpX state)) :: [String] return (ret, fopt, zip xnames xopt) diff --git a/examples/ExampleDsl/Types.hs b/examples/ExampleDsl/Types.hs --- a/examples/ExampleDsl/Types.hs +++ b/examples/ExampleDsl/Types.hs @@ -21,9 +21,7 @@ import Control.Lens import Casadi.MX ( MX ) -import Dyno.View.View ( J ) -import Dyno.View.JV ( JV ) -import Dyno.Vectorize ( Id ) +import Dyno.View.View ( S ) data Constraint a = Eq2 a a | Ineq2 a a @@ -32,7 +30,7 @@ data Objective a = ObjectiveUnset | Objective a data HomotopyParam a = HomotopyParamUnset | HomotopyParam a -type MXElement = J (JV Id) MX +type MXElement = S MX data NlpMonadState = NlpMonadState diff --git a/examples/Glider.hs b/examples/Glider.hs --- a/examples/Glider.hs +++ b/examples/Glider.hs @@ -9,7 +9,6 @@ import Dyno.Vectorize import Dyno.View.View -import Dyno.View.JV ( catJV ) import Dyno.Solvers --import Dyno.Sqp.Sqp --import Dyno.Sqp.LineSearch diff --git a/examples/Homotopy.hs b/examples/Homotopy.hs --- a/examples/Homotopy.hs +++ b/examples/Homotopy.hs @@ -12,9 +12,8 @@ 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.View +import Dyno.Vectorize ( Vectorize ) import Dyno.Nlp ( Nlp(..), Bounds ) import Dyno.NlpUtils ( HomotopyParams(..), solveNlpHomotopy ) import Dyno.Solvers @@ -29,7 +28,7 @@ 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) +data G a = G a -- (S a) deriving (Functor, Generic, Generic1, Show) instance Vectorize X @@ -57,11 +56,11 @@ bg :: J (JV G) (Vector Bounds) bg = catJV (G (Nothing, Just 0)) - fg :: J (JV X) MX -> J (JV P) MX -> (J (JV Id) MX, J (JV G) MX) - fg xy pxy = (f, catJV' g) + fg :: J (JV X) MX -> J (JV P) MX -> (S MX, J (JV G) MX) + fg xy pxy = (f, vcat g) where - X x y = splitJV' xy - P px _ = splitJV' pxy + X x y = vsplit xy + P px _ = vsplit pxy f = (1-x)**2 + 100*(y - x**2)**2 -- g = G x -- f = (x - px)**2 + (y - py)**2 diff --git a/examples/MultipleShooting.hs b/examples/MultipleShooting.hs --- a/examples/MultipleShooting.hs +++ b/examples/MultipleShooting.hs @@ -23,7 +23,6 @@ import Casadi.MX ( MX ) import Dyno.View.View -import Dyno.View.JV import Dyno.View.JVec import Dyno.Nlp import Dyno.NlpUtils diff --git a/examples/NlpSolverEx.hs b/examples/NlpSolverEx.hs --- a/examples/NlpSolverEx.hs +++ b/examples/NlpSolverEx.hs @@ -11,11 +11,11 @@ import Text.Printf ( printf ) import Casadi.MX ( MX ) +import Casadi.Viewable ( Viewable ) import Dyno.Vectorize ( Vectorize, Id(..), None(..), fill ) import Dyno.View.View -import Dyno.View.Viewable -import Dyno.View.JV -- ( JV ) +import Dyno.View.M ( vcat, vsplit ) import Dyno.Nlp import Dyno.NlpSolver import Dyno.NlpUtils @@ -53,10 +53,10 @@ bg :: G Bounds bg = G (Just 2, Nothing) - fg :: J (JV X) MX -> J (JV None) MX -> (J (JV Id) MX, J (JV G) MX) - fg xy _ = (f, catJV' g) + fg :: J (JV X) MX -> J (JV None) MX -> (S MX, J (JV G) MX) + fg xy _ = (f, vcat g) where - X x y = splitJV' xy + X x y = vsplit xy x' = 1e3*x y' = 1e-4*y f = x'**2 + y'**2 + 0.1*x' * y' @@ -97,10 +97,10 @@ kkts <- computeKKTs return ((unId (splitJV f), splitJV x, splitJV g), kkts) -data Sdv a = Sdv (J (JV Id) a) (J (JV X) a) (J (JV G) a) deriving (Generic) +data Sdv a = Sdv (S a) (J (JV X) a) (J (JV G) a) deriving (Generic) instance View Sdv -expand :: Viewable a => J Sdv a -> (J (JV Id) a, J (JV X) a, J (JV G) a) +expand :: Viewable a => J Sdv a -> (S a, J (JV X) a, J (JV G) a) expand sdv = (f, x, g) where Sdv f x g = split sdv diff --git a/examples/ParallelMap.hs b/examples/ParallelMap.hs --- a/examples/ParallelMap.hs +++ b/examples/ParallelMap.hs @@ -6,60 +6,80 @@ module Main ( main ) where import qualified Data.Map as M +import Data.Proxy ( Proxy(..) ) import Data.Time.Clock ( getCurrentTime, diffUTCTime ) +import Linear ( V2(..), V3(..) ) import Text.Printf ( printf ) import Casadi.DMatrix ( DMatrix ) import Casadi.SX ( SX ) +import Casadi.MX ( MX ) import Casadi.Option ( Opt(..) ) import qualified Dyno.TypeVecs as TV import Dyno.Vectorize ( Id(..) ) -import Dyno.View.Fun ( call, toSXFun, toMXFun, eval ) +import Dyno.View.Fun ( FunClass, Fun, SXFun, call, toSXFun, toMXFun, eval ) import Dyno.View.MapFun ( mapFun ) -import Dyno.View.M ( M, row ) -import Dyno.View.JV ( JV, catJV ) +import Dyno.View.M ( M, hcat', hsplit', vcat, vsplit ) import Dyno.View.JVec ( JVec(..) ) -import Dyno.View.View ( J, View(..), v2d ) +import Dyno.View.View ( J, JV ) type N = 300 --- todo(greg): one with different sized input/output and non-scalar input/output -- some random function -f0' :: J (JV Id) SX -> J (JV Id) SX -f0' x = g (100000 :: Int) x +f0' :: J (JV V2) SX -> J (JV V3) SX +f0' x = vcat $ V3 (g (100000 :: Int) x0) x1 (2*x1) where + V2 x0 x1 = vsplit x + g 0 y = y g k y = g (k-1) (sin y) main :: IO () main = do - let dummyInput :: J (JVec N (JV Id)) DMatrix - dummyInput = v2d $ cat $ JVec $ fmap (catJV . Id) (TV.tvlinspace 0 (2*pi)) - dummyInput' :: M (JV Id) (JVec N (JV Id)) DMatrix - dummyInput' = row dummyInput + let dummyInput :: M (JV V2) (JVec N (JV Id)) DMatrix + dummyInput = hcat' $ fmap (\x -> vcat (V2 x (2*x))) + (TV.tvlinspace 0 (2*pi)) + show dummyInput `seq` return () - show dummyInput' `seq` return () -- make a dummy function that's moderately expensive to evaluate putStrLn "creating dummy function..." f0 <- toSXFun "f0" f0' + :: IO (SXFun (J (JV V2)) (J (JV V3))) - let runOne name someMap input = do + let runOne :: FunClass fun + => String + -> fun + (M (JV V2) (JVec N (JV Id))) + (M (JV V3) (JVec N (JV Id))) + -> IO () + runOne name someMap = do putStrLn $ "evaluating " ++ name ++ "..." t0 <- getCurrentTime - _ <- eval someMap input + _ <- eval someMap dummyInput t1 <- getCurrentTime printf "evaluated %s in %.3f seconds\n" name (realToFrac (diffUTCTime t1 t0) :: Double) - naive <- toMXFun "naive map" $ - \xs -> cat $ JVec $ fmap (call f0) (unJVec (split xs)) - ser <- mapFun "serial symbolic map" f0 + let naiveFun :: M (JV V2) (JVec N (JV Id)) MX -> M (JV V3) (JVec N (JV Id)) MX + naiveFun xs = hcat' ys + where + ys :: TV.Vec N (M (JV V3) (JV Id) MX) + ys = fmap (call f0) xs' + + xs' :: TV.Vec N (M (JV V2) (JV Id) MX) + xs' = hsplit' xs + + naive <- toMXFun "naive map" naiveFun + ser <- mapFun (Proxy :: Proxy N) "serial symbolic map" f0 (M.fromList [("parallelization", Opt "serial")]) - par <- mapFun "parallel symbolic map" f0 + :: IO (Fun + (M (JV V2) (JVec N (JV Id))) + (M (JV V3) (JVec N (JV Id)))) + par <- mapFun (Proxy :: Proxy N) "parallel symbolic map" f0 (M.fromList [("parallelization", Opt "openmp")]) - runOne "naive map" naive dummyInput - runOne "serial symbolic map" ser dummyInput' - runOne "parallel symbolic map" par dummyInput' + runOne "naive map" naive + runOne "serial symbolic map" ser + runOne "parallel symbolic map" par diff --git a/examples/Quadrature.hs b/examples/Quadrature.hs --- a/examples/Quadrature.hs +++ b/examples/Quadrature.hs @@ -18,8 +18,7 @@ import Accessors ( Lookup ) import Dyno.Vectorize ( Vectorize(..), None(..), Id(..) ) -import Dyno.View.View ( View(..), J ) -import Dyno.View.JV ( splitJV, catJV ) +import Dyno.View.View ( View(..), J, splitJV, catJV ) import Dyno.Solvers import Dyno.Nlp ( NlpOut(..), Bounds ) import Dyno.NlpUtils diff --git a/examples/Rocket.hs b/examples/Rocket.hs --- a/examples/Rocket.hs +++ b/examples/Rocket.hs @@ -8,13 +8,11 @@ import GHC.Generics ( Generic, Generic1 ) -import qualified Data.Map as M import Data.Vector ( Vector ) import Accessors ( Lookup ) -import Dyno.View.View ( J, jfill ) -import Dyno.View.JV ( catJV ) +import Dyno.View.View ( J, jfill, catJV ) import Dyno.Nlp ( NlpOut(..), Bounds ) import Dyno.Ocp import Dyno.Vectorize ( Vectorize, None(..), fill ) diff --git a/examples/Sailboat.hs b/examples/Sailboat.hs --- a/examples/Sailboat.hs +++ b/examples/Sailboat.hs @@ -31,8 +31,7 @@ import Accessors ( Lookup ) import Dyno.Vectorize -import Dyno.View.View ( View(..), J ) -import Dyno.View.JV ( catJV, splitJV ) +import Dyno.View.View ( View(..), J, catJV, splitJV ) import Dyno.Solvers import Dyno.NlpUtils import Dyno.Nlp ( NlpOut(..) ) @@ -309,9 +308,10 @@ callback :: J (CollTraj' SailboatOcp NCollStages CollDeg) (Vector Double) -> b -> IO Bool callback traj _ = do plotPoints <- cpPlotPoints cp traj (catJV None) + :: IO (DynPlotPoints Double) -- dynoplot let dynoPlotMsg = encodeSerial (plotPoints, meta) - sendDynoPlotMsg "glider" dynoPlotMsg + sendDynoPlotMsg "dynoplot" dynoPlotMsg -- -- 3d vis -- let CollTraj tf' _ _ stages' xf = split traj diff --git a/examples/Spring.hs b/examples/Spring.hs --- a/examples/Spring.hs +++ b/examples/Spring.hs @@ -12,8 +12,7 @@ import Accessors ( Lookup ) -import Dyno.View.View ( J, jfill ) -import Dyno.View.JV ( catJV ) +import Dyno.View.View ( J, jfill, catJV ) import Dyno.Nlp ( Bounds ) import Dyno.Ocp import Dyno.Vectorize ( Vectorize, None(..), fill ) diff --git a/src/Dyno/AutoScaling.hs b/src/Dyno/AutoScaling.hs --- a/src/Dyno/AutoScaling.hs +++ b/src/Dyno/AutoScaling.hs @@ -21,15 +21,12 @@ import Casadi.MX ( MX ) import Casadi.Sparsity ( getRow, getCol ) -import Dyno.View.JV ( JV, splitJV ) import Dyno.View.M ( M ) import qualified Dyno.View.M as M import Dyno.Nlp ( KKT(..), Nlp(..) ) -import Dyno.View.Unsafe.View ( mkJ, unJ ) -import Dyno.View.Unsafe.M ( unM ) +import Dyno.View.Unsafe ( mkM, unM ) import Dyno.Vectorize ( Id(..) ) -import Dyno.View.View ( View(..), J, JNone(..), v2d, d2v, jfill) -import Dyno.View.Viewable ( Viewable ) +import Dyno.View.View ( View(..), J, S, JNone(..), v2d, d2v, jfill, splitJV ) toSparse :: (View f, View g) => String -> M f g DMatrix -> [(Int,Int,Double)] @@ -52,7 +49,7 @@ , showOne "hessF " (kktHessF kkt) , showOne "hessLamG " (kktHessLambdaG kkt) , showOne "jacG " (kktJacG kkt) - , showOne "gradF " (M.col (kktGradF kkt)) + , showOne "gradF " (kktGradF kkt) ] where showOne name m = @@ -100,8 +97,8 @@ toLogScaling :: forall x g sdv a - . (View x, View g, View sdv, Viewable a, CM.CMatrix a) - => KKT x g -> (J sdv a -> (J (JV Id) a, J x a, J g a)) -> J sdv a -> LogScaling (J (JV Id) a) + . (View x, View g, View sdv, CM.CMatrix a) + => KKT x g -> (J sdv a -> (S a, J x a, J g a)) -> J sdv a -> LogScaling (S a) toLogScaling kkt expand sdvs = LogScaling { lsJacG = jacGObjValues @@ -115,9 +112,9 @@ hessFMatValues = toSparse "hessF" (kktHessF kkt) hessLambdaGMatValues = toSparse "hessLamG" (kktHessLambdaG kkt) hessLagMatValues = toSparse "hessLag" (kktHessLag kkt) - gradFMatValues = toSparse "gradF" (M.col (kktGradF kkt)) + gradFMatValues = toSparse "gradF" (kktGradF kkt) - objScale' :: J (JV Id) a + objScale' :: S a x :: J x a g' :: J g a (objScale', x, g') = expand sdvs @@ -130,23 +127,23 @@ nx = size (reproxy x) ng = size (reproxy g) - xs,gs :: V.Vector (J (JV Id) a) - xs = fmap mkJ $ CM.vertsplit (unJ x) (V.fromList [0..nx]) - gs = fmap mkJ $ CM.vertsplit (unJ g) (V.fromList [0..ng]) + xs,gs :: V.Vector (S a) + xs = fmap mkM $ CM.vertsplit (unM x) (V.fromList [0..nx]) + gs = fmap mkM $ CM.vertsplit (unM g) (V.fromList [0..ng]) - gradFObjValues :: [J (JV Id) a] + gradFObjValues :: [S a] gradFObjValues = map (toSum xs (V.singleton objScale)) gradFMatValues - jacGObjValues :: [J (JV Id) a] + jacGObjValues :: [S a] jacGObjValues = map (toSum gs xs) jacGMatValues - hessFObjValues :: [J (JV Id) a] + hessFObjValues :: [S a] hessFObjValues = map ((+ objScale) . toSum xs xs) hessFMatValues - hessLambdaGObjValues :: [J (JV Id) a] + hessLambdaGObjValues :: [S a] hessLambdaGObjValues = map ((+ objScale) . toSum xs xs) hessLambdaGMatValues - hessLagObjValues :: [J (JV Id) a] + hessLagObjValues :: [S a] hessLagObjValues = map ((+ objScale) . toSum xs xs) hessLagMatValues @@ -172,7 +169,7 @@ scalingNlp :: forall x g sdv . (View x, View g, View sdv) - => KKT x g -> (J sdv MX -> (J (JV Id) MX, J x MX, J g MX)) + => KKT x g -> (J sdv MX -> (S MX, J x MX, J g MX)) -> Nlp sdv JNone JNone MX scalingNlp kkt expand = Nlp @@ -188,7 +185,7 @@ , nlpFG = fg } where - fg :: J sdv MX -> J JNone MX -> (J (JV Id) MX, J JNone MX) + fg :: J sdv MX -> J JNone MX -> (S MX, J JNone MX) fg sdvs _ = (obj, cat JNone) where obj = toObjective $ toLogScaling kkt expand sdvs @@ -197,7 +194,7 @@ beforeAndAfter :: (View x, View g, View sdv) => KKT x g - -> (J sdv DMatrix -> (J (JV Id) DMatrix, J x DMatrix, J g DMatrix)) + -> (J sdv DMatrix -> (S DMatrix, J x DMatrix, J g DMatrix)) -> J sdv (V.Vector Double) -> String beforeAndAfter kkts expand scalingSol = @@ -259,7 +256,7 @@ -- putStrLn "finished! analyzing..." -- let --JTuple f0' g0' = split fg -- --Id _f0 = splitJV (d2v f0') --- --_g0 = unJ $ d2v g0' +-- --_g0 = unM $ d2v g0' -- -- -- --dfgdx :: M -- -- (JTuple (JV Id) (CollOcpConstraints NCollStages CollDeg AcX AcX Bc PathC)) @@ -284,8 +281,8 @@ ---- | (abs lambda) > 1e-15 = True ---- | otherwise = False -- --- activeX = V.map isActive (unJ (lambdaXOpt' sol)) --- activeG = V.map isActive (unJ (lambdaGOpt' sol)) +-- activeX = V.map isActive (unM (lambdaXOpt' sol)) +-- activeG = V.map isActive (unM (lambdaGOpt' sol)) -- activeAll = activeX V.++ activeG -- -- activeXIndices = map fst $ filter snd $ zip [(0::Int)..] (V.toList activeX) diff --git a/src/Dyno/DirectCollocation.hs b/src/Dyno/DirectCollocation.hs new file mode 100644 --- /dev/null +++ b/src/Dyno/DirectCollocation.hs @@ -0,0 +1,19 @@ +{-# OPTIONS_GHC -Wall #-} + +-- | Meta-module to reexport Dyno.DirectCollocation.* +module Dyno.DirectCollocation + ( module X + ) where + +import Dyno.DirectCollocation.ActiveConstraints as X +import Dyno.DirectCollocation.CheckAccuracy as X +import Dyno.DirectCollocation.Dynamic as X +import Dyno.DirectCollocation.Export as X +import Dyno.DirectCollocation.Formulate as X +import Dyno.DirectCollocation.FormulateCov as X +import Dyno.DirectCollocation.Integrate as X +import Dyno.DirectCollocation.Interpolate as X +import Dyno.DirectCollocation.Quadratures as X +import Dyno.DirectCollocation.Robust as X +import Dyno.DirectCollocation.ScaleFactors as X +import Dyno.DirectCollocation.Types as X diff --git a/src/Dyno/DirectCollocation/ActiveConstraints.hs b/src/Dyno/DirectCollocation/ActiveConstraints.hs --- a/src/Dyno/DirectCollocation/ActiveConstraints.hs +++ b/src/Dyno/DirectCollocation/ActiveConstraints.hs @@ -19,7 +19,9 @@ import GHC.Generics ( Generic ) +import Accessors ( Lookup, Field(..), flatten', accessors, describeField ) import Control.Applicative +import Control.Lens ( (^.) ) import Data.List ( intercalate ) import Data.Maybe ( catMaybes ) import qualified Data.Foldable as F @@ -31,13 +33,10 @@ import Dyno.Ocp ( OcpPhase(..), OcpPhaseInputs(..) ) import Dyno.Nlp ( Bounds ) import Dyno.Vectorize ( Vectorize, Id(..) ) -import Dyno.View.View ( View(..), J ) -import Dyno.View.JV ( JV, splitJV ) +import Dyno.View.View ( View(..), J, JV, splitJV ) import Dyno.View.JVec ( unJVec ) import Dyno.TypeVecs ( Dim ) -import Accessors ( Lookup, Getter(..), flatten', accessors ) - data Active a = Active { activeLower :: a, activeUpper :: a } deriving (Functor, F.Foldable, T.Traversable, Generic) instance Lookup a => Lookup (Active a) @@ -110,11 +109,13 @@ , Lookup (h Int) , Lookup (c Int) ) => ActiveConstraints x z u p h c (Active Int) -> [([String], Active Int)] -flattenActiveConstraints activeCons = map report $ flatten' $ accessors lbs +flattenActiveConstraints activeCons = map report $ flatten' accessors where - report (name, GetInt get, _) = (name, Active (get lbs) (get ubs)) - report (name, _, _) = - error $ "the 'impossible' happened, flattenActiveConstraints got a non-int getter " ++ show name + report (name, FieldInt f) = (name, Active (lbs ^. f) (ubs ^. f)) + report (name, f) = + error $ "the 'impossible' happened, " ++ + "flattenActiveConstraints got a non-int getter " ++ show name ++ + " with type " ++ describeField f lbs = fmap activeLower activeCons ubs = fmap activeUpper activeCons diff --git a/src/Dyno/DirectCollocation/CheckAccuracy.hs b/src/Dyno/DirectCollocation/CheckAccuracy.hs new file mode 100644 --- /dev/null +++ b/src/Dyno/DirectCollocation/CheckAccuracy.hs @@ -0,0 +1,195 @@ +{-# OPTIONS_GHC -Wall #-} +{-# LANGUAGE ScopedTypeVariables #-} +{-# LANGUAGE DeriveFunctor #-} +{-# LANGUAGE DeriveGeneric #-} +{-# LANGUAGE FlexibleContexts #-} +{-# LANGUAGE PolyKinds #-} + +-- todo(greg): use this in the untit tests +module Dyno.DirectCollocation.CheckAccuracy + ( Err(..) + , Checks(..) + , CheckState(..) + , toErr + , checkIntegrationAccuracy + , summarizeAccuracy + ) where + +import GHC.Generics ( Generic, Generic1 ) + +import Accessors +import Control.Lens ( (^.) ) +import Data.List ( sortBy ) +import Data.Maybe ( isJust, fromJust ) +import Data.Proxy ( Proxy(..) ) +import Data.Foldable ( foldl', maximumBy ) +import qualified Data.Vector as V +import Linear ( Additive ) +import Text.Printf ( printf ) + +import Dyno.Integrate +import Dyno.Vectorize ( Vectorize(..), Id(..), None(..), fill ) +import Dyno.View.View ( View(..), J, splitJV ) +import Dyno.TypeVecs ( Vec, Dim ) +import qualified Dyno.TypeVecs as TV +import Dyno.DirectCollocation.Quadratures ( QuadratureRoots, collocationTimes ) +import Dyno.DirectCollocation.Types +import Dyno.LagrangePolynomials ( interpolate ) + +data Checks x n = + Checks + { checksStageMismatch :: Vec n (x (Err Double)) + , checksWorstStageMismatch :: x (Err Double) + , checksTrajMismatch :: x (Err Double) + } + +data CheckState x q a = + CheckState + { csX :: x a + , csQ :: q a + } deriving (Functor, Generic, Generic1) +instance (Vectorize x, Vectorize q) => Vectorize (CheckState x q) +instance (Lookup (x a), Lookup (q a), Lookup a) => Lookup (CheckState x q a) + +data Err a = + Err + { errRef :: a + , errVal :: a + , errAbs :: a + , errRel :: a + } + +summarizeAccuracy :: + forall x n + . (Vectorize x, Lookup (x Double), Dim n) + => Checks x n -> String +summarizeAccuracy (Checks _ worstStageMismatch trajMismatch) = + unlines $ + ("worst stage mismatches:" : map showOne stageMismatches) ++ + ("" : "worst overall mismatches:" : map showOne trajMismatches) + where + showOne :: (String, Err Double) -> String + showOne (name, err) = + printf "relerr: %.2g, abserr: %.2g - %s - dir coll: %.2g, rk45: %.2g" + (errRel err) (errAbs err) name (errRef err) (errVal err) + + acs = flatten accessors + + stageMismatches = sortBy (flip comp) $ map (report worstStageMismatch) acs + trajMismatches = sortBy (flip comp) $ map (report trajMismatch) acs + comp (_,x) (_,y) = compare (errRel x) (errRel y) + + report x (name, FieldDouble f) = (name, Err ref val abs' rel) + where + ref = (fmap errRef x) ^. f + val = (fmap errVal x) ^. f + abs' = (fmap errAbs x) ^. f + rel = (fmap errRel x) ^. f + report _ (name, f) = + error $ "summarizeAccuracy got a non-double getter for " ++ show name ++ + " with type " ++ describeField f + +toErr :: (Ord a, Fractional a) => Maybe a -> a -> a -> Err a +toErr mscale ref val = + Err + { errRef = ref + , errVal = val + , errAbs = abs (ref - val) + , errRel = relerr + } + where + relerr + | ref == 0 && val == 0 = 0 + | isJust mscale = abs (ref - val) / fromJust mscale + | ref == 0 = abs (ref - val) / (max 1e-15 (abs val)) + | val == 0 = abs (ref - val) / (max 1e-15 (abs ref)) + | otherwise = abs (ref - val) / (maximum [1e-15, abs ref, abs val]) + +checkIntegrationAccuracy + :: forall x q u p n deg + . (Vectorize x, Vectorize q, Vectorize u, Additive u, Vectorize p, Dim n, Dim deg) + => x (Maybe Double) + -> QuadratureRoots + -> J (CollTraj x None u p n deg) (V.Vector Double) + -> ( Double + -> u Double + -> p Double + -> CheckState x q Double + -> CheckState x q Double) + -> Vec n (q Double) + -> Checks (CheckState x q) n +checkIntegrationAccuracy xscale roots traj' ode qfs = + Checks + { checksStageMismatch = mismatch + , checksWorstStageMismatch = worstStageMismatches + , checksTrajMismatch = + toErr <$> scale <*> CheckState (TV.tvlast xfs) (TV.tvlast qfs) <*> integratedFullTraj + } + where + scale :: CheckState x q (Maybe Double) + scale = CheckState + { csX = xscale + , csQ = fill Nothing + } + + integrate :: Double + -> CheckState x q Double + -> Vec deg Double -> Vec deg (u Double) + -> CheckState x q Double + integrate t0 cs0 ts us = rk45 f (InitialTime t0) (TimeStep h) cs0 + where + f :: Double -> CheckState x q Double -> CheckState x q Double + f t = ode t u params + where + u :: u Double + u = interpolate ts us t + + integratedFullTraj :: CheckState x q Double + integratedFullTraj = foldl' g (CheckState (TV.tvhead x0s) (TV.tvhead q0s)) foldInputs + where + foldInputs :: Vec n (Double, Vec deg Double, Vec deg (u Double)) + foldInputs = TV.tvzipWith3 (\y0 y1 y2 -> (y0, y1, y2)) t0s utimes fullus + + g cs0 (t0, ts, us) = integrate t0 cs0 ts us + + params = splitJV params' + traj@(CollTraj tf params' _ _) = split traj' + + xs :: Vec n (x Double, Vec deg (x Double)) + fullus :: Vec n (Vec deg (u Double)) + ((xs, xf), _, fullus) = getXzus''' traj + + t0s :: Vec n Double + t0s = fmap fst times + + utimes :: Vec n (Vec deg Double) + utimes = fmap snd times + + times :: Vec n (Double, Vec deg Double) + times = collocationTimes 0 roots h + + h = unId (splitJV tf) / fromIntegral (TV.reflectDim (Proxy :: Proxy n)) + + q0s :: Vec n (q Double) + q0s = TV.tvshiftr (fill 0) (qfs) + + x0s :: Vec n (x Double) + x0s = fmap fst xs + + xfs :: Vec n (x Double) + xfs = TV.tvshiftl x0s xf + + cs0s = TV.tvzipWith CheckState x0s q0s + csfs = TV.tvzipWith CheckState xfs qfs + + integratedCsfs :: Vec n (CheckState x q Double) + integratedCsfs = integrate <$> t0s <*> cs0s <*> utimes <*> fullus + + mismatch :: Vec n (CheckState x q (Err Double)) + mismatch = TV.tvzipWith (\ref val -> toErr <$> scale <*> ref <*> val) csfs integratedCsfs + + worstStageMismatches :: CheckState x q (Err Double) + worstStageMismatches = fmap (maximumBy comp) (sequenceA mismatch) + where + comp :: Err Double -> Err Double -> Ordering + comp x y = compare (errRel x) (errRel y) 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 @@ -17,6 +17,7 @@ import GHC.Generics ( Generic ) +import Casadi.Viewable ( Viewable ) import Data.Proxy ( Proxy(..) ) import Data.List ( mapAccumL ) import Data.Tree ( Tree(..) ) @@ -34,17 +35,23 @@ import Accessors ( AccessorTree(..), Lookup(..), accessors ) import PlotHo ( Plotter, addChannel ) -import Dyno.View.Unsafe.View ( unJ, unJ' ) - +import Dyno.View.Unsafe ( unM, unM' ) import Dyno.Vectorize ( Vectorize(..), Id(..), fill ) -import Dyno.View.JV ( JV, splitJV ) -import Dyno.View.View ( View(..), J ) +import Dyno.View.View ( View(..), J, JV, splitJV ) +import Dyno.View.M ( M ) 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 ) +unM'' :: (View f, View g, Viewable a) => String -> M f g a -> a +unM'' msg x = case unM' x of + Left msg' -> + error $ + "Dyno.DirectCollocation.Dynamic: unM'' " ++ msg ++ ":\n" ++ msg' + Right r -> r + addCollocationChannel :: String -> (((DynPlotPoints Double, CollTrajMeta) -> IO ()) -> IO ()) -> Plotter () addCollocationChannel name action = addChannel name sameMeta toSignalTree action @@ -127,7 +134,7 @@ stages :: Vec n (CollStage (JV x) (JV z) (JV u) deg (Vector a)) stages = fmap split (unJVec (split stages')) - xss = xss' `V.snoc` (V.singleton (tf, unJ xf)) + xss = xss' `V.snoc` (V.singleton (tf, unM xf)) -- assumes initial time is 0 qss = V.singleton (0, vectorize (fill 0 :: Quadratures q qo a)) `V.cons` qss' @@ -161,7 +168,7 @@ xzus0 = fmap split (unJVec (split xzus')) :: Vec deg (CollPoint (JV x) (JV z) (JV u) (Vector a)) xs :: V.Vector (a, V.Vector a) - xs = (t0, unJ x0) `V.cons` xs' `V.snoc` (tnext, unJ (soXNext stageOutputs)) + xs = (t0, unM x0) `V.cons` xs' `V.snoc` (tnext, unM (soXNext stageOutputs)) qs :: V.Vector (a, V.Vector a) qs = qs' `V.snoc` (tnext, vectorize (soQNext stageOutputs)) @@ -187,13 +194,13 @@ , (a, V.Vector a) ) g (CollPoint x z u) (o,x',pathc,po,q,q') tau = - ( (t,unJ' "x" x) - , (t,unJ' "z" z) - , (t,unJ' "u" u) - , (t,unJ' "o" o) - , (t,unJ' "x'" x') - , (t,unJ' "h" pathc) - , (t,unJ' "po" po) + ( (t,unM'' "x" x) + , (t,unM'' "z" z) + , (t,unM'' "u" u) + , (t,unM'' "o" o) + , (t,unM'' "x'" x') + , (t,unM'' "h" pathc) + , (t,unM'' "po" po) , (t,vectorize q) , (t,vectorize q') ) @@ -223,7 +230,7 @@ namesFromAccTree x = (\(_,(_,y)) -> y) $ namesFromAccTree' 0 ("",x) namesFromAccTree' :: Int -> (String, AccessorTree a) -> (Int, (String, NameTree)) -namesFromAccTree' k (nm, ATGetter _) = (k+1, (nm, NameTreeLeaf k)) +namesFromAccTree' k (nm, Field _) = (k+1, (nm, NameTreeLeaf k)) namesFromAccTree' k0 (nm, Data names ats) = (k, (nm, NameTreeNode names children)) where (k, children) = mapAccumL namesFromAccTree' k0 ats @@ -240,7 +247,7 @@ oTree = blah (\(DynPlotPoints _ _ _ o _ _ _ _ _ ) -> o) "outputs" (ctmO meta) xdTree = blah (\(DynPlotPoints _ _ _ _ xd _ _ _ _ ) -> xd) "diff state derivatives" (ctmX meta) hTree = blah (\(DynPlotPoints _ _ _ _ _ h _ _ _ ) -> h) "path constraints" (ctmH meta) - poTree = blah (\(DynPlotPoints _ _ _ _ _ _ po _ _ ) -> po) "quadrature outputs" (ctmPo meta) + poTree = blah (\(DynPlotPoints _ _ _ _ _ _ po _ _ ) -> po) "plot outputs" (ctmPo meta) qTree = blah (\(DynPlotPoints _ _ _ _ _ _ _ q _ ) -> q) "quadrature states" (ctmQ meta) qdTree = blah (\(DynPlotPoints _ _ _ _ _ _ _ _ qd) -> qd) "ddt(quadrature states)" (ctmQ meta) @@ -267,14 +274,14 @@ => MetaProxy x z u p o q qo po h -> CollTrajMeta toMeta _ = CollTrajMeta - { ctmX = namesFromAccTree $ accessors (fill () :: x ()) - , ctmZ = namesFromAccTree $ accessors (fill () :: z ()) - , ctmU = namesFromAccTree $ accessors (fill () :: u ()) - , ctmP = namesFromAccTree $ accessors (fill () :: p ()) - , ctmO = namesFromAccTree $ accessors (fill () :: o ()) - , ctmQ = namesFromAccTree $ accessors (fill () :: Quadratures q qo ()) - , ctmH = namesFromAccTree $ accessors (fill () :: h ()) - , ctmPo = namesFromAccTree $ accessors (fill () :: po ()) + { ctmX = namesFromAccTree (accessors :: AccessorTree (x ())) + , ctmZ = namesFromAccTree (accessors :: AccessorTree (z ())) + , ctmU = namesFromAccTree (accessors :: AccessorTree (u ())) + , ctmP = namesFromAccTree (accessors :: AccessorTree (p ())) + , ctmO = namesFromAccTree (accessors :: AccessorTree (o ())) + , ctmQ = namesFromAccTree (accessors :: AccessorTree (Quadratures q qo ())) + , ctmH = namesFromAccTree (accessors :: AccessorTree (h ())) + , ctmPo = namesFromAccTree (accessors :: AccessorTree (po ())) } --unzip8 :: Vector (a, b, c, d, e, f, g, h) 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 @@ -18,6 +18,7 @@ , write ) where +import Control.Lens ( (^.) ) import Control.Monad ( unless ) import Data.List ( unzip6, intercalate ) import Data.Proxy ( Proxy(..) ) @@ -28,13 +29,12 @@ import qualified Control.Monad.State.Lazy as State import qualified Data.Set as S -import Accessors ( Lookup, Getter(..), flatten, flatten', accessors ) +import Accessors ( Lookup, Field(..), flatten, flatten', accessors ) import Dyno.Nlp ( NlpOut(..) ) import Dyno.TypeVecs ( Vec ) import Dyno.Vectorize ( Vectorize, Id(..), None(..), fill ) -import Dyno.View.View ( View(..) ) -import Dyno.View.JV ( splitJV, catJV ) +import Dyno.View.View ( View(..), splitJV, catJV ) import Dyno.DirectCollocation.Formulate ( CollProblem(..), DirCollOptions(..) ) import Dyno.DirectCollocation.Types ( CollTraj(..), CollOcpConstraints(..) , StageOutputs(..), Quadratures(..) @@ -249,13 +249,13 @@ npArray :: String -> String npArray str = "numpy.array(" ++ str ++ ")" -toDub :: Getter (xzu Double) -> xzu Double -> Double -toDub (GetDouble f) = f -toDub (GetFloat f) = realToFrac . f -toDub (GetInt f) = realToFrac . f -toDub (GetBool f) = fromIntegral . fromEnum . f -toDub (GetString _) = const (read "NaN") -toDub GetSorry = const (read "NaN") +toDub :: Field (xzu Double) -> xzu Double -> Double +toDub (FieldDouble f) = (^. f) +toDub (FieldFloat f) = realToFrac . (^. f) +toDub (FieldInt f) = realToFrac . (^. f) +toDub (FieldBool f) = fromIntegral . fromEnum . (^. f) +toDub (FieldString _) = const (read "NaN") +toDub FieldSorry = const (read "NaN") pythonParam :: forall p . (Vectorize p, Lookup (p Double)) @@ -266,7 +266,7 @@ pyParam (name, get) = putVal pyRetName (topNames ++ name) (show (get p)) at' :: [([String], p Double -> Double)] - at' = map (\(fn,g,_) -> (fn, toDub g)) $ flatten' $ accessors (fill (0 :: Double)) + at' = map (\(fn, f) -> (fn, toDub f)) $ flatten' accessors pythonTraj :: forall x . (Vectorize x, Lookup (x Double)) => String -> [String] -> [x Double] -> State PythonExporter () @@ -276,7 +276,7 @@ pyArray (name, get) = putVal pyRetName (topNames ++ name) (npArray (show (map get xs))) at' :: [([String], x Double -> Double)] - at' = map (\(fn,g,_) -> (fn, toDub g)) $ flatten' $ accessors (fill (0 :: Double)) + at' = map (\(fn, f) -> (fn, toDub f)) $ flatten' accessors matlabParam :: forall p . (Vectorize p, Lookup (p Double)) => String -> p Double -> [String] @@ -286,7 +286,7 @@ mlParam name get = topName ++ "." ++ name ++ " = " ++ show (get p) ++ ";" at :: [(String, p Double -> Double)] - at = map (\(fn,g,_) -> (fn, toDub g)) $ flatten $ accessors (fill (0 :: Double)) + at = map (\(fn, f) -> (fn, toDub f)) $ flatten accessors matlabTraj :: forall x . (Vectorize x, Lookup (x Double)) => String -> [x Double] -> [String] matlabTraj topName xs = map (uncurry mlArray) at @@ -296,7 +296,7 @@ topName ++ "." ++ name ++ " = " ++ show (map get xs) ++ ";" at :: [(String, x Double -> Double)] - at = map (\(fn,g,_) -> (fn, toDub g)) $ flatten $ accessors (fill (0 :: Double)) + at = map (\(fn, f) -> (fn, toDub f)) $ flatten accessors data PythonExporter = PythonExporter (S.Set [String], [String]) 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 @@ -39,10 +39,11 @@ import Casadi.SX ( SX ) import Dyno.Integrate ( InitialTime(..), TimeStep(..), rk45 ) -import Dyno.View.View ( View(..), J, jfill, JTuple(..), v2d, d2v ) -import Dyno.View.M ( M ) +import Dyno.View.View + ( View(..), JTuple(..), J, S, JV + , splitJV, catJV, jfill, v2d, d2v ) +import Dyno.View.M ( M, vcat, vsplit ) import qualified Dyno.View.M as M -import Dyno.View.JV ( JV, splitJV, catJV, splitJV', catJV' ) import Dyno.View.HList ( (:*:)(..) ) import Dyno.View.Fun import Dyno.View.MapFun @@ -109,18 +110,18 @@ data QuadraturePlottingIn x z u p o q qo fp a = -- x0 xF x z u p fp o q qo t T QuadraturePlottingIn (J x a) (J x a) (J x a) (J z a) (J u a) (J p a) (J o a) (J q a) (J qo a) (J fp a) - (J (JV Id) a) (J (JV Id) a) + (S a) (S a) deriving (Generic, Generic1) data QuadratureIn x z u p fp a = -- x' x z u p fp t T QuadratureIn (J x a) (J x a) (J z a) (J u a) (J p a) (J fp a) - (J (JV Id) a) (J (JV Id) a) + (S a) (S a) deriving (Generic, Generic1) data QuadratureStageIn x z u p fp deg a = -- xzus p fp ts h - QuadratureStageIn (J (CollStage x z u deg) a) (J p a) (J fp a) (J (JVec deg (JV Id)) a) (J (JV Id) a) + QuadratureStageIn (J (CollStage x z u deg) a) (J p a) (J fp a) (J (JVec deg (JV Id)) a) (S a) deriving (Generic, Generic1) data QuadratureStageOut q deg a = @@ -130,17 +131,17 @@ data PathCIn x z u p fp a = -- x' x z u p t - PathCIn (J x a) (J x a) (J z a) (J u a) (J p a) (J fp a) (J (JV Id) a) + PathCIn (J x a) (J x a) (J z a) (J u a) (J p a) (J fp a) (S a) deriving (Generic, Generic1) data PathCStageIn x z u p fp deg a = -- xzus p fp ts h - PathCStageIn (J (CollStage x z u deg) a) (J p a) (J fp a) (J (JVec deg (JV Id)) a) (J (JV Id) a) + PathCStageIn (J (CollStage x z u deg) a) (J p a) (J fp a) (J (JVec deg (JV Id)) a) (S a) deriving (Generic, Generic1) data DaeIn x z u p fp a = -- t p fp x' (CollPoint x z u) - DaeIn (J (JV Id) a) (J p a) (J fp a) (J x a) (J (CollPoint x z u) a) + DaeIn (S a) (J p a) (J fp a) (J x a) (J (CollPoint x z u) a) deriving (Generic, Generic1) data DaeOut r o a = @@ -189,19 +190,19 @@ Radau -> TV.tvlast $ unJVec $ split xs dynamicsFunction :: DaeIn (JV x) (JV z) (JV u) (JV p) (JV fp) SX -> DaeOut (JV r) (JV o) SX - dynamicsFunction (DaeIn t parm fixedParm x' collPoint) = DaeOut (catJV' r) (catJV' o) + dynamicsFunction (DaeIn t parm fixedParm x' collPoint) = DaeOut (vcat r) (vcat o) where CollPoint x z u = split collPoint (r,o) = ocpDae ocp - (splitJV' x') (splitJV' x) (splitJV' z) (splitJV' u) - (splitJV' parm) (splitJV' fixedParm) (unId (splitJV' t)) + (vsplit x') (vsplit x) (vsplit z) (vsplit u) + (vsplit parm) (vsplit fixedParm) (unId (vsplit t)) interpolateFun <- toMXFun "interpolate (JV x)" interpolate' >>= expandMXFun interpolateQFun <- toMXFun "interpolate (JV q)" interpolate' >>= expandMXFun interpolateQoFun <- toMXFun "interpolate (JV qo)" interpolate' >>= expandMXFun interpolateScalarFun <- toMXFun "interpolate (JV Id)" interpolate' >>= expandMXFun - let callInterpolateScalar :: J (JV Id) MX -> Vec deg (J (JV Id) MX) -> J (JV Id) MX + let callInterpolateScalar :: S MX -> Vec deg (S MX) -> S 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 @@ -220,9 +221,9 @@ DaeOut _ o = dynamicsFunction daeIn quad :: J (JV q) SX - quad = catJV' $ ocpQuadratures ocp - (splitJV' x) (splitJV' z) (splitJV' u) (splitJV' p) (splitJV' fp) (splitJV' o) - (unId (splitJV' t)) (unId (splitJV' tf)) + quad = vcat $ ocpQuadratures ocp + (vsplit x) (vsplit z) (vsplit u) (vsplit p) (vsplit fp) (vsplit o) + (unId (vsplit t)) (unId (vsplit tf)) let quadOutFun :: QuadratureIn (JV x) (JV z) (JV u) (JV p) (JV fp) SX -> J (JV qo) SX quadOutFun (QuadratureIn x' x z u p fp t tf) = quad @@ -231,20 +232,20 @@ DaeOut _ o = dynamicsFunction daeIn quad :: J (JV qo) SX - quad = catJV' $ ocpQuadratureOutputs ocp - (splitJV' x) (splitJV' z) (splitJV' u) (splitJV' p) (splitJV' fp) (splitJV' o) - (unId (splitJV' t)) (unId (splitJV' tf)) + quad = vcat $ ocpQuadratureOutputs ocp + (vsplit x) (vsplit z) (vsplit u) (vsplit p) (vsplit fp) (vsplit o) + (unId (vsplit t)) (unId (vsplit tf)) - let lagFun :: QuadratureIn (JV x) (JV z) (JV u) (JV p) (JV fp) SX -> J (JV Id) SX + let lagFun :: QuadratureIn (JV x) (JV z) (JV u) (JV p) (JV fp) SX -> S SX lagFun (QuadratureIn x' x z u p fp t tf) = lag where daeIn = DaeIn t p fp x' (cat (CollPoint x z u)) DaeOut _ o = dynamicsFunction daeIn - lag :: J (JV Id) SX - lag = catJV' $ Id $ ocpLagrange ocp - (splitJV' x) (splitJV' z) (splitJV' u) (splitJV' p) (splitJV' fp) (splitJV' o) - (unId (splitJV' t)) (unId (splitJV' tf)) + lag :: S SX + lag = vcat $ Id $ ocpLagrange ocp + (vsplit x) (vsplit z) (vsplit u) (vsplit p) (vsplit fp) (vsplit o) + (unId (vsplit t)) (unId (vsplit tf)) let pathCFun :: PathCIn (JV x) (JV z) (JV u) (JV p) (JV fp) SX -> J (JV h) SX pathCFun (PathCIn x' x z u p fp t) = h @@ -253,9 +254,9 @@ DaeOut _ o = dynamicsFunction daeIn h :: J (JV h) SX - h = catJV' $ ocpPathC ocp - (splitJV' x) (splitJV' z) (splitJV' u) (splitJV' p) (splitJV' fp) (splitJV' o) - (unId (splitJV' t)) + h = vcat $ ocpPathC ocp + (vsplit x) (vsplit z) (vsplit u) (vsplit p) (vsplit fp) (vsplit o) + (unId (vsplit t)) quadFunSX <- toSXFun "quadFun" quadFun quadOutFunSX <- toSXFun "quadOutFun" quadOutFun @@ -266,10 +267,10 @@ QuadraturePlottingIn (JV x) (JV z) (JV u) (JV p) (JV o) (JV q) (JV qo) (JV fp) SX -> J (JV po) SX quadraturePlottingFun (QuadraturePlottingIn x0 xF x z u p o q qo fp t tf) = - catJV' $ ocpPlotOutputs ocp (splitJV' x0, splitJV' xF) - (splitJV' x) (splitJV' z) (splitJV' u) (splitJV' p) - (splitJV' o) (splitJV' q) (splitJV' qo) (splitJV' fp) - (unId (splitJV' t)) (unId (splitJV' tf)) + vcat $ ocpPlotOutputs ocp (vsplit x0, vsplit xF) + (vsplit x) (vsplit z) (vsplit u) (vsplit p) + (vsplit o) (vsplit q) (vsplit qo) (vsplit fp) + (unId (vsplit t)) (unId (vsplit tf)) quadPlotFunSX <- toSXFun "quadPlotFun" quadraturePlottingFun let -- later we could use the intermediate points as outputs, or in path cosntraints @@ -298,9 +299,9 @@ pathCStageFunMX <- toMXFun "pathCStageFun" pathCStageFun - bcFun <- toSXFun "bc" $ \(x0:*:x1:*:x2:*:x3:*:x4:*:x5) -> catJV' $ ocpBc ocp (splitJV' x0) (splitJV' x1) (splitJV' x2) (splitJV' x3) (splitJV' x4) (unId (splitJV' x5)) + bcFun <- toSXFun "bc" $ \(x0:*:x1:*:x2:*:x3:*:x4:*:x5) -> vcat $ ocpBc ocp (vsplit x0) (vsplit x1) (vsplit x2) (vsplit x3) (vsplit x4) (unId (vsplit x5)) mayerFun <- toSXFun "mayer" $ \(x0:*:x1:*:x2:*:x3:*:x4:*:x5) -> - catJV' $ Id $ ocpMayer ocp (unId (splitJV' x0)) (splitJV' x1) (splitJV' x2) (splitJV' x3) (splitJV' x4) (splitJV' x5) + vcat $ Id $ ocpMayer ocp (unId (vsplit x0)) (vsplit x1) (vsplit x2) (vsplit x3) (vsplit x4) (vsplit x5) dynFun <- toSXFun "dynamics" dynamicsFunction @@ -310,7 +311,7 @@ (J (JV x) :*: J (JVec deg (JTuple (JV x) (JV z))) :*: J (JVec deg (JV u)) - :*: J (JV Id) + :*: S :*: J (JV p) :*: J (JV fp) :*: J (JVec deg (JV Id)) @@ -323,7 +324,7 @@ -- dt, parm, and fixedParm have to be repeated -- that is why they are row matrices - let stageFun :: (M (JV Id) (JV Id) + let stageFun :: (S :*: M (JV Id) (CollStage (JV x) (JV z) (JV u) deg) :*: M (JV Id) (JVec deg (JV Id)) :*: M (JV Id) (JV p) @@ -334,14 +335,14 @@ :*: M (JV Id) (JV x) ) MX stageFun (dt' :*: collStageRow :*: stageTimesRow :*: parm' :*: fixedParm') = - (M.row dc :*: M.row stageHs :*: M.row interpolatedX') + (M.trans dc :*: M.trans stageHs :*: M.trans interpolatedX') where - dt = M.unrow dt' - parm = M.unrow parm' - fixedParm = M.unrow fixedParm' + dt = M.trans dt' + parm = M.trans parm' + fixedParm = M.trans fixedParm' - stageTimes = M.unrow stageTimesRow - collStage = M.unrow collStageRow + stageTimes = M.trans stageTimesRow + collStage = M.trans collStageRow CollStage x0 xzus = split collStage dc :*: interpolatedX' = call dynamicsStageFun @@ -361,7 +362,7 @@ let mapOpts = case mapStrategy dirCollOpts of Unrolled -> M.empty Symbolic r -> r - mapStageFunMX <- mapFun'' (Proxy :: Proxy n) "mapDynamicsStageFun" stageFunMX mapOpts + mapStageFunMX <- mapFun' (Proxy :: Proxy n) "mapDynamicsStageFun" stageFunMX mapOpts -- use repeated outputs for now :: IO (Fun ( M (JV Id) (JVec n (JV Id)) @@ -377,7 +378,7 @@ ) ---- non-repeated outputs don't work yet, and we need them for exact hessian -- :: IO (Fun --- (M (JV Id) (JV Id) +-- (S -- :*: M (JV Id) (JVec n (CollStage (JV x) (JV z) (JV u) deg)) -- :*: M (JV Id) (JVec n (JVec deg (JV Id))) -- :*: M (JV Id) (JV p) @@ -390,7 +391,7 @@ -- ) let mapStageFun :: MapStrategy - -> ( J (JV Id) MX + -> ( S MX , J (JVec n (CollStage (JV x) (JV z) (JV u) deg)) MX , J (JVec n (JVec deg (JV Id))) MX , J (JV p) MX @@ -404,29 +405,29 @@ mapStageFun Unrolled (dt', stages, times, parm', fixedParm') = (cat (JVec dcs), cat (JVec hs), cat (JVec xnexts)) where - dt = M.row dt' - parm = M.row parm' - fixedParm = M.row fixedParm' + dt = M.trans dt' + parm = M.trans parm' + fixedParm = M.trans fixedParm' (dcs, hs, xnexts) = TV.tvunzip3 $ TV.tvzipWith f (unJVec (split stages)) (unJVec (split times)) - f stage stageTimes = (M.unrow dc, M.unrow h, M.unrow xnext) + f stage stageTimes = (M.trans dc, M.trans h, M.trans xnext) where dc :*: h :*: xnext = call stageFunMX - (dt :*: (M.row stage) :*: (M.row stageTimes) :*: parm :*: fixedParm) + (dt :*: (M.trans stage) :*: (M.trans stageTimes) :*: parm :*: fixedParm) -- dc :*: h :*: xnext = -- stageFun --- (dt :*: (M.row stage) :*: (M.row stageTimes) :*: parm :*: fixedParm) +-- (dt :*: (M.trans stage) :*: (M.trans stageTimes) :*: parm :*: fixedParm) - mapStageFun (Symbolic _) (x0', x1, x2, x3', x4') = (M.unrow y0, M.unrow y1, M.unrow y2) + mapStageFun (Symbolic _) (x0', x1, x2, x3', x4') = (M.trans y0, M.trans y1, M.trans y2) where x0 = jreplicate x0' :: J (JVec n (JV Id)) MX x3 = jreplicate x3' :: J (JVec n (JV p)) MX x4 = jreplicate x4' :: J (JVec n (JV fp)) MX y0 :*: y1 :*: y2 = call mapStageFunMX - (M.row x0 :*: M.row x1 :*: M.row x2 :*: M.row x3 :*: M.row x4) + (M.trans x0 :*: M.trans x1 :*: M.trans x2 :*: M.trans x3 :*: M.trans x4) let nlp :: Nlp (CollTraj x z u p n deg) (JV fp) (CollOcpConstraints x r c h n deg) MX nlp = Nlp { @@ -437,18 +438,18 @@ :*: J (JV q) :*: J (JV p) :*: J (JV fp) - :*: J (JV Id) + :*: S ) (J (JV c)) ) - (mayerFun :: SXFun ( J (JV Id) + (mayerFun :: SXFun ( S :*: J (JV x) :*: J (JV x) :*: J (JV q) :*: J (JV p) :*: J (JV fp) ) - (J (JV Id)) + S ) (call lagrangeStageFunMX) (call quadratureStageFunMX) @@ -487,15 +488,15 @@ evalQuadratures :: Vec n (Vec deg Double) -> Double -> IO Double evalQuadratures qs' tf' = do - let d2d :: Double -> J (JV Id) DMatrix + let d2d :: Double -> S DMatrix d2d = realToFrac qs :: Vec n (J (JVec deg (JV Id)) DMatrix) qs = fmap (cat . JVec . fmap d2d) qs' - tf :: J (JV Id) DMatrix + tf :: S DMatrix tf = realToFrac tf' - evalq :: J (JVec deg (JV Id)) DMatrix -> IO (J (JV Id) DMatrix) + evalq :: J (JVec deg (JV Id)) DMatrix -> IO (S DMatrix) evalq q = eval genericQuadraturesFun (q :*: tf) - stageIntegrals' <- T.mapM evalq qs :: IO (Vec n (J (JV Id) DMatrix)) + stageIntegrals' <- T.mapM evalq qs :: IO (Vec n (S DMatrix)) let stageIntegrals = fmap (unId . splitJV . d2v) stageIntegrals' :: Vec n Double return (F.sum stageIntegrals) @@ -532,8 +533,8 @@ -> MXFun ( J (CollStage (JV x) (JV z) (JV u) deg) :*: J (JV p) :*: J (JV fp) - :*: J (JV Id) - :*: J (JV Id) + :*: S + :*: S ) ( J (JVec deg (JV r)) :*: J (JVec deg (JV x)) @@ -573,11 +574,11 @@ callOutputFun :: (J (JV x) DMatrix, J (JV x) DMatrix) -> J (JV p) (Vector Double) -> J (JV fp) (Vector Double) - -> J (JV Id) (Vector Double) - -> J (JV Id) DMatrix + -> S (Vector Double) + -> S DMatrix -> Quadratures q qo Double -> ( J (CollStage (JV x) (JV z) (JV u) deg) (Vector Double) - , J (JV Id) (Vector Double) + , S (Vector Double) ) -> IO ( StageOutputs x o h q qo po deg Double , Quadratures q qo Double @@ -589,7 +590,7 @@ (_ :*: xdot :*: out :*: xnext) <- eval outputFun $ stage' :*: p' :*: fp' :*: (v2d h) :*: (v2d k) - let stageTimes :: Vec deg (J (JV Id) DMatrix) + let stageTimes :: Vec deg (S DMatrix) stageTimes = fmap (\tau -> t0 + realToFrac tau * h') taus where t0 = h' * v2d k @@ -658,7 +659,7 @@ vstages = unJVec (split stages) :: Vec n (J (CollStage (JV x) (JV z) (JV u) deg) (Vector Double)) - ks :: Vec n (J (JV Id) (Vector Double)) + ks :: Vec n (S (Vector Double)) ks = TV.mkVec' $ map (catJV . Id . realToFrac) (take n [(0::Int)..]) CollStage x0 _ = split (TV.tvhead vstages) @@ -706,24 +707,24 @@ :*: J (JV q) :*: J (JV p) :*: J (JV fp) - :*: J (JV Id) + :*: S ) (J (JV c)) -- mayerFun - -> SXFun ( J (JV Id) + -> SXFun ( S :*: J (JV x) :*: J (JV x) :*: J (JV q) :*: J (JV p) :*: J (JV fp) ) - (J (JV Id)) + S -- lagQuadFun - -> (QuadratureStageIn (JV x) (JV z) (JV u) (JV p) (JV fp) deg MX -> J (JV Id) MX) + -> (QuadratureStageIn (JV x) (JV z) (JV u) (JV p) (JV fp) deg MX -> S MX) -- quadFun -> (QuadratureStageIn (JV x) (JV z) (JV u) (JV p) (JV fp) deg MX -> J (JV q) MX) -- stageFun - -> ( ( J (JV Id) MX + -> ( ( S MX , J (JVec n (CollStage (JV x) (JV z) (JV u) deg)) MX , J (JVec n (JVec deg (JV Id))) MX , J (JV p) MX @@ -739,7 +740,7 @@ -- parameter -> J (JV fp) MX -- (objective, constraints) - -> (J (JV Id) MX, J (CollOcpConstraints x r c h n deg) MX) + -> (S MX, J (CollOcpConstraints x r c h n deg) MX) getFg taus bcFun mayerFun lagQuadFun quadFun mapStageFun collTraj fixedParm = (obj, cat g) where @@ -752,7 +753,7 @@ objMayer = call mayerFun (tf :*: x0 :*: xf :*: finalQuadratures :*: parm :*: fixedParm) - objLagrange :: J (JV Id) MX + objLagrange :: S MX objLagrange = F.sum $ TV.tvzipWith (oneQuadStage lagQuadFun) stages times' finalQuadratures :: J (JV q) MX @@ -774,7 +775,7 @@ n = reflectDim (Proxy :: Proxy n) -- times at each collocation point - times :: Vec n (Vec deg (J (JV Id) MX)) + times :: Vec n (Vec deg (S MX)) times = fmap snd $ timesFromTaus 0 (fmap realToFrac taus) dt times' :: Vec n (J (JVec deg (JV Id)) MX) @@ -859,7 +860,7 @@ -- state derivatives, maybe these could be useful as outputs xdots :: Vec deg (J x MX) - xdots = fmap (`M.vs` (1/h)) $ interpolateXDots cijs (x0 TV.<| xs) + xdots = fmap (`M.ms` (1/h)) $ interpolateXDots cijs (x0 TV.<| xs) quadratureIns :: Vec deg (QuadratureIn x z u p fp MX) quadratureIns = TV.tvzipWith3 (\x' (CollPoint x z u) t -> QuadratureIn x' x z u p fp t tf) @@ -868,7 +869,7 @@ qdots :: Vec deg (J q MX) qdots = fmap evalQuadDeriv quadratureIns - stageTimes :: Vec deg (J (JV Id) MX) + stageTimes :: Vec deg (S MX) stageTimes = unJVec (split stageTimes') qnext :: J q MX @@ -876,7 +877,7 @@ qs = fmap timesH qsOverH where - timesH q = M.uncol $ M.ms (M.col q) h + timesH q = M.ms q h qsOverH :: Vec deg (J q MX) qsOverH = cijInvFr !* qdots @@ -915,7 +916,7 @@ -- state derivatives, maybe these could be useful as outputs xdots :: Vec deg (J x MX) - xdots = fmap (`M.vs` (1/h)) $ interpolateXDots cijs (x0 TV.<| xs) + xdots = fmap (`M.ms` (1/h)) $ interpolateXDots cijs (x0 TV.<| xs) pathCIns :: Vec deg (PathCIn x z u p fp MX) pathCIns = TV.tvzipWith3 (\x' (CollPoint x z u) t -> PathCIn x' x z u p fp t) @@ -924,7 +925,7 @@ hs :: Vec deg (J h MX) hs = fmap evalPathC pathCIns - stageTimes :: Vec deg (J (JV Id) MX) + stageTimes :: Vec deg (S MX) stageTimes = unJVec (split stageTimes') @@ -933,20 +934,20 @@ genericQuadraturesFunction :: forall deg . Dim deg - => (J (JV Id) MX -> Vec deg (J (JV Id) MX) -> J (JV Id) MX) + => (S MX -> Vec deg (S MX) -> S MX) -> Vec (TV.Succ deg) (Vec (TV.Succ deg) Double) -> Int - -> (J (JVec deg (JV Id)) :*: J (JV Id)) MX - -> J (JV Id) MX + -> (J (JVec deg (JV Id)) :*: S) MX + -> S MX genericQuadraturesFunction interpolate' cijs' n (qdots' :*: tf) = dt * qnext where dt = tf / fromIntegral n - qdots :: Vec deg (J (JV Id) MX) + qdots :: Vec deg (S MX) qdots = unJVec $ split qdots' - qnext :: J (JV Id) MX + qnext :: S MX qnext = interpolate' 0 qs qs = cijInvFr !* qdots @@ -963,7 +964,7 @@ cijInv :: Vec deg (Vec deg Double) cijInv = TV.mkVec' (map TV.mkVec' (Mat.toLists cijInv')) - cijInvFr :: Vec deg (Vec deg (J (JV Id) MX)) + cijInvFr :: Vec deg (Vec deg (S MX)) cijInvFr = fmap (fmap realToFrac) cijInv @@ -996,7 +997,7 @@ => (J x MX -> Vec deg (J x MX) -> J x MX) -> Vec (TV.Succ deg) (Vec (TV.Succ deg) Double) -> SXFun (DaeIn x z u p fp) (DaeOut r o) - -> (J x :*: J (JVec deg (JTuple x z)) :*: J (JVec deg u) :*: J (JV Id) :*: J p :*: J fp :*: J (JVec deg (JV Id))) MX + -> (J x :*: J (JVec deg (JTuple x z)) :*: J (JVec deg u) :*: S :*: J p :*: J fp :*: J (JVec deg (JV Id))) MX -> (J (JVec deg r) :*: J x) MX toDynamicsStage interpolate' cijs dynFun (x0 :*: xzs' :*: us' :*: h :*: p :*: fp :*: stageTimes') = cat (JVec dynConstrs) :*: xnext @@ -1014,7 +1015,7 @@ dynConstrs :: Vec deg (J r MX) (dynConstrs, _) = TV.tvunzip $ TV.tvzipWith4 applyDae xdots xzs us stageTimes - applyDae :: J x MX -> JTuple x z MX -> J u MX -> J (JV Id) MX -> (J r MX, J o MX) + applyDae :: J x MX -> JTuple x z MX -> J u MX -> S MX -> (J r MX, J o MX) applyDae x' (JTuple x z) u t = (r, o) where DaeOut r o = call dynFun (DaeIn t p fp x' collPoint) @@ -1022,7 +1023,7 @@ -- state derivatives, maybe these could be useful as outputs xdots :: Vec deg (J x MX) - xdots = fmap (`M.vs` (1/h)) $ interpolateXDots cijs (x0 TV.<| xs) + xdots = fmap (`M.ms` (1/h)) $ interpolateXDots cijs (x0 TV.<| xs) xs :: Vec deg (J x MX) xs = fmap (\(JTuple x _) -> x) xzs @@ -1034,7 +1035,7 @@ => (J x MX -> Vec deg (J x MX) -> J x MX) -> Vec (TV.Succ deg) (Vec (TV.Succ deg) Double) -> Vec deg Double -> SXFun (DaeIn x z u p fp) (DaeOut r o) - -> (J (CollStage x z u deg) :*: J p :*: J fp :*: J (JV Id) :*: J (JV Id)) MX + -> (J (CollStage x z u deg) :*: J p :*: J fp :*: S :*: S) MX -> (J (JVec deg r) :*: J (JVec deg x) :*: J (JVec deg o) :*: J x) MX outputFunction callInterpolate cijs taus dynFun (collStage :*: p :*: fp :*: h :*: k) = cat (JVec dynConstrs) :*: cat (JVec xdots) :*: cat (JVec outputs) :*: xnext @@ -1042,7 +1043,7 @@ 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 (JV Id) MX) + stageTimes :: Vec deg (S MX) stageTimes = fmap (\tau -> t0 + realToFrac tau * h) taus t0 = k*h @@ -1053,14 +1054,14 @@ 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 (JV Id) MX -> (J r MX, J o MX) + applyDae :: J x MX -> J (CollPoint x z u) MX -> S MX -> (J r MX, J o MX) applyDae x' xzu t = (r, o) where DaeOut r o = call dynFun (DaeIn t p fp x' xzu) -- state derivatives, maybe these could be useful as outputs xdots :: Vec deg (J x MX) - xdots = fmap (`M.vs` (1/h)) $ interpolateXDots cijs (x0 TV.<| xs) + xdots = fmap (`M.ms` (1/h)) $ interpolateXDots cijs (x0 TV.<| xs) xs :: Vec deg (J x MX) xs = fmap ((\(CollPoint x _ _) -> x) . split) xzus diff --git a/src/Dyno/DirectCollocation/FormulateCov.hs b/src/Dyno/DirectCollocation/FormulateCov.hs --- a/src/Dyno/DirectCollocation/FormulateCov.hs +++ b/src/Dyno/DirectCollocation/FormulateCov.hs @@ -19,9 +19,9 @@ import Casadi.DMatrix ( DMatrix ) import Casadi.MX ( MX ) -import Dyno.View.View ( View(..), J, jfill, v2d, d2v ) +import Dyno.View.M ( vcat, vsplit ) +import Dyno.View.View ( View(..), J, S, JV, catJV, jfill, v2d, d2v ) import Dyno.View.Cov ( Cov ) -import Dyno.View.JV ( JV, catJV, catJV', splitJV' ) import Dyno.View.HList ( (:*:)(..) ) import Dyno.View.Fun import Dyno.View.JVec( JVec(..), jreplicate ) @@ -40,12 +40,14 @@ data CollCovProblem ocp n deg sx sw sh shr sc = CollCovProblem { ccpNlp :: Nlp - (CollTrajCov sx ocp n deg) + (CollTrajCov sx (X ocp) (Z ocp) (U ocp) (P ocp) n deg) (JV None) (CollOcpCovConstraints ocp n deg sh shr sc) MX - , ccpPlotPoints :: J (CollTrajCov sx ocp n deg) (Vector Double) -> IO (DynPlotPoints Double) + , ccpPlotPoints :: J (CollTrajCov sx (X ocp) (Z ocp) (U ocp) (P ocp) n deg) + (Vector Double) + -> IO (DynPlotPoints Double) , ccpOutputs :: - J (CollTrajCov sx ocp n deg) (Vector Double) + J (CollTrajCov sx (X ocp) (Z ocp) (U ocp) (P ocp) n deg) (Vector Double) -> IO ( Vec n (StageOutputs (X ocp) (O ocp) (H ocp) (Q ocp) (QO ocp) (PO ocp) deg Double) , Vec n (J (Cov (JV sx)) (Vector Double)) , J (Cov (JV sx)) (Vector Double) @@ -100,11 +102,11 @@ (computeSensitivities) (ocpCovSq ocpCov) sbcFun <- toSXFun "sbc" $ \(x0:*:x1) -> ocpCovSbc ocpCov x0 x1 - shFun <- toSXFun "sh" $ \(x0:*:x1) -> ocpCovSh ocpCov (splitJV' x0) x1 + shFun <- toSXFun "sh" $ \(x0:*:x1) -> ocpCovSh ocpCov (vsplit x0) x1 mayerFun <- toSXFun "cov mayer" $ \(x0:*:x1:*:x2:*:x3:*:x4) -> - catJV' $ Id $ ocpCovMayer ocpCov (unId (splitJV' x0)) (splitJV' x1) (splitJV' x2) x3 x4 + vcat $ Id $ ocpCovMayer ocpCov (unId (vsplit x0)) (vsplit x1) (vsplit x2) x3 x4 lagrangeFun <- toSXFun "cov lagrange" $ \(x0:*:x1:*:x2:*:x3) -> - catJV' $ Id $ ocpCovLagrange ocpCov (unId (splitJV' x0)) (splitJV' x1) x2 (unId (splitJV' x3)) + vcat $ Id $ ocpCovLagrange ocpCov (unId (vsplit x0)) (vsplit x1) x2 (unId (vsplit x3)) cp0 <- makeCollProblem dirCollOpts ocp ocpInputs guess @@ -114,7 +116,7 @@ gammas' = ocpCovGammas ocpCov :: shr Double gammas :: J (JV shr) MX - gammas = catJV' (fmap realToFrac gammas') + gammas = vcat (fmap realToFrac gammas') rpathCUb :: shr Bounds rpathCUb = fill (Nothing, Just 0) @@ -123,27 +125,28 @@ robustPathCUb = catJV rpathCUb -- the NLP - fg :: J (CollTrajCov sx ocp n deg) MX + fg :: J (CollTrajCov sx x z u p n deg) MX -> J (JV fp) MX - -> (J (JV Id) MX, J (CollOcpCovConstraints ocp n deg sh shr sc) MX) + -> (S MX, J (CollOcpCovConstraints ocp n deg 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 (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))) + (lagrangeFun :: SXFun (S :*: J (JV x) :*: J (Cov (JV sx)) :*: S) S) + (mayerFun :: SXFun (S :*: (J (JV x) :*: (J (JV x) :*: (J (Cov (JV sx)) :*: J (Cov (JV sx)))))) S) (nlpFG nlp0) computeCovariancesFun' <- toMXFun "compute covariances" (\(x :*: y) -> computeCovariances x y) -- callbacks - let getPlotPoints :: J (CollTrajCov sx ocp n deg) (Vector Double) -> IO (DynPlotPoints Double) + 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 (catJV None) - getOutputs :: J (CollTrajCov sx ocp n deg) (Vector Double) + getOutputs :: J (CollTrajCov sx x z u p n deg) (Vector Double) -> IO ( Vec n (StageOutputs x o h q qo po deg Double) , Vec n (J (Cov (JV sx)) (Vector Double)) , J (Cov (JV sx)) (Vector Double) @@ -231,15 +234,16 @@ -> SXFun (J (JV x) :*: J (Cov (JV sx))) (J sh) -- lagrangeFun -> SXFun - (J (JV Id) :*: J (JV x) :*: J (Cov (JV sx)) :*: J (JV Id)) (J (JV Id)) + (S :*: J (JV x) :*: J (Cov (JV sx)) :*: S) S -- mayerFun -> SXFun - (J (JV Id) :*: J (JV x) :*: J (JV x) :*: J (Cov (JV sx)) :*: J (Cov (JV sx))) (J (JV Id)) - -> (J (CollTraj' ocp n deg) MX -> J (JV fp) MX -> (J (JV Id) MX, J (CollOcpConstraints' ocp n deg) MX) + (S :*: J (JV x) :*: J (JV x) :*: J (Cov (JV sx)) :*: J (Cov (JV sx))) S + -> (J (CollTraj x z u p n deg) MX -> J (JV fp) MX + -> (S MX, J (CollOcpConstraints' ocp n deg) MX) ) - -> J (CollTrajCov sx ocp n deg) MX + -> J (CollTrajCov sx x z u p n deg) MX -> J (JV fp) MX - -> (J (JV Id) MX, J (CollOcpCovConstraints ocp n deg sh shr sc) MX) + -> (S MX, J (CollOcpCovConstraints ocp n deg sh shr sc) MX) getFgCov taus computeCovariances gammas robustify sbcFun shFun lagrangeFun mayerFun @@ -267,7 +271,7 @@ n = reflectDim (Proxy :: Proxy n) -- times at each collocation point - t0s :: Vec n (J (JV Id) MX) + t0s :: Vec n (S MX) (t0s, _) = TV.tvunzip $ timesFromTaus 0 (fmap realToFrac taus) dt -- initial point at each stage 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 @@ -3,6 +3,7 @@ {-# LANGUAGE TypeOperators #-} {-# LANGUAGE DeriveGeneric #-} {-# LANGUAGE FlexibleContexts #-} +{-# LANGUAGE PolyKinds #-} module Dyno.DirectCollocation.Integrate ( withIntegrator @@ -19,13 +20,13 @@ import Casadi.SX ( SX ) import Casadi.MX ( MX ) +import Casadi.Viewable ( Viewable ) -import Dyno.View.JV ( JV, splitJV, catJV, splitJV', catJV' ) -import Dyno.View.Viewable ( Viewable ) -import Dyno.View.View ( View(..), J, JNone, JTuple(..), jfill ) +import Dyno.View.View ( View(..), J, S, JV, JNone, JTuple(..), splitJV, catJV, jfill ) import Dyno.View.Fun ( SXFun, call, toSXFun, toMXFun, expandMXFun ) import Dyno.View.JVec ( JVec(..), jreplicate ) import Dyno.View.HList ( (:*:)(..) ) +import Dyno.View.M ( vcat, vsplit ) import qualified Dyno.View.M as M import Dyno.Vectorize ( Vectorize(..), Id(..), vzipWith ) import Dyno.TypeVecs ( Vec ) @@ -37,7 +38,7 @@ import Dyno.DirectCollocation.Types ( CollStage(..), CollPoint(..) ) import Dyno.DirectCollocation.Quadratures ( QuadratureRoots, mkTaus, interpolate, timesFromTaus ) -type Sxe = J (JV Id) SX +type Sxe = S SX data IntegratorX x z n deg a = IntegratorX @@ -46,7 +47,7 @@ } deriving (Generic) data IntegratorP u p n deg a = IntegratorP - { ipTf :: J (JV Id) a + { ipTf :: S a , ipParm :: J (JV p) a , ipU :: J (JVec n (JVec deg (JV u))) a } deriving (Generic) @@ -92,8 +93,8 @@ 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 (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 + -> SXFun (S :*: J p :*: J x :*: J (CollPoint x z u)) (J r) + -> (J x :*: J (JVec deg (JTuple x z)) :*: J (JVec deg u) :*: S :*: J p :*: J (JVec deg (JV Id))) MX -> (J (JVec deg r) :*: J x) MX dynStageConstraints' cijs taus dynFun (x0 :*: xzs' :*: us' :*: h :*: p :*: stageTimes') = cat (JVec dynConstrs) :*: xnext @@ -111,7 +112,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 (JV Id) MX -> J r MX + applyDae :: J x MX -> JTuple x z MX -> J u MX -> S MX -> J r MX applyDae x' (JTuple x z) u t = r where r = call dynFun (t :*: p :*: x' :*: collPoint) @@ -119,7 +120,7 @@ -- state derivatives, maybe these could be useful as outputs xdots :: Vec deg (J x MX) - xdots = fmap (`M.vs` (1/h)) $ interpolateXDots cijs (x0 TV.<| xs) + xdots = fmap (`M.ms` (1/h)) $ interpolateXDots cijs (x0 TV.<| xs) xs :: Vec deg (J x MX) xs = fmap (\(JTuple x _) -> x) xzs @@ -128,8 +129,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 (JV Id) a -> J r a) - -> (J (JV Id) :*: 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 -> S a -> J r a) + -> (S :*: J p :*: J x :*: J (CollPoint x z u)) a -> J r a dynamicsFunction' dae (t :*: parm :*: x' :*: collPoint) = dae x' x z u parm t where @@ -138,14 +139,15 @@ withIntegrator :: forall x z u p r deg n b . (Dim n, Dim deg, Vectorize x, Vectorize p, Vectorize u, Vectorize z, Vectorize r) - => Proxy (n, deg) + => Proxy n + -> Proxy deg -> QuadratureRoots -> x Double -> (x Sxe -> x Sxe -> z Sxe -> u Sxe -> p Sxe -> Sxe -> r Sxe) -> Solver -> ((x Double -> Either (u Double) (Vec n (Vec deg (u Double))) -> p Double -> Double -> IO (x Double)) -> IO b) -> IO b -withIntegrator _ roots initialX dae solver userFun = do +withIntegrator _ _ roots initialX dae solver userFun = do let -- the collocation points taus :: Vec deg Double taus = mkTaus roots @@ -158,9 +160,9 @@ dynFun <- toSXFun "dynamics" $ dynamicsFunction' $ \x0 x1 x2 x3 x4 x5 -> - let r = dae (splitJV' x0) (splitJV' x1) (splitJV' x2) (splitJV' x3) - (splitJV' x4) (unId (splitJV' x5)) - in catJV' r + let r = dae (vsplit x0) (vsplit x1) (vsplit x2) (vsplit x3) + (vsplit x4) (unId (vsplit x5)) + in vcat r dynStageConFun <- toMXFun "dynamicsStageCon" (dynStageConstraints' cijs taus dynFun) -- let callDynStageConFun = call dynStageConFun @@ -168,7 +170,7 @@ let fg :: J (IntegratorX x z n deg) MX -> J (IntegratorP u p n deg) MX - -> (J (JV Id) MX, J (IntegratorG x r n deg) MX) + -> (S MX, J (IntegratorG x r n deg) MX) fg = getFgIntegrator taus callDynStageConFun scaleX = Nothing @@ -257,10 +259,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 (JV Id) :*: J (JV p) :*: J (JVec deg (JV Id))) 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)) :*: S :*: 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 (JV Id) MX, J (IntegratorG x r n deg) MX) + -> (S MX, J (IntegratorG x r n deg) MX) getFgIntegrator taus stageFun ix' ip' = (0, cat g) where ix = split ix' @@ -282,7 +284,7 @@ n = reflectDim (Proxy :: Proxy n) -- times at each collocation point - times :: Vec n (Vec deg (J (JV Id) MX)) + times :: Vec n (Vec deg (S MX)) times = fmap snd $ timesFromTaus 0 (fmap realToFrac taus) dt times' :: Vec n (J (JVec deg (JV Id)) MX) diff --git a/src/Dyno/DirectCollocation/Interpolate.hs b/src/Dyno/DirectCollocation/Interpolate.hs --- a/src/Dyno/DirectCollocation/Interpolate.hs +++ b/src/Dyno/DirectCollocation/Interpolate.hs @@ -17,9 +17,8 @@ import Linear.V import Linear ( lerp ) -import Dyno.View.Unsafe.View ( unJ, mkJ ) -import Dyno.View.View ( View(..), J ) -import Dyno.View.JV ( JV ) +import Dyno.View.Unsafe ( mkM, unM ) +import Dyno.View.View ( View(..), J, JV ) import Dyno.View.JVec import Dyno.TypeVecs ( Vec ) import Dyno.Vectorize ( Vectorize ) @@ -63,7 +62,7 @@ (ts,xs) = TV.tvunzip txs ret :: J f (V.Vector Double) - ret = mkJ $ LP.interpolate ts (fmap unJ xs) t + ret = mkM $ LP.interpolate ts (fmap unM xs) t type Point x z u = CollPoint (JV x) (JV z) (JV u) @@ -205,5 +204,5 @@ -- if t is too big and there are others available linterp (_:others@((t1,_):_:_)) t | t > t1 = linterp others t -linterp acc@((t0,x0):(t1,x1):_) t = (acc, mkJ (lerp ((t - t0) / (t1 - t0)) (unJ x0) (unJ x1))) +linterp acc@((t0,x0):(t1,x1):_) t = (acc, mkM (lerp ((t - t0) / (t1 - t0)) (unM x0) (unM x1))) linterp _ _ = error "linear interpolation ran out of nodes" 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 @@ -24,17 +24,16 @@ import Casadi.MX ( MX ) import Casadi.SX ( SX ) import Casadi.DMatrix ( DMatrix ) +import Casadi.Viewable ( Viewable ) -import qualified Dyno.View.Unsafe.M as M ( mkM, blockSplit ) +import Dyno.View.Unsafe ( mkM ) -import Dyno.View.View ( View(..), J, JNone(..), JTuple(..), fromDMatrix ) -import Dyno.View.JV ( JV, catJV', splitJV' ) +import Dyno.View.View ( View(..), J, S, JV, JNone(..), JTuple(..) ) import Dyno.View.HList ( (:*:)(..) ) import Dyno.View.Cov ( Cov, toMat, fromMat ) import Dyno.View.Fun -import Dyno.View.Viewable ( Viewable ) +import Dyno.View.M ( M, vcat, vsplit ) 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 ) @@ -61,7 +60,7 @@ } deriving (Eq, Show, Generic, Generic1) instance (View xe, View we, Dim n) => Scheme (CovarianceSensitivities xe we n) -type Sxe = J (JV Id) SX +type Sxe = S SX mkComputeSensitivities :: forall x z u p sx sz sw sr deg n . @@ -85,9 +84,9 @@ errorDynFun <- toSXFun "error dynamics" $ errorDynamicsFunction $ \x0 x1 x2 x3 x4 x5 x6 x7 x8 x9 -> let r = covDae - (splitJV' x0) (splitJV' x1) (splitJV' x2) (splitJV' x3) (splitJV' x4) - (unId (splitJV' x5)) (splitJV' x6) (splitJV' x7) (splitJV' x8) (splitJV' x9) - in catJV' r + (vsplit x0) (vsplit x1) (vsplit x2) (vsplit x3) (vsplit x4) + (unId (vsplit x5)) (vsplit x6) (vsplit x7) (vsplit x8) (vsplit x9) + in vcat r edscf <- toMXFun "errorDynamicsStageCon" (errorDynStageConstraints cijs taus errorDynFun) errorDynStageConFunJac <- toFunJac edscf @@ -95,7 +94,7 @@ sensitivityStageFun' <- toMXFun "sensitivity stage function" $ sensitivityStageFunction (call errorDynStageConFunJac) let sensitivityStageFun = sensitivityStageFun' - let sens :: J (JV Id) MX + let sens :: S MX -> J (JV p) MX -> J (JVec deg (JV Id)) MX -> J (JV x) MX @@ -119,11 +118,11 @@ n = reflectDim (Proxy :: Proxy n) -- initial time at each collocation stage - t0s :: Vec n (J (JV Id) MX) + t0s :: Vec n (S MX) t0s = TV.mkVec' $ take n [dt * fromIntegral k | k <- [(0::Int)..]] -- times at each collocation point - times :: Vec n (Vec deg (J (JV Id) MX)) + times :: Vec n (Vec deg (S MX)) times = fmap (\t0 -> fmap (\tau -> t0 + realToFrac tau * dt) taus) t0s times' :: Vec n (J (JVec deg (JV Id)) MX) @@ -145,7 +144,7 @@ , 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 (JV Id) MX + => (M (JV sx) (JV sx) MX -> M (JV sx) (JV sw) MX -> J (Cov (JV sw)) MX -> S MX -> M (JV sx) (JV sx) MX) -> (J (CollTraj x z u p n deg) MX -> CovarianceSensitivities (JV sx) (JV sw) n MX) -> J (Cov (JV sw)) DMatrix @@ -170,7 +169,7 @@ TV.tvzip (M.vsplit' (csFs sensitivities)) (M.vsplit' (csWs sensitivities)) qc :: J (Cov (JV sw)) MX - qc = fromDMatrix qc' + qc = M.fromDMatrix qc' ffs :: J (Cov (JV sx)) MX -> (M (JV sx) (JV sx) MX, M (JV sx) (JV sw) MX) @@ -215,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 (JV Id) a + => (J x a -> J x a -> J z a -> J u a -> J p a -> S a -> J sx a -> J sx a -> J sz a -> J sw a -> J r a) - -> (J (JV Id) :*: J p :*: J x :*: J (CollPoint x z u) :*: J sx :*: J sx :*: J sz :*: J sw) a + -> (S :*: J p :*: J x :*: J (CollPoint x z u) :*: J sx :*: J sx :*: J sz :*: J sw) a -> J r a errorDynamicsFunction dae (t :*: parm :*: x' :*: collPoint :*: sx' :*: sx :*: sz :*: sw) = r @@ -227,7 +226,7 @@ data ErrorIn0 x z u p deg 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) + ErrorIn0 (J x a) (J (JVec deg (CollPoint x z u)) a) (S 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) @@ -247,7 +246,7 @@ View sr, View sw, View sz, View sx) => 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 sx :*: J sx :*: J sz :*: J sw) + -> SXFun (S :*: J p :*: J x :*: J (CollPoint x z u) :*: J sx :*: J sx :*: J sz :*: J sw) (J sr) -> JacIn (ErrorInD sx sw sz deg) (ErrorIn0 x z u p deg) MX -> JacOut (ErrorOut sr sx deg) (J JNone) MX @@ -263,7 +262,7 @@ xs = fmap ((\(CollPoint x _ _) -> x) . split) xzus xdots :: Vec deg (J x MX) - xdots = fmap (`M.vs` (1 / h)) $ interpolateXDots cijs (x0 TV.<| xs) + xdots = fmap (`M.ms` (1 / h)) $ interpolateXDots cijs (x0 TV.<| xs) -- -- interpolated final state -- xnext :: J x MX @@ -281,7 +280,7 @@ applyDae :: J sx MX -> J sx MX -> J sz MX - -> J x MX -> J (CollPoint x z u) MX -> J (JV Id) MX + -> J x MX -> J (CollPoint x z u) MX -> S MX -> J sr MX applyDae sx' sx sz x' xzu t = call dynFun @@ -289,7 +288,7 @@ -- error state derivatives sxdots :: Vec deg (J sx MX) - sxdots = fmap (`M.vs` (1/h)) $ interpolateXDots cijs (sx0 TV.<| sxs) + sxdots = fmap (`M.ms` (1/h)) $ interpolateXDots cijs (sx0 TV.<| sxs) sxs :: Vec deg (J sx MX) szs :: Vec deg (J sz MX) @@ -299,7 +298,7 @@ continuousToDiscreetNoiseApprox :: (View sx, View sw) - => M sx sx MX -> M sx sw MX -> J (Cov sw) MX -> J (JV Id) MX -> M sx sx MX + => M sx sx MX -> M sx sw MX -> J (Cov sw) MX -> S MX -> M sx sx MX continuousToDiscreetNoiseApprox _dsx1_dsx0 dsx1_dsw0 qs h = qd where -- Qs' = G * Qs * G.T @@ -313,8 +312,8 @@ propOneCov :: forall sx sw . (View sx, View sw) - => (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 + => (M sx sx MX -> M sx sw MX -> J (Cov sw) MX -> S MX -> M sx sx MX) + -> (M sx sx :*: M sx sw :*: J (Cov sx) :*: J (Cov sw) :*: S) MX -> J (Cov sx) MX propOneCov c2d (dsx1_dsx0 :*: dsx1_dsw0 :*: p0 :*: qs :*: h) = fromMat p1 where @@ -329,17 +328,17 @@ . (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 (JV Id) :*: J p :*: J (JVec deg (JV Id)) :*: J x :*: J (JVec deg (CollPoint x z u))) MX + -> (S :*: 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 where sx0 :: J sx MX - sx0 = M.uncol M.zeros + sx0 = M.zeros sw0 :: J sw MX - sw0 = M.uncol M.zeros + sw0 = M.zeros sxzs :: J (JVec deg (JTuple sx sz)) MX - sxzs = M.uncol M.zeros + sxzs = M.zeros mat :: M.M (ErrorOut sr sx deg) (ErrorInD sx sw sz deg) MX Jac mat _ _ = @@ -354,8 +353,8 @@ 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 (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)) + [[x00,x01,x02],[x10,x11,x12]] -> ((mkM x00, mkM x01, mkM x02), + (mkM x10, mkM x11, mkM x12)) _ -> error "stageFunction: got wrong number of elements in jacobian" -- TODO: this should be much simpler for radau @@ -376,7 +375,7 @@ -> IO (J (JV shr) MX -> J (JV p) MX -> J (JV x) MX -> J (Cov (JV sx)) MX -> J (JV shr) MX) mkRobustifyFunction project robustifyPathC = do proj <- toSXFun "errorSpaceProjection" $ - \(JacIn x0 x1) -> JacOut (catJV' (project (splitJV' x1) (splitJV' x0))) (cat JNone) + \(JacIn x0 x1) -> JacOut (vcat (project (vsplit x1) (vsplit x0))) (cat JNone) let _ = proj :: SXFun (JacIn (JV sx) (J (JV x))) (JacOut (JV x) (J JNone)) @@ -386,16 +385,16 @@ (JacIn (JV sx) (J (JV x))) (Jac (JV sx) (JV x) (J JNone)) - let zerosx = (M.uncol M.zeros) :: J (JV sx) SX + let zerosx = M.zeros :: J (JV sx) SX simplifiedPropJac <- toSXFun "simplified error space projection jacobian" $ \x0 -> (\(Jac j0 _ _) -> j0) (callSX projJac (JacIn zerosx x0)) let _ = simplifiedPropJac :: SXFun (J (JV x)) (M.M (JV x) (JV sx)) - let rpc (JacIn xe parm) = JacOut (catJV' lol) (cat JNone) + let rpc (JacIn xe parm) = JacOut (vcat lol) (cat JNone) where - lol = robustifyPathC (splitJV' x) (splitJV' e) (splitJV' parm) + lol = robustifyPathC (vsplit x) (vsplit e) (vsplit parm) JTuple x e = split xe robustH <- toSXFun "robust constraint" rpc let _ = robustH :: SXFun @@ -410,7 +409,7 @@ srh (x :*: p) = ret where - xe = M.uncol M.zeros :: J (JV sx) SX + xe = M.zeros :: J (JV sx) SX xxe = cat (JTuple x xe) :: J (JTuple (JV x) (JV sx)) SX ret :: Jac (JTuple (JV x) (JV sx)) (JV shr) (J JNone) SX @@ -424,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 = splitJV' gammas' :: shr (J (JV Id) MX) + gammas = vsplit gammas' :: shr (S MX) jHx :: M (JV shr) (JV x) MX jHe :: M (JV shr) (JV sx) MX @@ -452,27 +451,26 @@ jHes :: shr (M.M (JV Id) (JV sx) MX) jHes = M.vsplit jHe - shr' = splitJV' h0vec :: shr (J (JV Id) MX) + shr' = vsplit h0vec :: shr (S MX) rcs' :: J (JV shr) MX - rcs' = catJV' rcs + rcs' = vcat rcs - rcs :: shr (J (JV Id) MX) + rcs :: shr (S MX) rcs = vzipWith4 robustify gammas shr' jHxs jHes - robustify :: J (JV Id) MX - -> J (JV Id) MX + robustify :: S MX + -> S MX -> M.M (JV Id) (JV x) MX -> M.M (JV Id) (JV sx) MX - -> J (JV Id) MX - robustify gamma h0 gHx gHe = h0 + gamma * sqrt (M.uncol sigma2) + -> S MX + robustify gamma h0 gHx gHe = h0 + gamma * sqrt sigma2 where - sigma2 :: M.M (JV Id) (JV Id) MX + sigma2 :: S 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) - :: M.M (JV Id) (JV Id) MX retFun <- toMXFun "robust constraint violations" (\(x0 :*: x1 :*: x2 :*: x3) -> gogo x0 x1 x2 x3) -- >>= expandMXFun diff --git a/src/Dyno/DirectCollocation/ScaleFactors.hs b/src/Dyno/DirectCollocation/ScaleFactors.hs new file mode 100644 --- /dev/null +++ b/src/Dyno/DirectCollocation/ScaleFactors.hs @@ -0,0 +1,182 @@ +{-# OPTIONS_GHC -Wall #-} +{-# LANGUAGE ScopedTypeVariables #-} +{-# LANGUAGE FlexibleContexts #-} +{-# LANGUAGE DeriveFunctor #-} +{-# LANGUAGE DeriveFoldable #-} +{-# LANGUAGE DeriveTraversable #-} +{-# LANGUAGE DeriveGeneric #-} +{-# LANGUAGE PolyKinds #-} + +module Dyno.DirectCollocation.ScaleFactors + ( ScaleFactors(..), ScaleFactors', ScaleFactor(..) + , getScaleFactors, summarizeScaleFactors + ) where + +import GHC.Generics ( Generic, Generic1 ) + +import Control.Lens ( (.~) ) +import Data.Maybe ( catMaybes, fromMaybe ) +import Data.Serialize ( Serialize ) +import qualified Data.Foldable as F +import qualified Data.Traversable as T +import Data.Vector ( Vector ) +import Text.Printf ( printf ) + +import Dyno.DirectCollocation.Types +import Dyno.Nlp ( Bounds ) +import Dyno.Vectorize ( Vectorize(..), Id(..), fill ) +import Dyno.View.View ( View(..), splitJV ) +import Dyno.View.JVec ( unJVec ) +import Dyno.TypeVecs ( Dim ) +import Dyno.Ocp + +import Accessors ( Lookup, Field(..), accessors, describeField, flatten ) + +data ScaleFactor = + ScaleFactor + { sfMyScale :: Double + , sfBounds :: Bounds + , sfMagnitude :: Double + , sfRelDiff :: Double + , sfName :: String + } deriving Generic +instance Serialize ScaleFactor + +type ScaleFactors' ocp = ScaleFactors (X ocp) (Z ocp) (U ocp) (P ocp) (H ocp) (C ocp) + +data ScaleFactors x z u p h c a = + ScaleFactors + { xScale :: x a + , zScale :: z a + , uScale :: u a + , pScale :: p a + , pathConstraintScale :: h a + , boundaryConditionScale :: c a + , endTimeScale :: a + } deriving (Functor, F.Foldable, T.Traversable, Generic, Generic1) +instance ( Lookup (x a), Lookup (z a), Lookup (u a), Lookup (p a) + , Lookup (h a), Lookup (c a), Lookup a + ) => Lookup (ScaleFactors x z u p h c a) +instance ( Serialize (x a), Serialize (z a), Serialize (u a), Serialize (p a) + , Serialize (h a), Serialize (c a), Serialize a + ) => Serialize (ScaleFactors x z u p h c a) +instance ( Vectorize x, Vectorize z, Vectorize u, Vectorize p + , Vectorize h, Vectorize c + ) => Vectorize (ScaleFactors x z u p h c) + +summarizeScaleFactors :: + ( Vectorize x, Vectorize z, Vectorize u, Vectorize p, Vectorize h, Vectorize c + ) => ScaleFactors x z u p h c ScaleFactor -> Double -> String +summarizeScaleFactors sfs fracThreshold = + case catMaybes (map report (F.toList (vectorize sfs))) of + [] -> "" + xs -> unlines $ " ratio scale magnitude name" : xs + where + report :: ScaleFactor -> Maybe String + report (ScaleFactor {sfBounds = (Just 0, Just 0)}) = Nothing + report sf + | ratio < fracThreshold && 1/ratio < fracThreshold = Nothing + | otherwise = Just $ printf "%6.2g %9.2g %9.2g %s" + ratio (sfMyScale sf) (sfMagnitude sf) (sfName sf) + where + ratio = sfMyScale sf / sfMagnitude sf + +-- | get scale factors based on the largest magnitude of each type over a trajectory +getScaleFactors :: + forall x z u p h c n deg r fp o q qo po + . ( Vectorize x, Vectorize z, Vectorize u, Vectorize p, Vectorize h, Vectorize c, Vectorize r + , Applicative x, Applicative z, Applicative u, Applicative p, Applicative h, Applicative c + , Lookup (x String), Lookup (z String), Lookup (u String), Lookup (p String) + , Lookup (h String), Lookup (c String) + , Dim n, Dim deg + ) + => CollTraj x z u p n deg (Vector Double) + -> CollOcpConstraints x r c h n deg (Vector Double) + -> OcpPhase x z u p r o c h q qo po fp + -> OcpPhaseInputs x z u p c h fp + -> ScaleFactors x z u p h c ScaleFactor +getScaleFactors x g ocp inputs = + getScaleFactor <$> myScale <*> bounds <*> magnitude <*> names + where + getScaleFactor :: Double -> Bounds -> Double -> String -> ScaleFactor + getScaleFactor myscale' bounds' magnitude' name' = + ScaleFactor + { sfMyScale = myscale' + , sfBounds = bounds' + , sfMagnitude = magnitude' + , sfRelDiff = abs (magnitude' - myscale') / (0.5 * (magnitude' + myscale')) + , sfName = name' + } + + magnitude :: ScaleFactors x z u p h c Double + magnitude = getMagnitude x g + + myScale :: ScaleFactors x z u p h c Double + myScale = + ScaleFactors + { xScale = fromMaybe (fill 1) (ocpXScale ocp) + , zScale = fromMaybe (fill 1) (ocpZScale ocp) + , uScale = fromMaybe (fill 1) (ocpUScale ocp) + , pScale = fromMaybe (fill 1) (ocpPScale ocp) + , pathConstraintScale = fromMaybe (fill 1) (ocpPathCScale ocp) + , boundaryConditionScale = fromMaybe (fill 1) (ocpBcScale ocp) + , endTimeScale = fromMaybe 1 (ocpTScale ocp) + } + bounds :: ScaleFactors x z u p h c Bounds + bounds = + ScaleFactors + { xScale = ocpXbnd inputs + , zScale = ocpZbnd inputs + , uScale = ocpUbnd inputs + , pScale = ocpPbnd inputs + , pathConstraintScale = ocpPathCBnds inputs + , boundaryConditionScale = ocpBcBnds inputs + , endTimeScale = ocpTbnd inputs + } + + names :: ScaleFactors x z u p h c String + names = F.foldl' ff (fill "") (flatten accessors) + where + ff sf0 (name, FieldString f) = (f .~ name) sf0 + ff _ (name, f) = + error $ "the 'impossible' happened, got a non-strong getter for " + ++ show name ++ " with type " ++ describeField f + +getMagnitude :: + forall x z u p h c n deg r + . ( Vectorize x, Vectorize z, Vectorize u, Vectorize p, Vectorize h, Vectorize c + , Applicative x, Applicative z, Applicative u, Applicative p, Applicative h, Applicative c + , Dim n, Dim deg + ) + => CollTraj x z u p n deg (Vector Double) + -> CollOcpConstraints x r c h n deg (Vector Double) + -> ScaleFactors x z u p h c Double +getMagnitude traj@(CollTraj tf' p' _ _) g = + ScaleFactors + { xScale = getMagnitude' xs + , zScale = getMagnitude' zs + , uScale = getMagnitude' us + , pScale = p + , pathConstraintScale = getMagnitude' pathC + , boundaryConditionScale = bc + , endTimeScale = tf + } + where + getMagnitude' :: forall f . Applicative f => [f Double] -> f Double + getMagnitude' fs = fmap maximum (sequenceA fs) + + bc :: c Double + bc = splitJV (coBc g) + + pathC :: [h Double] + pathC = concatMap (map splitJV . F.toList . unJVec . split) $ F.toList $ unJVec $ split (coPathC g) + + ((xs',xf), zs', us') = getXzus''' traj + xs :: [x Double] + xs = map (fmap abs) $ concatMap (\(x0,xss) -> x0 : F.toList xss) (F.toList xs') ++ [xf] + zs :: [z Double] + zs = map (fmap abs) $ concatMap F.toList (F.toList zs') + us :: [u Double] + us = map (fmap abs) $ concatMap F.toList (F.toList us') + p = fmap abs (splitJV p') + tf = abs $ unId (splitJV tf') 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 @@ -42,14 +42,13 @@ import Data.Vector ( Vector ) import Data.Serialize ( Serialize ) +import Casadi.Viewable ( Viewable ) import Accessors ( Lookup ) import Dyno.Ocp -import Dyno.View.Viewable ( Viewable ) -import Dyno.View.View ( View(..), J, jfill ) +import Dyno.View.View ( View(..), J, S, JV, splitJV, catJV, jfill ) import Dyno.View.JVec ( JVec(..), jreplicate ) import Dyno.View.Cov ( Cov ) -import Dyno.View.JV ( JV, splitJV, catJV ) import Dyno.Vectorize ( Vectorize(..), Id(..) ) import Dyno.TypeVecs ( Vec ) import qualified Dyno.TypeVecs as TV @@ -61,15 +60,15 @@ -- design variables data CollTraj x z u p n deg a = CollTraj - { ctTf :: J (JV Id) a + { ctTf :: S a , ctP :: J (JV p) a , ctStages :: J (JVec n (CollStage (JV x) (JV z) (JV u) deg)) a , ctXf :: J (JV x) a } deriving (Eq, Generic, Show) -- design variables -data CollTrajCov sx ocp n deg a = - CollTrajCov (J (Cov (JV sx)) a) (J (CollTraj' ocp n deg) a) +data CollTrajCov sx x z u p n deg a = + CollTrajCov (J (Cov (JV sx)) a) (J (CollTraj x z u p n deg) a) deriving (Eq, Generic, Show) data CollStage x z u deg a = @@ -110,10 +109,9 @@ instance ( Vectorize x, Vectorize z, Vectorize u, Vectorize p , Dim n, Dim deg ) => View (CollTraj x z u p n deg) -instance ( Vectorize (X ocp), Vectorize (Z ocp), Vectorize (U ocp), Vectorize (P ocp) - , Vectorize sx +instance ( Vectorize sx, Vectorize x, Vectorize z, Vectorize u, Vectorize p , Dim n, Dim deg - ) => View (CollTrajCov sx ocp n deg) + ) => View (CollTrajCov sx x z u p n deg) instance (Vectorize x, Vectorize r, Dim deg) => View (CollStageConstraints x deg r) instance ( Vectorize x, Vectorize r, Vectorize c, Vectorize h @@ -266,7 +264,7 @@ fmapCollTraj' fx' fx fz fu fp ft (CollTraj tf1 p stages1 xf) = CollTraj tf2 (fj fp p) stages2 (fj fx' xf) where - tf2 :: J (JV Id) (Vector b) + tf2 :: S (Vector b) tf2 = catJV $ fmap ft (splitJV tf1) stages2 = cat $ fmapJVec (fmapStage fx' fx fz fu) (split stages1) diff --git a/src/Dyno/ExportCStruct.hs b/src/Dyno/ExportCStruct.hs new file mode 100644 --- /dev/null +++ b/src/Dyno/ExportCStruct.hs @@ -0,0 +1,242 @@ +-- todo(greg): +-- a better name for this module would be Dyno.TechnicalDebt +{-# OPTIONS_GHC -Wall #-} +{-# LANGUAGE ScopedTypeVariables #-} +{-# LANGUAGE GADTs #-} + +module Dyno.ExportCStruct + ( CStructExporter + , runCStructExporter + , putTypedef + , exportTypedef + , exportCData + , exportNames + ) where + +import Control.Lens ( (^.) ) +import Control.Monad.State.Lazy ( State ) +import qualified Control.Monad.State.Lazy as State +import qualified Data.Map as M +import Data.List +import Data.Proxy ( Proxy(..) ) +import Text.Printf ( printf ) +import Text.Read ( readMaybe ) + +import Accessors + ( Lookup, AccessorTree(..), Field(..) + , accessors, flatten, sameFieldType ) +import Dyno.Vectorize ( Vectorize, vlength ) + +runCStructExporter :: State CStructExporter a -> (a, String) +runCStructExporter action = + case State.runState action (CStructExporter (M.empty, [], [])) of + (ret, CStructExporter (_, typedefs, [])) -> (ret, unlines (intercalate [""] (reverse typedefs))) + (_, CStructExporter (_, _, stack)) -> + error $ "runCStructExporter: stack is not empty!!:\n" ++ unlines stack + +data CStructExporter = CStructExporter (M.Map String Fields, [[String]], [String]) + +data Fields where + Fields :: [(String, AccessorTree a)] -> Fields + +write :: String -> State CStructExporter () +write str = + State.modify $ + \(CStructExporter (set, typedefs, outs)) -> CStructExporter (set, typedefs, str: outs) + +typedefStruct :: String -> [(String, AccessorTree a)] -> State CStructExporter () +typedefStruct typeName fields = do + write "typedef struct {" + mapM_ (uncurry writeCField) fields + write $ "} " ++ typeName ++ ";" + + CStructExporter (set, typedefs, currentStack) <- State.get + State.put $ CStructExporter (set, reverse currentStack : typedefs, []) + +sameFields :: [(String, AccessorTree a)] -> [(String, AccessorTree b)] -> Bool +sameFields xs ys + | length xs /= length ys = False + | otherwise = all (uncurry same) (zip xs ys) + where + same (nx, fx) (ny, fy) = (nx == ny) && sameTree fx fy + +sameTree :: AccessorTree a -> AccessorTree b -> Bool +sameTree (Data (x0, x1) fx) (Data (y0, y1) fy) = x0 == y0 && x1 == y1 && sameFields fx fy +sameTree (Field fx) (Field fy) = sameFieldType fx fy +sameTree _ _ = False + +typedefStructIfMissing :: String -> [(String, AccessorTree a)] -> State CStructExporter String +typedefStructIfMissing typeName fields = do + CStructExporter (set0, typedefs, currentStack) <- State.get + case M.lookup typeName set0 of + -- haven't seen this type name yet, use it + Nothing -> do + State.put (CStructExporter (M.insert typeName (Fields fields) set0, typedefs, [])) + typedefStruct typeName fields + State.modify $ + \(CStructExporter (set, structs, _)) -> CStructExporter (set, structs, currentStack) + return typeName + -- have seen this type name already, check if it's the one we have seen already + Just (Fields fields0) + -- yeah it's the one we have seen already + | sameFields fields0 fields -> return typeName + -- uh oh, same name but different type, lets modify the name + | otherwise -> typedefStructIfMissing newTypeName fields + where + -- ideally this would sort types by complexity and give simple ones simple names + newTypeName = typeName ++ "_" +-- newTypeName = +-- error $ +-- printf "got two types with the same name: %s\nnew type:\n%s\nold type:\n%s\n" +-- (show typeName) +-- (show fields) +-- (show fields0) + + +parseVecName :: String -> Maybe Int +parseVecName ('V':'e':'c':' ':k) = readMaybe k +parseVecName _ = Nothing + +writeCField :: String -> AccessorTree a -> State CStructExporter () +writeCField fieldName (Field f) = + write $ printf " %s %s;" (primitiveName f) fieldName +writeCField fieldName (Data (typeName0, _) fields) = case parseVecName typeName0 of + Nothing -> do + typeName <- typedefStructIfMissing typeName0 fields + write $ printf " %s %s;" typeName fieldName + Just k -> do -- handle Vecs as arrays + childtype <- case fields of + [] -> error "writeCField: Vec child has no children" + ((_, Field f):_) -> return (primitiveName f) + ((_, Data (typeName0',_) childfields):_) -> + typedefStructIfMissing typeName0' childfields + write $ printf " %s %s[%d];" childtype fieldName k + + +primitiveName :: Field a -> String +primitiveName (FieldDouble _) = "double" +primitiveName (FieldInt _ ) = "int64_t" +primitiveName (FieldFloat _ ) = "float" +primitiveName (FieldString _) = error "writeCField: strings can't be struct fields :(" +primitiveName (FieldBool _ ) = error "writeCField: bools can't be struct fields :(" +primitiveName FieldSorry = + error "writeCField: found a GetSorry (generic-accessors doesn't support a type)" + + + +-- | convenience function to export only one struct +putTypedef :: forall a . Lookup a => Proxy a -> State CStructExporter String +putTypedef _ = + case handleM33 (accessors :: AccessorTree a) of + (Data (typeName, _) fields) -> typedefStructIfMissing typeName fields + (Field _) -> error "putStruct: accessors got Field instead of Data" + +-- | convenience function to export only one struct +exportTypedef :: Lookup a => Proxy a -> String +exportTypedef = snd . runCStructExporter . putTypedef + +-- | Export data as a C struct. +-- If a string with a variable name is given, the variable is declared. +exportCData :: forall a . Lookup a => Int -> Maybe String -> a -> String +exportCData spaces0 maybeVarName theData = case (acc, maybeVarName) of + (Data (typeName,_) fields, Nothing) -> exportStructData theData typeName spaces fields + (Data (typeName,_) fields, Just varName) -> + printf "%s%s %s = {\n%s;" spaces typeName varName + (exportStructData theData typeName (spaces ++ " ") fields) + (Field _, _) -> error "exportStructData: accessors got Field instead of Data" + where + spaces = replicate spaces0 ' ' + acc = handleM33 accessors + +handleM33 :: AccessorTree a -> AccessorTree a +handleM33 r@(Field _) = r +handleM33 (Data ("V3","V3") + [ ("x", Data ("V3","V3") [ ("x", Field field0) + , ("y", Field field1) + , ("z", Field field2) + ]) + , ("y", Data ("V3","V3") [ ("x", Field field3) + , ("y", Field field4) + , ("z", Field field5) + ]) + , ("z", Data ("V3","V3") [ ("x", Field field6) + , ("y", Field field7) + , ("z", Field field8) + ]) + ]) = r + where + r = Data ("M33","M33") + [ ("xx", Field field0) + , ("xy", Field field1) + , ("xz", Field field2) + , ("yx", Field field3) + , ("yy", Field field4) + , ("yz", Field field5) + , ("zx", Field field6) + , ("zy", Field field7) + , ("zz", Field field8) + ] +handleM33 (Data name fields) = Data name $ map (\(n,at) -> (n, handleM33 at)) fields + +exportStructData :: forall a . a -> String -> String -> [(String, AccessorTree a)] -> String +exportStructData theData comment spaces fields = + spaces ++ intercalate (",\n"++spaces) (map exportField' fields) + ++ "\n"++ spaces ++ "} /* " ++ comment ++ " */" + where + exportField' :: (String, AccessorTree a) -> String + exportField' (n,t) = exportField theData (Just n) spaces t + +toString :: a -> Field a -> String +toString theData (FieldDouble f) = case show (theData ^. f) of + "Infinity" -> "INFINITY" + "-Infinity" -> "-INFINITY" + r -> r +toString theData (FieldFloat f) = show (theData ^. f) +toString theData (FieldInt f) = show (theData ^. f) +toString theData (FieldBool f) = show (fromEnum (theData ^. f)) +toString _ (FieldString _) = "NAN" +toString _ FieldSorry = "NAN" + +exportField :: a -> Maybe String -> String -> AccessorTree a -> String +exportField theData (Just fieldName) _ (Field f) = + printf ".%s = %s" fieldName (toString theData f) +exportField theData Nothing _ (Field f) = toString theData f +exportField theData mfieldName spaces (Data (typeName,_) subfields) = + nameEq ++ "\n" ++ fields + where + nameEq = case mfieldName of + Just fieldName -> printf ".%s = {" fieldName + Nothing -> "{" + + comment = case mfieldName of + Just fieldName -> fieldName ++ " (" ++ typeName ++ ")" + Nothing -> typeName + + fields :: String + fields = case parseVecName typeName of + Nothing -> exportStructData theData comment (spaces ++ " ") subfields + Just _ -> + spaces ++ " " + ++ intercalate (",\n"++(spaces++" ")) + (map (exportField theData Nothing (spaces++" ") . snd) subfields) + ++ "\n"++spaces++" } /* " ++ comment ++ " */" + +exportNames :: forall f . (Vectorize f, Lookup (f ())) => Proxy f -> String -> (String, String) +exportNames _ functionName = (src, prototype) + where + src = + unlines + [ prototype ++ " {" + , printf " static const char names[%d][%d] = {%s};" + n maxLen (intercalate "," (map show names)) + , " return names[k];" + , "}" + ] + prototype = printf "const char * %s(const int k)" functionName + maxLen = 1 + maximum (map length names) + n + | length names == vlength (Proxy :: Proxy f) = vlength (Proxy :: Proxy f) + | otherwise = error "exportNames: length mismatch" + names :: [String] + names = map (\(name, _) -> name) $ flatten $ + handleM33 (accessors :: AccessorTree (f ())) diff --git a/src/Dyno/Fitting.hs b/src/Dyno/Fitting.hs new file mode 100644 --- /dev/null +++ b/src/Dyno/Fitting.hs @@ -0,0 +1,475 @@ +{-# OPTIONS_GHC -Wall #-} +{-# LANGUAGE DeriveGeneric #-} +{-# LANGUAGE TypeOperators #-} +{-# LANGUAGE ScopedTypeVariables #-} +{-# LANGUAGE RankNTypes #-} +{-# LANGUAGE PolyKinds #-} + +module Dyno.Fitting + ( l1Fit, l1Fits, withL1Fit + , l2Fit, l2Fits, withL2Fit + , lInfFit, lInfFits, withLInfFit + , L1X(..), GSlacks(..) + ) where + +import GHC.Generics ( Generic ) + +import Casadi.MX ( MX ) +import Casadi.Option ( Opt(..) ) +import Casadi.Overloading ( ArcTan2 ) +import qualified Data.Map as M +import Data.Vector ( Vector ) +import Data.Proxy ( Proxy(..) ) + +import Dyno.Nlp ( Bounds, NlpOut(..) ) +import Dyno.NlpSolver ( NlpSolver, withNlpSolver ) +import Dyno.Solvers ( Solver ) +import Dyno.Vectorize ( Vectorize, Id(..) ) +import Dyno.TypeVecs ( Dim, Vec ) +import qualified Dyno.TypeVecs as TV +import Dyno.View.Fun ( Fun, SXFun, call, toSXFun ) +import Dyno.View.HList ( (:*:)(..) ) +import Dyno.View.JVec ( JVec(..) ) +import Dyno.View.M ( M, reshape, sumRows, trans, vcat, vsplit ) +import Dyno.View.MapFun ( mapFun' ) +import Dyno.View.View ( J, S, View(..), JTuple(..), JV, catJV, splitJV, jfill) + +data L1X q n a = + L1X (J (JV q) a) (J (JVec n (JV Id)) a) + deriving Generic +instance (Vectorize q, Dim n) => View (L1X q n) + +data GSlacks g n a = + GSlacks (J (JV g) a) (J (JVec n (JV Id)) a) (J (JVec n (JV Id)) a) + deriving Generic +instance (Vectorize g, Dim n) => View (GSlacks g n) + + +-- | Minimize the L1 norm of model mismatch. +-- +-- > minimize: || f(x_k, q) - y_k ||_1 +-- > q +-- > +-- > subject to: qlb <= q <= qub +-- > glb <= g(q) <= gub +-- +-- reformulated as: +-- +-- > minimize: Sum(s_k) +-- > q, s_k +-- > subject to: qlb <= q <= qub +-- > glb <= g(q) <= gub +-- > y_x - f(x_k) - s_k <= 0 +-- > 0 <= y_x - f(x_k) + s_k +-- +-- where q is the parameter vector, x_k are features, y_k are data, +-- and g is a nonlinear constraint on the parameters. +l1Fit :: + forall n q g x + . (Vectorize q, Vectorize g, Vectorize x, Dim n) + => Solver + -> (forall a . (Floating a, ArcTan2 a) => q a -> x a -> a) + -> (forall a . (Floating a, ArcTan2 a) => q a -> g a) + -> Maybe (q Double) -> q Bounds -> g Bounds -> M.Map String Opt + -> Vec n (x Double, Double) -> IO (Either String (q Double)) +l1Fit solver fitModel qConstraints mq0 qbnds gbnds mapOpts featuresData = + unId <$> l1Fits solver fitModel qConstraints mapOpts (Id input) + where + input :: (Maybe (q Double), q Bounds, g Bounds, Vec n (x Double, Double)) + input = (mq0, qbnds, gbnds, featuresData) + + +-- | Solve multiple L1 fitting problems with the same structure. +-- This is equivilent to but more efficient than calling +-- 'l1Fit' many times. +l1Fits :: + forall n q g x t + . (Vectorize q, Vectorize g, Vectorize x, Traversable t, Dim n) + => Solver + -> (forall a . (Floating a, ArcTan2 a) => q a -> x a -> a) + -> (forall a . (Floating a, ArcTan2 a) => q a -> g a) + -> M.Map String Opt + -> t (Maybe (q Double), q Bounds, g Bounds, Vec n (x Double, Double)) + -> IO (t (Either String (q Double))) +l1Fits solver fitModel qConstraints mapOpts inputs = + withL1Fit solver fitModel qConstraints mapOpts (\fit -> mapM fit inputs) + + +-- | Low level interface to L1 fitting. +withL1Fit :: + forall n q g x b + . (Vectorize q, Vectorize g, Vectorize x, Dim n) + => Solver + -> (forall a . (Floating a, ArcTan2 a) => q a -> x a -> a) + -> (forall a . (Floating a, ArcTan2 a) => q a -> g a) + -> M.Map String Opt + -> (((Maybe (q Double), q Bounds, g Bounds, Vec n (x Double, Double)) + -> NlpSolver (L1X q n) + (JTuple (JVec n (JV x)) (JVec n (JV Id))) + (GSlacks g n) + (Either String (q Double)) + ) -> NlpSolver (L1X q n) + (JTuple (JVec n (JV x)) (JVec n (JV Id))) + (GSlacks g n) + b + ) -> IO b +withL1Fit solver fitModel qConstraints mapOpts userFun = do + let fitModel' (q :*: x :*: y :*: s) = f - y + s + where + f = vcat $ Id (fitModel (vsplit q) (vsplit x)) + + fitModelFun <- toSXFun "fit_model" fitModel' + :: IO (SXFun + (J (JV q) :*: J (JV x) :*: S :*: S) + S + ) + + mapFitModel <- mapFun' (Proxy :: Proxy n) "map_fit_model" fitModelFun mapOpts + :: IO (Fun + (J (JV q) + :*: M (JV x) (JVec n (JV Id)) + :*: M (JV Id) (JVec n (JV Id)) + :*: M (JV Id) (JVec n (JV Id)) + ) + (M (JV Id) (JVec n (JV Id))) + ) + let fg :: J (L1X q n) MX + -> J (JTuple (JVec n (JV x)) (JVec n (JV Id))) MX + -> (S MX, J (GSlacks g n) MX) + fg dvs featuresData = (f, cat g) + where + fitFeatures :: J (JVec n (JV x)) MX + fitData :: J (JVec n (JV Id)) MX + JTuple fitFeatures fitData = split featuresData + + q :: J (JV q) MX + s' :: J (JVec n (JV Id)) MX + L1X q s' = split dvs + + s :: M (JV Id) (JVec n (JV Id)) MX + s = trans s' + + ys :: M (JV Id) (JVec n (JV Id)) MX + ys = trans fitData + + xs :: M (JV x) (JVec n (JV Id)) MX + xs = reshape fitFeatures + + gs0 :: J (JVec n (JV Id)) MX + gs0 = trans $ call mapFitModel (q :*: xs :*: ys :*: (-s)) + + gs1 :: J (JVec n (JV Id)) MX + gs1 = trans $ call mapFitModel (q :*: xs :*: ys :*: s) + + f = sumRows s' + + g :: GSlacks g n MX + g = GSlacks (vcat (qConstraints (vsplit q))) gs0 gs1 + + let action solveOne = userFun solveOne' + where + solveOne' :: (Maybe (q Double), q Bounds, g Bounds, Vec n (x Double, Double)) + -> NlpSolver + (L1X q n) + (JTuple (JVec n (JV x)) (JVec n (JV Id))) + (GSlacks g n) + (Either String (q Double)) + solveOne' (mq0, qbnds, gbnds', featuresData) = + fmap (fmap toSol) (solveOne x0 p xbnds gbnds) + where + toSol out = splitJV xopt + where + L1X xopt _ = split (xOpt out) + + p :: J (JTuple (JVec n (JV x)) (JVec n (JV Id))) (Vector Double) + p = cat $ JTuple fs' ds' + where + fitFeatures :: Vec n (x Double) + (fitFeatures, fitData) = TV.tvunzip featuresData + fs' = cat $ JVec $ fmap catJV fitFeatures + ds' = cat $ JVec $ fmap (catJV . Id) fitData + xbnds :: J (L1X q n) (Vector Bounds) + xbnds = cat $ L1X (catJV qbnds) (jfill (Nothing, Nothing)) + gbnds :: J (GSlacks g n) (Vector Bounds) + gbnds = cat $ GSlacks (catJV gbnds') + (jfill (Nothing, Just 0)) (jfill ((Just 0, Nothing))) + x0 :: J (L1X q n) (Vector Double) + x0 = case mq0 of + Nothing -> jfill 0 + Just q0 -> cat (L1X (catJV q0) (jfill 0)) + + withNlpSolver solver fg Nothing Nothing Nothing Nothing action + + +-- | Minimize the L2 norm of model mismatch. +-- +-- > minimize: 0.5 * || f(x_k, q) - y_k ||_2^2 +-- > q +-- > +-- > subject to: qlb <= q <= qub +-- > glb <= g(q) <= gub +-- +-- where q is the parameter vector, x_k are features, y_k are data, +-- and g is a nonlinear constraint on the parameters. +l2Fit :: + forall n q g x + . (Vectorize q, Vectorize g, Vectorize x, Dim n) + => Solver + -> (forall a . (Floating a, ArcTan2 a) => q a -> x a -> a) + -> (forall a . (Floating a, ArcTan2 a) => q a -> g a) + -> Maybe (q Double) -> q Bounds -> g Bounds -> M.Map String Opt + -> Vec n (x Double, Double) -> IO (Either String (q Double)) +l2Fit solver fitModel qConstraints mq0 qbnds gbnds mapOpts featuresData = do + unId <$> l2Fits solver fitModel qConstraints mapOpts (Id input) + where + input :: (Maybe (q Double), q Bounds, g Bounds, Vec n (x Double, Double)) + input = (mq0, qbnds, gbnds, featuresData) + + +-- | Solve multiple L2 fitting problems with the same structure. +-- This is equivilent to but more efficient than calling +-- 'l2Fit' many times. +l2Fits :: + forall n q g x t + . (Vectorize q, Vectorize g, Vectorize x, Traversable t, Dim n) + => Solver + -> (forall a . (Floating a, ArcTan2 a) => q a -> x a -> a) + -> (forall a . (Floating a, ArcTan2 a) => q a -> g a) + -> M.Map String Opt + -> t (Maybe (q Double), q Bounds, g Bounds, Vec n (x Double, Double)) + -> IO (t (Either String (q Double))) +l2Fits solver fitModel qConstraints mapOpts inputs = + withL2Fit solver fitModel qConstraints mapOpts (\fit -> mapM fit inputs) + + +-- | Low level interface to L2 fitting. +withL2Fit :: + forall n q g x b + . (Vectorize q, Vectorize g, Vectorize x, Dim n) + => Solver + -> (forall a . (Floating a, ArcTan2 a) => q a -> x a -> a) + -> (forall a . (Floating a, ArcTan2 a) => q a -> g a) + -> M.Map String Opt + -> (((Maybe (q Double), q Bounds, g Bounds, Vec n (x Double, Double)) + -> NlpSolver (JV q) + (JTuple (JVec n (JV x)) (JVec n (JV Id))) + (JV g) + (Either String (q Double)) + ) -> NlpSolver (JV q) + (JTuple (JVec n (JV x)) (JVec n (JV Id))) + (JV g) + b + ) -> IO b +withL2Fit solver fitModel qConstraints mapOpts userFun = do + let fitModel' (q :*: x :*: y) = err * err + where + err = f - y + f = vcat $ Id (fitModel (vsplit q) (vsplit x)) + fitModelFun <- toSXFun "fit_model" fitModel' + :: IO (SXFun (J (JV q) :*: J (JV x) :*: S) S) + + mapFitModel <- mapFun' (Proxy :: Proxy n) "map_fit_model" fitModelFun mapOpts + :: IO (Fun + (J (JV q) + :*: M (JV x) (JVec n (JV Id)) + :*: M (JV Id) (JVec n (JV Id)) + ) + S + ) + let fg :: J (JV q) MX -> J (JTuple (JVec n (JV x)) (JVec n (JV Id))) MX + -> (S MX, J (JV g) MX) + fg q featuresData = (0.5 * f, g) + where + fitFeatures :: J (JVec n (JV x)) MX + fitData :: J (JVec n (JV Id)) MX + JTuple fitFeatures fitData = split featuresData + + -- fit data + ys :: M (JV Id) (JVec n (JV Id)) MX + ys = trans fitData + + -- fit features + xs :: M (JV x) (JVec n (JV Id)) MX + xs = reshape fitFeatures + + -- objective function + f :: S MX + f = call mapFitModel (q :*: xs :*: ys) + + -- nonlinear parameter constraints + g :: J (JV g) MX + g = vcat (qConstraints (vsplit q)) + + let action solveOne = userFun solveOne' + where + solveOne' :: (Maybe (q Double), q Bounds, g Bounds, Vec n (x Double, Double)) + -> NlpSolver + (JV q) + (JTuple (JVec n (JV x)) (JVec n (JV Id))) + (JV g) + (Either String (q Double)) + solveOne' (mq0, qbnds, gbnds', featuresData) = + fmap (fmap (splitJV . xOpt)) (solveOne x0 p xbnds gbnds) + where + p :: J (JTuple (JVec n (JV x)) (JVec n (JV Id))) (Vector Double) + p = cat $ JTuple fs' ds' + where + fitFeatures :: Vec n (x Double) + (fitFeatures, fitData) = TV.tvunzip featuresData + fs' = cat $ JVec $ fmap catJV fitFeatures + ds' = cat $ JVec $ fmap (catJV . Id) fitData + xbnds = catJV qbnds + gbnds = catJV gbnds' + x0 = case mq0 of + Nothing -> jfill 0 + Just q0 -> catJV q0 + withNlpSolver solver fg Nothing Nothing Nothing Nothing action + + +-- | Minimize the L-infinity norm of model mismatch. +-- +-- > minimize: || f(x_k, q) - y_k ||_inf +-- > q +-- > +-- > subject to: qlb <= q <= qub +-- > glb <= g(q) <= gub +-- +-- reformulated as: +-- +-- > minimize: s +-- > q, s +-- > subject to: qlb <= q <= qub +-- > glb <= g(q) <= gub +-- > y_x - f(x_k) - s <= 0 +-- > 0 <= y_x - f(x_k) + s +-- +-- where q is the parameter vector, x_k are features, y_k are data, +-- and g is a nonlinear constraint on the parameters. +lInfFit :: + forall n q g x + . (Vectorize q, Vectorize g, Vectorize x, Dim n) + => Solver + -> (forall a . (Floating a, ArcTan2 a) => q a -> x a -> a) + -> (forall a . (Floating a, ArcTan2 a) => q a -> g a) + -> Maybe (q Double) -> q Bounds -> g Bounds -> M.Map String Opt + -> Vec n (x Double, Double) -> IO (Either String (q Double)) +lInfFit solver fitModel qConstraints mq0 qbnds gbnds mapOpts featuresData = + unId <$> lInfFits solver fitModel qConstraints mapOpts (Id input) + where + input :: (Maybe (q Double), q Bounds, g Bounds, Vec n (x Double, Double)) + input = (mq0, qbnds, gbnds, featuresData) + + +-- | Solve multiple L-infinity fitting problems with the same structure. +-- This is equivilent to but more efficient than calling +-- 'lInfFit' many times. +lInfFits :: + forall n q g x t + . (Vectorize q, Vectorize g, Vectorize x, Traversable t, Dim n) + => Solver + -> (forall a . (Floating a, ArcTan2 a) => q a -> x a -> a) + -> (forall a . (Floating a, ArcTan2 a) => q a -> g a) + -> M.Map String Opt + -> t (Maybe (q Double), q Bounds, g Bounds, Vec n (x Double, Double)) + -> IO (t (Either String (q Double))) +lInfFits solver fitModel qConstraints mapOpts inputs = do + withLInfFit solver fitModel qConstraints mapOpts (\fit -> mapM fit inputs) + + +-- | Low-level interface to L-infinity fitting. +withLInfFit :: + forall n q g x b + . (Vectorize q, Vectorize g, Vectorize x, Dim n) + => Solver + -> (forall a . (Floating a, ArcTan2 a) => q a -> x a -> a) + -> (forall a . (Floating a, ArcTan2 a) => q a -> g a) + -> M.Map String Opt + -> (((Maybe (q Double), q Bounds, g Bounds, Vec n (x Double, Double)) + -> NlpSolver (JTuple (JV q) (JV Id)) + (JTuple (JVec n (JV x)) (JVec n (JV Id))) + (GSlacks g n) + (Either String (q Double)) + ) -> NlpSolver (JTuple (JV q) (JV Id)) + (JTuple (JVec n (JV x)) (JVec n (JV Id))) + (GSlacks g n) + b + ) -> IO b +withLInfFit solver fitModel qConstraints mapOpts userFun = do + let fitModel' (q :*: x :*: y :*: s) = f - y + s + where + f = vcat $ Id (fitModel (vsplit q) (vsplit x)) + + fitModelFun <- toSXFun "fit_model" fitModel' + :: IO (SXFun + (J (JV q) :*: J (JV x) :*: S :*: S) + S + ) + + mapFitModel <- mapFun' (Proxy :: Proxy n) "map_fit_model" fitModelFun mapOpts + :: IO (Fun + (J (JV q) + :*: M (JV x) (JVec n (JV Id)) + :*: M (JV Id) (JVec n (JV Id)) + :*: S + ) + (M (JV Id) (JVec n (JV Id))) + ) + + let fg :: J (JTuple (JV q) (JV Id)) MX + -> J (JTuple (JVec n (JV x)) (JVec n (JV Id))) MX + -> (S MX, J (GSlacks g n) MX) + fg dvs featuresData = (s, cat g) + where + fitFeatures :: J (JVec n (JV x)) MX + fitData :: J (JVec n (JV Id)) MX + JTuple fitFeatures fitData = split featuresData + + q :: J (JV q) MX + s :: S MX + JTuple q s = split dvs + + ys :: M (JV Id) (JVec n (JV Id)) MX + ys = trans fitData + + xs :: M (JV x) (JVec n (JV Id)) MX + xs = reshape fitFeatures + + gs0 :: J (JVec n (JV Id)) MX + gs0 = trans $ call mapFitModel (q :*: xs :*: ys :*: (-s)) + + gs1 :: J (JVec n (JV Id)) MX + gs1 = trans $ call mapFitModel (q :*: xs :*: ys :*: s) + + g :: GSlacks g n MX + g = GSlacks (vcat (qConstraints (vsplit q))) gs0 gs1 + + let action solveOne = userFun solveOne' + where + solveOne' :: (Maybe (q Double), q Bounds, g Bounds, Vec n (x Double, Double)) + -> NlpSolver + (JTuple (JV q) (JV Id)) + (JTuple (JVec n (JV x)) (JVec n (JV Id))) + (GSlacks g n) + (Either String (q Double)) + solveOne' (mq0, qbnds, gbnds', featuresData) = + fmap (fmap toSol) (solveOne x0 p xbnds gbnds) + where + toSol out = splitJV xopt + where + JTuple xopt _ = split (xOpt out) + + p :: J (JTuple (JVec n (JV x)) (JVec n (JV Id))) (Vector Double) + p = cat $ JTuple fs' ds' + where + fitFeatures :: Vec n (x Double) + (fitFeatures, fitData) = TV.tvunzip featuresData + fs' = cat $ JVec $ fmap catJV fitFeatures + ds' = cat $ JVec $ fmap (catJV . Id) fitData + xbnds = cat $ JTuple (catJV qbnds) (catJV (Id (Nothing, Nothing))) + gbnds = cat $ GSlacks (catJV gbnds') + (jfill (Nothing, Just 0)) (jfill (Just 0, Nothing)) + x0 = case mq0 of + Nothing -> jfill 0 + Just q0 -> cat (JTuple (catJV q0) (catJV (Id 0))) + + withNlpSolver solver fg Nothing Nothing Nothing Nothing action diff --git a/src/Dyno/GoldenSectionSearch.hs b/src/Dyno/GoldenSectionSearch.hs new file mode 100644 --- /dev/null +++ b/src/Dyno/GoldenSectionSearch.hs @@ -0,0 +1,41 @@ +{-# OPTIONS_GHC -Wall #-} + +module Dyno.GoldenSectionSearch + ( Golden(..) + , goldenSectionSearch + , goldenSectionSearch' + ) where + +tau :: Floating a => a +tau = 2 / (1 + sqrt 5) + +data Golden a = + Golden + { goldenX :: a + , goldenY :: a + , goldenBox :: (a, a) + } deriving (Show, Eq, Ord) + +-- | Iterate a golden section search until the +-- bounding box is withing a given tolerance. +goldenSectionSearch :: (Ord a, Floating a) => a -> (a -> a) -> (a, a) -> (a, a) +goldenSectionSearch eps f bnds = g $ goldenSectionSearch' f bnds + where + g ((Golden x fx (lbx, ubx)):gs) + | ubx - lbx < eps = (x, fx) + | otherwise = g gs + g _ = error "goldenSectionSearch': hit the end of an infinite list" + +-- | Return an infinite list of the iterations of a golden section search. +goldenSectionSearch' :: (Ord a, Floating a) => (a -> a) -> (a, a) -> [Golden a] +goldenSectionSearch' f (y0, y3) = gss (y0, y1, y2, y3) + where + y1 = y0 + (y3 - y0) * (1 - tau) + y2 = y0 + (y3 - y0) * tau + + gss (x0, x1, x2, x3) + | f x1 < f x2 = Golden x1 (f x1) (x0, x2) : gss (x0, x1', x1, x2) + | otherwise = Golden x2 (f x2) (x1, x3) : gss (x1, x2, x2', x3) + where + x1' = x0 + (x2 - x0) * (1 - tau) + x2' = x1 + (x3 - x1) * tau diff --git a/src/Dyno/Linearize.hs b/src/Dyno/Linearize.hs new file mode 100644 --- /dev/null +++ b/src/Dyno/Linearize.hs @@ -0,0 +1,170 @@ +{-# OPTIONS_GHC -Wall #-} +{-# LANGUAGE ScopedTypeVariables #-} + +module Dyno.Linearize + ( OdeJacobian + , ErrorOdeJacobian + , makeOdeJacobian + , makeErrorOdeJacobian + , evalOdeJacobian + , evalErrorOdeJacobian + ) where + +import Dyno.Vectorize ( Vectorize(..), Triple(..), fill ) +import Dyno.View.View +import Dyno.View.M +import Dyno.View.Fun +import Dyno.View.FunJac + +import Casadi.SX ( SX ) +import Casadi.DMatrix ( DMatrix ) + +toOdeSX :: + (Vectorize x, Vectorize u, Vectorize w, Vectorize p, Vectorize sc, Vectorize o) + => (x (S SX) -> u (S SX) -> w (S SX) -> p (S SX) -> sc (S SX) + -> (x (S SX), o (S SX))) + -> JacIn (JQuad (JV x) (JV u) (JV w) (JV p)) (J (JV sc)) SX + -> JacOut (JTuple (JV x) (JV o)) (J JNone) SX +toOdeSX ode jacIn = jacOut + where + jacOut = JacOut (cat (JTuple (vcat dx) (vcat outputs))) (cat JNone) + JacIn xuwp sc = jacIn + JQuad x u w p = split xuwp + (dx, outputs) = + ode (vsplit x) (vsplit u) (vsplit w) (vsplit p) (vsplit sc) + +toErrorOdeSX :: + ( Vectorize x, Vectorize e, Vectorize u, Vectorize w + , Vectorize p, Vectorize sc, Vectorize o) + => (x (S SX) -> e (S SX) -> u (S SX) -> u (S SX) -> w (S SX) -> p (S SX) + -> sc (S SX) -> (e (S SX), o (S SX))) + -> JacIn (JQuad (JV e) (JV u) (JV w) (JV p)) (J (JV (Triple x u sc))) SX + -> JacOut (JTuple (JV e) (JV o)) (J JNone) SX +toErrorOdeSX errorOde jacIn = jacOut + where + jacOut = JacOut (cat (JTuple (vcat de) (vcat outputs))) (cat JNone) + JacIn euwp nominalInputs = jacIn + JQuad e du w p = split euwp + Triple fs0 u0 sc = vsplit nominalInputs + (de, outputs) = errorOde fs0 (vsplit e) u0 (vsplit du) (vsplit w) + (vsplit p) sc + +newtype OdeJacobian x u w p sc o = + OdeJacobian + (SXFun + (JacIn + (JQuad (JV x) (JV u) (JV w) (JV p)) + (J (JV sc))) + (Jac + (JQuad (JV x) (JV u) (JV w) (JV p)) + (JTuple (JV x) (JV o)) + (J JNone))) + +newtype ErrorOdeJacobian x e u w p sc o = + ErrorOdeJacobian + (SXFun + (JacIn + (JQuad (JV e) (JV u) (JV w) (JV p)) + (J (JV (Triple x u sc)))) + (Jac + (JQuad (JV e) (JV u) (JV w) (JV p)) + (JTuple (JV e) (JV o)) + (J JNone))) + +makeOdeJacobian :: + forall x u w p sc o + . (Vectorize x, Vectorize u, Vectorize w, Vectorize p, Vectorize sc, Vectorize o) + => (x (S SX) -> u (S SX) -> w (S SX) -> p (S SX) -> sc (S SX) + -> (x (S SX), o (S SX))) + -> IO (OdeJacobian x u w p sc o) +makeOdeJacobian ode = do + f <- toSXFun "odeSX" (toOdeSX ode) + fmap OdeJacobian (toFunJac f) + +makeErrorOdeJacobian :: + ( Vectorize x, Vectorize e, Vectorize u, Vectorize w + , Vectorize p, Vectorize sc, Vectorize o) + => (x (S SX) -> e (S SX) -> u (S SX) -> u (S SX) -> w (S SX) -> p (S SX) + -> sc (S SX) -> (e (S SX), o (S SX))) + -> IO (ErrorOdeJacobian x e u w p sc o) +makeErrorOdeJacobian errorOde = do + f <- toSXFun "errorOdeSX" (toErrorOdeSX errorOde) + fmap ErrorOdeJacobian (toFunJac f) + + +evalOdeJacobian :: + forall x u w p sc o + . ( Vectorize x, Vectorize u, Vectorize w + , Vectorize p, Vectorize o, Vectorize sc + ) + => OdeJacobian x u w p sc o + -> x Double + -> u Double + -> p Double + -> sc Double + -> IO ( M (JV x) (JV x) DMatrix + , M (JV x) (JV u) DMatrix + , M (JV x) (JV w) DMatrix + , M (JV x) (JV p) DMatrix + , M (JV o) (JV x) DMatrix + , M (JV o) (JV u) DMatrix + , M (JV o) (JV w) DMatrix + , M (JV o) (JV p) DMatrix + , J (JV x) DMatrix + , J (JV o) DMatrix + ) +evalOdeJacobian (OdeJacobian fj) x0 u0 p0 sc0 = do + let w = vcat (fill 0) + x = vcat (fmap realToFrac x0) + u = vcat (fmap realToFrac u0) + p = vcat (fmap realToFrac p0) + sc = vcat (fmap realToFrac sc0) + jacIn = JacIn (cat (JQuad x u w p)) sc + jacOut <- eval fj jacIn + let Jac dxo_dxup xo' _ = jacOut + (x',o) = vsplitTup xo' + (dxo_dx,dxo_du,dxo_dw,dxo_dp) = hsplitQuad dxo_dxup + (dx_dx, do_dx) = vsplitTup dxo_dx + (dx_du, do_du) = vsplitTup dxo_du + (dx_dw, do_dw) = vsplitTup dxo_dw + (dx_dp, do_dp) = vsplitTup dxo_dp + return (dx_dx, dx_du, dx_dw, dx_dp, do_dx, do_du, do_dw, do_dp, x', o) + + +evalErrorOdeJacobian :: + forall x e u w p sc o + . ( Vectorize x, Vectorize e, Vectorize u, Vectorize w + , Vectorize p, Vectorize o, Vectorize sc + ) + => ErrorOdeJacobian x e u w p sc o + -> x Double + -> u Double + -> p Double + -> sc Double + -> IO ( M (JV e) (JV e) DMatrix + , M (JV e) (JV u) DMatrix + , M (JV e) (JV w) DMatrix + , M (JV e) (JV p) DMatrix + , M (JV o) (JV e) DMatrix + , M (JV o) (JV u) DMatrix + , M (JV o) (JV w) DMatrix + , M (JV o) (JV p) DMatrix + , J (JV e) DMatrix + , J (JV o) DMatrix + ) +evalErrorOdeJacobian (ErrorOdeJacobian fj) x0 u0 p0 sc0 = do + let e = vcat (fill 0) + w = vcat (fill 0) + du = vcat (fill 0) + p = vcat (fmap realToFrac p0) + x0u0sc0 = vcat $ fmap realToFrac $ Triple x0 u0 sc0 + jacIn = JacIn (cat (JQuad e du w p)) x0u0sc0 + jacOut <- eval fj jacIn + let Jac dxo_dxup xo' _ = jacOut + (x',o) = vsplitTup xo' + (dxo_dx,dxo_du,dxo_dw,dxo_dp) = hsplitQuad dxo_dxup + (dx_dx, do_dx) = vsplitTup dxo_dx + (dx_du, do_du) = vsplitTup dxo_du + (dx_dw, do_dw) = vsplitTup dxo_dw + (dx_dp, do_dp) = vsplitTup dxo_dp + return (dx_dx, dx_du, dx_dw, dx_dp, do_dx, do_du, do_dw, do_dp, x', o) diff --git a/src/Dyno/MultipleShooting.hs b/src/Dyno/MultipleShooting.hs --- a/src/Dyno/MultipleShooting.hs +++ b/src/Dyno/MultipleShooting.hs @@ -21,15 +21,14 @@ import Casadi.MX ( MX ) +import Dyno.Nlp ( Bounds, Nlp(..) ) import Dyno.TypeVecs -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.Vectorize ( Vectorize ) import Dyno.View.Fun ( MXFun, toMXFun, call ) +import Dyno.View.JVec ( JVec(..) ) +import Dyno.View.M ( vcat, vsplit ) import Dyno.View.Scheme ( Scheme ) -import Dyno.Vectorize ( Vectorize, Id ) -import Dyno.Nlp ( Bounds, Nlp(..) ) +import Dyno.View.View ( View(..), J, S, JV, JNone(..), JTuple(..), jfill, catJV ) data IntegratorIn x u p a = IntegratorIn (J (JV x) a) (J (JV u) a) (J (JV p) a) @@ -44,9 +43,9 @@ -- problem specification data MsOcp x u p = MsOcp - { msOde :: Ode x u p (J (JV Id) MX) - , msMayer :: x (J (JV Id) MX) -> J (JV Id) MX - , msLagrangeSum :: x (J (JV Id) MX) -> u (J (JV Id) MX) -> J (JV Id) MX + { msOde :: Ode x u p (S MX) + , msMayer :: x (S MX) -> S MX + , msLagrangeSum :: x (S MX) -> u (S MX) -> S MX , msX0 :: x (Maybe Double) , msXF :: x (Maybe Double) , msXBnds :: x Bounds @@ -95,15 +94,15 @@ => MsOcp x u p -> IO (Nlp (MsDvs x u p n) JNone (MsConstraints x n) MX) makeMsNlp msOcp = do let n = reflectDim (Proxy :: Proxy n) - integrate (IntegratorIn x0 u p) = IntegratorOut (catJV' (simulate nsteps ode x0' u' p' 0 dt)) + integrate (IntegratorIn x0 u p) = IntegratorOut (vcat (simulate nsteps ode x0' u' p' 0 dt)) where endTime = msEndTime msOcp dt = (realToFrac endTime) / fromIntegral n ode = msOde msOcp nsteps = fromMaybe 1 (msNumRk4Steps msOcp) - x0' = splitJV' x0 - u' = splitJV' u - p' = splitJV' p + x0' = vsplit x0 + u' = vsplit u + p' = vsplit p integrator <- toMXFun "my integrator" integrate let _ = integrator :: MXFun (IntegratorIn x u p) (IntegratorOut x) -- just for type signature @@ -146,7 +145,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 (JV Id) MX, J (MsConstraints x n) MX) + fg :: J (MsDvs x u p n) MX -> J JNone MX -> (S MX, J (MsConstraints x n) MX) fg dvs _ = (f, cat g) where MsDvs xus xf p = split dvs @@ -158,13 +157,13 @@ lagrangeSum = F.sum $ fmap callLagrangeSum (unJVec (split xus)) where - callLagrangeSum xu = msLagrangeSum msOcp (splitJV' x) (splitJV' u) + callLagrangeSum xu = msLagrangeSum msOcp (vsplit x) (vsplit u) where JTuple x u = split xu - mayer = msMayer msOcp (splitJV' xf) + mayer = msMayer msOcp (vsplit xf) - f :: J (JV Id) MX + f :: S MX f = mayer + lagrangeSum diff --git a/src/Dyno/Nlp.hs b/src/Dyno/Nlp.hs --- a/src/Dyno/Nlp.hs +++ b/src/Dyno/Nlp.hs @@ -15,11 +15,9 @@ import qualified Data.Vector as V import Data.Binary ( Binary ) import Data.Serialize ( Serialize ) +import Casadi.Viewable ( Viewable ) -import Dyno.Vectorize ( Id ) -import Dyno.View.View ( View(..), J ) -import Dyno.View.Viewable ( Viewable ) -import Dyno.View.JV ( JV ) +import Dyno.View.View ( View(..), J, S ) import Dyno.View.M ( M ) type Bounds = (Maybe Double, Maybe Double) @@ -36,7 +34,7 @@ -- data Nlp x p g a = Nlp - { nlpFG :: J x a -> J p a -> (J (JV Id) a, J g a) + { nlpFG :: J x a -> J p a -> (S a, J g a) , nlpBX :: J x (V.Vector Bounds) , nlpBG :: J g (V.Vector Bounds) , nlpX0 :: J x (V.Vector Double) @@ -51,7 +49,7 @@ -- | NLP output data NlpOut x g a = NlpOut - { fOpt :: J (JV Id) a + { fOpt :: S a , xOpt :: J x a , gOpt :: J g a , lambdaXOpt :: J x a @@ -70,7 +68,7 @@ , kktJacG :: M g x DMatrix , kktG :: J g DMatrix , kktGradF :: J x DMatrix - , kktF :: J (JV Id) DMatrix + , kktF :: S DMatrix } deriving (Generic, Eq, Show) instance (View x, View g) => Binary (KKT x g) instance (View x, View g) => Serialize (KKT x g) diff --git a/src/Dyno/NlpScaling.hs b/src/Dyno/NlpScaling.hs --- a/src/Dyno/NlpScaling.hs +++ b/src/Dyno/NlpScaling.hs @@ -12,19 +12,16 @@ import Casadi.CMatrix ( CMatrix, fromDVector ) -import Dyno.View.Unsafe.View ( unJ, mkJ ) - +import Dyno.View.Unsafe ( mkM, unM ) import Dyno.View.M ( M ) import qualified Dyno.View.M as M import Dyno.Vectorize ( Id(..) ) -import Dyno.View.View ( View, J, v2d, fromDMatrix ) -import Dyno.View.JV ( JV, catJV' ) -import Dyno.View.Viewable ( Viewable ) +import Dyno.View.View ( View, J, S, v2d ) data ScaleFuns x g a = ScaleFuns - { fToFBar :: J (JV Id) a -> J (JV Id) a - , fbarToF :: J (JV Id) a -> J (JV Id) a + { fToFBar :: S a -> S a + , fbarToF :: S a -> S a , xToXBar :: J x a -> J x a , xbarToX :: J x a -> J x a , gToGBar :: J g a -> J g a @@ -42,12 +39,12 @@ scaledFG :: forall x p g a . - (View x, View g, CMatrix a, Viewable a) + (View x, View g, CMatrix a) => ScaleFuns x g a - -> (J x a -> J p a -> (J (JV Id) a, J g a)) + -> (J x a -> J p a -> (S a, J g a)) -> J x a -> J p a - -> (J (JV Id) a, J g a) + -> (S a, J g a) scaledFG scaleFuns fg x p = (fToFBar scaleFuns f, gToGBar scaleFuns g) where (f, g) = fg (xbarToX scaleFuns x) p @@ -60,15 +57,15 @@ -- Doesn't seem to be a bottleneck mkScaleFuns :: forall x g a . - (View x, View g, CMatrix a, Viewable a) + (View x, View g, CMatrix a) => Maybe (J x (V.Vector Double)) -> Maybe (J g (V.Vector Double)) -> Maybe Double -> ScaleFuns x g a mkScaleFuns mx mg mf | any (not . allPositive) - [ fmap unJ mx - , fmap unJ mg + [ fmap unM mx + , fmap unM mg , fmap V.singleton mf ] = error "all scaling factors must be positive" | otherwise = @@ -90,10 +87,10 @@ } where xdiaginv :: Maybe (M x x a) - xdiaginv = fmap (\scl -> M.diag (fromDMatrix (1.0 / (v2d scl)))) mx + xdiaginv = fmap (\scl -> M.diag (M.fromDMatrix (1.0 / (v2d scl)))) mx gdiag :: Maybe (M g g a) - gdiag = fmap (\scl -> M.diag (fromDMatrix (v2d scl))) mg + gdiag = fmap (\scl -> M.diag (M.fromDMatrix (v2d scl))) mg jacGBarToJacG' :: M g x a -> M g x a jacGBarToJacG' g0 = gg0x @@ -111,7 +108,7 @@ hessFBarToHessF' :: M x x a -> M x x a hessFBarToHessF' h0 = case mf of Nothing -> h1 - Just fscl -> h1 `M.ms` (catJV' (Id (realToFrac fscl))) + Just fscl -> h1 `M.ms` (M.vcat (Id (realToFrac fscl))) where h1 = case xdiaginv of Nothing -> h0 @@ -119,8 +116,8 @@ (lamXToLamXBar', lamXBarToLamX') = case mf of Nothing -> (mulByXScale, divByXScale) - Just fscl -> ( \lamx -> mkJ ((unJ (mulByXScale lamx)) / fs) - , \lamx -> mkJ ((unJ (divByXScale lamx)) * fs) + Just fscl -> ( \lamx -> mkM ((unM (mulByXScale lamx)) / fs) + , \lamx -> mkM ((unM (divByXScale lamx)) * fs) ) where fs :: a @@ -128,8 +125,8 @@ (lamGToLamGBar', lamGBarToLamG') = case mf of Nothing -> (mulByGScale, divByGScale) - Just fscl -> ( \lamg -> mkJ ((unJ (mulByGScale lamg)) / fs) - , \lamg -> mkJ ((unJ (divByGScale lamg)) * fs) + Just fscl -> ( \lamg -> mkM ((unM (mulByGScale lamg)) / fs) + , \lamg -> mkM ((unM (divByGScale lamg)) * fs) ) where fs :: a @@ -139,30 +136,30 @@ divByXScale :: J x a -> J x a (mulByXScale, divByXScale) = case mx of Nothing -> (id, id) - Just xscl -> ( mkJ . (* s) . unJ - , mkJ . (/ s) . unJ + Just xscl -> ( mkM . (* s) . unM + , mkM . (/ s) . unM ) where s :: a - s = fromDVector (unJ xscl) + s = fromDVector (unM xscl) mulByGScale :: J g a -> J g a divByGScale :: J g a -> J g a (mulByGScale, divByGScale) = case mg of Nothing -> (id, id) - Just gscl -> ( mkJ . (* s) . unJ - , mkJ . (/ s) . unJ + Just gscl -> ( mkM . (* s) . unM + , mkM . (/ s) . unM ) where s :: a - s = fromDVector (unJ gscl) + s = fromDVector (unM gscl) - mulByFScale :: J (JV Id) a -> J (JV Id) a - divByFScale :: J (JV Id) a -> J (JV Id) a + mulByFScale :: S a -> S a + divByFScale :: S a -> S a (mulByFScale, divByFScale) = case mf of Nothing -> (id, id) - Just fscl -> ( mkJ . (* s) . unJ - , mkJ . (/ s) . unJ + Just fscl -> ( mkM . (* s) . unM + , mkM . (/ s) . unM ) 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 @@ -10,6 +10,8 @@ , RunNlpOptions(..) , runNlpSolverWith , defaultRunnerOptions + , withNlpSolver + , withNlpSolver' -- * solve , solve , solve' @@ -94,17 +96,14 @@ import Dyno.FormatTime ( formatSeconds ) import qualified Dyno.View.M as M -import Dyno.Nlp ( NlpOut(..), KKT(..) ) +import Dyno.Nlp ( NlpOut(..), KKT(..), Bounds ) import Dyno.NlpScaling ( ScaleFuns(..), scaledFG, mkScaleFuns ) import Dyno.SolverInternal ( SolverInternal(..) ) import Dyno.Solvers ( Solver(..), getSolverInternal ) import Dyno.Vectorize ( Id(..) ) -import Dyno.View.JV ( JV ) -import Dyno.View.View ( View(..), J, fmapJ, d2v, v2d, jfill ) +import Dyno.View.View ( View(..), J, S, JV, fmapJ, d2v, v2d, jfill, unzipJ ) import Dyno.View.M ( M ) -import Dyno.View.Unsafe.View ( unJ, mkJ ) -import Dyno.View.Unsafe.M ( mkM ) -import Dyno.View.Viewable ( Viewable ) +import Dyno.View.Unsafe ( mkM, unM ) type VD a = J a (Vector Double) type VMD a = J a (Vector (Maybe Double)) @@ -130,7 +129,7 @@ -> NlpSolver x p g () setInput scaleFun getLen name x0 = do nlpState <- ask - let x = unJ $ scaleFun (isScale nlpState) $ mkJ $ CM.fromDVector (unJ x0) + let x = unM $ scaleFun (isScale nlpState) $ mkM $ CM.fromDVector (unM x0) let nActual = (CM.size1 x, CM.size2 x) nTypeLevel = (getLen nlpState, 1) when (nTypeLevel /= nActual) $ error $ @@ -183,7 +182,7 @@ nlpState <- ask dmat <- liftIO $ C.ioInterfaceFunction_getInput__0 (isSolver nlpState) name let scale = scaleFun (isScale nlpState) - return (mkJ $ dnonzeros $ unJ $ scale (mkJ dmat)) + return (mkM $ dnonzeros $ unM $ scale (mkM dmat)) getX0 :: View x => NlpSolver x p g (VD x) getX0 = getInput xbarToX "x0" @@ -217,7 +216,7 @@ nlpState <- ask dmat <- liftIO $ C.ioInterfaceFunction_getOutput__0 (isSolver nlpState) name let scale = scaleFun (isScale nlpState) - return (mkJ $ dnonzeros $ unJ $ scale (mkJ dmat)) + return (mkM $ dnonzeros $ unM $ scale (mkM dmat)) getF :: NlpSolver x p g (VD (JV Id)) getF = getOutput fbarToF "f" @@ -236,7 +235,7 @@ evalScaledGradF :: forall x p g . (View x, View g, View p) - => NlpSolver x p g (J x DMatrix, J (JV Id) DMatrix) + => NlpSolver x p g (J x DMatrix, S DMatrix) evalScaledGradF = do x0bar <- getInput (const id) "x0" :: NlpSolver x p g (J x (Vector Double)) pbar <- getInput (const id) "p" :: NlpSolver x p g (J p (Vector Double)) @@ -245,18 +244,18 @@ let solver = isSolver nlpState :: C.NlpSolver liftIO $ do gradF <- C.nlpSolver_gradF solver - result <- evalDMatrix' gradF (M.fromList [("x", unJ (v2d x0bar)), ("p", unJ (v2d pbar))]) + result <- evalDMatrix' gradF (M.fromList [("x", unM (v2d x0bar)), ("p", unM (v2d pbar))]) let mret = do grad <- M.lookup "grad" result f <- M.lookup "f" result - return (mkJ grad, mkJ f) + return (mkM grad, mkM f) case mret of Nothing -> error $ "evalScaledGradF: error looking up output\n" ++ "fields available: " ++ show (M.keys result) Just r -> return r evalGradF :: forall x p g . (View x, View g, View p) - => NlpSolver x p g (J x DMatrix, J (JV Id) DMatrix) + => NlpSolver x p g (J x DMatrix, S DMatrix) evalGradF = do nlpState <- ask let scale = isScale nlpState @@ -273,14 +272,14 @@ let solver = isSolver nlpState :: C.NlpSolver -- todo: remove this workaround when casadi fixes https://github.com/casadi/casadi/issues/1345 if size (Proxy :: Proxy g) == 0 - then return (M.zeros, M.uncol M.zeros) + then return (M.zeros, M.zeros) else liftIO $ do jacG <- C.nlpSolver_jacG solver - result <- evalDMatrix' jacG (M.fromList [("x", unJ (v2d x0bar)), ("p", unJ (v2d pbar))]) + result <- evalDMatrix' jacG (M.fromList [("x", unM (v2d x0bar)), ("p", unM (v2d pbar))]) let mret = do jac <- M.lookup "jac" result g <- M.lookup "g" result - return (mkM jac, mkJ g) + return (mkM jac, mkM g) case mret of Nothing -> error $ "evalScaledJacG: error looking up output\n" ++"fields available: " ++ show (M.keys result) @@ -308,10 +307,10 @@ hessLag <- C.nlpSolver_hessLag solver result <- evalDMatrix' hessLag $ M.fromList - [ ("der_x", unJ (v2d x0bar)) - , ("der_p", unJ (v2d pbar)) + [ ("der_x", unM (v2d x0bar)) + , ("der_p", unM (v2d pbar)) , ("adj0_f", 1.0) - , ("adj0_g", unJ (v2d lamGbar)) + , ("adj0_g", unM (v2d lamGbar)) ] case M.lookup "jac" result of -- ???????????????? Nothing -> error $ "evalScaledHessLag: error looking up hess lag output\n" @@ -340,10 +339,10 @@ hessLag <- C.nlpSolver_hessLag solver result <- evalDMatrix' hessLag $ M.fromList - [ ("der_x", unJ (v2d x0bar)) - , ("der_p", unJ (v2d pbar)) + [ ("der_x", unM (v2d x0bar)) + , ("der_p", unM (v2d pbar)) , ("adj0_f", 1.0) - , ("adj0_g", unJ (v2d lamGbar)) + , ("adj0_g", unM (v2d lamGbar)) ] case M.lookup "jac" result of -- ???????????????? Nothing -> error $ "evalScaledHessF: error looking up hess lag output\n" @@ -371,10 +370,10 @@ hessLag <- C.nlpSolver_hessLag solver result <- evalDMatrix' hessLag $ M.fromList - [ ("der_x", unJ (v2d x0bar)) - , ("der_p", unJ (v2d pbar)) + [ ("der_x", unM (v2d x0bar)) + , ("der_p", unM (v2d pbar)) , ("adj0_f", 0.0) - , ("adj0_g", unJ (v2d lamGbar)) + , ("adj0_g", unM (v2d lamGbar)) ] case M.lookup "jac" result of -- ???????????????? Nothing -> error $ "evalScaledHessLambdaG: error looking up hess lag output\n" @@ -532,7 +531,7 @@ forall x p g a . (View x, View p, View g) => Solver - -> (J x MX -> J p MX -> (J (JV Id) MX, J g MX)) + -> (J x MX -> J p MX -> (S MX, J g MX)) -> Maybe (J x (Vector Double)) -> Maybe (J g (Vector Double)) -> Maybe Double @@ -546,7 +545,7 @@ (View x, View p, View g) => RunNlpOptions -> Solver - -> (J x MX -> J p MX -> (J (JV Id) MX, J g MX)) + -> (J x MX -> J p MX -> (S MX, J g MX)) -> Maybe (J x (Vector Double)) -> Maybe (J g (Vector Double)) -> Maybe Double @@ -554,18 +553,18 @@ -> NlpSolver x p g a -> IO a runNlpSolverWith runnerOptions solverStuff nlpFun scaleX scaleG scaleF callback' (NlpSolver nlpMonad) = do - inputsX <- mkJ <$> symV "x" (size (Proxy :: Proxy x)) - inputsP <- mkJ <$> symV "p" (size (Proxy :: Proxy p)) + inputsX <- mkM <$> symV "x" (size (Proxy :: Proxy x)) + inputsP <- mkM <$> symV "p" (size (Proxy :: Proxy p)) - let scale :: forall sfa . (CMatrix sfa, Viewable sfa) => ScaleFuns x g sfa + let scale :: forall sfa . CMatrix sfa => ScaleFuns x g sfa scale = mkScaleFuns scaleX scaleG scaleF (obj, g) = scaledFG scale nlpFun inputsX inputsP - inputsXMat = unJ inputsX - inputsPMat = unJ inputsP - objMat = unJ obj - gMat = unJ g + inputsXMat = unM inputsX + inputsPMat = unM inputsP + objMat = unM obj + gMat = unM g when (verbose runnerOptions) $ do putStrLn "************** initializing dynobud runNlpSolver ******************" @@ -586,7 +585,7 @@ -- let eval 0 = error "finished" -- eval k = do -- putStrLn "setting input" --- ioInterfaceFunction_setInput''' nlp (unJ nlpX0') (0::Int) +-- ioInterfaceFunction_setInput''' nlp (unM nlpX0') (0::Int) -- putStrLn $ "evaluating " ++ show k -- C.function_evaluate nlp -- eval (k-1 :: Int) @@ -607,7 +606,7 @@ callbackRet <- case callback' of Nothing -> return True Just callback -> do - xval <- fmap (d2v . xbarToX scale . mkJ . CM.densify) $ + xval <- fmap (d2v . xbarToX scale . mkM . CM.densify) $ C.ioInterfaceFunction_getOutput__2 function' 0 pval <- readIORef paramRef callback xval pval @@ -640,7 +639,7 @@ -- let eval 0 = error "finished" -- eval k = do -- putStrLn "setting input" --- ioInterfaceFunction_setInput''' jac_g (unJ nlpX0') (0::Int) +-- ioInterfaceFunction_setInput''' jac_g (unM nlpX0') (0::Int) -- putStrLn $ "evaluating " ++ show k -- C.function_evaluate jac_g -- eval (k-1 :: Int) @@ -670,3 +669,60 @@ (ret, retTime) <- timeIt $ liftIO $ runReaderT nlpMonad nlpState when (verbose runnerOptions) $ printf "ran NLP monad in %s\n" (formatSeconds retTime) return ret + + +withNlpSolver :: + forall x p g a . + (View x, View p, View g) + => Solver + -> (J x MX -> J p MX -> (S MX, J g MX)) + -> Maybe (J x (Vector Double)) + -> Maybe (J g (Vector Double)) + -> Maybe Double + -> Maybe (J x (Vector Double) -> J p (Vector Double) -> IO Bool) + -> ((J x (Vector Double) -> J p (Vector Double) + -> J x (Vector Bounds) -> J g (Vector Bounds) + -> NlpSolver x p g (Either String (NlpOut x g (Vector Double))) + ) -> NlpSolver x p g a) + -> IO a +withNlpSolver = withNlpSolver' defaultRunnerOptions + +withNlpSolver' :: + forall x p g a . + (View x, View p, View g) + => RunNlpOptions + -> Solver + -> (J x MX -> J p MX -> (S MX, J g MX)) + -> Maybe (J x (Vector Double)) + -> Maybe (J g (Vector Double)) + -> Maybe Double + -> Maybe (J x (Vector Double) -> J p (Vector Double) -> IO Bool) + -> ((J x (Vector Double) -> J p (Vector Double) + -> J x (Vector Bounds) -> J g (Vector Bounds) + -> NlpSolver x p g (Either String (NlpOut x g (Vector Double))) + ) -> NlpSolver x p g a) + -> IO a +withNlpSolver' opts solver fg sx sg sf cb userFun = + runNlpSolverWith opts solver fg sx sg sf cb action + where + action :: NlpSolver x p g a + action = userFun solveOne + where + solveOne :: + J x (Vector Double) -> J p (Vector Double) + -> J x (Vector Bounds) -> J g (Vector Bounds) + -> NlpSolver x p g (Either String (NlpOut x g (Vector Double))) + solveOne x0 p xbnds gbnds = do + setX0 x0 + setP p + let (lbx, ubx) = unzipJ xbnds + (lbg, ubg) = unzipJ gbnds + setLbx lbx + setUbx ubx + setLbg lbg + setUbg ubg + (status, out) <- solve' + return $ case status of + Left msg -> Left msg + Right _ -> Right out + diff --git a/src/Dyno/NlpUtils.hs b/src/Dyno/NlpUtils.hs --- a/src/Dyno/NlpUtils.hs +++ b/src/Dyno/NlpUtils.hs @@ -25,11 +25,10 @@ import Casadi.MX ( MX ) import qualified Casadi.GenericC as Gen -import Dyno.View.Unsafe.View ( unJ, mkJ ) - +import Dyno.View.M ( vcat, vsplit ) +import Dyno.View.Unsafe ( mkM, unM ) import Dyno.Vectorize ( Vectorize(..), Id(..) ) -import Dyno.View.JV ( JV, catJV, catJV', splitJV, splitJV' ) -import Dyno.View.View ( View(..), J, JNone(..), unzipJ ) +import Dyno.View.View ( View(..), J, S, JV, JNone(..), catJV, splitJV, unzipJ ) import Dyno.Nlp ( Nlp(..), NlpOut(..), Bounds ) import Dyno.Solvers ( Solver ) import Dyno.NlpSolver @@ -87,7 +86,7 @@ solverStuff nlp pFs callback callbackP = do when ((reduction hp) >= 1) $ error $ "homotopy reduction factor " ++ show (reduction hp) ++ " >= 1" when ((increase hp) <= 1) $ error $ "homotopy increase factor " ++ show (increase hp) ++ " <= 1" - let fg :: J x MX -> J p MX -> (J (JV Id) MX, J g MX) + let fg :: J x MX -> J p MX -> (S MX, J g MX) fg x p = nlpFG nlp x p runNlpSolverWith options solverStuff fg (nlpScaleX nlp) (nlpScaleG nlp) (nlpScaleF nlp) callback $ do @@ -143,8 +142,8 @@ where setAlpha :: Double -> NlpSolver x p g () setAlpha alpha = do - let p0'' = unJ p0' - let p = mkJ $ V.zipWith (+) p0'' (V.map (alpha*) (V.zipWith (-) (unJ pF') p0'')) + let p0'' = unM p0' + let p = mkM $ V.zipWith (+) p0'' (V.map (alpha*) (V.zipWith (-) (unM pF') p0'')) setP p tryStep :: Int -> Double -> Double @@ -211,23 +210,23 @@ Nlp { nlpFG = \x' _ -> let _ = x' :: J (JV x) MX - x = splitJV' x' :: x (J (JV Id) MX) - (obj,g) = fg x :: (J (JV Id) MX, g (J (JV Id) MX)) - --obj' = sxCatJV (Id obj) :: J (JV Id) MX + x = vsplit x' :: x (S MX) + (obj,g) = fg x :: (S MX, g (S MX)) + --obj' = sxCatJV (Id obj) :: S MX --g' = sxCatJV g :: J (JV g) MX - in (obj, catJV' g) - , nlpBX = catJV bx -- mkJ $ vectorize (nlpBX nlp) :: J (JV x) (V.Vector Bounds) - , nlpBG = catJV bg -- mkJ $ vectorize (nlpBG nlp) :: J (JV g) (V.Vector Bounds) - , nlpX0 = catJV x0 -- mkJ $ vectorize (nlpX0 nlp) :: J (JV x) (V.Vector Double) - , nlpP = cat JNone -- mkJ $ vectorize (nlpP nlp) :: J (JV p) (V.Vector Double) - , nlpLamX0 = Nothing --fmap (mkJ . vectorize) (nlpLamX0 nlp) + in (obj, vcat g) + , nlpBX = catJV bx + , nlpBG = catJV bg + , nlpX0 = catJV x0 + , nlpP = cat JNone -- mkM $ vectorize (nlpP nlp) :: J (JV p) (V.Vector Double) + , nlpLamX0 = Nothing --fmap (mkM . vectorize) (nlpLamX0 nlp) -- :: Maybe (J (JV x) (V.Vector Double)) - , nlpLamG0 = Nothing -- fmap (mkJ . vectorize) (nlpLamG0 nlp) + , nlpLamG0 = Nothing -- fmap (mkM . vectorize) (nlpLamG0 nlp) -- :: Maybe (J (JV g) (V.Vector Double)) , nlpScaleF = Nothing -- nlpScaleF nlp - , nlpScaleX = Nothing -- fmap (mkJ . vectorize) (nlpScaleX nlp) + , nlpScaleX = Nothing -- fmap (mkM . vectorize) (nlpScaleX nlp) -- :: Maybe (J (JV x) (V.Vector Double)) - , nlpScaleG = Nothing -- fmap (mkJ . vectorize) (nlpScaleG nlp) + , nlpScaleG = Nothing -- fmap (mkM . vectorize) (nlpScaleG nlp) -- :: Maybe (J (JV g) (V.Vector Double)) } @@ -242,11 +241,11 @@ Right _ -> Right $ (unId (splitJV (fOpt r1)), splitJV (xOpt r1)) -- let r1 :: NlpOut x g Double --- r1 = NlpOut { fOpt = V.head $ unJ (fOpt' r1') --- , xOpt = devectorize $ unJ (xOpt' r1') --- , gOpt = devectorize $ unJ (gOpt' r1') --- , lambdaXOpt = devectorize $ unJ $ lambdaXOpt' r1' --- , lambdaGOpt = devectorize $ unJ $ lambdaGOpt' r1' +-- r1 = NlpOut { fOpt = V.head $ unM (fOpt' r1') +-- , xOpt = devectorize $ unM (xOpt' r1') +-- , gOpt = devectorize $ unM (gOpt' r1') +-- , lambdaXOpt = devectorize $ unM $ lambdaXOpt' r1' +-- , lambdaGOpt = devectorize $ unM $ lambdaGOpt' r1' -- } -- -- return (r0, r1) diff --git a/src/Dyno/Ocp.hs b/src/Dyno/Ocp.hs --- a/src/Dyno/Ocp.hs +++ b/src/Dyno/Ocp.hs @@ -28,17 +28,15 @@ import Data.Serialize ( Serialize ) import Data.Vector ( Vector ) -import Dyno.View.JV ( JV ) -import Dyno.View.View ( J ) +import Dyno.View.View ( J, S, JV ) import Dyno.View.Cov ( Cov ) import Dyno.Nlp ( Bounds ) -import Dyno.Vectorize ( Id ) import Casadi.SX ( SX ) import Casadi.DMatrix ( DMatrix ) type Sx a = J a SX -type Sxe = J (JV Id) SX +type Sxe = S SX -- | differential state type family X a :: * -> * diff --git a/src/Dyno/OcpHomotopy.hs b/src/Dyno/OcpHomotopy.hs --- a/src/Dyno/OcpHomotopy.hs +++ b/src/Dyno/OcpHomotopy.hs @@ -14,8 +14,7 @@ import Dyno.Ocp import Dyno.Vectorize ( Vectorize ) -import Dyno.View.View ( J ) -import Dyno.View.JV ( JV, catJV ) +import Dyno.View.View ( J, JV, catJV ) import Dyno.TypeVecs ( Dim ) import Dyno.Solvers ( Solver ) import Dyno.Nlp ( Nlp(..), NlpOut(..) ) diff --git a/src/Dyno/Random.hs b/src/Dyno/Random.hs new file mode 100644 --- /dev/null +++ b/src/Dyno/Random.hs @@ -0,0 +1,34 @@ +{-# OPTIONS_GHC -Wall #-} +{-# LANGUAGE ScopedTypeVariables #-} +{-# LANGUAGE PolyKinds #-} + +module Dyno.Random + ( initRandomIO + ) where + +import Control.Monad ( replicateM ) +import Data.Proxy ( Proxy(..) ) +import Data.Foldable ( toList ) +import qualified Data.Vector as V +import System.Random.MWC +import System.Random.MWC.Distributions +import qualified Numeric.LinearAlgebra.Data as D +import qualified Numeric.LinearAlgebra.HMatrix as HM + +import Dyno.Vectorize ( Vectorize(..), devectorize, vlength ) + +initRandomIO :: forall w . (Vectorize w, Foldable w) => w (w Double) -> IO (IO (w Double)) +initRandomIO sq = do + gen0 <- createSystemRandom + let cov = D.fromLists $ map toList (toList sq) :: HM.Matrix Double + c = HM.chol (HM.sym cov) :: HM.Matrix Double + n = vlength (Proxy :: Proxy w) + + mkOne :: IO (w Double) + mkOne = do + vs' <- replicateM n (standard gen0) + let vs = HM.app c (HM.vector vs') :: HM.Vector Double + w = devectorize $ V.fromList (HM.toList vs) + return w + + return mkOne diff --git a/src/Dyno/SimpleOcp.hs b/src/Dyno/SimpleOcp.hs --- a/src/Dyno/SimpleOcp.hs +++ b/src/Dyno/SimpleOcp.hs @@ -7,7 +7,7 @@ module Dyno.SimpleOcp ( SimpleOcp(..) - , S + , Se , solveOcp ) where @@ -28,18 +28,17 @@ import Dyno.DirectCollocation.Formulate import Dyno.DirectCollocation.Types import Dyno.DirectCollocation.Quadratures -import Dyno.Vectorize ( Vectorize(..), Tuple(..), Id, None(..), fill, vzipWith ) +import Dyno.Vectorize ( Vectorize(..), Tuple(..), None(..), fill, vzipWith ) import Dyno.View.View -- ( View(..) ) -import Dyno.View.JV import Dyno.View.JVec -- | scalar symbolic type -newtype S = S {unS :: J (JV Id) SX} deriving (Num, Fractional, Floating) +newtype Se = Se {unSe :: S SX} deriving (Num, Fractional, Floating) data SimpleOcp x u = SimpleOcp - { ode :: x S -> u S -> x S - , objective :: x S -> u S -> S + { ode :: x Se -> u Se -> x Se + , objective :: x Se -> u Se -> Se , xBounds :: x (Double, Double) , uBounds :: u (Double, Double) , xInitial :: x Double @@ -60,12 +59,12 @@ toOcp simple = OcpPhase { ocpMayer = \_ _ _ _ _ _ -> 0 - , ocpLagrange = \(Tuple x u) _ u' _ _ _ _ _ -> 1e-9 * (u' `dot` u') + unS (objective simple (fmap S x) (fmap S u)) + , ocpLagrange = \(Tuple x u) _ u' _ _ _ _ _ -> 1e-9 * (u' `dot` u') + unSe (objective simple (fmap Se x) (fmap Se u)) , ocpQuadratures = \_ _ _ _ _ _ _ _ -> None , ocpQuadratureOutputs = \_ _ _ _ _ _ _ _ -> None , ocpDae = \(Tuple xd ud) (Tuple x u) _ u' _ _ _ -> let r = Tuple (xd `vminus` x') (ud `vminus` u') - x' = fmap unS $ ode simple (fmap S x) (fmap S u) + x' = fmap unSe $ ode simple (fmap Se x) (fmap Se u) in (r, None) , ocpBc = \(Tuple x0 _) (Tuple xf _) _ _ _ _ ->SimpleBc x0 xf , ocpPathC = \_ _ _ _ _ _ _ -> None diff --git a/src/Dyno/TypeVecs.hs b/src/Dyno/TypeVecs.hs --- a/src/Dyno/TypeVecs.hs +++ b/src/Dyno/TypeVecs.hs @@ -82,17 +82,15 @@ Left msg -> fail msg instance (Lookup a, Dim n) => Lookup (Vec n a) where - toAccessorTree vec get set = Data ("Vec " ++ show n, "Vec " ++ show n) $ map child (take n [0..]) + toAccessorTree lens0 = + Data ("Vec " ++ show n, "Vec " ++ show n) $ map child (take n [0..]) where n = reflectDim (Proxy :: Proxy n) - child k = ("v" ++ show k, toAccessorTree (getK vec) (getK . get) setK) + child k = ("v" ++ show k, toAccessorTree (lens0 . lensK)) where - setK vk new = set (devectorize (v V.// [(k,vk)])) new + lensK f (MkVec v) = fmap (\vk -> devectorize (v V.// [(k,vk)])) (f vk0) where - MkVec v = get new - - getK :: Vec n a -> a - getK (MkVec v) = v V.! k + vk0 = v V.! k instance Dim n => Distributive (Vec n) where distribute f = devectorize $ V.generate (reflectDim (Proxy :: Proxy n)) @@ -123,7 +121,8 @@ devectorize' :: V.Vector a -> Either String (Vec n a) devectorize' x | n == n' = Right (MkVec x) - | otherwise = Left $ "mkVec: length mismatch, " ++ show (n,n') + | otherwise = Left $ "mkVec: length mismatch, type-level: " + ++ show n ++ ", value-level: " ++ show n' where n = reflectDim (Proxy :: Proxy n) n' = V.length x @@ -142,7 +141,8 @@ 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') + | otherwise = error $ "unVec: length mismatch, type-level: " + ++ show n ++ ", value-level: " ++ show n' where n = reflectDim (Proxy :: Proxy n) n' = V.length x diff --git a/src/Dyno/Vectorize.hs b/src/Dyno/Vectorize.hs --- a/src/Dyno/Vectorize.hs +++ b/src/Dyno/Vectorize.hs @@ -28,11 +28,16 @@ , vzipWith , vzipWith3 , vzipWith4 + , vdiag + , vdiag' + , vnames + , vnames' , GVectorize(..) ) where import GHC.Generics +import Accessors ( Field, Lookup, accessors, flatten, flatten' ) import Control.Applicative import Data.Either ( partitionEithers ) import Data.Serialize ( Serialize ) @@ -41,15 +46,13 @@ import qualified Data.Traversable as T import Data.Proxy ( Proxy(..) ) import qualified Linear +import SpatialMath ( Euler ) +import SpatialMathT ( V3T, Rot ) import Text.Printf ( printf ) import Prelude -- BBP workaround -import SpatialMath ( Euler ) -import SpatialMathT ( V3T, Rot ) -import Accessors ( Lookup ) - -- | a length-0 vectorizable type data None a = None deriving (Eq, Ord, Generic, Generic1, Functor, F.Foldable, T.Traversable, Show) @@ -129,6 +132,22 @@ vzipWith4 f x y z w = devectorize $ V.zipWith4 f (vectorize x) (vectorize y) (vectorize z) (vectorize w) +-- | Make a diagonal "matrix" from a "vector". +-- Off-diagonal elements will be 0, thus the Num constraint. +vdiag :: forall f a . (Vectorize f, Num a) => f a -> f (f a) +vdiag = flip vdiag' 0 + +-- | Make a diagonal "matrix" from a "vector" with a given off-diagonal value. +vdiag' :: forall f a . Vectorize f => f a -> a -> f (f a) +vdiag' v0 offDiag = + devectorize $ V.generate n (\k -> devectorize (V.generate n (\j -> gen j k))) + where + v = vectorize v0 + n = vlength (Proxy :: Proxy f) + gen j k + | j /= k = offDiag + | otherwise = v V.! k + -- this could me more efficient as a class method, but this is safer vlength :: Vectorize f => Proxy f -> Int vlength = V.length . vectorize . (fill () `asFunctorOf`) @@ -328,3 +347,21 @@ -- break a vector jOuter vectors, each of length kInner splitsAt :: Int -> Int -> V.Vector a -> V.Vector (V.Vector a) splitsAt k j = V.fromList . splitsAt' k j + +-- | fill a vectorizable thing with its field names +vnames :: forall f . (Vectorize f, Lookup (f ())) => f String +vnames = case mr of + Left msg -> error $ "vnames devectorize error: " ++ msg + Right r -> r + where + mr = devectorize' $ V.fromList $ + fmap fst (flatten accessors :: [(String, Field (f ()))]) + +-- | fill a vectorizable thing with its field name heirarchy +vnames' :: forall f . (Vectorize f, Lookup (f ())) => f [String] +vnames' = case mr of + Left msg -> error $ "vnames' devectorize error: " ++ msg + Right r -> r + where + mr = devectorize' $ V.fromList $ + fmap fst (flatten' accessors :: [([String], Field (f ()))]) diff --git a/src/Dyno/View.hs b/src/Dyno/View.hs new file mode 100644 --- /dev/null +++ b/src/Dyno/View.hs @@ -0,0 +1,16 @@ +{-# OPTIONS_GHC -Wall #-} + +-- | Meta module for re-expording the View API. +module Dyno.View + ( module X + ) where + +import Dyno.View.Cov as X +import Dyno.View.Fun as X +import Dyno.View.FunJac as X +import Dyno.View.HList as X +import Dyno.View.JVec as X +import Dyno.View.M as X +import Dyno.View.MapFun as X +import Dyno.View.Scheme as X +import Dyno.View.View as X diff --git a/src/Dyno/View/Cov.hs b/src/Dyno/View/Cov.hs --- a/src/Dyno/View/Cov.hs +++ b/src/Dyno/View/Cov.hs @@ -10,9 +10,7 @@ , toHMatrix , toHMatrix' , fromMatrix - , diag , diag' - , diag'' , nOfVecLen ) where @@ -29,19 +27,15 @@ 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(..), vlength, devectorize ) -import Dyno.View.View ( View(..), J ) -import Dyno.View.JV ( JV ) -import Dyno.View.Viewable ( Viewable(..) ) +import Dyno.View.Unsafe ( M(UnsafeM), mkM, unM ) +import Dyno.Vectorize ( Vectorize(..) ) +import Dyno.View.View ( View(..), J, JV ) 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) + cat (Cov x) = mkM x + split x = Cov (unM x) size = const $ (n*n + n) `div` 2 where n = size (Proxy :: Proxy f) @@ -57,16 +51,16 @@ 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 :: (View f, CMatrix 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 :: forall f a . (View f, CMatrix 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 + --CM.setNZ m (CM.dense (unM c)) slice' + CM.setNZ m (unM c) slice' -- Joel says that "dense" isn't required here return (CM.triu2symm m) {-# NOINLINE toMatrix #-} @@ -74,30 +68,17 @@ 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 +toHMatrix' v = toHMatrix $ (mkM (CM.fromDVector (unM v)) :: J (Cov f) DMatrix) diag' :: Vectorize f => f a -> a -> J (Cov (JV f)) (Vector a) -diag' x offDiag = mkJ $ V.fromList $ concat $ zipWith f vx [0..] +diag' x offDiag = mkM $ V.fromList $ concat $ zipWith f vx [0..] where f y k = replicate k offDiag ++ [y] vx = V.toList $ vectorize x -diag'' :: forall f a . (Vectorize f, Num a) => f a -> f (f a) -diag'' v0 = devectorize $ V.generate n (\k -> devectorize (V.generate n (\j -> gen j k))) - where - v = vectorize v0 - n = vlength (Proxy :: Proxy f) - gen j k - | j /= k = 0 - | otherwise = v V.! k - - --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 = X (mkM 1) (mkM 2) :: X DMatrix --xx' = cat xx -- --dd :: J (Cov X) DMatrix @@ -109,9 +90,9 @@ --dd2 :: J (Cov X) DMatrix --dd2 = fromMatrix sp -fromMat :: (View f, CMatrix a, Viewable a) => M f f a -> J (Cov f) a +fromMat :: (View f, CMatrix 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.densify x)) slice' ---fromMatrix x = mkJ $ CM.getNZ (CM.triu x) slice' +fromMatrix :: (View f, CMatrix a) => a -> J (Cov f) a +fromMatrix x = mkM $ CM.getNZ (CM.triu (CM.densify x)) slice' +--fromMatrix x = mkM $ CM.getNZ (CM.triu x) slice' 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 @@ -41,6 +41,7 @@ import Casadi.Option import Casadi.DMatrix ( DMatrix ) import Casadi.CMatrix ( CMatrix ) +import Casadi.Viewable ( Viewable ) import qualified Casadi.Core.Classes.Function as F import qualified Casadi.Core.Classes.MXFunction as M import qualified Casadi.Core.Classes.Sparsity as C @@ -49,7 +50,6 @@ import Dyno.View.FunJac import Dyno.View.Scheme import Dyno.View.View ( View ) -import Dyno.View.Viewable ( Viewable ) newtype MXFun (f :: * -> *) (g :: * -> *) = MXFun C.MXFunction newtype SXFun (f :: * -> *) (g :: * -> *) = SXFun C.SXFunction @@ -106,7 +106,7 @@ C.callSX sxf (toVector x) (AlwaysInline False) (NeverInline False) mkSym :: forall a f . - (Scheme f, CMatrix a, Viewable a) + (Scheme f, CMatrix a) => (String -> Int -> Int -> IO a) -> String -> Proxy f -> IO (f a) mkSym mk name _ = do diff --git a/src/Dyno/View/FunJac.hs b/src/Dyno/View/FunJac.hs --- a/src/Dyno/View/FunJac.hs +++ b/src/Dyno/View/FunJac.hs @@ -24,12 +24,12 @@ reproxy = const Proxy fromVector v = JacIn j0 (fromVector (V.tail v)) where - j0 = case fromMat (V.head v) of + j0 = case fromFioMat (V.head v) of Left err -> error $ "JacIn fromVector error: " ++ err Right j0' -> j0' toVector (JacIn xj x) = V.cons (toFioMat xj) (toVector x) - sizeList p = matSizes (reproxy' p) : sizeList (reproxy p) + sizeList p = fioMatSizes (reproxy' p) : sizeList (reproxy p) where reproxy :: Proxy (JacIn xj x) -> Proxy x reproxy = const Proxy @@ -43,12 +43,12 @@ reproxy = const Proxy fromVector v = JacOut j0 (fromVector (V.tail v)) where - j0 = case fromMat (V.head v) of + j0 = case fromFioMat (V.head v) of Left err -> error $ "JacOut fromVector error: " ++ err Right j0' -> j0' toVector (JacOut fj f) = V.cons (toFioMat fj) (toVector f) - sizeList p = matSizes (reproxy' p) : sizeList (reproxy p) + sizeList p = fioMatSizes (reproxy' p) : sizeList (reproxy p) where reproxy :: Proxy (JacOut fj f) -> Proxy f reproxy = const Proxy @@ -63,14 +63,14 @@ reproxy = const Proxy fromVector v = Jac m fj (fromVector (V.drop 2 v)) where - m = case fromMat (v V.! 0) of + m = case fromFioMat (v V.! 0) of Left err -> error $ "Jac fromVector error: " ++ err Right j0' -> j0' - fj = case fromMat (v V.! 1) of + fj = case fromFioMat (v V.! 1) of Left err -> error $ "Jac fromVector error: " ++ err Right j0' -> j0' toVector (Jac m fj f) = V.fromList [toFioMat m, toFioMat fj] V.++ toVector f - sizeList p = matSizes (reproxy'' p) : matSizes (reproxy' p) : sizeList (reproxy p) + sizeList p = fioMatSizes (reproxy'' p) : fioMatSizes (reproxy' p) : sizeList (reproxy p) where reproxy'' :: Proxy (Jac xj fj f) -> Proxy (M fj xj) reproxy'' = const Proxy diff --git a/src/Dyno/View/HList.hs b/src/Dyno/View/HList.hs --- a/src/Dyno/View/HList.hs +++ b/src/Dyno/View/HList.hs @@ -57,7 +57,7 @@ (px, py) = reproxy pxy --instance (View f, View g) => View (f :*: g) where --- cat (x :*: y) = mkJ (vveccat (V.fromList [x', y'])) +-- cat (x :*: y) = mkM (vveccat (V.fromList [x', y'])) -- where -- UnsafeJ x' = cat x -- UnsafeJ y' = cat y @@ -70,7 +70,7 @@ -- Seq.EmptyR -> k0 -- _ Seq.:> k1' -> k1' ---- split :: forall a . Viewable a => J S a -> S a --- split = undefined -- S . unJ +-- split = undefined -- S . unM -- -- -- @@ -98,7 +98,7 @@ ---- hsplit = undefined -- hsizeList p = [size p] -- where --- hfromList (x:xs) = (mkJ x, xs) +-- hfromList (x:xs) = (mkM x, xs) -- ----hsplit :: HSplit f g => M f g a -> HSplitT f g a ----hsplit m@(UnsafeM mat) = undefined diff --git a/src/Dyno/View/JV.hs b/src/Dyno/View/JV.hs deleted file mode 100644 --- a/src/Dyno/View/JV.hs +++ /dev/null @@ -1,52 +0,0 @@ -{-# OPTIONS_GHC -Wall #-} -{-# LANGUAGE ScopedTypeVariables #-} -{-# LANGUAGE DeriveFunctor #-} -{-# LANGUAGE DeriveGeneric #-} -{-# LANGUAGE InstanceSigs #-} - -module Dyno.View.JV - ( JV - , splitJV - , catJV - , splitJV' - , catJV' - ) where - -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 Dyno.View.Unsafe.View ( mkJ, unJ ) - -import Dyno.View.View ( View(..), J ) -import Dyno.View.Viewable ( Viewable(..) ) -import Dyno.Vectorize ( Vectorize(..), Id, vlength, devectorize ) - --- | views into Vectorizable things -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 - cat = mkJ . vveccat . vectorize . unJV - size = const $ vlength (Proxy :: Proxy f) - sizes = const . Seq.singleton . (vlength (Proxy :: Proxy f) +) - split :: forall a . Viewable a => J (JV f) a -> JV f a - split = JV . devectorize . flip vvertsplit ks. unJ - where - ks = V.fromList (take (n+1) [0..]) - n = size (Proxy :: Proxy (JV f)) - -splitJV :: Vectorize f => J (JV f) (Vector a) -> f a -splitJV = devectorize . unJ - -catJV :: Vectorize f => f a -> J (JV f) (Vector a) -catJV = mkJ . vectorize - -splitJV' :: (Vectorize f, Viewable a) => J (JV f) a -> f (J (JV Id) a) -splitJV' = fmap mkJ . unJV . split - -catJV' :: (Vectorize f, Viewable a) => f (J (JV Id) a) -> J (JV f) a -catJV' = cat . JV . fmap unJ diff --git a/src/Dyno/View/JVec.hs b/src/Dyno/View/JVec.hs --- a/src/Dyno/View/JVec.hs +++ b/src/Dyno/View/JVec.hs @@ -16,19 +16,19 @@ import Linear.V ( Dim(..) ) import Data.Vector ( Vector ) import qualified Data.Vector as V +import Casadi.Viewable ( Viewable(..) ) -import Dyno.View.Unsafe.View ( mkJ, unJ ) +import Dyno.View.Unsafe ( mkM, unM ) import Dyno.TypeVecs ( Vec, unVec, reifyVector ) -import Dyno.View.Viewable ( Viewable(..) ) import Dyno.View.View ( View(..), J ) import Dyno.Vectorize ( devectorize ) -- | vectors in View newtype JVec (n :: k) f a = JVec { unJVec :: Vec n (J f a) } deriving ( Show ) instance (Dim n, View f) => View (JVec n f) where - cat = mkJ . vveccat . fmap unJ . unVec . unJVec - split = JVec . fmap mkJ . devectorize . flip vvertsplit ks . unJ + cat = mkM . vvertcat . fmap unM . unVec . unJVec + split = JVec . fmap mkM . devectorize . flip vvertsplit ks . unM where ks = V.fromList (take (n+1) [0,m..]) n = reflectDim (Proxy :: Proxy n) 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 @@ -9,12 +9,8 @@ ( M , sparse, dense , mm - , mv - , vm , ms , sm - , vs - , sv , trans , zeros , eye @@ -42,22 +38,24 @@ , vcatTup , vcatTrip , vcatQuad - , row - , col - , unrow - , uncol , solve , solve' + , sumRows, sumCols + , fromDMatrix , toHMat , fromHMat , fromHMat' + , blockSplit + , reshape -- * hmatrix wrappers , rcond , rank ) where import Data.Proxy ( Proxy(..) ) +import qualified Data.Foldable as F import qualified Data.Map as M +import Data.Vector ( Vector ) import qualified Data.Vector as V import qualified Numeric.LinearAlgebra as HMat @@ -65,15 +63,14 @@ import Casadi.CMatrix ( CMatrix ) import Casadi.DMatrix ( DMatrix, dnonzeros, dsparsify ) import qualified Casadi.CMatrix as CM +import Casadi.Viewable ( Viewable(..) ) -import Dyno.View.Unsafe.View ( unJ, mkJ ) -import Dyno.View.Unsafe.M ( M(UnsafeM), mkM, mkM', unM ) +import Dyno.View.Unsafe ( M(UnsafeM), mkM, mkM', unM ) import Dyno.Vectorize ( Vectorize(..), Id, fill, devectorize ) import Dyno.TypeVecs ( Vec, Dim(..) ) -import Dyno.View.View ( View(..), J, JTuple, JTriple, JQuad ) -import Dyno.View.JV ( JV ) +import qualified Dyno.TypeVecs as TV +import Dyno.View.View ( View(..), J, S, JV, JTuple, JTriple, JQuad ) import Dyno.View.JVec ( JVec ) -import Dyno.View.Viewable ( Viewable ) -- todo: generalize once casadi 2.3 is ready @@ -86,41 +83,29 @@ 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) -mv :: (View f, View g, CMatrix a, Viewable a) => M f g a -> J g a -> J f a -mv m v = uncol $ mm m (col v) - -vm :: (View f, View g, CMatrix a, Viewable a) => J f a -> M f g a -> J g a -vm v m = unrow $ mm (row v) m - -ms :: (View f, View h, Viewable a, CMatrix a) => M f g a -> J (JV Id) a -> M f h a -ms m0 m1 = mkM $ (unM m0) * (unJ m1) - -sm :: (View f, View h, Viewable a, CMatrix a) => J (JV Id) a -> M f g a -> M f h a -sm m0 m1 = mkM $ (unJ m0) * (unM m1) - -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 +ms :: (View f, View g, CMatrix a) => M f g a -> S a -> M f g a +ms m0 m1 = mkM $ (unM m0) * (unM m1) -sv :: (View f, Viewable a, CMatrix a) => J (JV Id) a -> J f a -> J f a -sv m0 m1 = uncol $ sm m0 (col m1) +sm :: (View f, View g, CMatrix a) => S a -> M f g a -> M f g a +sm m0 m1 = mkM $ (unM m0) * (unM 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, CMatrix a) + (Vectorize f, View g, Viewable a) => M (JV f) g a -> f (M (JV Id) g a) -vsplit (UnsafeM x) = fmap mkM $ devectorize $ CM.vertsplit x nrs +vsplit (UnsafeM x) = fmap mkM $ devectorize $ vvertsplit x nrs where nr = size (Proxy :: Proxy (JV f)) nrs = V.fromList [0,1..nr] vcat :: forall f g a . - (Vectorize f, View g, CMatrix a) + (Vectorize f, View g, Viewable a) => f (M (JV Id) g a) -> M (JV f) g a -vcat x = mkM $ CM.vertcat $ V.map unM (vectorize x) +vcat x = mkM $ vvertcat $ V.map unM (vectorize x) hsplit :: forall f g a . @@ -310,13 +295,13 @@ z = CM.eye n n = size (Proxy :: Proxy f) -diag :: forall f a . (View f, Viewable a, CMatrix a) => J f a -> M f f a +diag :: forall f a . (View f, CMatrix a) => J f a -> M f f a diag x = mkM z where - z = CM.diag (unJ x) + z = CM.diag (unM x) -takeDiag :: forall f a . (View f, Viewable a, CMatrix a) => M f f a -> J f a -takeDiag m = mkJ $ CM.diag (unM m) +takeDiag :: forall f a . (View f, CMatrix a) => M f f a -> J f a +takeDiag m = mkM $ CM.diag (unM m) ones :: forall f g a . (View f, View g, CMatrix a) => M f g a ones = mkM z @@ -337,18 +322,6 @@ rows = size (Proxy :: Proxy f) cols = size (Proxy :: Proxy g) -row :: (CMatrix a, View f, Viewable a) => J f a -> M (JV Id) f a -row = mkM . CM.trans . unJ - -col :: (CMatrix a, View f, Viewable a) => J f a -> M f (JV Id) a -col = mkM . unJ - -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, CMatrix a, View f) => M f (JV Id) a -> J f a -uncol (UnsafeM x) = mkJ x - solve :: (View g, View h, CMatrix a) => M f g a -> M f h a -> String -> M.Map String GenericType -> M g h a @@ -380,3 +353,44 @@ rank :: (View f, View g) => M f g DMatrix -> Int rank = HMat.rank . toHMat + +fromDMatrix :: (CM.CMatrix a, View f, View g) + => M f g DMatrix -> M f g a +fromDMatrix = mkM . CM.fromDMatrix . unM + +-- | Break a typed matrix into a list of its elements given by the +-- sizes of the View constructor. +-- For example: +-- > data F a = F (J (JV V2) a) (J (JV V3) a) +-- > data G a = G (J (JV V4) a) (J (JV V5) a) (J (JV V6) a) +-- > x :: M F G DMatrix +-- > x = ... +-- > +-- > y :: Vector (Vector DMatrix) +-- > y = blockSplit x +-- +-- > -- y is a 2x3 group with DMatrix dimensions: +-- > -- [ [ (2,4), (2,5), (2,6) ] +-- > -- , [ (3,4), (3,5), (3,6) ] +-- > -- ] +blockSplit :: forall f g a . (View f, View g, CMatrix a) => M f g a -> Vector (Vector a) +blockSplit (UnsafeM m) = CM.blocksplit m vsizes hsizes + where + vsizes = V.fromList $ 0 : (F.toList (sizes 0 (Proxy :: Proxy f))) + hsizes = V.fromList $ 0 : (F.toList (sizes 0 (Proxy :: Proxy g))) + +sumRows :: (View f, View g, CMatrix a) => M f g a -> M (JV Id) g a +sumRows (UnsafeM x) = mkM (CM.sumRows x) + +sumCols :: (View f, View g, CMatrix a) => M f g a -> M f (JV Id) a +sumCols (UnsafeM x) = mkM (CM.sumCols x) + +-- | reshape a vector into a column-major matrix +reshape :: + forall n f a + . (Dim n, View f, CMatrix a) + => J (JVec n f) a -> M f (JVec n (JV Id)) a +reshape (UnsafeM x) = mkM (CM.reshape x (nx, ny)) + where + nx = size (Proxy :: Proxy f) + ny = TV.reflectDim (Proxy :: Proxy n) diff --git a/src/Dyno/View/MapFun.hs b/src/Dyno/View/MapFun.hs --- a/src/Dyno/View/MapFun.hs +++ b/src/Dyno/View/MapFun.hs @@ -11,7 +11,6 @@ module Dyno.View.MapFun ( mapFun , mapFun' - , mapFun'' ) where import qualified Data.Foldable as F @@ -30,43 +29,26 @@ import Dyno.TypeVecs ( Dim ) import qualified Dyno.TypeVecs as TV -import Dyno.Vectorize ( Id ) import Dyno.View.Fun import Dyno.View.HList -import Dyno.View.JV ( JV ) import Dyno.View.JVec ( JVec ) -import Dyno.View.Unsafe.View ( J(..) ) import Dyno.View.M ( M ) import Dyno.View.Scheme ( Scheme ) import Dyno.View.View ( View ) --- | symbolic fmap -mapFun :: forall fun f g n - . (FunClass fun, View f, View g, Dim n) - => String - -> fun (J f) (J g) - -> M.Map String Opt - -> IO (Fun (M (JV Id) (JVec n f)) (M (JV Id) (JVec n g))) -mapFun name f' opts0 = do - opts <- T.mapM mkGeneric opts0 :: IO (M.Map String GenericType) - let Fun f = toFun f' - n = TV.reflectDim (Proxy :: Proxy n) - fm <- F.function_map__1 f name n opts :: IO C.Function - checkFunDimensionsWith "mapFun" (Fun fm) --- {-# NOINLINE mapFun #-} - - class ParScheme f where type Par f (n :: k) :: * -> * -instance View f => ParScheme (J f) where - type Par (J f) n = M (JV Id) (JVec n f) +-- normal +instance (View f, View g) => ParScheme (M f g) where + type Par (M f g) n = M f (JVec n g) +-- multiple inputs/outputs instance (ParScheme f, ParScheme g) => ParScheme (f :*: g) where type Par (f :*: g) n = (Par f n) :*: (Par g n) -- | symbolic fmap -mapFun' :: forall fun f g n +mapFun :: forall fun f g n . ( FunClass fun , Scheme (Par f n), Scheme (Par g n) , Dim n ) @@ -75,28 +57,27 @@ -> fun f g -> M.Map String Opt -> IO (Fun (Par f n) (Par g n)) -mapFun' _ name f' opts0 = do +mapFun _ name f' opts0 = do opts <- T.mapM mkGeneric opts0 :: IO (M.Map String GenericType) let Fun f = toFun f' n = TV.reflectDim (Proxy :: Proxy n) fm <- F.function_map__1 f name n opts :: IO C.Function checkFunDimensionsWith "mapFun'" (Fun fm) --- {-# NOINLINE mapFun' #-} +-- {-# NOINLINE mapFun #-} class ParScheme' f0 f1 where repeated :: Proxy f0 -> Proxy f1 -> Seq Bool -instance View f => ParScheme' (M (JV Id) f) (M (JV Id) (JVec n f)) where - repeated _ _ = S.singleton True - -instance View f => ParScheme' (J f) (M (JV f) (JVec n (JV Id))) where +-- normal +instance (View f, View g) => ParScheme' (M f g) (M f (JVec n g)) where repeated _ _ = S.singleton True ----- non-repeated ---instance View f => ParScheme' (J f) (M (JV Id) f) where --- repeated _ _ = S.singleton False +-- non-repeated +instance View f => ParScheme' (M f g) (M f g) where + repeated _ _ = S.singleton False +-- multiple inputs/output instance (ParScheme' f0 f1, ParScheme' g0 g1) => ParScheme' (f0 :*: g0) (f1 :*: g1) where repeated pfg0 pfg1 = repeated pf0 pf1 S.>< repeated pg0 pg1 where @@ -106,8 +87,8 @@ (pf0, pg0) = splitProxy pfg0 (pf1, pg1) = splitProxy pfg1 --- | symbolic fmap -mapFun'' :: forall fun i0 i1 o0 o1 n +-- | symbolic fmap which can do non-repeated inputs/outputs +mapFun' :: forall fun i0 i1 o0 o1 n . ( FunClass fun , ParScheme' i0 i1, ParScheme' o0 o1 , Scheme i0, Scheme o0 @@ -119,7 +100,7 @@ -> fun i0 o0 -> M.Map String Opt -> IO (Fun i1 o1) -mapFun'' _ name f0 opts0 = do +mapFun' _ name f0 opts0 = do -- let fds = checkFunDimensions f0 -- putStrLn "mapFun'' input dimensions:" -- case fds of @@ -137,4 +118,4 @@ fm <- C.map__1 name (unFun (toFun f0)) n repeatedIn repeatedOut opts :: IO C.Map checkFunDimensionsWith "mapFun''" (Fun (F.castFunction fm)) --- {-# NOINLINE mapFun'' #-} +-- {-# NOINLINE mapFun' #-} 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 @@ -24,57 +24,39 @@ import Casadi.CMatrix ( CMatrix ) -import Dyno.View.Unsafe.View ( unsafeUnJ, mkJ' ) -import Dyno.View.Unsafe.M ( unM, mkM' ) +import Dyno.View.Unsafe ( mkM', unsafeUnM ) import qualified Dyno.View.M as M -import Dyno.View.View ( View(..), J, JQuad, JTriple, JTuple ) -import Dyno.View.Viewable ( Viewable ) +import Dyno.View.View ( View(..), JQuad, JTriple, JTuple ) instance (View f0, View f1, View f2, View f3) => Scheme (JQuad f0 f1 f2 f3) instance (View f0, View f1, View f2) => Scheme (JTriple f0 f1 f2) instance (View f0, View f1) => Scheme (JTuple f0 f1) class FunctionIO (f :: * -> *) where - fromMat :: (CMatrix a, Viewable a) => a -> Either String (f a) + fromFioMat :: CMatrix a => a -> Either String (f a) toFioMat :: f a -> a - matSizes :: Proxy f -> (Int,Int) - -instance View x => Scheme (J x) where - numFields = const 1 - fromVector v = case V.toList v of - [m] -> case fromMat m of - Left err -> error $ "Scheme fromVector J error: " ++ err - Right m' -> m' - _ -> error $ "Scheme fromVector (J x) length mismatch, should be 1 but got: " - ++ show (V.length v) - toVector = V.singleton . toFioMat - sizeList p = [matSizes p] + fioMatSizes :: Proxy f -> (Int,Int) instance (View f, View g) => Scheme (M.M f g) where numFields = const 1 fromVector v = case V.toList v of - [m] -> case fromMat m of + [m] -> case fromFioMat m of Left err -> error $ "Scheme fromVector M error: " ++ err Right m' -> m' _ -> error $ "Scheme fromVector (M f g) length mismatch, should be 1 but got: " ++ show (V.length v) toVector = V.singleton . toFioMat - sizeList p = [matSizes p] - -instance View f => FunctionIO (J f) where - toFioMat = unsafeUnJ - fromMat = mkJ' - matSizes = const (size (Proxy :: Proxy f), 1) + sizeList p = [fioMatSizes p] instance (View f, View g) => FunctionIO (M.M f g) where - toFioMat = unM - fromMat = mkM' - matSizes = const (size (Proxy :: Proxy f), size (Proxy :: Proxy g)) + toFioMat = unsafeUnM + fromFioMat = mkM' + fioMatSizes = const (size (Proxy :: Proxy f), size (Proxy :: Proxy g)) class Scheme (f :: * -> *) where numFields :: Proxy f -> Int - fromVector :: (CMatrix a, Viewable a) => V.Vector a -> f a + fromVector :: CMatrix a => V.Vector a -> f a toVector :: f a -> V.Vector a sizeList :: Proxy f -> [(Int,Int)] @@ -93,7 +75,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, CMatrix a, Viewable a ) + , Generic (f a), Datatype d, CMatrix a ) => Vector a -> f a fromVector vs = out' where @@ -149,7 +131,7 @@ reproxy :: Proxy (M1 i d f p) -> Proxy (f p) reproxy = const Proxy instance FunctionIO f => GSizeList (Rec0 (f p)) where - gsizeList = Seq.singleton . matSizes . reproxy + gsizeList = Seq.singleton . fioMatSizes . reproxy where reproxy :: Proxy (Rec0 (f p) q) -> Proxy f reproxy = const Proxy @@ -188,19 +170,19 @@ reproxy :: Proxy (C1 c f a) -> Proxy (f a) reproxy = const Proxy -instance (GFromVector f a) => GFromVector (S1 s f) a where +instance GFromVector f a => GFromVector (S1 s f) a where gfromVector name vs = M1 . gfromVector name vs . reproxy where reproxy :: Proxy (S1 s f a) -> Proxy (f a) reproxy = const Proxy -instance (FunctionIO f, Viewable a) => GFromVector (Rec0 (f a)) a where +instance FunctionIO f => GFromVector (Rec0 (f a)) a where gfromVector name ms = const (K1 j) where - j = case fromMat m of + j = case fromFioMat m of Right j' -> j' Left err -> - error $ "\"" ++ name ++ "\" GFromVector fromMat error: " ++ err + error $ "\"" ++ name ++ "\" GFromVector fromFioMat error: " ++ err m = case V.toList ms of [m'] -> m' _ -> error $ "\"" ++ name ++ "\" GFromVector Rec0 length error, " ++ @@ -217,11 +199,8 @@ instance GToVector f a => GToVector (M1 i d f) a where gtoVector = gtoVector . unM1 -instance View f => GToVector (Rec0 (J f a)) a where - gtoVector = Seq.singleton . unsafeUnJ . unK1 - instance (View f, View g) => GToVector (Rec0 (M.M f g a)) a where - gtoVector = Seq.singleton . unM . unK1 + gtoVector = Seq.singleton . unsafeUnM . unK1 --instance GToVector U1 a where -- gtoVector = const Seq.empty diff --git a/src/Dyno/View/Unsafe.hs b/src/Dyno/View/Unsafe.hs new file mode 100644 --- /dev/null +++ b/src/Dyno/View/Unsafe.hs @@ -0,0 +1,334 @@ +{-# 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(..), Viewable(..), M(..), J, S, JV + , mkM, mkM', unM, unM' + ) where + +import GHC.Generics hiding ( S ) + +import qualified Data.Foldable as F +import qualified Data.Sequence as Seq +import Data.Proxy ( Proxy(..) ) +import qualified Data.Vector as V +import qualified Data.Binary as B +import qualified Data.Serialize as S + +import Casadi.CMatrix ( CMatrix ) +import qualified Casadi.CMatrix as CM +import Casadi.Overloading ( ArcTan2(..), Erf(..), Fmod(..), SymOrd(..) ) +import Casadi.Viewable ( Viewable(..) ) + +import Dyno.Vectorize ( Vectorize(..), Id, devectorize, vlength ) + +-- | Matrix with dimensions encoded as Views. +newtype M (f :: * -> *) (g :: * -> *) (a :: *) = + UnsafeM { unsafeUnM :: a } deriving (Eq, Functor, Generic) + +-- | Type alias for a column vector view. +type J f = M f (JV Id) + +-- | Type alias for a scalar view. +type S = M (JV Id) (JV Id) + +instance (View f, View g, Viewable a, B.Binary a) => B.Binary (M f g a) where + put = B.put . unM + get = do + x <- B.get + case mkM' x of + Right y -> return y + Left msg -> fail msg + +instance (View f, View g, Viewable a, S.Serialize a) => S.Serialize (M f g a) where + put = S.put . unM + get = do + x <- S.get + case mkM' x of + Right y -> return y + Left msg -> fail msg + +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 x = mkM $ fromRational x * 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 + +instance (View f, View g, CMatrix a) => Erf (M f g a) where + erf = over erf + erfinv = over erfinv + +mkM' :: forall f g a + . (View f, View g, Viewable 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' = vsize1 x + ny' = vsize2 x + zeros = mkM (vrecoverDimension x (nx, ny)) + +unM' :: forall f g a + . (View f, View g, Viewable a) + => M f g a -> Either String a +unM' (UnsafeM x) + | nx == nx' && ny == ny' = Right x + | otherwise = Left $ "unM' length mismatch: " ++ + "typed size: " ++ show (nx, ny) ++ + ", actual size: " ++ show (nx', ny') + where + nx = size (Proxy :: Proxy f) + ny = size (Proxy :: Proxy g) + nx' = vsize1 x + ny' = vsize2 x + +mkM :: (View f, View g, Viewable a) => a -> M f g a +mkM x = case mkM' x of + Right r -> r + Left msg -> error msg + +unM :: (View f, View g, Viewable a) => M f g a -> a +unM x = case unM' x of + Right r -> r + Left msg -> error msg + + +-- | 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 = mkM . vvertcat . 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 (unM x')) + --, "leftovers: " ++ show leftovers + , "errors: " ++ show (reverse errors) + ] + where + x = unM 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 (unM 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 (mkM 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) + +------------------------------- JV ----------------------------------- +-- | views into Vectorizable things +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 + cat = mkM . vvertcat . vectorize . unJV + size = const $ vlength (Proxy :: Proxy f) + sizes = const . Seq.singleton . (vlength (Proxy :: Proxy f) +) + split :: forall a . Viewable a => J (JV f) a -> JV f a + split = JV . devectorize . flip vvertsplit ks . unM + where + ks = V.fromList (take (n+1) [0..]) + n = size (Proxy :: Proxy (JV f)) diff --git a/src/Dyno/View/Unsafe/M.hs b/src/Dyno/View/Unsafe/M.hs deleted file mode 100644 --- a/src/Dyno/View/Unsafe/M.hs +++ /dev/null @@ -1,125 +0,0 @@ -{-# OPTIONS_GHC -Wall #-} -{-# LANGUAGE ScopedTypeVariables #-} -{-# LANGUAGE KindSignatures #-} -{-# LANGUAGE FlexibleInstances #-} -{-# LANGUAGE DeriveFunctor #-} -{-# LANGUAGE DeriveGeneric #-} - -module Dyno.View.Unsafe.M - ( M(..) - , mkM - , mkM' - , blockSplit - ) where - -import GHC.Generics ( Generic ) - -import Data.Proxy -import qualified Data.Binary as B -import qualified Data.Serialize as S -import qualified Data.Foldable as F -import qualified Data.Vector as V -import Data.Vector ( Vector ) - -import Casadi.Overloading ( Fmod(..), ArcTan2(..), SymOrd(..) ) -import Casadi.DMatrix ( DMatrix ) -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 (View f, View g) => B.Binary (M f g DMatrix) where - put = B.put . unM - get = fmap mkM B.get - -instance (View f, View g) => S.Serialize (M f g DMatrix) where - put = S.put . unM - get = fmap mkM S.get - -instance Show a => Show (M f g a) where - showsPrec p (UnsafeM x) = showsPrec p x - -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 deleted file mode 100644 --- a/src/Dyno/View/Unsafe/View.hs +++ /dev/null @@ -1,273 +0,0 @@ -{-# 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 qualified Data.Vector as V -import qualified Data.Binary as B -import qualified Data.Serialize as S - -import qualified Casadi.CMatrix as CM - -import Dyno.View.Viewable ( Viewable(..) ) - -newtype J (f :: * -> *) (a :: *) = UnsafeJ { unsafeUnJ :: a } deriving (Eq, Generic) - -instance (View f, B.Binary a, Viewable a) => B.Binary (J f a) where - put = B.put . unJ - get = do - x <- B.get - case mkJ' x of - Right y -> return y - Left msg -> fail msg -instance (View f, S.Serialize a, Viewable a) => S.Serialize (J f a) where - put = S.put . unJ - get = do - x <- S.get - case mkJ' x of - Right y -> return y - Left msg -> fail msg - -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,1::Int) ++ - ", actual size: " ++ show (nx',ny') - 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 @@ -6,13 +6,13 @@ {-# LANGUAGE DeriveTraversable #-} module Dyno.View.View - ( View(..) - , J + ( View(..), JV + , J, S , JNone(..), JTuple(..), JTriple(..), JQuad(..) , jfill , v2d, d2v , fmapJ, unzipJ - , fromDMatrix + , splitJV, catJV ) where import GHC.Generics ( Generic, Generic1 ) @@ -26,11 +26,8 @@ import qualified Casadi.DMatrix as DMatrix import qualified Casadi.CMatrix as CM -import Dyno.View.Viewable ( Viewable(..) ) -import Dyno.Vectorize ( Vectorize(..) ) - - -import Dyno.View.Unsafe.View +import Dyno.Vectorize ( Vectorize(..), devectorize ) +import Dyno.View.Unsafe ( View(..), J, S, JV, mkM, unM ) -- some helper types data JNone a = JNone deriving ( Eq, Generic, Generic1, Show, Functor, F.Foldable, T.Traversable ) @@ -44,23 +41,26 @@ instance (View f0, View f1, View f2, View f3) => View (JQuad f0 f1 f2 f3) jfill :: forall a f . View f => a -> J f (Vector a) -jfill x = mkJ (V.replicate n x) +jfill x = mkM (V.replicate n x) where n = size (Proxy :: Proxy f) -fromDMatrix :: (CM.CMatrix a, Viewable a, View f) => J f DMatrix.DMatrix -> J f a -fromDMatrix = mkJ . CM.fromDMatrix . unJ - v2d :: View f => J f (V.Vector Double) -> J f DMatrix.DMatrix -v2d = mkJ . CM.fromDVector . unJ +v2d = mkM . CM.fromDVector . unM d2v :: View f => J f DMatrix.DMatrix -> J f (V.Vector Double) -d2v = mkJ . DMatrix.dnonzeros . CM.densify . unJ +d2v = mkM . DMatrix.dnonzeros . CM.densify . unM fmapJ :: View f => (a -> b) -> J f (Vector a) -> J f (Vector b) -fmapJ f = mkJ . V.map f . unJ +fmapJ f = mkM . V.map f . unM unzipJ :: View f => J f (Vector (a,b)) -> (J f (Vector a), J f (Vector b)) -unzipJ v = (mkJ x, mkJ y) +unzipJ v = (mkM x, mkM y) where - (x,y) = V.unzip (unJ v) + (x,y) = V.unzip (unM v) + +splitJV :: Vectorize f => J (JV f) (Vector a) -> f a +splitJV = devectorize . unM + +catJV :: Vectorize f => f a -> J (JV f) (Vector a) +catJV = mkM . vectorize diff --git a/src/Dyno/View/Viewable.hs b/src/Dyno/View/Viewable.hs deleted file mode 100644 --- a/src/Dyno/View/Viewable.hs +++ /dev/null @@ -1,72 +0,0 @@ -{-# OPTIONS_GHC -Wall #-} - -module Dyno.View.Viewable - ( Viewable(..) - ) where - -import qualified Data.Vector as V - -import qualified Casadi.SX as SX -import qualified Casadi.MX as MX -import qualified Casadi.DMatrix as DMatrix -import qualified Casadi.CMatrix as CM - -class Viewable a where - vvertsplit :: a -> V.Vector Int -> V.Vector a - vhorzsplit :: a -> V.Vector Int -> V.Vector a - vveccat :: V.Vector a -> a - vsize1 :: a -> Int - vsize2 :: a -> Int - vrecoverDimension :: a -> Int -> a - -instance Viewable SX.SX where - vveccat = CM.veccat - vvertsplit = CM.vertsplit - vhorzsplit = CM.horzsplit - vsize1 = CM.size1 - vsize2 = CM.size2 - vrecoverDimension _ k = CM.zeros (k,1) - -instance Viewable MX.MX where - vveccat = CM.veccat - vvertsplit = CM.vertsplit - vhorzsplit = CM.horzsplit - vsize1 = CM.size1 - vsize2 = CM.size2 - vrecoverDimension _ k = CM.zeros (k,1) - -instance Viewable DMatrix.DMatrix where - vveccat = CM.veccat - vvertsplit = CM.vertsplit - vhorzsplit = CM.horzsplit - vsize1 = CM.size1 - vsize2 = CM.size2 - vrecoverDimension _ k = CM.zeros (k,1) - ---instance CM.CasadiMat a => Viewable a where --- vveccat = CM.veccat --- vvertsplit = CM.vertsplit --- vhorzsplit = CM.horzsplit --- vsize1 x --- | CM.size2 x == 1 = CM.size1 x --- | otherwise = error "Dyno.View.Viewable(vsize1): not a column!!" - -instance Viewable (V.Vector a) where - vsize1 = V.length - vsize2 = const 1 - vveccat = V.concat . V.toList - vvertsplit x ks = V.fromList (split x (V.toList ks)) - vhorzsplit _ _ = error "vhorzsplit not defined for Vector" - vrecoverDimension x _ = x - -split :: V.Vector a -> [Int] -> [V.Vector a] -split v xs@(0:_) = split' v xs -split _ _ = error "split: first index must be 0" - -split' :: V.Vector a -> [Int] -> [V.Vector a] -split' _ [] = error "can't split with no input" -split' x [kf] - | V.length x == kf = [] - | otherwise = error "split: last index must be length of vector" -split' x (k0:k1:ks) = V.slice k0 (k1 - k0) x : split' x (k1:ks) - diff --git a/tests/FittingTests.hs b/tests/FittingTests.hs new file mode 100644 --- /dev/null +++ b/tests/FittingTests.hs @@ -0,0 +1,95 @@ +{-# OPTIONS_GHC -Wall #-} +{-# LANGUAGE DataKinds #-} +{-# LANGUAGE RankNTypes #-} + +module FittingTests + ( fittingTests + ) where + +import Casadi.Option ( Opt(..) ) +import Casadi.Overloading ( ArcTan2 ) +import qualified Data.Map as M +import qualified Test.HUnit.Base as HUnit +import Test.Framework ( Test, testGroup ) +import Test.Framework.Providers.HUnit ( testCase ) +import Text.Printf ( printf ) + +import Dyno.Fitting ( l1Fit, l2Fit, lInfFit ) +import Dyno.Nlp ( Bounds ) +import Dyno.Solvers ( Solver, ipoptSolver ) +import Dyno.TypeVecs ( Vec ) +import qualified Dyno.TypeVecs as TV +import Dyno.Vectorize + +toHUnit :: IO (Maybe String) -> HUnit.Assertion +toHUnit f = HUnit.assert $ do + r <- f + case r of + Just msg -> return (HUnit.assertString msg) + Nothing -> return (HUnit.assertBool "LGTM" True) + +solver :: Solver +solver = ipoptSolver + +-- Our data set is [1, 2, 1] +-- +-- y ^ +-- 2.0 - | * +-- 1.5 - | +-- 1.0 - | * * +-- 0.5 - | +-- 0.0 - | +-- +------------> +-- x +-- +-- The model is f(c, x) = c +-- So the L1 minimum should be 1 +-- L2 minimum should be 4/3 +-- Linf minimum should be 3/2 + +fitModel :: Id a -> None a -> a +fitModel (Id c) None = c + +qbounds :: Id Bounds +qbounds = Id (Nothing, Nothing) + +gbounds :: None Bounds +gbounds = None + +fitData :: Vec 3 (None Double, Double) +fitData = fmap (\x -> (None, x)) $ TV.mkVec' [1, 2, 1] + +mapOptions :: M.Map String Opt +mapOptions = + M.fromList + [("parallelization", Opt "serial")] + --[("parallelization", Opt "openmp")] + +testFit :: + Double + -> (Solver + -> (forall a . (Floating a, ArcTan2 a) => Id a -> None a -> a) + -> (forall a . (Floating a, ArcTan2 a) => Id a -> None a) + -> Maybe (Id Double) + -> Id Bounds + -> None Bounds + -> M.Map String Opt + -> Vec 3 (None Double, Double) + -> IO (Either String (Id Double)) + ) + -> HUnit.Assertion +testFit expectedValue fit = toHUnit $ do + ret <- fit solver fitModel (const None) Nothing qbounds gbounds mapOptions fitData + return $ case ret of + Left msg -> Just msg + Right (Id x) + | abs (x - expectedValue) <= 1e-9 -> Nothing + | otherwise -> Just $ printf "expected %.4f, got %.4f" expectedValue x + +fittingTests :: Test +fittingTests = + testGroup "fitting tests" + [ testCase "L1 fit" (testFit 1 l1Fit) + , testCase "L2 fit" (testFit (4/3) l2Fit) + , testCase "L-infinity fit" (testFit (3/2) lInfFit) + ] diff --git a/tests/IntegrationTests.hs b/tests/IntegrationTests.hs --- a/tests/IntegrationTests.hs +++ b/tests/IntegrationTests.hs @@ -26,8 +26,7 @@ import Linear ( Additive ) import Dyno.Vectorize ( Vectorize(..), None(..), devectorize, fill ) -import Dyno.View.View ( View(..), J ) -import Dyno.View.JV ( splitJV ) +import Dyno.View.View ( View(..), J, splitJV ) import Dyno.TypeVecs ( Dim ) import Dyno.Solvers import Dyno.Nlp ( NlpOut(..) ) diff --git a/tests/MapTests.hs b/tests/MapTests.hs new file mode 100644 --- /dev/null +++ b/tests/MapTests.hs @@ -0,0 +1,223 @@ +{-# OPTIONS_GHC -Wall #-} +{-# LANGUAGE DataKinds #-} +{-# LANGUAGE TypeOperators #-} + +module MapTests + ( mapTests + ) where + +import qualified Casadi.CMatrix as CM +import Casadi.DMatrix ( DMatrix ) +import Casadi.Option ( Opt ) +import Casadi.SX ( SX ) +import qualified Data.Map as M +import Data.Proxy ( Proxy(..) ) +import qualified Data.Vector as V +import Linear +import qualified Test.HUnit.Base as HUnit +import Test.Framework ( Test, testGroup ) +import Test.Framework.Providers.HUnit ( testCase ) +import Text.Printf ( printf ) + +import Dyno.Vectorize +import Dyno.View.Fun +import Dyno.View.HList +import Dyno.View.M ( M, hcat, hsplit, vcat, vsplit ) +import Dyno.View.MapFun +import Dyno.View.JVec +import Dyno.View.View +import Dyno.View.Unsafe ( mkM ) + +toHUnit :: IO (Maybe String) -> HUnit.Assertion +toHUnit f = HUnit.assert $ do + r <- f + case r of + Just msg -> return (HUnit.assertString msg) + Nothing -> return (HUnit.assertBool "LGTM" True) + +blockcat' :: [[DMatrix]] -> DMatrix +blockcat' = CM.blockcat . fmap V.fromList . V.fromList + +testFun0 :: + (Proxy 4 -> String -> SXFun (J (JV V2)) (J (JV V3)) -> M.Map String Opt + -> IO (Fun + (M (JV V2) (JVec 4 (JV Id))) + (M (JV V3) (JVec 4 (JV Id))) + ) + ) + -> HUnit.Assertion +testFun0 theMapFun = toHUnit $ do + let f :: J (JV V2) SX -> J (JV V3) SX + f x = vcat $ V3 (10*x0) (100*x1) (1000*x1) + where + V2 x0 x1 = vsplit x + + fun <- toSXFun "v2_in_v3_out" f :: IO (SXFun (J (JV V2)) (J (JV V3))) + mapF <- theMapFun Proxy "map_v2_in_v3_out" fun M.empty + + let input :: M (JV V2) (JVec 4 (JV Id)) DMatrix + input = mkM $ blockcat' + [ [1, 3, 5, 7] + , [2, 4, 6, 8] + ] + + out <- eval mapF input :: IO (M (JV V3) (JVec 4 (JV Id)) DMatrix) + let expectedOut = mkM $ blockcat' + [ [ 10, 30, 50, 70] + , [ 200, 400, 600, 800] + , [2000, 4000, 6000, 8000] + ] + + return $ + if out == expectedOut + then Nothing + else Just $ printf "expected: %s\nactual: %s" (show expectedOut) (show out) + +testFun1 :: + (Proxy 4 -> String -> SXFun (J (JV V2) :*: S) (J (JV V3) :*: S) + -> M.Map String Opt + -> IO (Fun + (M (JV V2) (JVec 4 (JV Id)) :*: M (JV Id) (JVec 4 (JV Id))) + (M (JV V3) (JVec 4 (JV Id)) :*: M (JV Id) (JVec 4 (JV Id))) + ) + ) + -> HUnit.Assertion +testFun1 theMapFun = toHUnit $ do + let f :: (J (JV V2) :*: S) SX -> (J (JV V3) :*: S) SX + f (x :*: y) = o0 :*: o1 + where + o0 = vcat $ V3 (10*x0) (100*x1) (1000*x1) + o1 = vcat $ Id (2*y0) + V2 x0 x1 = vsplit x + Id y0 = vsplit y + + fun <- toSXFun "v2id_in_v3id_out" f + mapF <- theMapFun Proxy "map_v2id_in_v3id_out" fun M.empty + + let input0 :: M (JV V2) (JVec 4 (JV Id)) DMatrix + input0 = mkM $ blockcat' + [ [1, 3, 5, 7] + , [2, 4, 6, 8] + ] + input1 :: M (JV Id) (JVec 4 (JV Id)) DMatrix + input1 = mkM $ blockcat' + [ [1, 2, 3, 4] + ] + + out0 :*: out1 <- eval mapF (input0 :*: input1) + let expectedOut0 = mkM $ blockcat' + [ [ 10, 30, 50, 70] + , [ 200, 400, 600, 800] + , [2000, 4000, 6000, 8000] + ] + expectedOut1 = mkM $ blockcat' [[2, 4, 6, 8]] + + msg0 = printf "output 0\nexpected: %s\nactual: %s" (show expectedOut0) (show out0) + msg1 = printf "output 1\nexpected: %s\nactual: %s" (show expectedOut1) (show out1) + return $ case (out0 == expectedOut0, out1 == expectedOut1) of + (True, True) -> Nothing + (False, True) -> Just msg0 + (True, False) -> Just msg1 + (False, False) -> Just (msg0 ++ "\n" ++ msg1) + + +testFun2 :: + (Proxy 2 -> String -> SXFun (M (JV V2) (JV V3)) (M (JV V3) (JV V4)) + -> M.Map String Opt + -> IO (Fun + (M (JV V2) (JVec 2 (JV V3))) + (M (JV V3) (JVec 2 (JV V4))) + ) + ) + -> HUnit.Assertion +testFun2 theMapFun = toHUnit $ do + let f :: M (JV V2) (JV V3) SX -> M (JV V3) (JV V4) SX + f x = vcat (V3 o0 o1 o2) + where + V2 x0 x1 = vsplit x + V3 x00 x01 x02 = hsplit x0 + V3 x10 x11 x12 = hsplit x1 + + o0 = hcat $ V4 (x00) (2*x01) (3*x02) 8 + o1 = hcat $ V4 (x10) (2*x11) (3*x12) 9 + o2 = hcat $ V4 (4*x00) (5*x01) (6*x02) 10 + + fun <- toSXFun "f" f + mapF <- theMapFun Proxy "map_f" fun M.empty + + let input :: M (JV V2) (JVec 2 (JV V3)) DMatrix + input = mkM $ blockcat' + [ [1, 3, 5, 10, 12, 14] + , [2, 4, 6, 11, 13, 15] + ] + + out <- eval mapF input + let expectedOut = mkM $ blockcat' + [ [1, 6, 15, 8, 10, 24, 42, 8] + , [2, 8, 18, 9, 11, 26, 45, 9] + , [4, 15, 30, 10, 40, 60, 84, 10] + ] + return $ + if out == expectedOut + then Nothing + else Just $ printf "expected: %s\nactual: %s" (show expectedOut) (show out) + +testFunNonRepeated :: HUnit.Assertion +testFunNonRepeated = toHUnit $ do + let f :: (J (JV V2) :*: S) SX -> (J (JV V3) :*: S) SX + f (x :*: y) = o0 :*: o1 + where + o0 = vcat $ V3 (10*x0) (100*x1) (1000*x1) + o1 = vcat $ Id (2*y0) + V2 x0 x1 = vsplit x + Id y0 = vsplit y + + fun <- toSXFun "f" f + mapF <- mapFun' (Proxy :: Proxy 5) "map_f" fun M.empty + + let input0 :: M (JV V2) (JV Id) DMatrix + input0 = mkM $ blockcat' + [ [1] + , [2] + ] + input1 :: M (JV Id) (JVec 5 (JV Id)) DMatrix + input1 = mkM $ blockcat' + [ [1, 2, 3, 4, 5] + ] + + out0 :*: out1 <- eval mapF (input0 :*: input1) + let expectedOut0 ::M (JV V3) (JV Id) DMatrix + expectedOut0 = mkM $ blockcat' + [ [ 50] + , [ 1000] + , [10000] + ] + expectedOut1 ::M (JV Id) (JVec 5 (JV Id)) DMatrix + expectedOut1 = mkM $ blockcat' [[2, 4, 6, 8 ,10]] + + msg0 = printf "output 0\nexpected: %s\nactual: %s" (show expectedOut0) (show out0) + msg1 = printf "output 1\nexpected: %s\nactual: %s" (show expectedOut1) (show out1) + return $ case (out0 == expectedOut0, out1 == expectedOut1) of + (True, True) -> Nothing + (False, True) -> Just msg0 + (True, False) -> Just msg1 + (False, False) -> Just (msg0 ++ "\n" ++ msg1) + + +mapTests :: Test +mapTests = + testGroup "map tests" + [ testGroup "V2 in, V3 out" + [ testCase "mapFun" $ testFun0 mapFun + , testCase "mapFun'" $ testFun0 mapFun' + ] + , testGroup "(V2 :*: Id) in, (V3 :*: Id) out" + [ testCase "mapFun" $ testFun1 mapFun + , testCase "mapFun'" $ testFun1 mapFun' + ] + , testGroup "(M V2 V3) in, (M V3 V4) out" + [ testCase "mapFun" $ testFun2 mapFun + , testCase "mapFun'" $ testFun2 mapFun' + ] + , testCase "non-repeated" testFunNonRepeated + ] diff --git a/tests/NewUnitTests.hs b/tests/NewUnitTests.hs --- a/tests/NewUnitTests.hs +++ b/tests/NewUnitTests.hs @@ -3,20 +3,25 @@ module Main ( main ) where import qualified Data.Monoid as Mo -import Test.Framework ( Test, ColorMode(..), RunnerOptions'(..), TestOptions'(..) - , defaultMainWithOpts ) +import Test.Framework + ( Test, ColorMode(..), RunnerOptions'(..), TestOptions'(..) + , defaultMainWithOpts ) import QuadratureTests ( quadratureTests ) import VectorizeTests ( vectorizeTests ) import ViewTests ( viewTests ) import IntegrationTests ( integrationTests ) +import MapTests ( mapTests ) +import FittingTests ( fittingTests ) main :: IO () main = defaultMainWithOpts tests opts tests :: [Test] tests = - [ quadratureTests + [ fittingTests + , mapTests + , quadratureTests , integrationTests , vectorizeTests , viewTests diff --git a/tests/QuadratureTests.hs b/tests/QuadratureTests.hs --- a/tests/QuadratureTests.hs +++ b/tests/QuadratureTests.hs @@ -18,8 +18,7 @@ import Text.Printf ( printf ) import Dyno.Vectorize ( Vectorize(..), None(..), Id(..) ) -import Dyno.View.View ( View(..), J ) -import Dyno.View.JV ( splitJV ) +import Dyno.View.View ( View(..), J, splitJV ) import Dyno.Solvers import Dyno.Nlp ( NlpOut(..), Bounds ) import Dyno.NlpUtils diff --git a/tests/VectorizeTests.hs b/tests/VectorizeTests.hs --- a/tests/VectorizeTests.hs +++ b/tests/VectorizeTests.hs @@ -20,8 +20,10 @@ import Linear import Linear.V +import qualified Test.HUnit.Base as HUnit import Test.QuickCheck import Test.Framework ( Test, testGroup ) +import Test.Framework.Providers.HUnit ( testCase ) import Test.Framework.Providers.QuickCheck2 ( testProperty ) import Dyno.Vectorize @@ -170,9 +172,36 @@ prop_transpose :: Dims -> Dims -> Bool prop_transpose (Dims _ n) (Dims _ m) = transposeUnTranspose n m +test_vdiag :: HUnit.Assertion +test_vdiag = HUnit.assertEqual "" x y + where + x :: V3 (V3 Int) + x = V3 + (V3 7 0 0) + (V3 0 8 0) + (V3 0 0 9) + + y :: V3 (V3 Int) + y = vdiag (V3 7 8 9) + +test_vdiag' :: HUnit.Assertion +test_vdiag' = HUnit.assertEqual "" x y + where + x :: V3 (V3 Int) + x = V3 + (V3 7 3 3) + (V3 3 8 3) + (V3 3 3 9) + + y :: V3 (V3 Int) + y = vdiag' (V3 7 8 9) 3 + + vectorizeTests :: Test vectorizeTests = testGroup "vectorize tests" [ testProperty "vec . devec" prop_vecThenDevec , testProperty "transposeUnTranspose" prop_transpose + , testCase "vdiag" test_vdiag + , testCase "vdiag'" test_vdiag' ] diff --git a/tests/ViewTests.hs b/tests/ViewTests.hs --- a/tests/ViewTests.hs +++ b/tests/ViewTests.hs @@ -3,6 +3,7 @@ {-# LANGUAGE GADTs #-} {-# LANGUAGE DeriveGeneric #-} {-# LANGUAGE FlexibleInstances #-} +{-# LANGUAGE DataKinds #-} {-# LANGUAGE PolyKinds #-} module ViewTests @@ -11,36 +12,38 @@ , viewTests ) where -import GHC.Generics ( Generic1 ) +import GHC.Generics ( Generic, Generic1 ) import qualified Data.Map as M import Data.Proxy ( Proxy(..) ) import qualified Data.Binary as B import qualified Data.Serialize as S import qualified Data.Traversable as T +import Linear ( V1(..), V2(..), V3(..), V4(..) ) import qualified Numeric.LinearAlgebra as Mat +import Data.Vector ( Vector ) import qualified Data.Vector as V -import GHC.Generics ( Generic ) import System.IO.Unsafe ( unsafePerformIO ) +import qualified Test.HUnit.Base as HUnit import Test.QuickCheck import Test.Framework ( Test, testGroup ) +import Test.Framework.Providers.HUnit ( testCase ) import Test.Framework.Providers.QuickCheck2 ( testProperty ) import Casadi.Function ( evalDMatrix ) import Casadi.MXFunction ( mxFunction ) import Casadi.CMatrix ( CMatrix ) +import qualified Casadi.CMatrix as CM 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 Casadi.Viewable ( Viewable ) +import Dyno.View.Unsafe ( M(UnsafeM), mkM ) import Dyno.TypeVecs ( Vec, Dim ) import Dyno.Vectorize ( Vectorize(..), Id, fill ) -import Dyno.View.View ( View(..), JNone, JTuple, JTriple, JQuad ) -import Dyno.View.JV ( JV ) -import Dyno.View.Viewable ( Viewable ) +import Dyno.View.View ( View(..), J, JV, JNone, JTuple, JTriple, JQuad ) +import Dyno.View.JVec ( JVec ) import Dyno.View.M import Dyno.View.Cov ( Cov, fromMat, toMat ) @@ -97,8 +100,6 @@ , 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) @@ -128,8 +129,6 @@ class MyEq a where myEq :: a -> a -> Bool -instance MyEq a => MyEq (J f a) where - myEq (UnsafeJ x) (UnsafeJ y) = myEq x y instance MyEq a => MyEq (M f g a) where myEq (UnsafeM x) (UnsafeM y) = myEq x y instance MyEq SX where @@ -297,7 +296,7 @@ => 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 + let xj0 = mkM xm0 :: J (JV f) a xj1 = split xj0 :: JV f a xj2 = cat xj1 :: J (JV f) a return $ beEqual xj0 xj2 @@ -485,10 +484,153 @@ return (beEqual m0 m1) +---------- this next part is to test blockcat/blocksplit ----------- +data BV a = BV (J (JV V3) a) (J (JV V1) a) (J (JV V2) a) (J (JV V1) a) + deriving Generic +data BH a = BH (J (JV V2) a) (J (JV V4) a) deriving Generic +instance View BV +instance View BH + +blockcat' :: [[DMatrix]] -> DMatrix +blockcat' = CM.blockcat . V.fromList . map V.fromList + +blockcatScalars :: Num a => [[a]] +blockcatScalars = + [ [ 0, 1, 2, 3, 4, 5] + , [ 6, 7, 8, 9, 10, 11] + , [12, 13, 14, 15, 16, 17] + + , [18, 19, 20, 21, 22, 23] + + , [24, 25, 26, 27, 28, 29] + , [30, 31, 32, 33, 34, 35] + + , [36, 37, 38, 39, 40, 41] + ] + +blockcatBlocks :: Vector (Vector DMatrix) +blockcatBlocks = V.fromList $ map V.fromList + [ [x00, x01] + , [x10, x11] + , [x20, x21] + , [x30, x31] + ] + where + x00 = blockcat' + [ [ 0, 1] + , [ 6, 7] + , [12, 13] + ] + + x01 = blockcat' + [ [ 2, 3, 4, 5] + , [ 8, 9, 10, 11] + , [14, 15, 16, 17] + ] + + x10 = blockcat' [[18, 19]] + x11 = blockcat' [[20, 21, 22, 23]] + + x20 = blockcat' + [ [24, 25] + , [30, 31] + ] + + x21 = blockcat' + [ [26, 27, 28, 29] + , [32, 33, 34, 35] + ] + + x30 = blockcat' [[36, 37]] + x31 = blockcat' [[38, 39, 40, 41]] + +blockCountUp :: M BV BH DMatrix +blockCountUp = countUp + +test_blockcatScalars :: HUnit.Assertion +test_blockcatScalars = HUnit.assertEqual "" x y + where + x :: M BV BH DMatrix + x = blockCountUp + + y :: M BV BH DMatrix + y = mkM $ blockcat' blockcatScalars + +test_blockcatBlocks :: HUnit.Assertion +test_blockcatBlocks = HUnit.assertEqual "" x y + where + x :: M BV BH DMatrix + x = blockCountUp + + y :: M BV BH DMatrix + y = mkM $ CM.blockcat blockcatBlocks + +test_blockSplit :: HUnit.Assertion +test_blockSplit = HUnit.assertEqual "" x y + where + x, y :: V.Vector (V.Vector DMatrix) + x = blockcatBlocks + y = blockSplit blockCountUp + +----------------- sumRows/sumCols --------------- +sumInput :: M (JV V2) (JV V3) DMatrix +sumInput = countUp + +-- make sure the countUp is doing what I expect +test_sumInput :: HUnit.Assertion +test_sumInput = HUnit.assertEqual "" x sumInput + where + x :: M (JV V2) (JV V3) DMatrix + x = vcat (V2 r0 r1) + + r0, r1 :: M (JV Id) (JV V3) DMatrix + r0 = hcat $ V3 0 1 2 + r1 = hcat $ V3 3 4 5 + +test_sumRows :: HUnit.Assertion +test_sumRows = HUnit.assertEqual "" x y + where + x :: M (JV Id) (JV V3) DMatrix + x = hcat (V3 3 5 7) + + y :: M (JV Id) (JV V3) DMatrix + y = sumRows sumInput + +test_sumCols :: HUnit.Assertion +test_sumCols = HUnit.assertEqual "" x y + where + x :: M (JV V2) (JV Id) DMatrix + x = vcat (V2 3 12) + + y :: M (JV V2) (JV Id) DMatrix + y = sumCols sumInput + +test_reshape :: HUnit.Assertion +test_reshape = HUnit.assertEqual "" x y + where + j :: J (JVec 3 (JV V2)) DMatrix + j = countUp + + x :: M (JV V2) (JVec 3 (JV Id)) DMatrix + x = mkM $ blockcat' + [ [0, 2, 4] + , [1, 3, 5] + ] + + y :: M (JV V2) (JVec 3 (JV Id)) DMatrix + y = reshape j + viewTests :: Test viewTests = - testGroup "view tests" - [ prop_VSplitVCat + testGroup "view tests" $ + [ testCase "blockcat scalars" test_blockcatScalars + , testCase "blockcat blocks" test_blockcatBlocks + , testCase "blocksplit" test_blockSplit + , testCase "reshape" test_reshape + , testCase "sumInput" test_sumInput + , testCase "sumRows" test_sumRows + , testCase "sumCols" test_sumCols + , prop_VSplitVCat , prop_HSplitHCat , prop_VSplitVCat' , prop_HSplitHCat'