dynobud 1.7.1.0 → 1.8.0.0
raw patch · 29 files changed
+911/−507 lines, 29 filesdep ~casadi-bindingsdep ~casadi-bindings-coredep ~containersnew-component:exe:parallel-map
Dependency ranges changed: casadi-bindings, casadi-bindings-core, containers
Files
- dynobud.cabal +23/−4
- examples/DaePendulum.hs +10/−2
- examples/ExampleDsl/NlpMonad.hs +26/−28
- examples/ExampleDsl/Types.hs +22/−22
- examples/Glider.hs +7/−1
- examples/ParallelMap.hs +65/−0
- examples/Quadrature.hs +7/−1
- examples/Rocket.hs +12/−3
- examples/Sailboat.hs +8/−1
- examples/Spring.hs +10/−2
- src/Dyno/AutoScaling.hs +10/−10
- src/Dyno/DirectCollocation/Export.hs +5/−4
- src/Dyno/DirectCollocation/Formulate.hs +181/−84
- src/Dyno/DirectCollocation/FormulateCov.hs +7/−6
- src/Dyno/DirectCollocation/Robust.hs +2/−2
- src/Dyno/LagrangePolynomials.lhs +2/−3
- src/Dyno/NlpSolver.hs +116/−105
- src/Dyno/NlpUtils.hs +40/−38
- src/Dyno/OcpHomotopy.hs +11/−10
- src/Dyno/SimpleOcp.hs +8/−2
- src/Dyno/Solvers.hs +2/−2
- src/Dyno/View/Fun.hs +140/−96
- src/Dyno/View/M.hs +14/−5
- src/Dyno/View/MapFun.hs +140/−0
- src/Dyno/View/Scheme.hs +21/−12
- src/Dyno/View/Symbolic.hs +0/−56
- tests/IntegrationTests.hs +6/−1
- tests/QuadratureTests.hs +14/−4
- tests/ViewTests.hs +2/−3
dynobud.cabal view
@@ -1,5 +1,5 @@ name: dynobud-version: 1.7.1.0+version: 1.8.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@@ -45,8 +45,8 @@ Dyno.View.JV Dyno.View.JVec Dyno.View.M+ Dyno.View.MapFun Dyno.View.Scheme- Dyno.View.Symbolic Dyno.View.Unsafe.View Dyno.View.Unsafe.M Dyno.View.View@@ -64,9 +64,10 @@ other-modules: build-depends: base >=4.6 && < 5,- casadi-bindings-core >= 2.3.0.0,- casadi-bindings >= 2.3.0.0,+ casadi-bindings-core >= 2.4.1.0,+ casadi-bindings >= 2.4.1.0, -- casadi-bindings-internal,+ data-default-class, jacobi-roots >=0.2 && <0.3, spatial-math >= 0.2.1.0, vector >=0.10,@@ -214,6 +215,22 @@ ghc-options: -threaded -O2 +executable parallel-map+ if flag(examples)+ Buildable: True+ else+ Buildable: False+ hs-source-dirs: examples+ main-is: ParallelMap.hs+ default-language: Haskell2010+ build-depends: dynobud,+ containers,+ casadi-bindings,+ time,+ base >=4.6 && < 5++ ghc-options: -threaded -O2+ executable spring if flag(examples) Buildable: True@@ -244,6 +261,7 @@ build-depends: base >=4.6 && < 5 , dynobud , casadi-bindings+ , containers , vector , generic-accessors , bytestring@@ -465,6 +483,7 @@ test-framework, test-framework-hunit, test-framework-quickcheck2,+ containers, vector, linear, binary,
examples/DaePendulum.hs view
@@ -21,7 +21,9 @@ import Dyno.Nlp import Dyno.NlpUtils import Dyno.Ocp-import Dyno.DirectCollocation.Formulate ( CollProblem(..), makeCollProblem, makeGuess )+import Dyno.DirectCollocation.Formulate+ ( CollProblem(..), DirCollOptions(..), MapStrategy(..)+ , makeCollProblem, makeGuess ) import Dyno.DirectCollocation.Types ( CollTraj' ) import Dyno.DirectCollocation.Dynamic ( toMeta ) import Dyno.DirectCollocation.Quadratures ( QuadratureRoots(..) )@@ -197,10 +199,16 @@ solver2 :: Solver solver2 = ipoptSolver { options = [("expand", Opt True)] } +dirCollOpts :: DirCollOptions+dirCollOpts =+ DirCollOptions+ { mapStrategy = Unrolled+ , collocationRoots = Legendre+ } main :: IO () main = do- cp <- makeCollProblem Legendre pendOcp pendOcpInputs guess+ cp <- makeCollProblem dirCollOpts pendOcp pendOcpInputs guess withCallback $ \send -> do let nlp = cpNlp cp meta = toMeta (cpMetaProxy cp)
examples/ExampleDsl/NlpMonad.hs view
@@ -24,6 +24,7 @@ import qualified Data.Foldable as F import qualified Data.HashSet as HS import qualified Data.Sequence as S+import qualified Data.Map.Lazy as LM import qualified Data.Map.Strict as M import Data.Sequence ( (|>) ) import Data.Vector ( Vector )@@ -31,10 +32,8 @@ import Linear.V ( Dim(..) ) import Data.Proxy -import Casadi.SharedObject ( soInit )-import Casadi.MX ( MX )-import Casadi.SX ( SX )-import Casadi.SXFunction+import Casadi.MX ( MX, sym )+import Casadi.MXFunction import Casadi.Function import Casadi.CMatrix ( veccat ) import qualified Casadi.CMatrix as CM@@ -46,7 +45,6 @@ import Dyno.View.View ( View(..), JNone(..), jfill ) import Dyno.View.JV ( JV ) import Dyno.View.JVec ( JVec )-import qualified Dyno.View.Symbolic as Sym import qualified Dyno.TypeVecs as TV import Dyno.Solvers ( Solver ) import Dyno.NlpUtils ( solveNlp )@@ -55,15 +53,15 @@ import ExampleDsl.LogsAndErrors import ExampleDsl.Types -type SXElement = J (JV Id) SX+type MXElement = J (JV Id) MX -sxElementSym :: String -> IO SXElement-sxElementSym = Sym.sym+mxElementSym :: String -> IO MXElement+mxElementSym name = mkJ <$> sym name -sxElementToSX :: SXElement -> SX-sxElementToSX (UnsafeJ x)+mxElementToMX :: MXElement -> MX+mxElementToMX (UnsafeJ x) | (1,1) == sizes' = x- | otherwise = error $ "sxElementToSX: got non-scalar of size " ++ show sizes'+ | otherwise = error $ "mxElementToMX: got non-scalar of size " ++ show sizes' where sizes' = (CM.size1 x, CM.size2 x) @@ -95,21 +93,21 @@ ((result,logs),state) <- flip runStateT nlp0 . runWriterT . runExceptT . runNlp $ builder return (result, logs, state) -designVar :: String -> NlpMonad SXElement+designVar :: String -> NlpMonad MXElement designVar name = do debug $ "adding design variable \""++name++"\"" state0 <- get let map0 = nlpXSet state0- sym <- liftIO (sxElementSym name)+ newSym <- liftIO (mxElementSym name) when (HS.member name map0) $ err $ name ++ " already in symbol map"- let state1 = state0 { nlpX = nlpX state0 |> (name, sym)+ let state1 = state0 { nlpX = nlpX state0 |> (name, newSym) , nlpXSet = HS.insert name map0 } put state1- return sym+ return newSym infix 4 ===-(===) :: SXElement -> SXElement -> NlpMonad ()+(===) :: MXElement -> MXElement -> NlpMonad () (===) lhs rhs = do debug $ "adding equality constraint: " -- ++ withEllipse 30 (show lhs) ++ " == " ++ withEllipse 30 (show rhs)@@ -117,7 +115,7 @@ put $ state0 { nlpConstraints = nlpConstraints state0 |> Eq2 lhs rhs } infix 4 <==-(<==) :: SXElement -> SXElement -> NlpMonad ()+(<==) :: MXElement -> MXElement -> NlpMonad () (<==) lhs rhs = do debug $ "adding inequality constraint: " -- ++ withEllipse 30 (show lhs) ++ " <= " ++ withEllipse 30 (show rhs)@@ -125,14 +123,14 @@ put $ state0 { nlpConstraints = nlpConstraints state0 |> Ineq2 lhs rhs } infix 4 >==-(>==) :: SXElement -> SXElement -> NlpMonad ()+(>==) :: MXElement -> MXElement -> NlpMonad () (>==) lhs rhs = do debug $ "adding inequality constraint: " -- ++ withEllipse 30 (show lhs) ++ " >= " ++ withEllipse 30 (show rhs) state0 <- get put $ state0 { nlpConstraints = nlpConstraints state0 |> Ineq2 rhs lhs } -bound :: SXElement -> (Double,Double) -> NlpMonad ()+bound :: MXElement -> (Double,Double) -> NlpMonad () bound mid (lhs, rhs) = do debug $ "adding inequality bound: " -- ++ -- withEllipse 30 (show lhs) ++ " <= " ++@@ -141,7 +139,7 @@ state0 <- get put $ state0 { nlpConstraints = nlpConstraints state0 |> Ineq3 mid (lhs, rhs) } -minimize :: SXElement -> NlpMonad ()+minimize :: MXElement -> NlpMonad () minimize obj = do debug $ "setting objective function: " -- ++ withEllipse 30 (show obj) state0 <- get@@ -154,13 +152,13 @@ ObjectiveUnset -> put $ state0 { nlpObj = Objective obj } -constr :: Constraint SXElement -> (SXElement, Bounds)+constr :: Constraint MXElement -> (MXElement, Bounds) constr (Eq2 lhs rhs) = (lhs - rhs, (Just 0, Just 0)) constr (Ineq2 lhs rhs) = (lhs - rhs, (Nothing, Just 0)) constr (Ineq3 x (lhs,rhs)) = (x, (Just lhs, Just rhs)) -toG :: Dim ng => S.Seq (Constraint SXElement) -> Vec ng (SXElement, Bounds)+toG :: Dim ng => S.Seq (Constraint MXElement) -> Vec ng (MXElement, Bounds) toG nlpConstraints' = devectorize $ V.fromList $ F.toList $ fmap constr nlpConstraints' buildNlp :: forall nx ng .@@ -170,24 +168,24 @@ Objective obj' -> return obj' ObjectiveUnset -> error "solveNlp: objective unset" - let inputs :: Vector SXElement+ let inputs :: Vector MXElement inputs = V.fromList $ map snd $ F.toList (nlpX state) - g :: Vec ng SXElement+ g :: Vec ng MXElement gbnd :: Vec ng Bounds (g, gbnd) = TV.tvunzip $ toG (nlpConstraints state) xbnd :: Vec nx Bounds xbnd = fill (Nothing, Nothing) - svector = veccat . fmap sxElementToSX+ svector = veccat . fmap mxElementToMX - sxfun <- sxFunction (V.fromList [svector inputs]) (V.fromList [svector (V.singleton obj), svector (TV.unVec g)])- soInit sxfun+ 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)) where- ret = callMX sxfun (V.singleton (unJ x))+ ret = callMX mxfun (V.singleton (unJ x))+ (AlwaysInline False) (NeverInline False) return Nlp { nlpFG = fg , nlpBX = mkJ (TV.unVec xbnd)
examples/ExampleDsl/Types.hs view
@@ -20,7 +20,7 @@ import qualified Data.Map as M import Control.Lens -import Casadi.SX ( SX )+import Casadi.MX ( MX ) import Dyno.View.View ( J ) import Dyno.View.JV ( JV ) import Dyno.Vectorize ( Id )@@ -32,59 +32,59 @@ data Objective a = ObjectiveUnset | Objective a data HomotopyParam a = HomotopyParamUnset | HomotopyParam a -type SXElement = J (JV Id) SX+type MXElement = J (JV Id) MX data NlpMonadState = NlpMonadState- { nlpX :: S.Seq (String, SXElement)+ { nlpX :: S.Seq (String, MXElement) , nlpXSet :: HS.HashSet String- , nlpConstraints :: S.Seq (Constraint SXElement)- , nlpObj :: Objective SXElement- , nlpHomoParam :: HomotopyParam SXElement+ , nlpConstraints :: S.Seq (Constraint MXElement)+ , nlpObj :: Objective MXElement+ , nlpHomoParam :: HomotopyParam MXElement } -data OcpState = OcpState { ocpPathConstraints :: S.Seq (Constraint SXElement)- , ocpLagrangeObj :: Objective SXElement- , ocpHomoParam :: HomotopyParam SXElement+data OcpState = OcpState { ocpPathConstraints :: S.Seq (Constraint MXElement)+ , ocpLagrangeObj :: Objective MXElement+ , ocpHomoParam :: HomotopyParam MXElement } -data DaeState = DaeState { _daeXDot :: S.Seq (String, SXElement)- , _daeX :: S.Seq (String, SXElement)- , _daeZ :: S.Seq (String, SXElement)- , _daeU :: S.Seq (String, SXElement)- , _daeP :: S.Seq (String, SXElement)- , _daeO :: M.Map String SXElement+data DaeState = DaeState { _daeXDot :: S.Seq (String, MXElement)+ , _daeX :: S.Seq (String, MXElement)+ , _daeZ :: S.Seq (String, MXElement)+ , _daeU :: S.Seq (String, MXElement)+ , _daeP :: S.Seq (String, MXElement)+ , _daeO :: M.Map String MXElement , daeNameSet :: HS.HashSet String- , daeConstraints :: S.Seq (SXElement, SXElement)+ , daeConstraints :: S.Seq (MXElement, MXElement) } --makeLenses ''DaeState-daeXDot :: Lens' DaeState (S.Seq (String, SXElement))+daeXDot :: Lens' DaeState (S.Seq (String, MXElement)) daeXDot f (DaeState xdot' x z u p o ss c) = (\xdot -> DaeState xdot x z u p o ss c) `fmap` f xdot' {-# INLINE daeXDot #-} -daeX :: Lens' DaeState (S.Seq (String, SXElement))+daeX :: Lens' DaeState (S.Seq (String, MXElement)) daeX f (DaeState xdot x' z u p o ss c) = (\x -> DaeState xdot x z u p o ss c) `fmap` f x' {-# INLINE daeX #-} -daeZ :: Lens' DaeState (S.Seq (String, SXElement))+daeZ :: Lens' DaeState (S.Seq (String, MXElement)) daeZ f (DaeState xdot x z' u p o ss c) = (\z -> DaeState xdot x z u p o ss c) `fmap` f z' {-# INLINE daeZ #-} -daeU :: Lens' DaeState (S.Seq (String, SXElement))+daeU :: Lens' DaeState (S.Seq (String, MXElement)) daeU f (DaeState xdot x z u' p o ss c) = (\u -> DaeState xdot x z u p o ss c) `fmap` f u' {-# INLINE daeU #-} -daeP :: Lens' DaeState (S.Seq (String, SXElement))+daeP :: Lens' DaeState (S.Seq (String, MXElement)) daeP f (DaeState xdot x z u p' o ss c) = (\p -> DaeState xdot x z u p o ss c) `fmap` f p' {-# INLINE daeP #-} -daeO :: Lens' DaeState (M.Map String SXElement)+daeO :: Lens' DaeState (M.Map String MXElement) daeO f (DaeState xdot x z u p o' ss c) = (\o -> DaeState xdot x z u p o ss c) `fmap` f o' {-# INLINE daeO #-}
examples/Glider.hs view
@@ -138,11 +138,17 @@ (V3 0 1 0) (V3 0 0 1) +dirCollOpts :: DirCollOptions+dirCollOpts =+ DirCollOptions+ { mapStrategy = Unrolled+ , collocationRoots = Legendre+ } main :: IO () main = do let guess = jfill 1 :: J (CollTraj' GliderOcp NCollStages CollDeg) (Vector Double)- cp <- makeCollProblem Legendre ocp ocpInputs guess+ cp <- makeCollProblem dirCollOpts ocp ocpInputs guess let nlp = cpNlp cp withCallback $ \send -> do let meta = toMeta (cpMetaProxy cp)
+ examples/ParallelMap.hs view
@@ -0,0 +1,65 @@+-- | How to use symbolic map (serial and parallel).++{-# OPTIONS_GHC -Wall #-}+{-# LANGUAGE DataKinds #-}++module Main ( main ) where++import qualified Data.Map as M+import Data.Time.Clock ( getCurrentTime, diffUTCTime )+import Text.Printf ( printf )++import Casadi.DMatrix ( DMatrix )+import Casadi.SX ( SX )+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.MapFun ( mapFun )+import Dyno.View.M ( M, row )+import Dyno.View.JV ( JV, catJV )+import Dyno.View.JVec ( JVec(..) )+import Dyno.View.View ( J, View(..), v2d )++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+ where+ 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+ 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'++ let runOne name someMap input = do+ putStrLn $ "evaluating " ++ name ++ "..."+ t0 <- getCurrentTime+ _ <- eval someMap input+ 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+ (M.fromList [("parallelization", Opt "serial")])+ par <- mapFun "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'
examples/Quadrature.hs view
@@ -201,7 +201,13 @@ compareIntegration :: (QuadratureRoots, StateOrOutput, QuadOrLagrange) -> IO () compareIntegration (roots, stateOrOutput, quadOrLag) = do withCallback $ \send -> do- cp <- makeCollProblem roots (quadOcp stateOrOutput quadOrLag) quadOcpInputs (guess roots)+ let dirCollOpts :: DirCollOptions+ dirCollOpts =+ DirCollOptions+ { mapStrategy = Unrolled+ , collocationRoots = roots+ }+ cp <- makeCollProblem dirCollOpts (quadOcp stateOrOutput quadOrLag) quadOcpInputs (guess roots) let nlp = cpNlp cp meta = toMeta (cpMetaProxy cp) cb traj _ = do
examples/Rocket.hs view
@@ -8,6 +8,7 @@ import GHC.Generics ( Generic, Generic1 ) +import qualified Data.Map as M import Data.Vector ( Vector ) import Accessors ( Lookup )@@ -20,7 +21,8 @@ import Dyno.Solvers ( Solver(..), Opt(..), ipoptSolver ) import Dyno.NlpUtils ( solveNlp ) import Dyno.DirectCollocation.ActiveConstraints-import Dyno.DirectCollocation.Formulate ( CollProblem(..), makeCollProblem )+import Dyno.DirectCollocation.Formulate+ ( CollProblem(..), DirCollOptions(..), MapStrategy(..), makeCollProblem ) import Dyno.DirectCollocation.Types ( CollTraj' ) import Dyno.DirectCollocation.Dynamic ( toMeta ) import Dyno.DirectCollocation.Quadratures ( QuadratureRoots(..) )@@ -166,11 +168,18 @@ type NCollStages = 100 type CollDeg = 3 +dirCollOpts :: DirCollOptions+dirCollOpts =+ DirCollOptions+ { collocationRoots = Legendre+ , mapStrategy = Unrolled+ }+ main :: IO ()-main = +main = withCallback $ \send -> do - cp <- makeCollProblem Legendre rocketOcp rocketOcpInputs guess+ cp <- makeCollProblem dirCollOpts rocketOcp rocketOcpInputs guess let nlp = cpNlp cp meta = toMeta (cpMetaProxy cp)
examples/Sailboat.hs view
@@ -290,9 +290,16 @@ solver = ipoptSolver --solver = snoptSolver { options = [("detect_linear", Opt False)] } +dirCollOpts :: DirCollOptions+dirCollOpts =+ DirCollOptions+ { mapStrategy = Unrolled+ , collocationRoots = Legendre+ }+ main :: IO () main = do- cp <- makeCollProblem Legendre ocp ocpInputs (cat initialGuess)+ cp <- makeCollProblem dirCollOpts ocp ocpInputs (cat initialGuess) let nlp = cpNlp cp ZMQ.withContext $ \context -> withPublisher context urlDynoPlot $ \sendDynoPlotMsg -> do
examples/Spring.hs view
@@ -19,7 +19,8 @@ import Dyno.Vectorize ( Vectorize, None(..), fill ) import Dyno.Solvers ( Solver(..), Opt(..), ipoptSolver ) import Dyno.NlpUtils ( solveNlp )-import Dyno.DirectCollocation.Formulate ( CollProblem(..), makeCollProblem )+import Dyno.DirectCollocation.Formulate+ ( CollProblem(..), DirCollOptions(..), MapStrategy(..), makeCollProblem ) import Dyno.DirectCollocation.Types ( CollTraj' ) import Dyno.DirectCollocation.Dynamic ( toMeta ) import Dyno.DirectCollocation.Quadratures ( QuadratureRoots(..) )@@ -153,11 +154,18 @@ type NCollStages = 100 type CollDeg = 3 +dirCollOpts :: DirCollOptions+dirCollOpts =+ DirCollOptions+ { mapStrategy = Unrolled+ , collocationRoots = Legendre+ }+ main :: IO () main = withCallback $ \send -> do - cp <- makeCollProblem Legendre springOcp springOcpInputs guess+ cp <- makeCollProblem dirCollOpts springOcp springOcpInputs guess let nlp = cpNlp cp meta = toMeta (cpMetaProxy cp)
src/Dyno/AutoScaling.hs view
@@ -16,21 +16,20 @@ --import qualified Numeric.LinearAlgebra.HMatrix as HMat import Text.Printf ( printf ) -import Casadi.Sparsity ( getRow, getCol )-import Casadi.SX ( SX )-import Casadi.DMatrix ( DMatrix, dnonzeros ) import qualified Casadi.CMatrix as CM+import Casadi.DMatrix ( DMatrix, dnonzeros )+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.Vectorize ( Id(..) )-import Dyno.Nlp ( KKT(..), Nlp(..) ) import Dyno.View.View ( View(..), J, JNone(..), v2d, d2v, jfill) import Dyno.View.Viewable ( Viewable )-import qualified Dyno.View.M as M-import Dyno.View.M ( M )-import Dyno.View.JV ( JV, splitJV ) toSparse :: (View f, View g) => String -> M f g DMatrix -> [(Int,Int,Double)]@@ -173,7 +172,8 @@ scalingNlp :: forall x g sdv . (View x, View g, View sdv)- => KKT x g -> (J sdv SX -> (J (JV Id) SX, J x SX, J g SX)) -> Nlp sdv JNone JNone SX+ => KKT x g -> (J sdv MX -> (J (JV Id) MX, J x MX, J g MX))+ -> Nlp sdv JNone JNone MX scalingNlp kkt expand = Nlp { nlpBX = jfill (Nothing, Nothing)@@ -188,7 +188,7 @@ , nlpFG = fg } where- fg :: J sdv SX -> J JNone SX -> (J (JV Id) SX, J JNone SX)+ fg :: J sdv MX -> J JNone MX -> (J (JV Id) MX, J JNone MX) fg sdvs _ = (obj, cat JNone) where obj = toObjective $ toLogScaling kkt expand sdvs
src/Dyno/DirectCollocation/Export.hs view
@@ -35,7 +35,7 @@ import Dyno.Vectorize ( Vectorize, Id(..), None(..), fill ) import Dyno.View.View ( View(..) ) import Dyno.View.JV ( splitJV, catJV )-import Dyno.DirectCollocation.Formulate ( CollProblem(..) )+import Dyno.DirectCollocation.Formulate ( CollProblem(..), DirCollOptions(..) ) import Dyno.DirectCollocation.Types ( CollTraj(..), CollOcpConstraints(..) , StageOutputs(..), Quadratures(..) , getXzus'''@@ -114,7 +114,8 @@ let _ = outs :: Vec n (StageOutputs x o h q qo po deg Double) _ = finalQuads :: Quadratures q qo Double - let taus :: Vec deg Double+ let roots = collocationRoots (cpDirCollOpts cp)+ taus :: Vec deg Double taus = cpTaus cp Id tf = splitJV tf' @@ -200,7 +201,7 @@ , matlabRetName ++ ".T = " ++ show tf ++ ";" , matlabRetName ++ ".N = " ++ show n ++ ";" , matlabRetName ++ ".deg = " ++ show (reflectDim (Proxy :: Proxy deg)) ++ ";"- , matlabRetName ++ ".collocationRoots = '" ++ show (cpRoots cp) ++ "';"+ , matlabRetName ++ ".collocationRoots = '" ++ show roots ++ "';" ] runRet :: State PythonExporter ()@@ -233,7 +234,7 @@ putVal pyRetName ["T"] (show tf) putVal pyRetName ["N"] (show n) putVal pyRetName ["deg"] (show (reflectDim (Proxy :: Proxy deg)))- putVal pyRetName ["collocationRoots"] ("'" ++ show (cpRoots cp) ++ "'")+ putVal pyRetName ["collocationRoots"] ("'" ++ show roots ++ "'") return $ Export { exportMatlab = matlabOut
src/Dyno/DirectCollocation/Formulate.hs view
@@ -7,7 +7,8 @@ module Dyno.DirectCollocation.Formulate ( CollProblem(..)- , StageOutputs(..)+ , DirCollOptions(..)+ , MapStrategy(..) , makeCollProblem , mkTaus , makeGuess@@ -20,6 +21,9 @@ import Control.Applicative import Control.Monad.State ( StateT(..), runStateT )+import Data.Default.Class ( Default(..) )+import Data.Map ( Map )+import qualified Data.Map as M import Data.Maybe ( fromMaybe ) import Data.Proxy ( Proxy(..) ) import Data.Vector ( Vector )@@ -31,17 +35,20 @@ import Casadi.DMatrix ( DMatrix ) import Casadi.MX ( MX )+import Casadi.Option ( Opt(..) ) 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 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 import Dyno.View.JVec( JVec(..), jreplicate ) import Dyno.View.Scheme ( Scheme )-import Dyno.Vectorize ( Vectorize(..), Id(..), fill, vlength, vzipWith )+import Dyno.Vectorize ( Vectorize(..), Id(..), fill, vlength ) import Dyno.TypeVecs ( Vec, Dim, reflectDim ) import qualified Dyno.TypeVecs as TV import Dyno.LagrangePolynomials ( lagrangeDerivCoeffs )@@ -74,12 +81,31 @@ -> J (JV fp) (Vector Double) -> IO (Vec n (StageOutputs x o h q qo po deg Double)) , cpTaus :: Vec deg Double- , cpRoots :: QuadratureRoots+ , cpDirCollOpts :: DirCollOptions , cpEvalQuadratures :: Vec n (Vec deg Double) -> Double -> IO Double , cpMetaProxy :: MetaProxy x z u p o q qo po h+-- , cpJacSparsitySpy :: String+-- , cpHessSparsitySpy :: String } +data MapStrategy =+ Unrolled -- ^ split vector then use haskell fmap+ | Symbolic (Map String Opt) -- ^ use casadi symbolic map, with options+ deriving Show +data DirCollOptions =+ DirCollOptions+ { collocationRoots :: QuadratureRoots -- ^ which collocation roots to use+ , mapStrategy :: MapStrategy+ } deriving Show++instance Default DirCollOptions where+ def =+ DirCollOptions+ { mapStrategy = Unrolled+ , collocationRoots = Radau+ }+ 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)@@ -133,38 +159,6 @@ instance (View r, View o) => Scheme (DaeOut r o) ---toQuadratureOcp :: (Vectorize x, Vectorize q, Vectorize c, Vectorize r)--- => q Double--- -> q Double--- -> q Double--- -> OcpPhase x z u p r o c h q qo po fp--- -> OcpPhase (Tuple x q) z u p (Tuple r q) o (Tuple c q) h None qo po fp---toQuadratureOcp qscale q0scale qdotScale ocp0 =--- OcpPhase--- { ocpMayer = \tf (Tuple x0 _) (Tuple xf qf) None p fp -> ocpMayer ocp0 tf x0 xf qf p fp--- , ocpLagrange = \(Tuple x _) z u p fp o t -> ocpLagrange ocp0 x z u p fp o t--- , ocpQuadratures = \_ _ _ _ _ _ _ _ -> None--- , ocpQuadratureOutputs = \(Tuple x _) -> ocpQuadratureOutputs ocp0 x--- , ocpDae = \(Tuple x' q') (Tuple x _) z u p fp t ->--- let (res0, o) = ocpDae ocp0 x' x z u p fp t--- tf = error "toQuadratureOcp: quadrature derivatives can't use end time"--- dq = ocpQuadratures ocp0 x z u p fp o t tf--- in (Tuple res0 (vzipWith (-) q' dq), o)--- , ocpBc = \(Tuple x0 q0) (Tuple xf qf) None p fp tf ->--- Tuple (ocpBc ocp0 x0 xf qf p fp tf) q0--- , ocpPathC = \(Tuple x _) -> ocpPathC ocp0 x--- , ocpPlotOutputs = \(Tuple x q) z u p o None -> ocpPlotOutputs ocp0 x z u p o q--- , ocpObjScale = ocpObjScale ocp0--- , ocpTScale = ocpTScale ocp0--- , ocpXScale = Just $ Tuple (fromMaybe (fill 1) (ocpXScale ocp0)) qscale--- , ocpZScale = ocpZScale ocp0--- , ocpUScale = ocpUScale ocp0--- , ocpPScale = ocpPScale ocp0--- , ocpResidualScale = Just $ Tuple (fromMaybe (fill 1) (ocpResidualScale ocp0)) qdotScale--- , ocpBcScale = Just $ Tuple (fromMaybe (fill 1) (ocpBcScale ocp0)) q0scale--- , ocpPathCScale = ocpPathCScale ocp0--- }- makeCollProblem :: forall x z u p r o c h q qo po fp deg n . ( Dim deg, Dim n@@ -172,13 +166,14 @@ , Vectorize r, Vectorize o, Vectorize h, Vectorize c, Vectorize q , Vectorize po, Vectorize fp, Vectorize qo )- => QuadratureRoots+ => DirCollOptions -> OcpPhase x z u p r o c h q qo po fp -> OcpPhaseInputs x z u p c h fp -> J (CollTraj x z u p n deg) (Vector Double) -> IO (CollProblem x z u p r o c h q qo po fp n deg)-makeCollProblem roots ocp ocpInputs guess = do+makeCollProblem dirCollOpts ocp ocpInputs guess = do let -- the collocation points+ roots = collocationRoots dirCollOpts taus :: Vec deg Double taus = mkTaus roots @@ -268,7 +263,8 @@ pathCFunSX <- toSXFun "pathCFun" pathCFun let quadraturePlottingFun ::- QuadraturePlottingIn (JV x) (JV z) (JV u) (JV p) (JV o) (JV q) (JV qo) (JV fp) SX -> J (JV po) SX+ 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)@@ -309,8 +305,129 @@ dynFun <- toSXFun "dynamics" dynamicsFunction dynamicsStageFun <- toMXFun "dynamicsStageFunction" (toDynamicsStage callInterpolate cijs dynFun)- callDynamicsStageFun <- fmap call (expandMXFun dynamicsStageFun)+ >>= expandMXFun+ :: IO (SXFun+ (J (JV x)+ :*: J (JVec deg (JTuple (JV x) (JV z)))+ :*: J (JVec deg (JV u))+ :*: J (JV Id)+ :*: J (JV p)+ :*: J (JV fp)+ :*: J (JVec deg (JV Id))+ )+ (J (JVec deg (JV r))+ :*: J (JV x)+ )+ )+-- let callDynamicsStageFun = call dynamicsStageFun + -- dt, parm, and fixedParm have to be repeated+ -- that is why they are row matrices+ let stageFun :: (M (JV Id) (JV Id)+ :*: M (JV Id) (CollStage (JV x) (JV z) (JV u) deg)+ :*: M (JV Id) (JVec deg (JV Id))+ :*: M (JV Id) (JV p)+ :*: M (JV Id) (JV fp)+ ) MX ->+ (M (JV Id) (JVec deg (JV r))+ :*: M (JV Id) (JVec deg (JV h))+ :*: M (JV Id) (JV x)+ ) MX+ stageFun (dt' :*: collStageRow :*: stageTimesRow :*: parm' :*: fixedParm') =+ (M.row dc :*: M.row stageHs :*: M.row interpolatedX')+ where+ dt = M.unrow dt'+ parm = M.unrow parm'+ fixedParm = M.unrow fixedParm'++ stageTimes = M.unrow stageTimesRow+ collStage = M.unrow collStageRow+ CollStage x0 xzus = split collStage+ dc :*: interpolatedX' =+ call dynamicsStageFun+ (x0 :*: xzs :*: us :*: dt :*: parm :*: fixedParm :*: stageTimes)++ pathCStageIn = PathCStageIn collStage parm fixedParm stageTimes dt+ stageHs = pathCStageFun pathCStageIn++ xzs = cat (JVec xzs') :: J (JVec deg (JTuple (JV x) (JV z))) MX+ us = cat (JVec us') :: J (JVec deg (JV u)) MX+ (xzs', us') = TV.tvunzip $ fmap toTuple $ unJVec (split xzus)+ toTuple xzu = (cat (JTuple x z), u)+ where+ CollPoint x z u = split xzu+ stageFunMX <- toMXFun "stageFun" stageFun++ let mapOpts = case mapStrategy dirCollOpts of+ Unrolled -> M.empty+ Symbolic r -> r+ mapStageFunMX <- mapFun'' (Proxy :: Proxy n) "mapDynamicsStageFun" stageFunMX mapOpts+-- use repeated outputs for now+ :: IO (Fun+ ( M (JV Id) (JVec n (JV Id))+ :*: 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) (JVec n (JV p))+ :*: M (JV Id) (JVec n (JV fp))+ )+ ( M (JV Id) (JVec n (JVec deg (JV r)))+ :*: M (JV Id) (JVec n (JVec deg (JV h)))+ :*: M (JV Id) (JVec n (JV x))+ )+ )+---- non-repeated outputs don't work yet, and we need them for exact hessian+-- :: IO (Fun+-- (M (JV Id) (JV Id)+-- :*: 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)+-- :*: M (JV Id) (JV fp)+-- )+-- (M (JV Id) (JVec n (JVec deg (JV r)))+-- :*: M (JV Id) (JVec n (JVec deg (JV h)))+-- :*: M (JV Id) (JVec n (JV x))+-- )+-- )+ let mapStageFun ::+ MapStrategy+ -> ( J (JV Id) 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+ , J (JV fp) MX+ )+ -> ( J (JVec n (JVec deg (JV r))) MX+ , J (JVec n (JVec deg (JV h))) MX+ , J (JVec n (JV x)) MX+ )++ 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'++ (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)+ where+ dc :*: h :*: xnext =+ call stageFunMX+ (dt :*: (M.row stage) :*: (M.row stageTimes) :*: parm :*: fixedParm)+-- dc :*: h :*: xnext =+-- stageFun+-- (dt :*: (M.row stage) :*: (M.row stageTimes) :*: parm :*: fixedParm)++ mapStageFun (Symbolic _) (x0', x1, x2, x3', x4') = (M.unrow y0, M.unrow y1, M.unrow 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)+ let nlp :: Nlp (CollTraj x z u p n deg) (JV fp) (CollOcpConstraints x r c h n deg) MX nlp = Nlp { nlpFG =@@ -335,8 +452,7 @@ ) (call lagrangeStageFunMX) (call quadratureStageFunMX)- (call pathCStageFunMX)- (callDynamicsStageFun)+ (mapStageFun (mapStrategy dirCollOpts)) , nlpBX = cat (ocpPhaseBx ocpInputs) , nlpBG = cat (ocpPhaseBg ocpInputs) , nlpX0 = guess :: J (CollTraj x z u p n deg) (Vector Double)@@ -383,6 +499,7 @@ let stageIntegrals = fmap (unId . splitJV . d2v) stageIntegrals' :: Vec n Double return (F.sum stageIntegrals) + nlpConstraints <- toMXFun "nlp_constraints" (\(x:*:p) -> snd (nlpFG nlp x p)) let evalConstraints x p = do g <- eval nlpConstraints (v2d x :*: v2d p)@@ -395,7 +512,7 @@ , cpConstraints = evalConstraints , cpOutputs = getOutputs , cpTaus = taus- , cpRoots = roots+ , cpDirCollOpts = dirCollOpts , cpEvalQuadratures = evalQuadratures , cpMetaProxy = MetaProxy }@@ -605,20 +722,17 @@ -> (QuadratureStageIn (JV x) (JV z) (JV u) (JV p) (JV fp) deg MX -> J (JV Id) MX) -- quadFun -> (QuadratureStageIn (JV x) (JV z) (JV u) (JV p) (JV fp) deg MX -> J (JV q) MX)- -- pathCStageFun- -> (PathCStageIn (JV x) (JV z) (JV u) (JV p) (JV fp) deg MX -> J (JVec deg (JV h)) MX) -- stageFun- -> ( ( J (JV x)- :*: J (JVec deg (JTuple (JV x) (JV z)))- :*: J (JVec deg (JV u))- :*: J (JV Id)- :*: J (JV p)- :*: J (JV fp)- :*: J (JVec deg (JV Id))- ) MX- -> ( J (JVec deg (JV r))- :*: J (JV x)- ) MX+ -> ( ( J (JV Id) 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+ , J (JV fp) MX+ )+ -> ( J (JVec n (JVec deg (JV r))) MX+ , J (JVec n (JVec deg (JV h))) MX+ , J (JVec n (JV x)) MX+ ) ) -- collTraj -> J (CollTraj x z u p n deg) MX@@ -626,7 +740,8 @@ -> J (JV fp) MX -- (objective, constraints) -> (J (JV Id) MX, J (CollOcpConstraints x r c h n deg) MX)-getFg taus bcFun mayerFun lagQuadFun quadFun pathCStageFun dynamicsStageFun collTraj fixedParm = (obj, cat g)+getFg taus bcFun mayerFun lagQuadFun quadFun+ mapStageFun collTraj fixedParm = (obj, cat g) where -- split up the design vars CollTraj tf parm stages' xf = split collTraj@@ -675,37 +790,19 @@ x0 = (\(CollStage x0' _) -> x0') (TV.tvhead spstages) g = CollOcpConstraints- { coCollPoints = cat $ JVec dcs- , coContinuity = cat $ JVec integratorMatchingConstraints- , coPathC = cat $ JVec hs+ { coCollPoints = dcs+ , coContinuity = integratorMatchingConstraints+ , coPathC = hs , coBc = call bcFun (x0 :*: xf :*: finalQuadratures :*: parm :*: fixedParm :*: tf) } - integratorMatchingConstraints :: Vec n (J (JV x) MX) -- THIS SHOULD BE A NONLINEAR FUNCTION- integratorMatchingConstraints = vzipWith (-) interpolatedXs xfs-- dcs :: Vec n (J (JVec deg (JV r)) MX)- hs :: Vec n (J (JVec deg (JV h)) MX)- interpolatedXs :: Vec n (J (JV x) MX)- (dcs, hs, interpolatedXs) = TV.tvunzip3 $ fmap fff $ TV.tvzip spstages times'- fff :: (CollStage (JV x) (JV z) (JV u) deg MX, J (JVec deg (JV Id)) MX) ->- (J (JVec deg (JV r)) MX, J (JVec deg (JV h)) MX, J (JV x) MX)- fff (CollStage x0' xzus, stageTimes) = (dc, stageHs, interpolatedX')- where- -- todo: could share xdot here instead of embedding in pathc and dynamics- dc :*: interpolatedX' =- dynamicsStageFun (x0' :*: xzs :*: us :*: dt :*: parm :*: fixedParm :*: stageTimes)-- -- todo: don't split/cat this- pathCStageIn = PathCStageIn (cat (CollStage x0 xzus)) parm fixedParm stageTimes dt- stageHs = pathCStageFun pathCStageIn+ integratorMatchingConstraints :: J (JVec n (JV x)) MX -- THIS SHOULD BE A NONLINEAR FUNCTION+ integratorMatchingConstraints = interpolatedXs - (cat (JVec xfs)) - xzs = cat (JVec xzs') :: J (JVec deg (JTuple (JV x) (JV z))) MX- us = cat (JVec us') :: J (JVec deg (JV u)) MX- (xzs', us') = TV.tvunzip $ fmap toTuple $ unJVec (split xzus)- toTuple xzu = (cat (JTuple x z), u)- where- CollPoint x z u = split xzu+ dcs :: J (JVec n (JVec deg (JV r))) MX+ hs :: J (JVec n (JVec deg (JV h))) MX+ interpolatedXs :: J (JVec n (JV x)) MX+ (dcs, hs, interpolatedXs) = mapStageFun (dt, stages', cat (JVec times'), parm, fixedParm) ocpPhaseBx :: forall x z u p c h fp n deg .@@ -981,7 +1078,7 @@ -> p Double -> CollTraj x z u p n deg (Vector Double) makeGuess quadratureRoots tf guessX guessZ guessU parm =- CollTraj (jfill tf) (catJV parm) guesses (catJV (guessX tf))+ CollTraj (catJV (Id tf)) (catJV parm) guesses (catJV (guessX tf)) where -- timestep dt = tf / fromIntegral n
src/Dyno/DirectCollocation/FormulateCov.hs view
@@ -33,7 +33,7 @@ import Dyno.DirectCollocation.Types import Dyno.DirectCollocation.Dynamic ( DynPlotPoints )-import Dyno.DirectCollocation.Quadratures ( QuadratureRoots(..), timesFromTaus )+import Dyno.DirectCollocation.Quadratures ( timesFromTaus ) import Dyno.DirectCollocation.Robust import Dyno.DirectCollocation.Formulate @@ -56,7 +56,7 @@ , ccpCovariances :: MXFun (J (Cov (JV sx)) :*: J (CollTraj (X ocp) (Z ocp) (U ocp) (P ocp) n deg)) (J (CovTraj sx n))- , ccpRoots :: QuadratureRoots+ , ccpDirCollOpts :: DirCollOptions } @@ -83,14 +83,15 @@ , fp ~ None , None ~ FP ocp )- => QuadratureRoots+ => DirCollOptions -> OcpPhase' ocp -> OcpPhaseInputs x z u p c h fp -> OcpPhaseWithCov ocp sx sz sw sr sh shr sc -> J (CollTraj x z u p n deg) (Vector Double) -> IO (CollCovProblem ocp n deg sx sw sh shr sc)-makeCollCovProblem roots ocp ocpInputs ocpCov guess = do+makeCollCovProblem dirCollOpts ocp ocpInputs ocpCov guess = do let -- the collocation points+ roots = collocationRoots dirCollOpts taus :: Vec deg Double taus = mkTaus roots @@ -105,7 +106,7 @@ lagrangeFun <- toSXFun "cov lagrange" $ \(x0:*:x1:*:x2:*:x3) -> catJV' $ Id $ ocpCovLagrange ocpCov (unId (splitJV' x0)) (splitJV' x1) x2 (unId (splitJV' x3)) - cp0 <- makeCollProblem roots ocp ocpInputs guess+ cp0 <- makeCollProblem dirCollOpts ocp ocpInputs guess robustify <- mkRobustifyFunction (ocpCovProjection ocpCov) (ocpCovRobustifyPathC ocpCov) @@ -199,7 +200,7 @@ , ccpOutputs = getOutputs , ccpSensitivities = computeSensitivitiesFun' , ccpCovariances = computeCovariancesFun'- , ccpRoots = roots+ , ccpDirCollOpts = dirCollOpts }
src/Dyno/DirectCollocation/Robust.hs view
@@ -361,8 +361,8 @@ -- TODO: this should be much simpler for radau -- TODO: check these next 4 lines- dsxz_dsx0 = - (M.solve df_dsxz df_dsx0) :: M (JVec deg (JTuple sx sz)) sx MX- dsxz_dsw0 = - (M.solve df_dsxz df_dsw0) :: M (JVec deg (JTuple sx sz)) sw MX+ dsxz_dsx0 = - (M.solve' df_dsxz df_dsx0) :: M (JVec deg (JTuple sx sz)) sx MX+ dsxz_dsw0 = - (M.solve' df_dsxz df_dsw0) :: M (JVec deg (JTuple sx sz)) sw MX dsx1_dsx0 = dg_dsx0 + dg_dsxz `M.mm` dsxz_dsx0 :: M sx sx MX dsx1_dsw0 = dg_dsw0 + dg_dsxz `M.mm` dsxz_dsw0 :: M sx sw MX
src/Dyno/LagrangePolynomials.lhs view
@@ -109,13 +109,13 @@ , interpolate ) where +import qualified Data.Map as M import qualified Data.Foldable as F import qualified Data.Vector as V import Linear ( Additive, (^*), sumV ) import Casadi.SXFunction ( sxFunction ) import Casadi.Function ( evalDMatrix )-import Casadi.SharedObject ( soInit ) import Casadi.SX ( SX, ssym, sgradient ) import Casadi.DMatrix ( DMatrix, dnonzeros ) import Casadi.CMatrix ( densify )@@ -285,8 +285,7 @@ zs = map (lagrangeXis taus tau) [0..deg] inputs = tau : taus zdot = map (`sgradient` tau) zs- zdotAlg <- sxFunction (V.fromList inputs) (V.fromList zdot)- soInit zdotAlg+ zdotAlg <- sxFunction "zdotAlg" (V.fromList inputs) (V.fromList zdot) M.empty --mapM_ print zdot'
src/Dyno/NlpSolver.hs view
@@ -52,28 +52,30 @@ , evalScaledKKT -- * options , Op.Opt(..)- , setOption- , reinit -- * other , MonadIO , liftIO , generateAndCompile ) where -import Text.Printf ( printf )-import Data.Time.Clock ( getCurrentTime, diffUTCTime )-import Data.Proxy ( Proxy(..) )-import System.Process ( callProcess, showCommandForUser ) import Control.Exception ( AsyncException( UserInterrupt ), try ) import Control.Concurrent ( forkIO, newEmptyMVar, takeMVar, putMVar ) import qualified Control.Applicative as A import Control.Monad ( when, void ) import "mtl" Control.Monad.Reader ( MonadIO(..), MonadReader(..), ReaderT(..) )-import Data.Maybe ( fromMaybe ) import Data.IORef ( newIORef, readIORef, writeIORef )+import qualified Data.Map as M+import Data.Maybe ( fromMaybe )+import Data.Proxy ( Proxy(..) )+import qualified Data.Traversable as T+import Data.Time.Clock ( getCurrentTime, diffUTCTime ) import Data.Vector ( Vector ) import qualified Data.Vector as V+import Foreign.C.Types ( CInt ) +import System.Process ( callProcess, showCommandForUser )+import Text.Printf ( printf )+ import Casadi.Core.Enums ( InputOutputScheme(..) ) import qualified Casadi.Core.Classes.Function as C import qualified Casadi.Core.Classes.NlpSolver as C@@ -81,29 +83,28 @@ import qualified Casadi.Core.Classes.IOInterfaceFunction as C import Casadi.Callback ( makeCallback )+import Casadi.CMatrix ( CMatrix )+import qualified Casadi.CMatrix as CM import Casadi.DMatrix ( DMatrix, dnonzeros )-import Casadi.Function ( Function, externalFunction, generateCode )+import Casadi.Function ( Function, evalDMatrix', externalFunction, generateCode )+import Casadi.IOScheme ( mxFunctionWithSchemes )+import Casadi.MX ( MX, symV ) import qualified Casadi.Option as Op import qualified Casadi.GenericC as Gen-import Casadi.SharedObject ( soInit )-import Casadi.CMatrix ( CMatrix )-import qualified Casadi.CMatrix as CM -import Dyno.View.Unsafe.View ( unJ, mkJ )-import Dyno.View.Unsafe.M ( mkM )- import Dyno.FormatTime ( formatSeconds )+import qualified Dyno.View.M as M+import Dyno.Nlp ( NlpOut(..), KKT(..) )+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.M ( M )-import qualified Dyno.View.M as M-import Dyno.View.Symbolic ( Symbolic, sym, mkScheme, mkFunction )+import Dyno.View.Unsafe.View ( unJ, mkJ )+import Dyno.View.Unsafe.M ( mkM ) import Dyno.View.Viewable ( Viewable )-import Dyno.Nlp ( NlpOut(..), KKT(..) )-import Dyno.NlpScaling ( ScaleFuns(..), scaledFG, mkScaleFuns )-import Dyno.Solvers ( Solver(..), getSolverInternal )-import Dyno.SolverInternal ( SolverInternal(..) ) type VD a = J a (Vector Double) type VMD a = J a (Vector (Maybe Double))@@ -180,7 +181,7 @@ -> String -> NlpSolver x p g (J xg (Vector Double)) getInput scaleFun name = do nlpState <- ask- dmat <- liftIO $ C.ioInterfaceFunction_input__0 (isSolver nlpState) name+ dmat <- liftIO $ C.ioInterfaceFunction_getInput__0 (isSolver nlpState) name let scale = scaleFun (isScale nlpState) return (mkJ $ dnonzeros $ unJ $ scale (mkJ dmat)) @@ -214,7 +215,7 @@ -> String -> NlpSolver x p g (J xg (Vector Double)) getOutput scaleFun name = do nlpState <- ask- dmat <- liftIO $ C.ioInterfaceFunction_output__0 (isSolver nlpState) name+ dmat <- liftIO $ C.ioInterfaceFunction_getOutput__0 (isSolver nlpState) name let scale = scaleFun (isScale nlpState) return (mkJ $ dnonzeros $ unJ $ scale (mkJ dmat)) @@ -244,12 +245,15 @@ let solver = isSolver nlpState :: C.NlpSolver liftIO $ do gradF <- C.nlpSolver_gradF solver- C.ioInterfaceFunction_setInput__0 gradF (unJ (v2d x0bar)) "x"- C.ioInterfaceFunction_setInput__0 gradF (unJ (v2d pbar)) "p"- C.function_evaluate gradF- gradF' <- C.ioInterfaceFunction_output__0 gradF "grad"- f' <- C.ioInterfaceFunction_output__0 gradF "f"- return (mkJ gradF', mkJ f')+ result <- evalDMatrix' gradF (M.fromList [("x", unJ (v2d x0bar)), ("p", unJ (v2d pbar))])+ let mret = do+ grad <- M.lookup "grad" result+ f <- M.lookup "f" result+ return (mkJ grad, mkJ 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)@@ -272,12 +276,15 @@ then return (M.zeros, M.uncol M.zeros) else liftIO $ do jacG <- C.nlpSolver_jacG solver- C.ioInterfaceFunction_setInput__0 jacG (unJ (v2d x0bar)) "x"- C.ioInterfaceFunction_setInput__0 jacG (unJ (v2d pbar)) "p"- C.function_evaluate jacG- jacG' <- C.ioInterfaceFunction_output__0 jacG "jac"- g' <- C.ioInterfaceFunction_output__0 jacG "g"- return (mkM jacG', mkJ g')+ result <- evalDMatrix' jacG (M.fromList [("x", unJ (v2d x0bar)), ("p", unJ (v2d pbar))])+ let mret = do+ jac <- M.lookup "jac" result+ g <- M.lookup "g" result+ return (mkM jac, mkJ g)+ case mret of+ Nothing -> error $ "evalScaledJacG: error looking up output\n"+ ++"fields available: " ++ show (M.keys result)+ Just r -> return r evalJacG :: forall x p g . (View x, View g, View p) => NlpSolver x p g (M g x DMatrix, J g DMatrix)@@ -299,13 +306,17 @@ let solver = isSolver nlpState :: C.NlpSolver liftIO $ do hessLag <- C.nlpSolver_hessLag solver- C.ioInterfaceFunction_setInput__0 hessLag (unJ (v2d x0bar)) "x"- C.ioInterfaceFunction_setInput__0 hessLag (unJ (v2d pbar)) "p"- C.ioInterfaceFunction_setInput__0 hessLag (unJ (v2d lamGbar)) "lam_g"- C.ioInterfaceFunction_setInput__0 hessLag 1.0 "lam_f"- C.function_evaluate hessLag- hess' <- C.ioInterfaceFunction_output__0 hessLag "hess"- return (mkM hess')+ result <- evalDMatrix' hessLag $+ M.fromList+ [ ("der_x", unJ (v2d x0bar))+ , ("der_p", unJ (v2d pbar))+ , ("adj0_f", 1.0)+ , ("adj0_g", unJ (v2d lamGbar))+ ]+ case M.lookup "jac" result of -- ????????????????+ Nothing -> error $ "evalScaledHessLag: error looking up hess lag output\n"+ ++ "available fields are: " ++ show (M.keys result)+ Just r -> return (mkM r) -- | only valid at the solution evalHessLag :: forall x p g . (View x, View g, View p)@@ -327,13 +338,17 @@ let solver = isSolver nlpState :: C.NlpSolver liftIO $ do hessLag <- C.nlpSolver_hessLag solver- C.ioInterfaceFunction_setInput__0 hessLag (unJ (v2d x0bar)) "x"- C.ioInterfaceFunction_setInput__0 hessLag (unJ (v2d pbar)) "p"- C.ioInterfaceFunction_setInput__0 hessLag (unJ (v2d lamGbar)) "lam_g"- C.ioInterfaceFunction_setInput__0 hessLag 1.0 "lam_f"- C.function_evaluate hessLag- hess' <- C.ioInterfaceFunction_output__0 hessLag "hess"- return (mkM hess')+ result <- evalDMatrix' hessLag $+ M.fromList+ [ ("der_x", unJ (v2d x0bar))+ , ("der_p", unJ (v2d pbar))+ , ("adj0_f", 1.0)+ , ("adj0_g", unJ (v2d lamGbar))+ ]+ case M.lookup "jac" result of -- ????????????????+ Nothing -> error $ "evalScaledHessF: error looking up hess lag output\n"+ ++ "available fields are: " ++ show (M.keys result)+ Just r -> return (mkM r) evalHessF :: forall x p g . (View x, View g, View p) => NlpSolver x p g (M x x DMatrix)@@ -354,13 +369,17 @@ let solver = isSolver nlpState :: C.NlpSolver liftIO $ do hessLag <- C.nlpSolver_hessLag solver- C.ioInterfaceFunction_setInput__0 hessLag (unJ (v2d x0bar)) "x"- C.ioInterfaceFunction_setInput__0 hessLag (unJ (v2d pbar)) "p"- C.ioInterfaceFunction_setInput__0 hessLag (unJ (v2d lamGbar)) "lam_g"- C.ioInterfaceFunction_setInput__0 hessLag 0.0 "lam_f"- C.function_evaluate hessLag- hess' <- C.ioInterfaceFunction_output__0 hessLag "hess"- return (mkM hess')+ result <- evalDMatrix' hessLag $+ M.fromList+ [ ("der_x", unJ (v2d x0bar))+ , ("der_p", unJ (v2d pbar))+ , ("adj0_f", 0.0)+ , ("adj0_g", unJ (v2d lamGbar))+ ]+ case M.lookup "jac" result of -- ????????????????+ Nothing -> error $ "evalScaledHessLambdaG: error looking up hess lag output\n"+ ++ "available fields are: " ++ show (M.keys result)+ Just r -> return (mkM r) -- | only valid at solution@@ -412,19 +431,6 @@ } -setOption :: Gen.GenericC a => String -> a -> NlpSolver x p g ()-setOption name val = do- nlpState <- ask- let nlp = isSolver nlpState- liftIO $ Op.setOption nlp name val---reinit :: NlpSolver x p g ()-reinit = do- nlpState <- ask- let nlp = isSolver nlpState- liftIO $ soInit nlp- -- | solve with current inputs, return success or failure code solve :: NlpSolver x p g (Either String String) solve = do@@ -503,13 +509,13 @@ generateAndCompile :: String -> Function -> IO Function generateAndCompile name f = do putStrLn $ "generating " ++ name ++ ".c"- writeFile (name ++ ".c") (generateCode f True)--- C.function_generateCode__1 f (name ++ ".c") True+ let opts = M.fromList [("generate_main", Op.Opt True)]+ writeFile (name ++ ".c") (generateCode f opts) let cmd = "clang" args = ["-fPIC","-shared","-Wall","-Wno-unused-variable",name++".c","-o",name++".so"] putStrLn (showCommandForUser cmd args) callProcess cmd args- externalFunction ("./"++name++".so")+ externalFunction ("./"++name++".so") M.empty data RunNlpOptions = RunNlpOptions@@ -523,10 +529,10 @@ } runNlpSolver ::- forall x p g a s .- (View x, View p, View g, Symbolic s)+ forall x p g a .+ (View x, View p, View g) => Solver- -> (J x s -> J p s -> (J (JV Id) s, J g s))+ -> (J x MX -> J p MX -> (J (JV Id) MX, J g MX)) -> Maybe (J x (Vector Double)) -> Maybe (J g (Vector Double)) -> Maybe Double@@ -536,11 +542,11 @@ runNlpSolver = runNlpSolverWith defaultRunnerOptions runNlpSolverWith ::- forall x p g a s .- (View x, View p, View g, Symbolic s)+ forall x p g a .+ (View x, View p, View g) => RunNlpOptions -> Solver- -> (J x s -> J p s -> (J (JV Id) s, J g s))+ -> (J x MX -> J p MX -> (J (JV Id) MX, J g MX)) -> Maybe (J x (Vector Double)) -> Maybe (J g (Vector Double)) -> Maybe Double@@ -548,32 +554,30 @@ -> NlpSolver x p g a -> IO a runNlpSolverWith runnerOptions solverStuff nlpFun scaleX scaleG scaleF callback' (NlpSolver nlpMonad) = do- inputsX <- sym "x"- inputsP <- sym "p"+ inputsX <- mkJ <$> symV "x" (size (Proxy :: Proxy x))+ inputsP <- mkJ <$> symV "p" (size (Proxy :: Proxy p)) let scale :: forall sfa . (CMatrix sfa, Viewable sfa) => ScaleFuns x g sfa scale = mkScaleFuns scaleX scaleG scaleF - let (obj, g) = scaledFG scale nlpFun inputsX inputsP+ (obj, g) = scaledFG scale nlpFun inputsX inputsP - let inputsXMat = unJ inputsX+ inputsXMat = unJ inputsX inputsPMat = unJ inputsP objMat = unJ obj gMat = unJ g - inputScheme <- mkScheme SCHEME_NLPInput [("x", inputsXMat), ("p", inputsPMat)]- outputScheme <- mkScheme SCHEME_NLPOutput [("f", objMat), ("g", gMat)]- when (verbose runnerOptions) $ do putStrLn "************** initializing dynobud runNlpSolver ******************" putStrLn "making nlp..."- (nlp, nlpTime) <- timeIt $ mkFunction "nlp" inputScheme outputScheme- when (verbose runnerOptions) $ printf "made nlp in %s\n" (formatSeconds nlpTime)- mapM_ (\(l,Op.Opt o) -> Op.setOption nlp l o) (functionOptions solverStuff)- when (verbose runnerOptions) $ putStrLn "init nlp..."- (_, nlpInitTime) <- timeIt $ soInit nlp- when (verbose runnerOptions) $ printf "nlp initialized in %s\n" (formatSeconds nlpInitTime) + (nlp, nlpTime) <- timeIt $ mxFunctionWithSchemes "nlp"+ (SCHEME_NLPInput, M.fromList [("x", inputsXMat), ("p", inputsPMat)])+ (SCHEME_NLPOutput, M.fromList [("f", objMat), ("g", gMat)])+ (M.fromList (functionOptions solverStuff))+ when (verbose runnerOptions) $ printf "nlp initialized in %s\n"+ (formatSeconds nlpTime)+ when (verbose runnerOptions) $ putStrLn "function call..." -- in case the user wants to do something (like codegen?) (_, functionCallTime) <- timeIt $ functionCall solverStuff nlp@@ -594,26 +598,40 @@ -- jac_sparsity <- C.function_jacSparsity nlp 0 1 True False -- C.sparsity_spyMatlab jac_sparsity "jac_sparsity_reorder.m" - when (verbose runnerOptions) $ putStrLn "create solver..."- (solver, solverCreateTime) <- timeIt $ C.nlpSolver__0 (solverName (getSolverInternal solverStuff)) nlp- when (verbose runnerOptions) $ printf "created solver in %s\n" (formatSeconds solverCreateTime)- -- add callback if user provides it+ when (verbose runnerOptions) $ putStrLn "create callback..." intref <- newIORef False paramRef <- newIORef (jfill 0)- let cb function' = do+ let cb :: Function -> IO CInt+ cb function' = do callbackRet <- case callback' of Nothing -> return True Just callback -> do xval <- fmap (d2v . xbarToX scale . mkJ . CM.densify) $- C.ioInterfaceFunction_output__2 function' 0+ C.ioInterfaceFunction_getOutput__2 function' 0 pval <- readIORef paramRef callback xval pval interrupt <- readIORef intref return $ if callbackRet && not interrupt then 0 else fromIntegral (solverInterruptCode (getSolverInternal solverStuff))- casadiCallback <- makeCallback cb >>= C.genericType__0- Op.setOption solver "iteration_callback" casadiCallback+ casadiCallback <- makeCallback cb >>= C.genericType__1++ -- make the solver+ solverOptions <-+ T.mapM Gen.mkGeneric $+ M.fromList $+ ("iteration_callback", Op.Opt casadiCallback)+ : defaultSolverOptions (getSolverInternal solverStuff)+ ++ options solverStuff+ when (verbose runnerOptions) $ putStrLn "create solver..."+ (solver, solverInitTime) <-+ timeIt $ C.nlpSolver__5 "nlpSolver"+ (solverName (getSolverInternal solverStuff)) (C.castFunction nlp)+ solverOptions+ when (verbose runnerOptions) $+ printf "solver initialized in %s\n" (formatSeconds solverInitTime)++ -- grad_f <- gradient nlp 0 0 -- soInit grad_f -- jac_g <- jacobian nlp 0 1 True False@@ -635,12 +653,6 @@ -- Op.setOption solver "grad_f" grad_f' -- Op.setOption solver "jac_g" jac_g' - -- set all the user options- mapM_ (\(l,Op.Opt o) -> Op.setOption solver l o) (defaultSolverOptions (getSolverInternal solverStuff)- ++ options solverStuff)- when (verbose runnerOptions) $ putStrLn "initialize solver..."- (_, solverInitTime) <- timeIt $ soInit solver- when (verbose runnerOptions) $ printf "solver initialized in %s\n" (formatSeconds solverInitTime) let proxy :: J f b -> Proxy f proxy = const Proxy@@ -658,4 +670,3 @@ (ret, retTime) <- timeIt $ liftIO $ runReaderT nlpMonad nlpState when (verbose runnerOptions) $ printf "ran NLP monad in %s\n" (formatSeconds retTime) return ret-
src/Dyno/NlpUtils.hs view
@@ -22,7 +22,7 @@ import System.IO ( hFlush, stdout ) import Text.Printf ( printf ) -import Casadi.SX ( SX )+import Casadi.MX ( MX ) import qualified Casadi.GenericC as Gen import Dyno.View.Unsafe.View ( unJ, mkJ )@@ -30,7 +30,6 @@ 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.Symbolic ( Symbolic ) import Dyno.Nlp ( Nlp(..), NlpOut(..), Bounds ) import Dyno.Solvers ( Solver ) import Dyno.NlpSolver@@ -63,11 +62,11 @@ -- | solve a homotopy nlp solveNlpHomotopy ::- forall x p g t a .- (View x, View p, View g, T.Traversable t, Symbolic a)+ forall x p g t .+ (View x, View p, View g, T.Traversable t) => Double -> HomotopyParams -> Solver- -> Nlp x p g a -> t (J p (Vector Double))+ -> Nlp x p g MX -> t (J p (Vector Double)) -> Maybe (J x (Vector Double) -> J p (Vector Double) -> IO Bool) -> Maybe (J x (Vector Double) -> J p (Vector Double) -> Double -> IO ()) -> IO (t (NlpOut x g (Vector Double)))@@ -75,12 +74,12 @@ -- | solve a homotopy nlp solveNlpHomotopyWith ::- forall x p g t a .- (View x, View p, View g, T.Traversable t, Symbolic a)+ forall x p g t .+ (View x, View p, View g, T.Traversable t) => RunNlpOptions -> Double -> HomotopyParams -> Solver- -> Nlp x p g a -> t (J p (Vector Double))+ -> Nlp x p g MX -> t (J p (Vector Double)) -> Maybe (J x (Vector Double) -> J p (Vector Double) -> IO Bool) -> Maybe (J x (Vector Double) -> J p (Vector Double) -> Double -> IO ()) -> IO (t (NlpOut x g (Vector Double)))@@ -88,7 +87,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 a -> J p a -> (J (JV Id) a, J g a)+ let fg :: J x MX -> J p MX -> (J (JV Id) MX, J g MX) fg x p = nlpFG nlp x p runNlpSolverWith options solverStuff fg (nlpScaleX nlp) (nlpScaleG nlp) (nlpScaleF nlp) callback $ do@@ -207,27 +206,30 @@ -> Maybe (x Double -> IO Bool) -> IO (Either String (Double, x Double)) solveNlpV solverStuff fg bx bg x0 cb = do- let nlp :: Nlp (JV x) JNone (JV g) SX- nlp = Nlp { nlpFG = \x' _ -> let _ = x' :: J (JV x) SX- x = splitJV' x' :: x (J (JV Id) SX)- (obj,g) = fg x :: (J (JV Id) SX, g (J (JV Id) SX))- --obj' = sxCatJV (Id obj) :: J (JV Id) SX- --g' = sxCatJV g :: J (JV g) SX- in (obj, catJV' g)- , nlpBX = catJV bx -- mkJ $ vectorize (nlpBX nlp) :: J (JV x) (V.Vector Bounds)- , nlpBG = catJV bg -- mkJ $ vectorize (nlpBG nlp) :: J (JV g) (V.Vector Bounds)- , nlpX0 = catJV x0 -- mkJ $ vectorize (nlpX0 nlp) :: J (JV x) (V.Vector Double)- , nlpP = cat JNone -- mkJ $ vectorize (nlpP nlp) :: J (JV p) (V.Vector Double)- , nlpLamX0 = Nothing --fmap (mkJ . vectorize) (nlpLamX0 nlp)- -- :: Maybe (J (JV x) (V.Vector Double))- , nlpLamG0 = Nothing -- fmap (mkJ . vectorize) (nlpLamG0 nlp)- -- :: Maybe (J (JV g) (V.Vector Double))- , nlpScaleF = Nothing -- nlpScaleF nlp- , nlpScaleX = Nothing -- fmap (mkJ . vectorize) (nlpScaleX nlp)+ let nlp :: Nlp (JV x) JNone (JV g) MX+ nlp =+ 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+ --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) -- :: Maybe (J (JV x) (V.Vector Double))- , nlpScaleG = Nothing -- fmap (mkJ . vectorize) (nlpScaleG nlp)- -- :: Maybe (J (JV g) (V.Vector Double))- }+ , nlpLamG0 = Nothing -- fmap (mkJ . vectorize) (nlpLamG0 nlp)+ -- :: Maybe (J (JV g) (V.Vector Double))+ , nlpScaleF = Nothing -- nlpScaleF nlp+ , nlpScaleX = Nothing -- fmap (mkJ . vectorize) (nlpScaleX nlp)+ -- :: Maybe (J (JV x) (V.Vector Double))+ , nlpScaleG = Nothing -- fmap (mkJ . vectorize) (nlpScaleG nlp)+ -- :: Maybe (J (JV g) (V.Vector Double))+ } callback :: Maybe (J (JV x) (Vector Double) -> J JNone (Vector Double) -> IO Bool) callback = case cb of@@ -252,26 +254,26 @@ -- | convenience function to solve a pure Nlp solveNlp ::- (View x, View p, View g, Symbolic a)+ (View x, View p, View g) => Solver- -> Nlp x p g a -> Maybe (J x (Vector Double) -> J p (Vector Double) -> IO Bool)+ -> Nlp x p g MX -> Maybe (J x (Vector Double) -> J p (Vector Double) -> IO Bool) -> IO (Either String String, NlpOut x g (Vector Double)) solveNlp solverStuff nlp callback = runNlp solverStuff nlp callback solve' -- | convenience function to solve a pure Nlp solveNlpWith ::- (View x, View p, View g, Symbolic a)+ (View x, View p, View g) => RunNlpOptions -> Solver- -> Nlp x p g a -> Maybe (J x (Vector Double) -> J p (Vector Double) -> IO Bool)+ -> Nlp x p g MX -> Maybe (J x (Vector Double) -> J p (Vector Double) -> IO Bool) -> IO (Either String String, NlpOut x g (Vector Double)) solveNlpWith opts solverStuff nlp callback = runNlpWith opts solverStuff nlp callback solve' -- | set all inputs-setNlpInputs :: (View x, View p, View g, Symbolic a) => Nlp x p g a -> NlpSolver x p g ()+setNlpInputs :: (View x, View p, View g) => Nlp x p g MX -> NlpSolver x p g () setNlpInputs nlp = do let (lbx,ubx) = unzipJ (nlpBX nlp) (lbg,ubg) = unzipJ (nlpBG nlp)@@ -292,19 +294,19 @@ -- | set all inputs, handle scaling, and let the user run a NlpMonad runNlp ::- (View x, View p, View g, Symbolic a)+ (View x, View p, View g) => Solver- -> Nlp x p g a -> Maybe (J x (Vector Double) -> J p (Vector Double) -> IO Bool)+ -> Nlp x p g MX -> Maybe (J x (Vector Double) -> J p (Vector Double) -> IO Bool) -> NlpSolver x p g b -> IO b runNlp = runNlpWith defaultRunnerOptions -- | set all inputs, handle scaling, and let the user run a NlpMonad runNlpWith ::- (View x, View p, View g, Symbolic a)+ (View x, View p, View g) => RunNlpOptions -> Solver- -> Nlp x p g a -> Maybe (J x (Vector Double) -> J p (Vector Double) -> IO Bool)+ -> Nlp x p g MX -> Maybe (J x (Vector Double) -> J p (Vector Double) -> IO Bool) -> NlpSolver x p g b -> IO b runNlpWith options solverStuff nlp callback runMe =
src/Dyno/OcpHomotopy.hs view
@@ -22,8 +22,8 @@ import Dyno.NlpSolver ( RunNlpOptions, defaultRunnerOptions ) import Dyno.NlpUtils ( HomotopyParams(..), solveNlpWith, solveNlpHomotopyWith ) import Dyno.DirectCollocation.Types ( CollTraj(..), CollOcpConstraints )-import Dyno.DirectCollocation.Formulate ( CollProblem(..), makeCollProblem )-import Dyno.DirectCollocation.Quadratures ( QuadratureRoots )+import Dyno.DirectCollocation.Formulate+ ( CollProblem(..), DirCollOptions, makeCollProblem ) runOcpHomotopyWith ::@@ -34,23 +34,24 @@ , Vectorize q, Vectorize po, Vectorize qo , Vectorize fp , T.Traversable t )- => RunNlpOptions+ => DirCollOptions -> RunNlpOptions -> Double -> HomotopyParams -> OcpPhase x z u p r o c h q qo po fp -> OcpPhaseInputs x z u p c h fp -> J (CollTraj x z u p n deg) (Vector Double)- -> QuadratureRoots -> Bool -> Bool -> Solver -> Solver+ -> Bool -> Bool -> Solver -> Solver -> t (fp Double) -> (CollProblem x z u p r o c h q qo po fp n deg- -> IO ([String] -> J (CollTraj x z u p n deg) (Vector Double) -> J (JV fp) (Vector Double) -> IO Bool)+ -> IO ([String] -> J (CollTraj x z u p n deg) (Vector Double)+ -> J (JV fp) (Vector Double) -> IO Bool) ) -> IO (t (NlpOut (CollTraj x z u p n deg) (CollOcpConstraints x r c h n deg) (Vector Double)))-runOcpHomotopyWith opts step0 homotopyParams ocpHomotopy ocpHomotopyInputs guess roots+runOcpHomotopyWith dirCollOpts opts step0 homotopyParams ocpHomotopy ocpHomotopyInputs guess useStartupCallback useHomotopyCallback startupSolver homotopySolver nominalParams makeCallback = do- cp0 <- makeCollProblem roots ocpHomotopy ocpHomotopyInputs guess+ cp0 <- makeCollProblem dirCollOpts ocpHomotopy ocpHomotopyInputs guess callback <- makeCallback cp0 let nlpHomotopy :: Nlp (CollTraj x z u p n deg)@@ -104,11 +105,11 @@ , Vectorize q, Vectorize po, Vectorize qo , Vectorize fp , T.Traversable t )- => Double -> HomotopyParams+ => DirCollOptions -> Double -> HomotopyParams -> OcpPhase x z u p r o c h q qo po fp -> OcpPhaseInputs x z u p c h fp -> J (CollTraj x z u p n deg) (Vector Double)- -> QuadratureRoots -> Bool -> Bool -> Solver -> Solver+ -> Bool -> Bool -> Solver -> Solver -> t (fp Double) -> (CollProblem x z u p r o c h q qo po fp n deg -> IO ([String] -> J (CollTraj x z u p n deg) (Vector Double) -> J (JV fp) (Vector Double) -> IO Bool)@@ -116,4 +117,4 @@ -> IO (t (NlpOut (CollTraj x z u p n deg) (CollOcpConstraints x r c h n deg) (Vector Double)))-runOcpHomotopy = runOcpHomotopyWith defaultRunnerOptions+runOcpHomotopy dirCollOpts = runOcpHomotopyWith dirCollOpts defaultRunnerOptions
src/Dyno/SimpleOcp.hs view
@@ -119,10 +119,16 @@ let ocp = toOcp simple ocpInputs = toOcpInputs simple tf = endTime simple- roots = Legendre+ dirCollOpts =+ DirCollOptions+ { collocationRoots = Radau+ , mapStrategy = Unrolled+ } -- todo(greg): = def+ roots = collocationRoots dirCollOpts+ guess :: CollTraj (Tuple x u) None u None n deg (Vector Double) guess = makeGuess roots tf (\t -> Tuple (initialGuess simple t) (fill 0)) (const None) (const (fill 0)) None- cp <- makeCollProblem roots ocp ocpInputs (cat guess)+ cp <- makeCollProblem dirCollOpts ocp ocpInputs (cat guess) let _ = cp :: CollProblem (Tuple x u) None u None (Tuple x u) None (SimpleBc x) None None None None None n deg (emsg, opt) <- solveNlp solver (cpNlp cp) Nothing case emsg of
src/Dyno/Solvers.hs view
@@ -6,7 +6,7 @@ , getSolverInternal ) where -import Casadi.Core.Classes.Function ( Function )+import Casadi.MXFunction ( MXFunction ) import Casadi.Option ( Opt(..) ) import Dyno.SolverInternal ( SolverInternal(..) )@@ -15,7 +15,7 @@ Solver { options :: [(String,Opt)] , functionOptions :: [(String, Opt)]- , functionCall :: Function -> IO ()+ , functionCall :: MXFunction -> IO () , solverInternal :: SolverInternal }
src/Dyno/View/Fun.hs view
@@ -1,46 +1,55 @@ {-# OPTIONS_GHC -Wall #-} {-# LANGUAGE ScopedTypeVariables #-}+{-# LANGUAGE PolyKinds #-} {-# LANGUAGE KindSignatures #-} module Dyno.View.Fun ( FunClass(..)+ , AlwaysInline(..)+ , NeverInline(..) , MXFun , SXFun , Fun(..) , toMXFun+ , toMXFun' , toSXFun+ , toSXFun' , eval , call , callSX , expandMXFun , toFunJac+ , checkFunDimensions+ , checkFunDimensionsWith ) where -import Control.Monad ( zipWithM )+import Control.Monad ( (>=>), zipWithM )+import qualified Data.Map as M+import Data.Maybe ( catMaybes ) import Data.Proxy import qualified Data.Vector as V import Data.Vector ( Vector )+import Text.Printf ( printf )+import System.IO.Unsafe ( unsafePerformIO ) -import Casadi.MX ( symM )-import Casadi.SX ( ssymM )+import Casadi.MX ( MX, symM )+import Casadi.SX ( SX, ssymM )+import Casadi.Function ( AlwaysInline(..), NeverInline(..) ) import qualified Casadi.Function as C import qualified Casadi.MXFunction as C import qualified Casadi.SXFunction as C import Casadi.Option-import Casadi.SharedObject-import Casadi.MX ( MX )-import Casadi.SX ( SX ) import Casadi.DMatrix ( DMatrix ) import Casadi.CMatrix ( CMatrix )- import qualified Casadi.Core.Classes.Function as F import qualified Casadi.Core.Classes.MXFunction as M-import qualified Casadi.Core.Classes.SharedObject as C+import qualified Casadi.Core.Classes.Sparsity as C import qualified Casadi.Core.Classes.OptionsFunctionality as C -import Dyno.View.Viewable ( Viewable )-import Dyno.View.Scheme 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@@ -67,7 +76,6 @@ return (SXFun sxf) toFun (SXFun f) = Fun (F.castFunction f) - instance FunClass MXFun where fromFun (Fun f) = do mxf <- C.mxFunctionFromFunction f@@ -82,13 +90,20 @@ -- | call a function on MX inputs, yielding MX outputs call :: (FunClass fun, Scheme f, Scheme g) => fun f g -> f MX -> g MX-call f' = fromVector . C.callMX f . toVector+call f x = call' f x (AlwaysInline False) (NeverInline False)++-- | call a function on MX inputs, yielding MX outputs+call' :: (FunClass fun, Scheme f, Scheme g)+ => fun f g -> f MX -> AlwaysInline -> NeverInline -> g MX+call' f' x ai ni = fromVector $ C.callMX f (toVector x) ai ni where Fun f = toFun f' --- | call an SXFunction on symbolic inputs, getting symbolic outputs+---- | call an SXFunction on symbolic inputs, getting symbolic outputs callSX :: (Scheme f, Scheme g) => SXFun f g -> f SX -> g SX-callSX (SXFun sxf) = fromVector . C.callSX sxf . toVector+callSX (SXFun sxf) x =+ fromVector $+ C.callSX sxf (toVector x) (AlwaysInline False) (NeverInline False) mkSym :: forall a f . (Scheme f, CMatrix a, Viewable a)@@ -102,100 +117,129 @@ ms <- zipWithM f sizes [(0::Int)..] return $ fromVector (V.fromList ms) -mkFun :: forall f g fun fun' a- . (Scheme f, Scheme g, Viewable a, C.SharedObjectClass fun, C.OptionsFunctionalityClass fun)- => (Vector a -> Vector a -> IO fun)- -> (String -> Proxy f -> IO (f a))- -> (fun -> fun' f g)- -> String- -> (f a -> g a)- -> IO (fun' f g)-mkFun mkfun mksym con name userf = do+mkToFun ::+ forall f g fun fun' a+ . ( Scheme f, Scheme g, Viewable a, FunClass fun'+ , C.OptionsFunctionalityClass fun+ )+ => String+ -> (String -> Vector a -> Vector a -> M.Map String Opt -> IO fun)+ -> (String -> Proxy f -> IO (f a))+ -> (fun -> fun' f g)+ -> String+ -> (f a -> g a)+ -> M.Map String Opt+ -> IO (fun' f g)+mkToFun errName mkfun mksym con name userf opts = do inputs <- mksym "x" (Proxy :: Proxy f)- fun <- mkfun (toVector inputs) (toVector (userf inputs))- setOption fun "name" name- soInit fun- return (con fun)+ fun <- mkfun name (toVector inputs) (toVector (userf inputs)) opts+ checkFunDimensionsWith (errName ++ " (" ++ name ++ ")") (con fun) --- | make an MXFunction-toMXFun :: forall f g . (Scheme f, Scheme g) => String -> (f MX -> g MX) -> IO (MXFun f g)-toMXFun name fun = mkFun C.mxFunction (mkSym symM) MXFun name fun+-- | make an MXFunction with name+toMXFun :: forall f g+ . (Scheme f, Scheme g)+ => String -> (f MX -> g MX)+ -> IO (MXFun f g)+toMXFun n f = toMXFun' n f M.empty --- | make an MXFunction-toSXFun :: forall f g . (Scheme f, Scheme g) => String -> (f SX -> g SX) -> IO (SXFun f g)-toSXFun name fun = mkFun C.sxFunction (mkSym ssymM) SXFun name fun+-- | make an MXFunction with name and options+toMXFun' :: forall f g+ . (Scheme f, Scheme g)+ => String -> (f MX -> g MX) -> M.Map String Opt+ -> IO (MXFun f g)+toMXFun' = mkToFun "toMXFun" C.mxFunction (mkSym symM) MXFun +-- | make an SXFunction with name+toSXFun :: forall f g+ . (Scheme f, Scheme g)+ => String -> (f SX -> g SX)+ -> IO (SXFun f g)+toSXFun n f = toSXFun' n f M.empty++-- | make an SXFunction with name and options+toSXFun' :: forall f g+ . (Scheme f, Scheme g)+ => String -> (f SX -> g SX) -> M.Map String Opt+ -> IO (SXFun f g)+toSXFun' = mkToFun "toSXFun" C.sxFunction (mkSym ssymM) SXFun+ -- | expand an MXFunction-expandMXFun :: MXFun f g -> IO (SXFun f g)+expandMXFun :: (Scheme f, Scheme g) => MXFun f g -> IO (SXFun f g) expandMXFun (MXFun mxf) = do sxf <- M.mxFunction_expand__0 mxf- C.sharedObject_init__0 sxf- return (SXFun sxf)+ checkFunDimensionsWith "expandMXFun" (SXFun sxf) +-- partial version of checkFunDimensions which throws an error+checkFunDimensionsWith ::+ forall fun f g+ . (FunClass fun, Scheme f, Scheme g)+ => String -> fun f g -> IO (fun f g)+checkFunDimensionsWith name fun = do+ case checkFunDimensions fun of+ Left msg -> error $ name ++ " error:\n" ++ msg+ Right _ -> return fun++-- if dimensions are good, return Nothing, otherwise return error message+checkFunDimensions ::+ forall fun f g+ . (FunClass fun, Scheme f, Scheme g)+ => fun f g -> Either String String+checkFunDimensions f' = unsafePerformIO $ do+ let f :: F.Function+ Fun f = toFun f'+ nInRuntime <- F.function_nIn f+ nOutRuntime <- F.function_nOut f+ let nInType = numFields (Proxy :: Proxy f)+ nOutType = numFields (Proxy :: Proxy g)+ ioLenErr name nIOType nIORuntime+ | nIOType == nIORuntime = Nothing+ | otherwise =+ Just $ printf "num %s incorrect: type: %d, runtime: %d"+ name nIOType nIORuntime++ case catMaybes [ ioLenErr "inputs" nInType nInRuntime+ , ioLenErr "outputs" nOutType nOutRuntime+ ] of+ errs@(_:_) -> return $ Left $ unlines+ ("checkFunDimensions got ill-dimensioned function:":errs)+ [] -> do+ let getSize sp = do+ s1 <- C.sparsity_size1 sp+ s2 <- C.sparsity_size2 sp+ return (s1, s2)+ sInsRuntime <- mapM (F.function_inputSparsity__2 f >=> getSize)+ (take nInRuntime [0..])+ sOutsRuntime <- mapM (F.function_outputSparsity__2 f >=> getSize)+ (take nOutRuntime [0..])+ let sInsType = sizeList (Proxy :: Proxy f)+ sOutsType = sizeList (Proxy :: Proxy g)+ ioSizeErr name k sType sRuntime+ | sType == sRuntime = Nothing+ | sType == (1,0) && sRuntime == (0,1) = Nothing+ | otherwise =+ Just $ printf "%s %d dimension mismatch! type: %s, runtime: %s"+ name (k :: Int) (show sType) (show sRuntime)+ sizeErrs =+ (zipWith3 (ioSizeErr "input") [0..] sInsType sInsRuntime) +++ (zipWith3 (ioSizeErr "output") [0..] sOutsType sOutsRuntime)+ return $ case catMaybes sizeErrs of+ [] -> Right $+ unlines+ [ "checkFunDimensions got well-dimensioned function"+ , printf "%d inputs, %d outputs" nInType nOutType+ , "input sizes: " ++ show sInsType+ , "output sizes: " ++ show sOutsType+ ]+ errs -> Left $ unlines+ ("checkFunDimensions got ill-dimensioned function:":errs)+ -- | make a function which also contains a jacobian toFunJac ::- FunClass fun =>+ (FunClass fun, View xj, View fj, Scheme x, Scheme f) => fun (JacIn xj x) (JacOut fj f) -> IO (fun (JacIn xj x) (Jac xj fj f)) toFunJac fun0 = do let Fun fun = toFun fun0- maybeName <- getOption fun "name"- let name = case maybeName of Nothing -> "no_name"- Just n -> n- let compact = False+ compact = False symmetric = False funJac <- C.jacobian fun 0 0 compact symmetric- setOption funJac "name" (name ++ "_dynobudJac")- soInit funJac-- fromFun (Fun funJac)-----toFunJac' ::--- forall x y f . (SymInputs x MX, SymInputs y MX, FunArgs f MX, FunArgs f (J x MX))--- => String -> ((x MX, y MX) -> f MX) -> IO ((x MX, y MX) -> Vector (Vector MX))---toFunJac' name f0 = do--- (diffInputs',_) <- sym' 0 (Proxy :: Proxy (x MX))--- let nsyms = F.sum $ fmap vsize1 (vectorize diffInputs')--- diffInputsCat <- symV "dx" nsyms--- let inputSizes = V.fromList ((0:) $ F.toList (sizeList 0 (Proxy :: Proxy (x MX))))--- diffInputs = vvertsplit diffInputsCat inputSizes------ (inputs,_) <- sym' 0 (Proxy :: Proxy (y MX))--- let diffOutputs = f0 (devectorize diffInputs, inputs)--- diffOutputsCat = vveccat (vectorize diffOutputs)------ allInputs = V.cons diffInputsCat (vectorize inputs)--- allOutputs = V.singleton diffOutputsCat------ mxf <- mxFunction allInputs allOutputs--- setOption mxf "name" name--- soInit mxf--- let compact = False--- symmetric = False--- mxfJac <- jacobian mxf 0 0 compact symmetric--- soInit mxfJac------ let callMe :: (x MX, y MX) -> Vector (Vector MX) -- , f MX)--- callMe (x',y')--- | 2 /= V.length vouts =--- error "toFunJac': bad number of outputs :("--- | otherwise = rows -- , devectorize fs)--- where--- --retJac :: f (J x MX)--- --retJac = devectorize retJac'--- --retJac' :: Vector (J x MX)--- --retJac' = fmap devectorize rows--- rows :: Vector (Vector MX)--- rows = fmap (`vhorzsplit` horzsizes) $ vvertsplit jac vertsizes--- vertsizes = V.fromList ((0:) $ F.toList (sizeList 0 (Proxy :: Proxy (f MX))))--- horzsizes = V.fromList ((0:) $ F.toList (sizeList 0 (Proxy :: Proxy (x MX))))------ --fs = vvertsplit f vertsizes--- x = vveccat (vectorize x')------ jac = vouts V.! 0--- --f = vouts V.! 1------ vouts = callMX mxfJac $ V.cons x (vectorize y')--- --- return callMe+ fromFun (Fun funJac) >>= checkFunDimensionsWith "toFunJac"
src/Dyno/View/M.hs view
@@ -47,6 +47,7 @@ , unrow , uncol , solve+ , solve' , toHMat , fromHMat , fromHMat'@@ -55,16 +56,18 @@ , rank ) where -import qualified Data.Vector as V import Data.Proxy ( Proxy(..) )+import qualified Data.Map as M+import qualified Data.Vector as V+import qualified Numeric.LinearAlgebra as HMat++import Casadi.GenericC ( GenericType ) import Casadi.CMatrix ( CMatrix ) import Casadi.DMatrix ( DMatrix, dnonzeros, dsparsify ) import qualified Casadi.CMatrix as CM-import qualified Numeric.LinearAlgebra as HMat import Dyno.View.Unsafe.View ( unJ, mkJ ) import Dyno.View.Unsafe.M ( 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 )@@ -346,8 +349,14 @@ 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 -> M g h a-solve (UnsafeM x) (UnsafeM y) = mkM (CM.solve x y)+solve :: (View g, View h, CMatrix a)+ => M f g a -> M f h a -> String -> M.Map String GenericType+ -> M g h a+solve (UnsafeM x) (UnsafeM y) n options = mkM (CM.solve x y n options)++solve' :: (View g, View h, CMatrix a) => M f g a -> M f h a -> M g h a+solve' (UnsafeM x) (UnsafeM y) = mkM (CM.solve' x y)+{-# DEPRECATED solve' "use the new solve, this one is going away" #-} toHMat :: forall n m . (View n, View m)
+ src/Dyno/View/MapFun.hs view
@@ -0,0 +1,140 @@+{-# OPTIONS_GHC -Wall #-}+{-# LANGUAGE TypeOperators #-}+{-# LANGUAGE ScopedTypeVariables #-}+{-# LANGUAGE TypeFamilies #-}+{-# LANGUAGE PolyKinds #-}+{-# LANGUAGE FlexibleInstances #-}+{-# LANGUAGE FlexibleContexts #-}+{-# LANGUAGE KindSignatures #-}+{-# LANGUAGE MultiParamTypeClasses #-}++module Dyno.View.MapFun+ ( mapFun+ , mapFun'+ , mapFun''+ ) where++import qualified Data.Foldable as F+import qualified Data.Map as M+import Data.Proxy+import Data.Sequence ( Seq )+import qualified Data.Sequence as S+import qualified Data.Traversable as T+import qualified Data.Vector as V++import qualified Casadi.Function as C+import Casadi.Option++import qualified Casadi.Core.Classes.Function as F+import qualified Casadi.Core.Classes.Map as C++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)++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+ . ( FunClass fun+ , Scheme (Par f n), Scheme (Par g n)+ , Dim n )+ => Proxy n+ -> String+ -> fun f g+ -> M.Map String Opt+ -> IO (Fun (Par f n) (Par g n))+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' 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+ repeated _ _ = S.singleton True++---- non-repeated+--instance View f => ParScheme' (J f) (M (JV Id) f) where+-- repeated _ _ = S.singleton False++instance (ParScheme' f0 f1, ParScheme' g0 g1) => ParScheme' (f0 :*: g0) (f1 :*: g1) where+ repeated pfg0 pfg1 = repeated pf0 pf1 S.>< repeated pg0 pg1+ where+ splitProxy :: Proxy (f :*: g) -> (Proxy f, Proxy g)+ splitProxy _ = (Proxy, Proxy)++ (pf0, pg0) = splitProxy pfg0+ (pf1, pg1) = splitProxy pfg1++-- | symbolic fmap+mapFun'' :: forall fun i0 i1 o0 o1 n+ . ( FunClass fun+ , ParScheme' i0 i1, ParScheme' o0 o1+ , Scheme i0, Scheme o0+ , Scheme i1, Scheme o1+ , Dim n+ )+ => Proxy n+ -> String+ -> fun i0 o0+ -> M.Map String Opt+ -> IO (Fun i1 o1)+mapFun'' _ name f0 opts0 = do+-- let fds = checkFunDimensions f0+-- putStrLn "mapFun'' input dimensions:"+-- case fds of+-- Left msg -> putStrLn msg+-- Right msg -> putStrLn msg+ _ <- checkFunDimensionsWith "mapFun'' input fun" (toFun f0)+ opts <- T.mapM mkGeneric opts0 :: IO (M.Map String GenericType)+ let n = TV.reflectDim (Proxy :: Proxy n)+ repeatedIn =+ V.fromList $ F.toList $ repeated (Proxy :: Proxy i0) (Proxy :: Proxy i1)+ repeatedOut =+ V.fromList $ F.toList $ repeated (Proxy :: Proxy o0) (Proxy :: Proxy o1)+-- putStrLn $ "repeated in: " ++ show repeatedIn+-- putStrLn $ "repeated out: " ++ show repeatedOut++ fm <- C.map__1 name (unFun (toFun f0)) n repeatedIn repeatedOut opts :: IO C.Map+ checkFunDimensionsWith "mapFun''" (Fun (F.castFunction fm))+-- {-# NOINLINE mapFun'' #-}
src/Dyno/View/Scheme.hs view
@@ -43,8 +43,9 @@ 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'+ [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@@ -53,8 +54,9 @@ instance (View f, View g) => Scheme (M.M f g) where numFields = const 1 fromVector v = case V.toList v of- [m] -> case fromMat m of Left err -> error $ "Scheme fromVector M error: " ++ err- Right m' -> m'+ [m] -> case fromMat 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@@ -76,13 +78,15 @@ toVector :: f a -> V.Vector a sizeList :: Proxy f -> [(Int,Int)] - default numFields :: (GNumFields (Rep (f ())), Generic (f ())) => Proxy f -> Int+ default numFields :: (GNumFields (Rep (f ())), Generic (f ()))+ => Proxy f -> Int numFields = gnumFields . reproxy where reproxy :: Proxy g -> Proxy ((Rep (g ())) p) reproxy = const Proxy - default sizeList :: (GSizeList (Rep (f ())), Generic (f ())) => Proxy f -> [(Int,Int)]+ default sizeList :: (GSizeList (Rep (f ())), Generic (f ()))+ => Proxy f -> [(Int,Int)] sizeList = F.toList . gsizeList . reproxy where reproxy :: Proxy g -> Proxy ((Rep (g ())) p)@@ -151,11 +155,14 @@ reproxy = const Proxy --------------------- GFromVector -----------------------------instance (GFromVector f a, GFromVector g a, GNumFields f, GNumFields g) => GFromVector (f :*: g) a where+instance (GFromVector f a, GFromVector g a, GNumFields f, GNumFields g)+ => GFromVector (f :*: g) a where gfromVector name vs pxy- | V.length vs == nx + ny = gfromVector name vx px :*: gfromVector name vy py- | otherwise = error $ "\"" ++ name ++ "\" GFromVector (:*:) length error, need " ++- show (nx,ny) ++ " but got " ++ show (V.length vs)+ | V.length vs == nx + ny =+ gfromVector name vx px :*: gfromVector name vy py+ | otherwise =+ error $ "\"" ++ name ++ "\" GFromVector (:*:) length error, need " +++ show (nx,ny) ++ " but got " ++ show (V.length vs) where nx = gnumFields px ny = gnumFields py@@ -192,7 +199,8 @@ where j = case fromMat m of Right j' -> j'- Left err -> error $ "\"" ++ name ++ "\" GFromVector fromMat error: " ++ err+ Left err ->+ error $ "\"" ++ name ++ "\" GFromVector fromMat error: " ++ err m = case V.toList ms of [m'] -> m' _ -> error $ "\"" ++ name ++ "\" GFromVector Rec0 length error, " ++@@ -202,7 +210,8 @@ --------------------- GToVector -----------------------------instance (GToVector f a, GToVector g a, GNumFields f, GNumFields g) => GToVector (f :*: g) a where+instance (GToVector f a, GToVector g a, GNumFields f, GNumFields g)+ => GToVector (f :*: g) a where gtoVector (x :*: y) = gtoVector x Seq.>< gtoVector y instance GToVector f a => GToVector (M1 i d f) a where
− src/Dyno/View/Symbolic.hs
@@ -1,56 +0,0 @@-{-# OPTIONS_GHC -Wall #-}-{-# LANGUAGE ScopedTypeVariables #-}--module Dyno.View.Symbolic- ( Symbolic(..)- ) where--import Data.Proxy ( Proxy(..) )-import Data.Vector ( Vector )--import Casadi.Core.Classes.SharedObject-import Casadi.Core.Classes.Function ( Function, castFunction )-import Casadi.Core.Classes.SXFunction-import Casadi.Core.Classes.MXFunction-import Casadi.Core.Enums ( InputOutputScheme(..) )--import Casadi.CMatrix ( CMatrix(..) )-import Casadi.SX ( SX, ssymV )-import Casadi.Option ( setOption )-import Casadi.MX ( MX, symV )-import Casadi.IOSchemes--import Dyno.View.Unsafe.View ( mkJ )--import Dyno.View.View ( View(..), J )-import Dyno.View.Viewable ( Viewable(..) )--class (Viewable a, CMatrix a) => Symbolic a where- -- | creating symbolics- sym :: View f => String -> IO (J f a)- mkScheme :: InputOutputScheme -> [(String,a)] -> IO (Vector a)- mkFunction :: String -> Vector a -> Vector a -> IO Function-instance Symbolic SX where- sym = mkSym ssymV- mkScheme = mkSchemeSX- mkFunction name x y = do- f <- sxFunction__0 x y- setOption f "name" name- sharedObject_init__0 f- return (castFunction f)--instance Symbolic MX where- sym = mkSym symV- mkScheme = mkSchemeMX- mkFunction name x y = do- f <- mxFunction__0 x y- setOption f "name" name- sharedObject_init__0 f- return (castFunction f)--mkSym :: forall f a . (View f, Viewable a) => (String -> Int -> IO a) -> String -> IO (J f a)-mkSym vsym name = ret- where- ret :: IO (J f a)- ret = fmap mkJ (vsym name n)- n = size (Proxy :: Proxy f)
tests/IntegrationTests.hs view
@@ -113,7 +113,12 @@ } let guess :: J (CollTraj x None None p n deg) (Vector Double) guess = cat $ makeGuessSim roots tf x0 (\_ x _ -> ode x p 0) (\_ _ -> None) p- cp <- makeCollProblem roots ocp ocpInputs guess :: IO (CollProblem x None None p x None x None None None None None n deg)+ dirCollOpts =+ DirCollOptions+ { collocationRoots = roots+ , mapStrategy = Unrolled+ }+ cp <- makeCollProblem dirCollOpts ocp ocpInputs guess :: IO (CollProblem x None None p x None x None None None None None n deg) (msg, opt') <- solveNlp solver (cpNlp cp) Nothing return $ case msg of Left m -> Left m
tests/QuadratureTests.hs view
@@ -10,6 +10,7 @@ import GHC.Generics ( Generic, Generic1 ) +import qualified Data.Map as M import Data.Vector ( Vector ) import qualified Test.HUnit.Base as HUnit import Test.Framework ( Test, testGroup )@@ -191,9 +192,14 @@ CollTraj _ _ _ xf' = split (xOpt out) Id f = splitJV (fOpt out) -compareIntegration :: (QuadratureRoots, StateOrOutput, QuadOrLagrange) -> HUnit.Assertion-compareIntegration (roots, stateOrOutput, quadOrLag) = HUnit.assert $ do- cp <- makeCollProblem roots (quadOcp stateOrOutput quadOrLag) quadOcpInputs (guess roots)+compareIntegration :: (MapStrategy, QuadratureRoots, StateOrOutput, QuadOrLagrange) -> HUnit.Assertion+compareIntegration (mapStrat, roots, stateOrOutput, quadOrLag) = HUnit.assert $ do+ let dirCollOpts =+ DirCollOptions+ { mapStrategy = mapStrat+ , collocationRoots = roots+ }+ cp <- makeCollProblem dirCollOpts (quadOcp stateOrOutput quadOrLag) quadOcpInputs (guess roots) let nlp = cpNlp cp (ret, out) <- solveNlp solver nlp Nothing case ret of@@ -208,5 +214,9 @@ | root <- [Radau, Legendre] , stateOrOutput <- [TestState, TestOutput] , quadOrLagr <- [TestQuadratures, TestLagrangeTerm]- , let input = (root, stateOrOutput, quadOrLagr)+ , mapStrat <- [ Unrolled+ , Symbolic (M.fromList [("parallelization", Opt "serial")])+ , Symbolic (M.fromList [("parallelization", Opt "openmp")])+ ]+ , let input = (mapStrat, root, stateOrOutput, quadOrLagr) ]
tests/ViewTests.hs view
@@ -13,6 +13,7 @@ import GHC.Generics ( Generic1 ) +import qualified Data.Map as M import Data.Proxy ( Proxy(..) ) import qualified Data.Binary as B import qualified Data.Serialize as S@@ -27,7 +28,6 @@ import Casadi.Function ( evalDMatrix ) import Casadi.MXFunction ( mxFunction )-import Casadi.SharedObject ( soInit ) import Casadi.CMatrix ( CMatrix ) import Casadi.DMatrix ( DMatrix ) import Casadi.MX ( MX )@@ -105,8 +105,7 @@ evalMX :: MX -> DMatrix evalMX x = unsafePerformIO $ do- f <- mxFunction V.empty (V.singleton x)- soInit f+ f <- mxFunction "evalMX" V.empty (V.singleton x) M.empty ret <- evalDMatrix f V.empty return (V.head ret)