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