dynobud 1.8.0.0 → 1.9.0.0
raw patch · 64 files changed
+2964/−1223 lines, 64 filesdep +mwc-randomdep ~Plot-ho-maticdep ~casadi-bindingsdep ~generic-accessors
Dependencies added: mwc-random
Dependency ranges changed: Plot-ho-matic, casadi-bindings, generic-accessors, linear
Files
- dynobud.cabal +21/−10
- examples/BasicNlp.hs +4/−4
- examples/DaePendulum.hs +1/−2
- examples/ExampleDsl/NlpMonad.hs +14/−16
- examples/ExampleDsl/Types.hs +2/−4
- examples/Glider.hs +0/−1
- examples/Homotopy.hs +7/−8
- examples/MultipleShooting.hs +0/−1
- examples/NlpSolverEx.hs +7/−7
- examples/ParallelMap.hs +41/−21
- examples/Quadrature.hs +1/−2
- examples/Rocket.hs +1/−3
- examples/Sailboat.hs +3/−3
- examples/Spring.hs +1/−2
- src/Dyno/AutoScaling.hs +21/−24
- src/Dyno/DirectCollocation.hs +19/−0
- src/Dyno/DirectCollocation/ActiveConstraints.hs +9/−8
- src/Dyno/DirectCollocation/CheckAccuracy.hs +195/−0
- src/Dyno/DirectCollocation/Dynamic.hs +30/−23
- src/Dyno/DirectCollocation/Export.hs +14/−14
- src/Dyno/DirectCollocation/Formulate.hs +93/−92
- src/Dyno/DirectCollocation/FormulateCov.hs +25/−21
- src/Dyno/DirectCollocation/Integrate.hs +22/−20
- src/Dyno/DirectCollocation/Interpolate.hs +4/−5
- src/Dyno/DirectCollocation/Robust.hs +43/−45
- src/Dyno/DirectCollocation/ScaleFactors.hs +182/−0
- src/Dyno/DirectCollocation/Types.hs +8/−10
- src/Dyno/ExportCStruct.hs +242/−0
- src/Dyno/Fitting.hs +475/−0
- src/Dyno/GoldenSectionSearch.hs +41/−0
- src/Dyno/Linearize.hs +170/−0
- src/Dyno/MultipleShooting.hs +16/−17
- src/Dyno/Nlp.hs +5/−7
- src/Dyno/NlpScaling.hs +27/−30
- src/Dyno/NlpSolver.hs +93/−37
- src/Dyno/NlpUtils.hs +23/−24
- src/Dyno/Ocp.hs +2/−4
- src/Dyno/OcpHomotopy.hs +1/−2
- src/Dyno/Random.hs +34/−0
- src/Dyno/SimpleOcp.hs +7/−8
- src/Dyno/TypeVecs.hs +9/−9
- src/Dyno/Vectorize.hs +41/−4
- src/Dyno/View.hs +16/−0
- src/Dyno/View/Cov.hs +16/−35
- src/Dyno/View/Fun.hs +2/−2
- src/Dyno/View/FunJac.hs +7/−7
- src/Dyno/View/HList.hs +3/−3
- src/Dyno/View/JV.hs +0/−52
- src/Dyno/View/JVec.hs +4/−4
- src/Dyno/View/M.hs +63/−49
- src/Dyno/View/MapFun.hs +17/−36
- src/Dyno/View/Scheme.hs +17/−38
- src/Dyno/View/Unsafe.hs +334/−0
- src/Dyno/View/Unsafe/M.hs +0/−125
- src/Dyno/View/Unsafe/View.hs +0/−273
- src/Dyno/View/View.hs +17/−17
- src/Dyno/View/Viewable.hs +0/−72
- tests/FittingTests.hs +95/−0
- tests/IntegrationTests.hs +1/−2
- tests/MapTests.hs +223/−0
- tests/NewUnitTests.hs +8/−3
- tests/QuadratureTests.hs +1/−2
- tests/VectorizeTests.hs +29/−0
- tests/ViewTests.hs +157/−15
dynobud.cabal view
@@ -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 <http://www.github.com/ghorn/dynobud http://www.github.com/ghorn/dynobud> 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
examples/BasicNlp.hs view
@@ -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
examples/DaePendulum.hs view
@@ -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
examples/ExampleDsl/NlpMonad.hs view
@@ -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)
examples/ExampleDsl/Types.hs view
@@ -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
examples/Glider.hs view
@@ -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
examples/Homotopy.hs view
@@ -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
examples/MultipleShooting.hs view
@@ -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
examples/NlpSolverEx.hs view
@@ -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
examples/ParallelMap.hs view
@@ -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
examples/Quadrature.hs view
@@ -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
examples/Rocket.hs view
@@ -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 )
examples/Sailboat.hs view
@@ -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
examples/Spring.hs view
@@ -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 )
src/Dyno/AutoScaling.hs view
@@ -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)
+ src/Dyno/DirectCollocation.hs view
@@ -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
src/Dyno/DirectCollocation/ActiveConstraints.hs view
@@ -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
+ src/Dyno/DirectCollocation/CheckAccuracy.hs view
@@ -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)
src/Dyno/DirectCollocation/Dynamic.hs view
@@ -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)
src/Dyno/DirectCollocation/Export.hs view
@@ -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])
src/Dyno/DirectCollocation/Formulate.hs view
@@ -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
src/Dyno/DirectCollocation/FormulateCov.hs view
@@ -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
src/Dyno/DirectCollocation/Integrate.hs view
@@ -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)
src/Dyno/DirectCollocation/Interpolate.hs view
@@ -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"
src/Dyno/DirectCollocation/Robust.hs view
@@ -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
+ src/Dyno/DirectCollocation/ScaleFactors.hs view
@@ -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')
src/Dyno/DirectCollocation/Types.hs view
@@ -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)
+ src/Dyno/ExportCStruct.hs view
@@ -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 ()))
+ src/Dyno/Fitting.hs view
@@ -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
+ src/Dyno/GoldenSectionSearch.hs view
@@ -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
+ src/Dyno/Linearize.hs view
@@ -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)
src/Dyno/MultipleShooting.hs view
@@ -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
src/Dyno/Nlp.hs view
@@ -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)
src/Dyno/NlpScaling.hs view
@@ -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
src/Dyno/NlpSolver.hs view
@@ -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+
src/Dyno/NlpUtils.hs view
@@ -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)
src/Dyno/Ocp.hs view
@@ -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 :: * -> *
src/Dyno/OcpHomotopy.hs view
@@ -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(..) )
+ src/Dyno/Random.hs view
@@ -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
src/Dyno/SimpleOcp.hs view
@@ -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
src/Dyno/TypeVecs.hs view
@@ -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
src/Dyno/Vectorize.hs view
@@ -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 ()))])
+ src/Dyno/View.hs view
@@ -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
src/Dyno/View/Cov.hs view
@@ -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'
src/Dyno/View/Fun.hs view
@@ -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
src/Dyno/View/FunJac.hs view
@@ -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
src/Dyno/View/HList.hs view
@@ -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
− src/Dyno/View/JV.hs
@@ -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
src/Dyno/View/JVec.hs view
@@ -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)
src/Dyno/View/M.hs view
@@ -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)
src/Dyno/View/MapFun.hs view
@@ -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' #-}
src/Dyno/View/Scheme.hs view
@@ -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
+ src/Dyno/View/Unsafe.hs view
@@ -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))
− src/Dyno/View/Unsafe/M.hs
@@ -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
− src/Dyno/View/Unsafe/View.hs
@@ -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)
src/Dyno/View/View.hs view
@@ -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
− src/Dyno/View/Viewable.hs
@@ -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)-
+ tests/FittingTests.hs view
@@ -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)+ ]
tests/IntegrationTests.hs view
@@ -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(..) )
+ tests/MapTests.hs view
@@ -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+ ]
tests/NewUnitTests.hs view
@@ -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
tests/QuadratureTests.hs view
@@ -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
tests/VectorizeTests.hs view
@@ -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' ]
tests/ViewTests.hs view
@@ -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'