diff --git a/LICENSE b/LICENSE new file mode 100644 --- /dev/null +++ b/LICENSE @@ -0,0 +1,29 @@ +Copyright (c) 2002-2009, Henrik Nilsson and Yale University. +All rights reserved. + +Redistribution and use in source and binary forms, with or without +modification, are permitted provided that the following conditions +are met: + +- Redistributions of source code must retain the above copyright notice, +this list of conditions and the following disclaimer. + +- Redistributions in binary form must reproduce the above copyright +notice, this list of conditions and the following disclaimer in the +documentation and/or other materials provided with the distribution. + +- Neither name of the copyright holders nor the names of its +contributors may be used to endorse or promote products derived from +this software without specific prior written permission. + +THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND THE CONTRIBUTORS +"AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT +LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR +A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT +HOLDERS OR THE CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, +INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, +BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS +OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND +ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR +TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE +USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. diff --git a/Setup.hs b/Setup.hs new file mode 100644 --- /dev/null +++ b/Setup.hs @@ -0,0 +1,2 @@ +import Distribution.Simple +main = defaultMain diff --git a/YFrob.cabal b/YFrob.cabal new file mode 100644 --- /dev/null +++ b/YFrob.cabal @@ -0,0 +1,59 @@ +Name: YFrob +Version: 0.4 +Cabal-Version: >= 1.2 +License: BSD3 +License-File: LICENSE +Copyright: (c) 2002-2009 Hanrik Nilsson and Yale University +Author: Henrik Nilsson +Maintainer: Henrik Nilsson (nhn@cs.nott.ac.uk) +Homepage: http://www.haskell.org/yampa/ +Category: Reactivity, FRP, Yampa +Synopsis: Yampa-based library for programming robots +Description: + Yampa-based, domain-specific language embedded in Haskell for programming + robots. At present, only simulated robots. However, the infrastructure is + separated into generic and robot-specific parts, and set up so as to make + it possible to write robot code that depends only on specific features, as + opposed to specific platforms or versions of those platforms. Thus, it is + in principle possible to write quite generic robot code. (Once upon a time, + the Pioneer platform, a real robot, was supported.) +Tested-With: GHC +Build-Type: Simple +Extra-Source-Files: + robotsim-test/Makefile + robotsim-test/Setup.hs + robotsim-test/YFrobRobotSimTest.cabal + robotsim-test/Main.hs + afp-demos/Makefile + afp-demos/Setup.hs + afp-demos/YFrobAFPDemos.cabal + afp-demos/Main.hs + afp-demos/AFPDemos.hs + afp-demos/ITest.hs + +Library + Hs-Source-Dirs: src + GHC-Options : -O2 -Wall -fno-warn-name-shadowing + Build-Depends: base >= 3 && < 5, array, HGL, Yampa >= 0.9 + Exposed-Modules: + FRP.YFrob.Common + FRP.YFrob.RobotSim + Other-Modules: + FRP.YFrob.Common.Diagnostics + FRP.YFrob.Common.PhysicalDimensions + FRP.YFrob.Common.RobotIO + FRP.YFrob.RobotSim.Animate + FRP.YFrob.RobotSim.ColorBindings + FRP.YFrob.RobotSim.Colors + FRP.YFrob.RobotSim.Command + FRP.YFrob.RobotSim.IdentityList + FRP.YFrob.RobotSim.IO + FRP.YFrob.RobotSim.Object + FRP.YFrob.RobotSim.ObjectPhysics + FRP.YFrob.RobotSim.ObjectTemplate + FRP.YFrob.RobotSim.Parser + FRP.YFrob.RobotSim.RenderFixedWalls + FRP.YFrob.RobotSim.RenderObject + FRP.YFrob.RobotSim.Simulator + FRP.YFrob.RobotSim.WorldGeometry + FRP.YFrob.RobotSim.World diff --git a/afp-demos/AFPDemos.hs b/afp-demos/AFPDemos.hs new file mode 100644 --- /dev/null +++ b/afp-demos/AFPDemos.hs @@ -0,0 +1,127 @@ +{-# LANGUAGE Arrows #-} + +module AFPDemos where + +import FRP.YFrob.RobotSim + +---------------------------------------------------------------------- + +-- Template illustrating how to select specific controller based on the +-- robot identity. + +rcA :: SimbotController +rcA rProps = + case rpId rProps of + 1 -> rcA1 rProps + 2 -> rcA2 rProps + 3 -> rcA3 rProps + +rcA1, rcA2, rcA3 :: SimbotController +rcA1 = undefined +rcA2 = undefined +rcA3 = undefined + +---------------------------------------------------------------------- + +rcStop :: SimbotController +rcStop _ = constant (mrFinalize ddBrake) + +rcBlind1 _ = constant (mrFinalize $ ddVelDiff 10 10) + +rcBlind2 rps = + let max = rpWSMax rps + in constant (mrFinalize $ ddVelDiff (max/2) (max/2)) + +rcTurn :: Velocity -> SimbotController +rcTurn vel rps = + let vMax = rpWSMax rps + rMax = 2 * (vMax - vel) / rpDiameter rps + in constant (mrFinalize $ ddVelTR vel rMax) + +rcReverse :: Velocity -> SimbotController +rcReverse v rps = beh `dSwitch` const (rcReverse (-v) rps) + where beh = proc sbi -> do + stuckE <- rsStuck -< sbi + let mr = ddVelDiff v v `mrMerge` + tcoPrintMessage (tag stuckE "Ouch!!") + returnA -< (mrFinalize mr, stuckE) + +rcReverse' v rps = + (rsStuck >>> arr fun) `dSwitch` const (rcReverse' (-v) rps) + where fun stuckE = + let mr = ddVelDiff v v `mrMerge` + tcoPrintMessage (tag stuckE "Ouch!!") + in (mrFinalize mr, stuckE) + +rcHeading :: Velocity -> Heading -> SimbotController +rcHeading vel hd rps = + let vMax = rpWSMax rps + vel' = lim vMax vel + k = 2 + in proc sbi -> do + let phi = normalizeAngle (hd - odometryHeading sbi) + let vel'' = (1 - abs phi / pi) * vel' + returnA -< mrFinalize (ddVelTR vel'' (k*phi)) + +rcHeading' :: Velocity -> Heading -> SimbotController +rcHeading' vel hd rps = + proc sbi -> do + rcHeadingAux rps -< (sbi, vel, hd) + +rcHeadingAux :: SimbotProperties -> + SF (SimbotInput,Velocity,Heading) SimbotOutput +rcHeadingAux rps = + let vMax = rpWSMax rps + k = 2 + in proc (sbi,vel,hd) -> do + let vel' = lim vMax vel + let phi = normalizeAngle (hd - odometryHeading sbi) + let vel'' = (1 - abs phi / pi) * vel' + returnA -< mrFinalize (ddVelTR vel'' (k*phi)) + +rcMoveTo :: Velocity -> Position2 -> SimbotController +rcMoveTo vd pd rps = proc sbi -> do + let (d,h) = vector2RhoTheta (pd .-. odometryPosition sbi) + vel = if d>2 then vd else vd*(d/2) + rcHeadingAux rps -< (sbi, vel, h) + +rcGoToBall :: Velocity -> SimbotController +rcGoToBall vd rps = proc sbi -> do + let (phi, d) = head (aotBalls sbi ++ [(0.0,0.0)]) + h = odometryHeading sbi + rcHeadingAux rps -< (sbi, vd, h + phi) + +rcGoToBall2 :: Velocity -> SimbotController +rcGoToBall2 vd rps = + let loop = switch (rcGoToBall vd rps &&& rsStuck) $ \_ -> + switch (constant (mrFinalize (ddVelTR (-vd) 0.3)) + &&& after 2.5 ()) $ \_ -> + loop + in + loop + + +lim m y = max (-m) (min m y) + +rcFollowLeftWall :: Velocity -> Distance -> SimbotController +rcFollowLeftWall v d _ = proc sbi -> do + let r = rfLeft sbi + dr <- derivative -< r + let omega = kp*(r-d) + kd*dr + kd = 5 + kp = v*(kd^2)/4 + returnA -< mrFinalize (ddVelTR v (lim 0.2 omega)) + +rcAlign :: Velocity -> SimbotController +rcAlign v rps = proc sbi -> do + let neighbors = aotOtherRobots sbi + vs = map (\(_,_,a,d) -> vector2Polar d a) neighbors + avg = if vs==[] then zeroVector + else foldl1 (^+^) vs ^/ fromInteger (toInteger (length vs)) + heading = vector2Theta avg + odometryHeading sbi + rcHeadingAux rps -< (sbi, v, heading) + -- o <- rcHeadingAux' rps -< (sbi, v, heading) + -- printE <- repeatedly 1.0 () -< () + -- returnA -< mrFinalize (mrMerge o + -- (tcoPrintMessage (tag printE (show (vs,heading))))) + diff --git a/afp-demos/ITest.hs b/afp-demos/ITest.hs new file mode 100644 --- /dev/null +++ b/afp-demos/ITest.hs @@ -0,0 +1,103 @@ +-- +-- ITest.hs -- An interactive command-line interpreter for running +-- a series of interactive tests. + +module ITest where + +import System.IO (hFlush, stdout) +import Data.List (sortBy, isPrefixOf) + +-- type synonym for a test-case that is a simple IO action: +type IOTest=(String,IO ()) + +-- utility function: +fst3 (x,_,_) = x + +-- testShell is the top-level command interpreter. +-- arguments: +-- tests -- a list of simple IOTests +-- prompt -- the prompt to display to the user +-- args -- command-line arguments +testShell :: [IOTest] -> String -> [String] -> IO () +testShell tests prompt args = + if ((length args)==0) + then putStrLn "\":help\" for help." >> iShell tests prompt + else loop args + where loop [] = return () + loop (tname:ts) = do done <- runTest tests tname + if done then return () else loop ts + +-- interactive shell: +iShell :: [IOTest] -> String -> IO () +iShell tests prompt = + do putStr prompt + hFlush stdout + line <- getLine + done <- case words line of + ((':':cmd):args) -> handleBuiltin tests cmd args + (tname:args) -> runTest tests tname + _ -> return False + if done + then return () + else iShell tests prompt + +-- runSafe: run a specific IO action, and catch any exceptions: +runSafe :: IO () -> IO () +runSafe act = + catch act (\e -> do putStrLn ("*** Exception running test: ") + putStrLn (show e) + putStrLn "***") + +-- run a specific simple IO test +runTest :: [IOTest] -> String -> IO Bool +runTest tests tname = + let ts = filter ((== tname) . fst) tests + in do case ts of + ((nm,tf):[]) -> do putStrLn ("executing test '" ++ nm++ "'") + runSafe tf + putStrLn ("(test complete)") + _ -> do putStrLn ("unknown or ambiguous test '" ++ tname ++ "'") + putStrLn ("Use ':list' to list available tests.") + return False + +builtins :: [(String,String,[IOTest] -> [String] -> IO Bool)] +builtins = [("quit","exit the test shell",(\_ _ -> return True)) + , ("help","display this list of commands", binHelp) + , ("list","list available test cases", binList) + ] + +-- help builtin: +binHelp :: [IOTest] -> [String] -> IO Bool +binHelp tests args = + do putStrLn ("Command summary:") + mapM_ putStrLn hlist + return False + where hlist = map aux (sortBy ccomp builtins) + aux (cmd,desc,_) = (" :" ++ cmd ++ " -- " ++ desc) + ccomp (cmd1,_,_) (cmd2,_,_) = compare cmd1 cmd2 + +-- list builtin: +binList :: [IOTest] -> [String] -> IO Bool +binList tests args = + do putStrLn ("Available Tests:") + mapM_ putStrLn tnms + return False + where tnms = map aux (sortBy tcomp tests) + aux (tnm,_) = " " ++ tnm + tcomp (tnm1,_) (tnm2,_) = compare tnm1 tnm2 + +-- process a built-in command +handleBuiltin :: [IOTest] -> String -> [String] -> IO Bool +handleBuiltin tstate cmd args = + let bins = filter (isPrefixOf cmd . fst3) builtins + in case bins of + ((_,_,cmdf):[]) -> cmdf tstate args + _ -> binError cmd + +binError :: String -> IO Bool +binError cmd = + do putStrLn ("Unknown or ambiguous command ':" ++ cmd ++ "'") + putStrLn ("Use ':help' for help.") + return False + + diff --git a/afp-demos/Main.hs b/afp-demos/Main.hs new file mode 100644 --- /dev/null +++ b/afp-demos/Main.hs @@ -0,0 +1,83 @@ +module Main where + +import System.Environment (getArgs) + +import FRP.YFrob.RobotSim +import AFPDemos +import ITest + + +world :: WorldTemplate +world = [ OTSimbotA {otRId = 1, otPos = Point2 (-3) (-4), otHdng=(pi/2)}, + OTSimbotB {otRId = 2, otPos = Point2 3 (-4), otHdng=(pi/2)} + ] + +-- for rcTurn: +tworld :: WorldTemplate +tworld = [ OTSimbotA {otRId = 1, otPos = Point2 (-3) (-4), otHdng=(pi/2)}, + OTSimbotB {otRId = 2, otPos = Point2 3 (-1), otHdng=(pi/2)} + ] + +-- for blind and reverse tests: A world with some obstacles: +world2 :: WorldTemplate +world2 = [ OTSimbotA {otRId = 1, otPos = Point2 (-2) (-2), otHdng=pi/4}, + OTSimbotA {otRId = 2, otPos = Point2 (-2) 0, otHdng=0}, + OTSimbotA {otRId = 3, otPos = Point2 (-2) 2, otHdng=(-pi/4)}, + OTSimbotA {otRId = 4, otPos = Point2 (-3) 1, otHdng=pi/8}, + OTSimbotB {otRId = 1, otPos = Point2 3 (-4), otHdng=(pi/2)}, + OTSimbotB {otRId = 2, otPos = Point2 1 1, otHdng=0}, + OTBlock {otPos = Point2 (-4) 2}, + OTNSWall {otPos = Point2 (-4) 4}, + OTEWWall {otPos = Point2 (-2) (-2)}, + OTVWall {otPos = Point2 (-3) 0}, + OTHWall {otPos = Point2 3 2}, + OTBall {otPos = Point2 3.1 (-2)}, + OTBall {otPos = Point2 2 2} + ] + +-- for align test: +world3 :: WorldTemplate +world3 = [ OTSimbotA {otRId = 1, otPos = Point2 (-4) (4), otHdng=pi/4}, + OTSimbotA {otRId = 2, otPos = Point2 (-3) 1, otHdng=pi/16}, + OTSimbotA {otRId = 3, otPos = Point2 (-2) (-2), otHdng=0}, + OTSimbotA {otRId = 4, otPos = Point2 0 0, otHdng=(-pi/4)} + -- OTSimbotA {otRId = 3, otPos = Point2 (-2) 2, otHdng=(-pi/4)}, + -- OTSimbotA {otRId = 4, otPos = Point2 (-3) 1, otHdng=pi/8}, + -- OTSimbotB {otRId = 3, otPos = Point2 3 (-4), otHdng=(pi/2)}, + -- OTSimbotB {otRId = 4, otPos = Point2 1 1, otHdng=0} + ] + +-- for wall following test: +world4 :: WorldTemplate +world4 = [ OTSimbotA {otRId = 1, otPos = Point2 (-4) (-3.3), otHdng=(pi/2)}, + OTSimbotB {otRId = 2, otPos = Point2 3 (-3.5), otHdng=(pi)} + ] + +afpDemos = [ {- ("rc",(Just world,rc,rc)), -} + ("rcStop",(Just world, rcStop, rcStop)), + ("rcBlind1",(Just world2, rcBlind1, rcBlind1)), + ("rcBlind2",(Just world2, rcBlind2, rcBlind2)), + ("rcTurn",(Just tworld, rcTurn 1, rcTurn 1.5)), + ("rcReverse",(Just world2, rcReverse 1, rcReverse 1)), + ("rcReverse'",(Just world2, rcReverse' 1, rcReverse' 1)), + ("rcHeading",(Just world, rcHeading 1 (pi/4), rcHeading 1 pi)), + ("rcHeading'",(Just world, rcHeading' 1 (pi/4), rcHeading' 1 pi)), + ("rcMoveTo", + (Just world,rcMoveTo 1 (Point2 3 4),rcMoveTo 1 (Point2 (-4) 4))), + ("rcGoToBall",(Just world2, rcReverse 1, rcGoToBall2 1)), + ("rcFollowLeftWall", + (Just world4,rcFollowLeftWall 0.5 0.5,rcFollowLeftWall 0.7 0.5)), + ("rcAlign", + (Just world3,rcAlign 1,rcAlign 1)) + ] + +ioDemos = map (\ (nm, (w,rc1,rc2)) -> (nm, runSim w rc1 rc2)) afpDemos + +main :: IO () +main = + do args <- getArgs + testShell ioDemos "AFP Demos> " args + + + + diff --git a/afp-demos/Makefile b/afp-demos/Makefile new file mode 100644 --- /dev/null +++ b/afp-demos/Makefile @@ -0,0 +1,22 @@ +# Makefile for YFrob AFP demos + +.PHONY: all configure build install clean + +TARGET = afp-demos + +all: build + +configure: + runhaskell Setup.hs configure --ghc + +build: configure + runhaskell Setup.hs build + -rm ${TARGET} + ln -s dist/build/${TARGET}/${TARGET} ${TARGET} + +install: build + runhaskell Setup.hs install + +clean: + runhaskell Setup.hs clean + -rm ${TARGET} diff --git a/afp-demos/Setup.hs b/afp-demos/Setup.hs new file mode 100644 --- /dev/null +++ b/afp-demos/Setup.hs @@ -0,0 +1,2 @@ +import Distribution.Simple +main = defaultMain diff --git a/afp-demos/YFrobAFPDemos.cabal b/afp-demos/YFrobAFPDemos.cabal new file mode 100644 --- /dev/null +++ b/afp-demos/YFrobAFPDemos.cabal @@ -0,0 +1,26 @@ +Name: YFrobAFPDemos +Version: 0.1 +Cabal-Version: >= 1.2 +License: BSD3 +License-File: ../LICENSE +Copyright: (c) 2002-2009 Hanrik Nilsson and Yale University +Author: Henrik Nilsson +Maintainer: Henrik Nilsson (nhn@cs.nott.ac.uk) +Homepage: http://www.haskell.org/yampa/ +Category: Reactivity, FRP, Yampa, YFrob +Synopsis: AFP 2002 YFrob demonstrations +Description: + A suite of YFrob demonstrations originating form the Summer School on + Advanced Functional Programming (AFP) 2002. A (very) simple text-based + user interface is used to select a demonstration to run. To get back + to the user interface, close the demonstration window. +Tested-With: GHC +Build-Type: Simple + +Executable afp-demos + GHC-Options : -O2 -Wall -fno-warn-name-shadowing + Build-Depends: base, YFrob >= 0.4 + Main-Is: Main.hs + Other-Modules: + AFPDemos + ITest diff --git a/robotsim-test/Main.hs b/robotsim-test/Main.hs new file mode 100644 --- /dev/null +++ b/robotsim-test/Main.hs @@ -0,0 +1,221 @@ +{-# LANGUAGE Arrows #-} + +module Main where + +import Data.List (sortBy) + +import FRP.YFrob.RobotSim + +{- +-- Robot B tends to get stuck. Why??? +world :: WorldTemplate +world = [ OTSimbotA {otRId = 1, otPos = Point2 (-2) (-2), otHdng=pi/4}, + OTSimbotA {otRId = 2, otPos = Point2 (-2) 0, otHdng=0}, + OTSimbotA {otRId = 3, otPos = Point2 (-2) 2, otHdng=(-pi/4)}, + OTSimbotA {otRId = 4, otPos = Point2 (-3) 1, otHdng=pi/8}, + -- OTSimbotB {otRId = 1, otPos = Point2 3 (-4), otHdng=(pi/2)}, + OTSimbotB {otRId = 2, otPos = Point2 1 1, otHdng=0}, + OTBlock {otPos = Point2 (-4) 2}, + OTNSWall {otPos = Point2 (-4) 4}, + OTEWWall {otPos = Point2 (-2) (-2)}, + OTVWall {otPos = Point2 (-3) 0}, + OTHWall {otPos = Point2 3 2}, + OTBall {otPos = Point2 3 (-2)}, + OTBall {otPos = Point2 2 2} + ] + + +main = runSim (Just world) (rcReverse 0.5) (rcReverse2 1.0) -- (rcAlign 0.0) (rcAlign 0.0) -- sc1 (rcReverse 1.0) +-} + +world :: WorldTemplate +world = [ OTSimbotA {otRId = 1, otPos = Point2 4.5 3.5, otHdng=0}, + OTSimbotB {otRId = 2, otPos = Point2 2.5 1.5, otHdng=0}, + OTSimbotB {otRId = 3, otPos = Point2 3 3.5, otHdng=0}, + OTBlock {otPos = Point2 (4.5) 2}, + OTNSWall {otPos = Point2 (4) 3.5}, + OTEWWall {otPos = Point2 (3.5) (1)} + ] + + +main = runSim (Just world) sc1 rcStop + + +sc1 :: SimbotController +sc1 sp = proc si -> do + e <- repeatedly 0.5 () -< () + let etco = e `tag` show (rfLeft si) + returnA -< mrFinalize (ddVelTR 0.0 0.2 `mrMerge` tcoPrintMessage etco) + + +sc2 :: SimbotController +sc2 sp = proc _ -> do + returnA -< mrFinalize (ddVelTR 0.5 0.5) + + +sc3 :: SimbotController +sc3 sp = gotoXYs tps 0.90 + where + tps = [Point2 (-4) (-4), Point2 0 4, Point2 4 (-4)] + +sc4 :: SimbotController +sc4 sp = gotoXYs (cycle tps) 2.0 + where + tps = [Point2 3 3, Point2 (-3) 3, Point2 (-3) (-3), Point2 3 (-3)] + + +gotoXYs :: [Position2] -> Velocity -> SF SimbotInput SimbotOutput +gotoXYs [] _ = stop +gotoXYs (tp : tps) v = switch (gotoXY tp v) $ \_ -> + gotoXYs tps v + + +gotoXY :: Position2 -> Velocity -> SF SimbotInput (SimbotOutput, Event ()) +gotoXY tp v = towardsTarget &&& closeEnough + where + towardsTarget = proc si -> do + let tv = tp .-. odometryPosition si + (td, th) = (vector2RhoTheta tv) + dh = normalizeHeading (th - odometryHeading si) + -- Eager to turn towards goal. + v_l = cos dh * (min (td^2) v) - sin (dh / 2) * v + v_r = cos dh * (min (td^2) v) + sin (dh / 2) * v + -- Reluctant to turn when facing 180° from goal dir. + -- v_l = cos dh * (min (td^2) v) - sin dh * v + -- v_r = cos dh * (min (td^2) v) + sin dh * v + returnA -< mrFinalize (ddVelDiff v_l v_r) + + closeEnough = proc si -> do + let td = norm (tp .-. odometryPosition si) < 0.05 + e <- iEdge False -< td + returnA -< e + + +stop :: SF a SimbotOutput +stop = constant (mrFinalize ddBrake) + + +------------------------------------------------------------------------------ +-- Paul's controllers +------------------------------------------------------------------------------ + +rcStop :: SimbotController +rcStop _ = constant (mrFinalize ddBrake) + +rcBlind1 _ = constant (mrFinalize $ ddVelDiff 10 10) + +rcBlind2 rps = + let max = rpWSMax rps + in constant (mrFinalize $ ddVelDiff (max/2) (max/2)) + +rcTurn :: Velocity -> SimbotController +rcTurn vel rps = + let vMax = rpWSMax rps + rMax = 2 * (vMax - vel) / rpDiameter rps + in constant (mrFinalize $ ddVelTR (vMax/2) rMax) + +rcReverse :: Velocity -> SimbotController +rcReverse v rps = beh `dSwitch` const (rcReverse (-v) rps) + where beh = proc sbi -> do + stuckE <- rsStuck -< sbi + let mr = ddVelDiff v v + `mrMerge` tcoPrintMessage (tag stuckE "Ouch!!") + returnA -< (mrFinalize mr, stuckE) + +rcReverse2 :: Velocity -> SimbotController +rcReverse2 v rps = beh `dSwitch` const (rcReverse2 (-v) rps) + where beh = proc sbi -> do + stuckE <- rsStuck -< sbi + e <- repeatedly 1.0 () -< () + st <- systemTime -< sbi + let mr = ddVelDiff v v + `mrMerge` tcoPrintMessage (tag stuckE "Ouch!!") + `mrMerge` tcoPrintMessage (tag e (show st)) + returnA -< (mrFinalize mr, stuckE) + +rcReverse' v rps = + (rsStuck >>> arr fun) `dSwitch` const (rcReverse' (-v) rps) + where fun stuckE = + let mr = ddVelDiff v v `mrMerge` + tcoPrintMessage (tag stuckE "Ouch!!") + in (mrFinalize mr, stuckE) + +rcHeading :: Velocity -> Heading -> SimbotController +rcHeading vel hd rps = + let vMax = rpWSMax rps + rMax = 2 * (vMax - vel) / rpDiameter rps + in proc sbi -> do + let he = normalizeAngle (hd - odometryHeading sbi) + rot = if he < 0 -- (pi Heading -> SimbotController +rcHeading1 vel hd rps = + let vMax = rpWSMax rps + rMax = 2 * (vMax - vel) / rpDiameter rps + in proc sbi -> do + let ang = normalizeAngle (hd - odometryHeading sbi) + rot = if ang < 0 then -rMax else rMax + returnA -< mrFinalize (ddVelTR (vMax/2) {- he -} rot) + +rcHeading2 :: Velocity -> Heading -> SimbotController +rcHeading2 vel hd rps = + let vMax = rpWSMax rps + rMax = 2 * (vMax - vel) / rpDiameter rps + in proc sbi -> do + let ang = normalizeAngle (hd - odometryHeading sbi) + returnA -< mrFinalize (ddVelTR (vMax/2) (2 * ang)) + +rcHeading' :: Velocity -> Heading -> SimbotController +rcHeading' vel hd rps = + proc sbi -> do + rcHeadingAux rps -< (sbi, vel, hd) + +rcHeading'' :: Velocity -> Heading -> SimbotController +rcHeading'' vel hd rps = + proc sbi -> do + t <- localTime -< () + rcHeadingAux rps -< (sbi, vel, hd + sin(2 * pi * t)) + + +rcHeadingAux :: SimbotProperties -> + SF (SimbotInput,Velocity,Heading) SimbotOutput +rcHeadingAux rps = + let vMax = rpWSMax rps + in proc (sbi,vel,hd) -> do + let rMax = 2 * (vMax - vel) / rpDiameter rps + he = hd - odometryHeading sbi + rot = if (pi Position2 -> SimbotController +rcMoveTo vd pd rps = proc sbi -> do + let (d,h) = vector2RhoTheta (pd .-. odometryPosition sbi) + vel = if d>2 then vd else vd*(d/2) + rcHeadingAux rps -< (sbi, vel, h) + +lim m y = max (-m) (min m y) + +rcFollowLeftWall :: Velocity -> Distance -> SimbotController +rcFollowLeftWall v d _ = proc sbi -> do + let r = rfLeft sbi + dr <- derivative -< r + let omega = kp*(r-d) + kd*dr + kd = 5 + kp = v*(kd^2)/4 + returnA -< mrFinalize (ddVelTR v (lim 0.2 omega)) + +rcAlign :: Velocity -> SimbotController +rcAlign v rps = proc sbi -> do + let neighbors = sortBy (\(_,_,_,d1) (_,_,_,d2) -> compare d1 d2) + (aotOtherRobots sbi) + (l1:l2:l3:_) = map (\(_,_,a,d) -> vector2Polar d a) neighbors + vhat = vector2Polar v (odometryHeading sbi) + heading l = vector2Theta (vhat ^+^ l) + dl1 <- derivative -< l1 + dl2 <- derivative -< l2 + dl3 <- derivative -< l3 + let hAvg = sum (map heading [dl1,dl2,dl3]) / 3 + rcHeadingAux rps -< (sbi, v, hAvg) diff --git a/robotsim-test/Makefile b/robotsim-test/Makefile new file mode 100644 --- /dev/null +++ b/robotsim-test/Makefile @@ -0,0 +1,22 @@ +# Makefile for YFrob RobotSim test + +.PHONY: all configure build install clean + +TARGET = robotsim-test + +all: build + +configure: + runhaskell Setup.hs configure --ghc + +build: configure + runhaskell Setup.hs build + -rm ${TARGET} + ln -s dist/build/${TARGET}/${TARGET} ${TARGET} + +install: build + runhaskell Setup.hs install + +clean: + runhaskell Setup.hs clean + -rm ${TARGET} diff --git a/robotsim-test/Setup.hs b/robotsim-test/Setup.hs new file mode 100644 --- /dev/null +++ b/robotsim-test/Setup.hs @@ -0,0 +1,2 @@ +import Distribution.Simple +main = defaultMain diff --git a/robotsim-test/YFrobRobotSimTest.cabal b/robotsim-test/YFrobRobotSimTest.cabal new file mode 100644 --- /dev/null +++ b/robotsim-test/YFrobRobotSimTest.cabal @@ -0,0 +1,21 @@ +Name: YFrobRobotSimTest +Version: 0.1 +Cabal-Version: >= 1.2 +License: BSD3 +License-File: ../LICENSE +Copyright: (c) 2002-2009 Hanrik Nilsson and Yale University +Author: Henrik Nilsson +Maintainer: Henrik Nilsson (nhn@cs.nott.ac.uk) +Homepage: http://www.haskell.org/yampa/ +Category: Reactivity, FRP, Yampa, YFrob +Synopsis: Test of YFrob robot simulator +Description: + Simple test of of the YFrob robot simulator. Just to make sure things can + be comiled and that basic graphics can be displayed on the screen. +Tested-With: GHC +Build-Type: Simple + +Executable robotsim-test + GHC-Options : -O2 -Wall -fno-warn-name-shadowing + Build-Depends: base, YFrob >= 0.4 + Main-Is: Main.hs diff --git a/src/FRP/YFrob/Common.hs b/src/FRP/YFrob/Common.hs new file mode 100644 --- /dev/null +++ b/src/FRP/YFrob/Common.hs @@ -0,0 +1,34 @@ +{- +****************************************************************************** +* Y F R O B / C O M M O N * +* * +* Module: Common * +* Purpose: Top level module for the generic part of YFrob * +* Author: Henrik Nilsson * +* * +****************************************************************************** +-} + +module FRP.YFrob.Common ( + module FRP.Yampa, + module FRP.Yampa.Task, + module FRP.Yampa.Geometry, -- Used for PhysicalDimensions + module FRP.Yampa.MergeableRecord, + module FRP.YFrob.Common.Diagnostics, + module FRP.YFrob.Common.PhysicalDimensions, + module FRP.YFrob.Common.RobotIO +) where + + +-- Yampa and Yampa extensions that are judged sufficiently useful in the YFrob +-- conext to be provided by default. +import FRP.Yampa +import FRP.Yampa.Task +import FRP.Yampa.Geometry hiding ((*^), (^+^), (^-^), (^/), dot, negateVector, + norm, normalize, zeroVector, VectorSpace) +import FRP.Yampa.MergeableRecord + +-- YFrob modules. +import FRP.YFrob.Common.Diagnostics +import FRP.YFrob.Common.PhysicalDimensions +import FRP.YFrob.Common.RobotIO diff --git a/src/FRP/YFrob/Common/Diagnostics.hs b/src/FRP/YFrob/Common/Diagnostics.hs new file mode 100644 --- /dev/null +++ b/src/FRP/YFrob/Common/Diagnostics.hs @@ -0,0 +1,19 @@ +{- +****************************************************************************** +* Y F R O B / C O M M O N * +* * +* Module: Diagnostics * +* Purpose: Standardized error-reporting for YFrob * +* Authors: Henrik Nilsson * +* * +****************************************************************************** +-} + +module FRP.YFrob.Common.Diagnostics where + +usrErr :: String -> String -> String -> a +usrErr mn fn msg = error ("YFrob." ++ mn ++ "." ++ fn ++ ": " ++ msg) + +intErr :: String -> String -> String -> a +intErr mn fn msg = error ("[internal error] YFrob." ++ mn ++ "." ++ fn ++ ": " + ++ msg) diff --git a/src/FRP/YFrob/Common/PhysicalDimensions.hs b/src/FRP/YFrob/Common/PhysicalDimensions.hs new file mode 100644 --- /dev/null +++ b/src/FRP/YFrob/Common/PhysicalDimensions.hs @@ -0,0 +1,129 @@ +{- +****************************************************************************** +* Y F R O B / C O M M O N * +* * +* Module: PhysicalDimensions * +* Purpose: Type synonyms for physical dimensions and some * +* related operations. * +* Author: Henrik Nilsson * +* * +****************************************************************************** +-} + +module FRP.YFrob.Common.PhysicalDimensions ( + YFrobReal, + +-- One dimensional + Frequency, + Mass, + Length, + Distance, + Position, + Speed, + Velocity, + Acceleration, + Angle, + Heading, + Bearing, + RotVel, + RotAcc, + +-- Two dimensional + Distance2, + Position2, + Velocity2, + Acceleration2, + +-- Three dimensional + Distance3, + Position3, + Velocity3, + Acceleration3, + +-- Operations + normalizeAngle, -- :: Angle -> Angle + normalizeHeading, -- :: Heading -> Heading + bearingToHeading, -- :: Bearing -> Heading + headingToBearing -- :: Heading -> Bearing +) where + +import FRP.Yampa (Time) +import FRP.Yampa.Miscellany (fMod) +import FRP.Yampa.Geometry (Vector2, Vector3, Point2, Point3) + + +-- Many of the physical dimensions below are related to time, and variables +-- of these types can thus be expected to occur in numerical expressions along +-- with variables of type time. To facilitate things, they should thus share +-- the same representation. Maybe it is a mistake that Yampa has fixed the +-- type of Time (currently to Double)? + +-- Dimensionless type. Same representation as AFRP's Time. +type YFrobReal = Time + +------------------------------------------------------------------------------ +-- One-dimensional types +------------------------------------------------------------------------------ + +type Frequency = YFrobReal -- [Hz] +type Mass = YFrobReal -- [kg] +type Length = YFrobReal -- [m] +type Position = YFrobReal -- [m] (absolute) +type Distance = YFrobReal -- [m] (relative) +type Speed = YFrobReal -- [m/s] (unsigned, speed = abs(velocity)) +type Velocity = YFrobReal -- [m/s] (signed) +type Acceleration = YFrobReal -- [m/s^2] +type Angle = YFrobReal -- [rad] (relative) +type Heading = YFrobReal -- [rad] (angle relative to x-axis = east) +type Bearing = YFrobReal -- [deg] (compass direction, 0 = N, 90 = E) +type RotVel = YFrobReal -- [rad/s] +type RotAcc = YFrobReal -- [rad/s^2] + + +------------------------------------------------------------------------------ +-- Two-dimensional types +------------------------------------------------------------------------------ + +type Position2 = Point2 Position -- [m] (absolute) +type Distance2 = Vector2 Distance -- [m] (relative) +type Velocity2 = Vector2 Velocity -- [m/s] +type Acceleration2 = Vector2 Acceleration -- [m/s^2] + + +------------------------------------------------------------------------------ +-- Three-dimensional types +------------------------------------------------------------------------------ + +type Position3 = Point3 Position -- [m] (absolute) +type Distance3 = Vector3 Distance -- [m] (relative) +type Velocity3 = Vector3 Velocity -- [m/s] +type Acceleration3 = Vector3 Acceleration -- [m/s^2] + + +------------------------------------------------------------------------------ +-- Operations +------------------------------------------------------------------------------ + +-- The resulting angle is in the interval [-pi, pi). +normalizeAngle :: Angle -> Angle +normalizeAngle d = fMod (d + pi) (2 * pi) - pi + + +-- The resulting heading is in the interval [-pi, pi). +normalizeHeading :: Heading -> Heading +normalizeHeading = normalizeAngle + + +-- Bearings in degrees are understood as on a compass; i.e., north is 0, +-- east is 90, south is 180, west is 270. +-- Heading is understood as the angle (in radians) relative to the "x-axis" +-- which is supposed to point East. + +-- The resulting heading is in the interval [-pi, pi). +bearingToHeading :: Bearing -> Heading +bearingToHeading b = (fMod (270 - b) 360 - 180) * pi / 180 + + +-- The resulting bearing is in the interval [0, 360). +headingToBearing :: Heading -> Bearing +headingToBearing d = fMod (90 - d * 180 / pi) 360 diff --git a/src/FRP/YFrob/Common/RobotIO.hs b/src/FRP/YFrob/Common/RobotIO.hs new file mode 100644 --- /dev/null +++ b/src/FRP/YFrob/Common/RobotIO.hs @@ -0,0 +1,246 @@ +{- +****************************************************************************** +* Y F R O B / C O M M O N * +* * +* Module: RobotIO * +* Purpose: Type classes and related definitions for robot I/O. * +* Author: Henrik Nilsson * +* * +****************************************************************************** +-} + +-- These classes are to be instatiated by various I/O types for specific +-- robots, thus allowing generic controller code working across a range +-- of robots to be written. + +-- To do/think about: +-- * Maybe classes/parts of classes providing access to constants ought +-- to be separated from true input classes. A robot XXX would then +-- have a record XXXConstants instantiating the relevant constant classes, +-- and XXXInput instantiating the relevant input classes. A typical +-- controller would have a type like +-- ctrlr :: (HasRobotProperties c, ..., HasOdometry i, ...) => c -> SF i o +-- Problem: some classes contain both constant aspects and true, variable +-- entities. The separation into constant and variable entities could be +-- rather awkward. +-- Furthermore! The geometry of a robot could potentially change dynamically, +-- things like maximal acceleration could certainly change (e.g. depending +-- on the battery status), and so on. So maybe it isn't really to bad to +-- make these "constants" part of the input. +-- * Maybe _some_ of this should move into Yampa at some point? +-- E.g. if one wants a "standardized" way of doing console I/O and GUI I/O? +-- Or is this strictly the business of the application specific layer +-- around Yampa? +-- * (This is also Yampa-related: the note also appears in YampaEvent.) +-- Question: How do we handle "events" from the outside world? Do we allow +-- events from the outside world in the first place??? Currently, the +-- answer is "no" because Event is an abstract type, and there are +-- (intentionally) no constructors for constructing events pointwise, e.g. +-- from a Bool or Maybe. Instead, events are the result of (stateful) edge +-- detection. +-- There are a number of reasons for this: +-- - Potentially, we may be semantically better off by not committing to an +-- Maybe/Event isomorphism by providing an interface which implies such +-- a relationship. Whether the present interface commits to this +-- isomorphism anyway is an open question. +-- - Events are supposed to occur instantaneously. Can one rely on the +-- outside world to maintain this invariant in general? As long as +-- we maintain control of event generation inside Yampa, this is a +-- non-issue. +-- - When observing events from the outside for the purpose of switching, +-- we often want to do that through an edge detector anyway. Otherwise, +-- if we simply observed an event signal generated from the outside and +-- did an immediate switch, then the event condition would remain AFTER +-- the switch, potentially causeing another immediate switch. This is of +-- course no reason to not allow events from the outside in themselves +-- (one could always do edge detection anyway), but just to say that +-- events from the outside does not seem that useful. +-- Regarding the last point, note that we'll have that problem anytime +-- we feed events into a signal transformer: if it does immediate switching +-- internally, the event condition will persist after the switch. + + +module FRP.YFrob.Common.RobotIO where + +import FRP.Yampa +import FRP.Yampa.MergeableRecord +import FRP.YFrob.Common.PhysicalDimensions + + +------------------------------------------------------------------------------ +-- Types related to the interface classes +------------------------------------------------------------------------------ + +type RobotType = String -- "SimbotA", "SimbotB", possibly "Pioneer" ... +type RobotId = Int + + +data BatteryStatus = BSHigh + | BSLow + | BSCritical + deriving (Eq, Show) + + +------------------------------------------------------------------------------ +-- Property classes and related utility functions/signal transformers +------------------------------------------------------------------------------ + +class HasRobotProperties p where + rpType :: p -> RobotType -- Type of the robot, e.g. "SimbotA". + rpId :: p -> RobotId -- Identity of the robot. + rpDiameter :: p -> Length -- Distance between the wheels. + rpAccMax :: p -> Acceleration -- Maximal translational acc. + rpWSMax :: p -> Speed -- Maximal peripheral wheel speed. + + +------------------------------------------------------------------------------ +-- Input classes and related utility functions/signal transformers +------------------------------------------------------------------------------ + +class HasSystemTime i where + stSystemTime :: i -> Time -- Time since system start. + + +-- Similar to "localTime". +systemTime :: HasSystemTime i => SF i Time +systemTime = arr stSystemTime + + +-- Note: Don't make any assumptions about how long the stuck condition +-- will persist. It could be quite transient, e.g. only while trying to +-- move and getting nowhere, or it could even be a "true event", such as +-- a flag indicating that the condition has occurred which is reset +-- by the act of reading. But if observed through edge detectors such as +-- those defined below, this should not matter. + +class HasRobotStatus i where + rsBattStat :: i -> BatteryStatus -- Curent battery status. + rsIsStuck :: i -> Bool -- Currently stuck or not. + + +rsBattStatChanged :: HasRobotStatus i => SF i (Event BatteryStatus) +rsBattStatChanged = + arr rsBattStat + >>> edgeBy (\bs bs' -> if bs == bs' then Nothing else Just bs') BSHigh + + +rsBattStatLow :: HasRobotStatus i => SF i (Event ()) +rsBattStatLow = arr (\i -> rsBattStat i == BSLow) >>> edge + + +rsBattStatCritical :: HasRobotStatus i => SF i (Event ()) +rsBattStatCritical = arr (\i -> rsBattStat i == BSCritical) >>> edge + + +rsStuck :: HasRobotStatus i => SF i (Event ()) +rsStuck = arr rsIsStuck >>> edge + + +class HasOdometry i where + odometryPosition :: i -> Position2 -- Current position. + odometryHeading :: i -> Heading -- Current heading. + + +-- Simple abstraction over a variety of devices capable of providing range +-- information (laser rangefinders, sonar, omnicam, ...). (The word "range" +-- is used in the meaning "distance to target"). rfRange provides an +-- omnidirectional range map giving the (estimated) distance to the closest +-- obstacle for any angle, thus hiding underlying details concerning sensor +-- types, their number and directions, the field of view for each, etc. +-- At least the range information for 0, pi/2, -pi/2, pi are accurate +-- (no instance should be provided for this class otherwise). +-- rfMaxRange gives (the upper limit of) the maximal target distance the +-- underlying device(s) is capable of detecting. Any larger reported distance +-- means "no target in range". + +class HasRangeFinder i where + rfRange :: i -> Angle -> Distance + rfMaxRange :: i -> Distance + + +-- Used to indicate out of range signals. But to test for this, just compare +-- against rfMaxRange. +rfOutOfRange :: Distance +rfOutOfRange = 1.0e100 + + +rfFront :: HasRangeFinder i => i -> Distance +rfFront i = rfRange i 0.0 + +rfBack :: HasRangeFinder i => i -> Distance +rfBack i = rfRange i pi + +rfLeft :: HasRangeFinder i => i -> Distance +rfLeft i = rfRange i (pi/2) + +rfRight :: HasRangeFinder i => i -> Distance +rfRight i = rfRange i (-pi/2) + + +-- Interface to sensors that keep track of animate objects, i.e. other +-- robots and balls. Could be an overhead camera, or perhaps all animate +-- objects are equipped with some kind of beacon that makes it possible to +-- track the other robots, or maybe all robots broadcast their positions over +-- radio. Anyway, this is a rather platform-specific interface, and should +-- probably be moved to the platform-specific part of the library at some +-- point. +-- +-- For each other robot, the robot type, the angle (relative to own heading), +-- and distance is provided. The distance is center to center. For each ball, +-- the relative angle and distance is provided. + +class HasAnimateObjectTracker i where + aotOtherRobots :: i -> [(RobotType, RobotId, Angle, Distance)] + aotBalls :: i -> [(Angle, Distance)] + + +-- Very simple textual console input. Not good enough for handling e.g. +-- shift, ctrl, function keys, ... Currently somewhere between event-land +-- and continous land. Should not assume that a key only occurs momentarily +-- (see comments about events above), even if that's likely what's going on. +-- On the other hand, if the interface truly represented a continuous view +-- of a keyboard, the type would be a LIST (possibly empty) of KEYS currently +-- being down. But that might be a bit fragile (leading to "stuck keys"). +-- So one would probably have to also subscribe to focus events to clear +-- the list? Maybe such sophistication is more appropriate for a GUI input +-- class? + +class HasTextualConsoleInput i where + tciKey :: i -> Maybe Char + + +-- Detects new key presses without insisting on all keys being released +-- in between (2-key rollover). Thus needs to be initialized with the +-- previous key status. +tciNewKeyDown :: HasTextualConsoleInput i => Maybe Char -> SF i (Event Char) +tciNewKeyDown mk = arr tciKey >>> edgeBy newKeyDown mk + where + newKeyDown Nothing Nothing = Nothing + newKeyDown Nothing mk'@(Just _) = mk' + newKeyDown (Just k) mk'@(Just k') | k' /= k = mk' + | otherwise = Nothing + newKeyDown (Just _) Nothing = Nothing + + +-- Detects key presses, insisting on all keys being released in between +tciKeyDown :: HasTextualConsoleInput i => SF i (Event Char) +tciKeyDown = arr tciKey >>> edgeJust + + +------------------------------------------------------------------------------ +-- Output classes and related types and utility functions/signal transformers +------------------------------------------------------------------------------ + +class MergeableRecord o => HasDiffDrive o where + -- Brake both wheels. + ddBrake :: MR o + + -- Set wheel velocities individually. + ddVelDiff :: Velocity -> Velocity -> MR o + + -- Set wheel velocities in terms of overall transl. and rot. velocity. + ddVelTR :: Velocity -> RotVel -> MR o + + +class MergeableRecord o => HasTextConsoleOutput o where + tcoPrintMessage :: Event String -> MR o diff --git a/src/FRP/YFrob/RobotSim.hs b/src/FRP/YFrob/RobotSim.hs new file mode 100644 --- /dev/null +++ b/src/FRP/YFrob/RobotSim.hs @@ -0,0 +1,185 @@ +{-# LANGUAGE Arrows #-} + +{- +****************************************************************************** +* Y F R O B / R O B O T S I M * +* * +* Module: RobotSim * +* Purpose: Top-level robot simulator module. * +* To be imported into a top-level module supplying * +* robot control code. * +* Author: Henrik Nilsson * +* * +****************************************************************************** +-} + +module FRP.YFrob.RobotSim ( + module FRP.YFrob.Common, + SimbotProperties, + SimbotInput, + SimbotOutput, + SimbotController, + WorldTemplate, + ObjectTemplate(..), + BoundingBox, + worldXMin, + worldYMin, + worldXMax, + worldYMax, + boundingBox, + runSim, + playSoccer -- Probably temporary. +) where + +import qualified Graphics.HGL as HGL + +import FRP.YFrob.Common + +import FRP.YFrob.RobotSim.WorldGeometry +import FRP.YFrob.RobotSim.Colors +import FRP.YFrob.RobotSim.ColorBindings +import FRP.YFrob.RobotSim.ObjectTemplate -- !!! Only to supprt playSoccer! +import FRP.YFrob.RobotSim.Object (Object(..)) +import FRP.YFrob.RobotSim.IO +import FRP.YFrob.RobotSim.RenderFixedWalls (fixedWalls) +import FRP.YFrob.RobotSim.RenderObject (renderObjects) +import FRP.YFrob.RobotSim.Animate +import FRP.YFrob.RobotSim.Parser +import FRP.YFrob.RobotSim.Simulator (SimbotController, simWorld, simWorld') + + +title :: String +title = "Y F R O B / R O B O T S I M" +width, height :: Int +width = 600 +height = 600 + +fr :: Frequency +fr = 20 -- Hz + + +-- !!! Providing no initial world should be used to invoke the editor. +-- !!! Eventually, we might want a different runSim to do that. Or pass a flag. +-- !!! We might also want to make runSim take a single controller, +-- !!! whereas runSim2 should take 2. + +runSim :: Maybe WorldTemplate -> SimbotController -> SimbotController -> IO () +runSim mwt sca scb = + animate fr title width height + -- !!! HGL seems to leak memory. Invoke only fixedWalls below + -- !!! to see a MUCH slower growth rate ... Sigh. + (\(cfbs, (objs, _)) -> renderObjects objs + `HGL.overGraphic` HGL.text (0,0) cfbs + `HGL.overGraphic` fixedWalls) + (\(_, (_, ems)) -> event [] id ems) + (parseWinInput >>> cmdString &&& simulator wt) + where +{- + -- For testing purposes. Draws initial world repeatedly. + simulator wt = + (proc inp -> do + (w, _) <- simWorld wt sca scb -< inp + e <- now () -< () + returnA -< (w, e `tag` w) + ) `switch` (\w -> constant w) +-} + simulator wt = switch (simWorld wt sca scb) $ editor + + editor wt = switch (constant undefined &&& now wt) $ simulator + + wt = maybe dfltWT id mwt + + dfltWT = [ + OTSimbotA { + otRId = 1, + otPos = Point2 (-2.5) 0.0, + otHdng = 0.0 + }, + OTSimbotB { + otRId = 1, + otPos = (Point2 2.5 0.0), + otHdng = (-pi) + } + ] + + +------------------------------------------------------------------------------ +-- Soccer game setup +------------------------------------------------------------------------------ + +soccerWorld :: WorldTemplate +soccerWorld = [ + OTSimbotB {otRId = 1, otPos = Point2 (-4) 0, otHdng=0}, + OTSimbotB {otRId = 2, otPos = Point2 (-2) 3, otHdng=0}, + OTSimbotB {otRId = 3, otPos = Point2 (-2) (-3), otHdng=0}, + OTSimbotB {otRId = 11, otPos = Point2 4 0, otHdng=pi}, + OTSimbotB {otRId = 12, otPos = Point2 2 (-3), otHdng=pi}, + OTSimbotB {otRId = 13, otPos = Point2 2 3, otHdng=pi}, + OTBlock {otPos = Point2 (-4.75) 1.25}, + OTBlock {otPos = Point2 (-4.25) 1.25}, + OTBlock {otPos = Point2 (-4.75) (-1.25)}, + OTBlock {otPos = Point2 (-4.25) (-1.25)}, + OTBlock {otPos = Point2 4.75 1.25}, + OTBlock {otPos = Point2 4.25 1.25}, + OTBlock {otPos = Point2 4.75 (-1.25)}, + OTBlock {otPos = Point2 4.25 (-1.25)}, + OTBall {otPos = Point2 0 0} + ] + + +playSoccer :: [SimbotController] -> [SimbotController] -> IO () +playSoccer team1 team2 = + animate fr title width height + (\((score1, score2, mg), (objs, _)) -> + renderObjects objs + `HGL.overGraphic` (maybe HGL.emptyGraphic + (\g->HGL.text (230,250) g) mg) + `HGL.overGraphic` HGL.text (0,0) (show score1) + `HGL.overGraphic` HGL.text (580,0) (show score2) + `HGL.overGraphic` fixedWalls) + (\(_, (_, ems)) -> event [] id ems) + (simloop soccerWorld 0 0) + where + simloop :: WorldTemplate -> Int -> Int + -> SF a ((Int, Int, Maybe String), + ([Object], Event [String])) + simloop wt score1 score2 = + switch (simulator wt score1 score2) $ \((score1, score2), w) -> + switch (constant ((score1, score2, Just "G O O O O O A L ! ! !"), + (w, noEvent)) + &&& after 5 ((score1, score2), + map objectToOT w)) $ \((score1,score2),wt) -> + simloop (ballToCenter wt) score1 score2 + + simulator wt score1 score2 = proc _ -> do + (objs, ems) <- simWorld' wt rcStop rc -< () + scored1 <- edgeTag (score1, score2 + 1) -< ballInLeftGoal objs + scored2 <- edgeTag (score1 + 1, score2) -< ballInRightGoal objs + returnA -< (((score1, score2, Nothing), (objs, ems)), + (scored1 `merge` scored2) `attach` objs) + + rc rp = let rid = rpId rp + in + if 1 <= rid && rid <= length team1 then + (team1 !! (rid - 1)) rp + else if 11 <= rid && rid <= 10 + length team2 then + (team2 !! (rid - 11)) rp + else + rcStop rp + + rcStop :: SimbotController + rcStop _ = constant (mrFinalize ddBrake) + + ballToCenter [] = [] + ballToCenter (OTBall {} : ots) = OTBall { otPos = Point2 0 0 } : ots + ballToCenter (ot : ots) = ot : ballToCenter ots + + ballInLeftGoal [] = False + ballInLeftGoal (ObjBall { objPos = Point2 x y } : _) = + x <= (-4.2) && (-1) <= y && y <= 1 + ballInLeftGoal (_ : objs) = ballInLeftGoal objs + + ballInRightGoal [] = False + ballInRightGoal (ObjBall { objPos = Point2 x y } : _) = + x >= 4.2 && (-1) <= y && y <= 1 + ballInRightGoal (_ : objs) = ballInRightGoal objs diff --git a/src/FRP/YFrob/RobotSim/Animate.hs b/src/FRP/YFrob/RobotSim/Animate.hs new file mode 100644 --- /dev/null +++ b/src/FRP/YFrob/RobotSim/Animate.hs @@ -0,0 +1,208 @@ +{- +****************************************************************************** +* Y F R O B / R O B O T S I M * +* * +* Module: Animate * +* Purpose: Animation of graphical signal functions. * +* Author: Henrik Nilsson * +* * +****************************************************************************** +-} + +-- For now, simple animation, no support for I/O. Maybe the way to go is +-- an IOTask. See notes in Yampa. +-- +-- We could probably have got by without breaking the Event abstraction. +-- If nothing else, turning a maybe into event is easily done by a +-- signal function which could be added as a pre-processor. E.g. +-- eventBy (const id) >>> sf + +-- Approach: The signal function is sampled as frequently as possible. It's +-- the OS's task to allocate resources, so we can just as well use up all the +-- CPU cycles we get. But since drawing is very time consuming, we draw at a +-- fixed, user-defineable, presumably lower rate, thus allowing the user to +-- control the ratio between the cycles spent on drawing and the cycles spent +-- on editing/simulation. This approach may result in rather uneven sampling +-- intervals, but embedSynch can be used to provide a stable time base when +-- that is important, e.g. for simulation, as long as there are enough cycles +-- on average to keep up. +-- +-- For some reason, context switching does not work as it should unless +-- the window tick mechanism is enabled. For that reason, we use a high +-- frequency tick (1kHz). (Alternatively, passing the -C runtime flag (e.g. +-- +RTS -C1) forces regular context switches. Moreover, getting events +-- without delay seems to require yielding to ensure that the thread +-- receiving them gets a chance to run. This can be done using yield prior to +-- galling HGL.maybeGetWindowEvent. Alternatively, we can get the window tick, +-- and since the tick frequency is high, no major waiting should ensue. This +-- is the current method, although it seems as if this method means that +-- window close events often will be missed. + +module FRP.YFrob.RobotSim.Animate (WinInput, animate) where + +import Control.Monad (when) +-- import Data.Maybe (isJust, fromJust) +-- import Posix (SysVar(..), ProcessTimes, ClockTick, +-- getSysVar, getProcessTimes, elapsedTime) +-- import Concurrent (yield) +import Data.IORef (IORef, newIORef, readIORef, writeIORef) +import qualified Graphics.HGL as HGL + +import FRP.YFrob.Common +-- Actually, Events are not abstract in the publicly available Yampa 0.9.2.3 +import FRP.Yampa.Internals -- Breaking the Event abstraction barrier here! +import FRP.Yampa.Forceable + + +type WinInput = Event HGL.Event + + +------------------------------------------------------------------------------ +-- Animation +------------------------------------------------------------------------------ + +-- Animate a signal function +-- fr ......... Frame rate. +-- title ...... Window title. +-- width ...... Window width in pixels. +-- height ..... Window height in pixels. +-- render ..... Renderer; invoked at the frame rate. +-- tco ........ Text Console Output; invoked at every step. +-- sf ......... Signal function to animate. + +-- !!! Note: it would be easy to add an argument (a -> IO b) as well, allowing +-- !!! arbitrary I/O. One could even replace the text output by such a +-- !!! function. Could one possibly somehow get data back into the signal +-- !!! function by means of continuations? Or maybe the IOTask monad is the +-- !!! way to go, with a special "reactimateIOTask". + +animate :: Forceable a => + Frequency -> String -> Int -> Int + -> (a -> HGL.Graphic) + -> (a -> [String]) + -> (SF WinInput a) + -> IO () +animate fr title width height render tco sf = HGL.runGraphics $ + do + win <- HGL.openWindowEx title + Nothing -- Initial position. + (width, height) -- Window size. + HGL.DoubleBuffered -- Painfully SLOW!!! + -- HGL.Unbuffered -- Flickers! + (Just 1) -- For scheduling!?! + (init, getTimeInput, isClosed) <- mkInitAndGetTimeInput win + reactimate init + getTimeInput + (\_ ea@(e,a) -> do + updateWin render win ea + forAll (tco a) putStrLn + isClosed) + (repeatedly (1/fr) () &&& sf) + HGL.closeWindow win + + +------------------------------------------------------------------------------ +-- Support for reading time and input +------------------------------------------------------------------------------ + +mkInitAndGetTimeInput + :: HGL.Window + -> IO (IO WinInput, Bool -> IO (DTime,Maybe WinInput), IO Bool) +mkInitAndGetTimeInput win = do + -- clkRes <- fmap fromIntegral (getSysVar ClockTick) + let clkRes = 1000 + tpRef <- newIORef errInitNotCalled + wepRef <- newIORef errInitNotCalled + weBufRef <- newIORef Nothing + closedRef <- newIORef False + let init = do + t0 <- getElapsedTime + writeIORef tpRef t0 + mwe <- getWinInput win weBufRef + writeIORef wepRef mwe + return (maybeToEvent mwe) + let getTimeInput _ = do + tp <- readIORef tpRef + t <- getElapsedTime `repeatUntil` (/= tp) -- Wrap around possible! + let dt = if t > tp then fromIntegral (t-tp)/clkRes else 1/clkRes + writeIORef tpRef t + mwe <- getWinInput win weBufRef + mwep <- readIORef wepRef + writeIORef wepRef mwe + -- putStrLn ("dt = " ++ show dt) + -- when (isJust mwe) (putStrLn ("Event = " ++ show (fromJust mwe))) + -- Simplistic "delta encoding": detects only repeated NoEvent. + case (mwep, mwe) of + (Nothing, Nothing) -> return (dt, Nothing) + (_, Just HGL.Closed) -> do + writeIORef closedRef True + return (dt, Just(maybeToEvent mwe)) + _ -> return (dt, Just (maybeToEvent mwe)) + return (init, getTimeInput, readIORef closedRef) + where + errInitNotCalled = intErr "RSAnimate" + "mkInitAndGetTimeInput" + "Init procedure not called." + + -- Accurate enough? Resolution seems to be 0.01 s, which could lead + -- to substantial busy waiting above. + -- getElapsedTime :: IO ClockTick + -- getElapsedTime = fmap elapsedTime getProcessTimes + + -- Use this for now. Have seen delta times down to 0.001 s. But as + -- the complexity of the simulator signal function gets larger, the + -- processing time for one iteration will presumably be > 0.01 s, + -- and a clock resoltion of 0.01 s vs. 0.001 s becomes a non issue. + getElapsedTime :: IO HGL.Time + getElapsedTime = HGL.getTime + + maybeToEvent :: Maybe a -> Event a + maybeToEvent = maybe NoEvent Event + + +-- Get window input, with "redundant" mouse moves removed. +getWinInput :: HGL.Window -> IORef (Maybe HGL.Event) -> IO (Maybe HGL.Event) +getWinInput win weBufRef = do + mwe <- readIORef weBufRef + case mwe of + Just _ -> do + writeIORef weBufRef Nothing + return mwe + Nothing -> do + mwe' <- gwi win + case mwe' of + Just (HGL.MouseMove {}) -> mmFilter mwe' + _ -> return mwe' + where + mmFilter jmme = do + mwe' <- gwi win + case mwe' of + Nothing -> return jmme + Just (HGL.MouseMove {}) -> mmFilter mwe' + Just _ -> writeIORef weBufRef mwe' + >> return jmme + + -- Seems as if we either have to yield or wait for a tick in order + -- to ensure that the thread receiving events gets a chance to + -- work. For some reason, yielding seems to result in window close + -- events getting through, wheras waiting often means they don't. + -- Maybe the process typically dies before the waiting time is up in + -- the latter case? + gwi win = do + -- yield + HGL.getWindowTick win + mwe <- HGL.maybeGetWindowEvent win + return mwe + + +------------------------------------------------------------------------------ +-- Support for output +------------------------------------------------------------------------------ + +-- Need to force non-displayed elements to avoid space leaks. +-- We also explicitly force displayed elements in case the renderer does not +-- force everything. +updateWin :: + Forceable a => (a -> HGL.Graphic) -> HGL.Window -> (Event (), a) -> IO () +updateWin render win (e, a) = when (force a `seq` isEvent e) + (HGL.setGraphic win (render a)) diff --git a/src/FRP/YFrob/RobotSim/ColorBindings.hs b/src/FRP/YFrob/RobotSim/ColorBindings.hs new file mode 100644 --- /dev/null +++ b/src/FRP/YFrob/RobotSim/ColorBindings.hs @@ -0,0 +1,38 @@ +{- +****************************************************************************** +* Y F R O B / R O B O T S I M * +* * +* Module: ColorBindings * +* Purpose: Definition of colours for various objects. * +* Author: Henrik Nilsson * +* * +****************************************************************************** +-} + +module FRP.YFrob.RobotSim.ColorBindings where + +import FRP.YFrob.RobotSim.Colors + + +------------------------------------------------------------------------------ +-- Fixed walls colour bindings +------------------------------------------------------------------------------ + +outerWallColor = DarkOliveGreen -- DarkGrey + + +------------------------------------------------------------------------------ +-- Object colour bindings +------------------------------------------------------------------------------ + +blockColor = DarkKhaki +nsWallColor = SlateGrey +ewWallColor = SlateGrey +simbotAColor = RoyalBlue +simbotAAltColor = MediumVioletRed -- Quick hack +simbotANoseColor = MediumOrchid +simbotBColor = SeaGreen +simbotBAltColor = Goldenrod -- Quick hack +simbotBNoseColor = NavyBlue +ballColor = Orange +bboxColor = Red diff --git a/src/FRP/YFrob/RobotSim/Colors.hs b/src/FRP/YFrob/RobotSim/Colors.hs new file mode 100644 --- /dev/null +++ b/src/FRP/YFrob/RobotSim/Colors.hs @@ -0,0 +1,150 @@ +{- +****************************************************************************** +* Y F R O B / R O B O T S I M * +* * +* Module: Colors * +* Purpose: Colour definitions. * +* Author: Henrik Nilsson * +* * +****************************************************************************** +-} + +module FRP.YFrob.RobotSim.Colors (Color(..), RGB, colorTable) where + +import Data.Array +import Graphics.HGL (RGB(..)) + + +------------------------------------------------------------------------------ +-- Color definitions +------------------------------------------------------------------------------ + +-- Pretty arbitrary selection of colours. +data Color = +-- Basic colours. + Black + | Blue + | Green + | Cyan + | Red + | Magenta + | Yellow + | White +-- Various greys. + | DarkGrey + | DimGrey + | Grey + | LightGrey + | DarkSlateGrey + | SlateGrey + | LightSlateGrey +-- Various blues/cyan. + | MidnightBlue + | NavyBlue + | CornflowerBlue + | DarkSlateBlue + | SlateBlue + | LightSlateBlue + | MediumBlue + | RoyalBlue + | DeepSkyBlue + | SteelBlue + | CadetBlue +-- Various greens/olive greens/khaki. + | DarkGreen + | DarkOliveGreen + | SeaGreen + | MediumSeaGreen + | LawnGreen + | LimeGreen + | ForestGreen + | OliveDrab + | DarkKhaki + | Khaki +-- Various oranges/browns. + | Goldenrod + | DarkGoldenrod + | SaddleBrown + | Orange +-- Various violets/purples. + | Maroon + | MediumVioletRed + | VioletRed + | Violet + | Plum + | Orchid + | MediumOrchid + | DarkOrchid + | BlueViolet + | Purple + deriving (Eq, Ord, Bounded, Enum, Ix) + +colorList :: [(Color, RGB)] +colorList = + [ + -- Basic colours. + (Black, RGB 0 0 0), + (Blue, RGB 0 0 255), + (Green, RGB 0 255 0), + (Cyan, RGB 0 255 255), + (Red, RGB 255 0 0), + (Magenta, RGB 255 0 255), + (Yellow, RGB 255 255 0), + (White, RGB 255 255 255), + + -- Various greys. + (DarkGrey, RGB 64 64 64), + (DimGrey, RGB 105 105 105), + (Grey, RGB 190 190 190), + (LightGrey, RGB 211 211 211), + (DarkSlateGrey, RGB 47 79 79), + (SlateGrey, RGB 112 128 144), + (LightSlateGrey, RGB 119 136 153), + + -- Various blues/cyan. + (MidnightBlue, RGB 25 25 112), + (NavyBlue, RGB 0 0 128), + (CornflowerBlue, RGB 100 149 237), + (DarkSlateBlue, RGB 72 61 139), + (SlateBlue, RGB 106 90 205), + (LightSlateBlue, RGB 132 112 255), + (MediumBlue, RGB 0 0 205), + (RoyalBlue, RGB 65 105 225), + (DeepSkyBlue, RGB 0 191 255), + (SteelBlue, RGB 70 130 180), + (CadetBlue, RGB 95 158 160), + + -- Various greens/olive greens/khaki. + (DarkGreen, RGB 0 100 0), + (DarkOliveGreen, RGB 85 107 47), + (SeaGreen, RGB 46 139 87), + (MediumSeaGreen, RGB 60 179 113), + (LawnGreen, RGB 124 252 0), + (LimeGreen, RGB 50 205 50), + (ForestGreen, RGB 34 139 34), + (OliveDrab, RGB 107 142 35), + (DarkKhaki, RGB 189 183 107), + (Khaki, RGB 240 230 140), + + -- Various oranges/browns. + (Goldenrod, RGB 218 165 32), + (DarkGoldenrod, RGB 184 134 11), + (SaddleBrown, RGB 139 69 19), + (Orange, RGB 255 165 0), + + -- Various violets/purples. + (Maroon, RGB 176 48 96), + (MediumVioletRed, RGB 199 21 133), + (VioletRed, RGB 208 32 144), + (Violet, RGB 238 130 238), + (Plum, RGB 221 160 221), + (Orchid, RGB 218 112 214), + (MediumOrchid, RGB 186 85 211), + (DarkOrchid, RGB 153 50 204), + (BlueViolet, RGB 138 43 226), + (Purple, RGB 160 32 240) + ] + + +colorTable :: Array Color RGB +colorTable = array (minBound, maxBound) colorList diff --git a/src/FRP/YFrob/RobotSim/Command.hs b/src/FRP/YFrob/RobotSim/Command.hs new file mode 100644 --- /dev/null +++ b/src/FRP/YFrob/RobotSim/Command.hs @@ -0,0 +1,41 @@ +{- +****************************************************************************** +* Y F R O B / R O B O T S I M * +* * +* Module: Command * +* Purpose: The simulator command type. * +* Author: Henrik Nilsson * +* * +****************************************************************************** +-} + +module FRP.YFrob.RobotSim.Command ( + Command(..) +) where + +import FRP.YFrob.Common.PhysicalDimensions (Angle) +import FRP.YFrob.RobotSim.Object (ObjClass) + + +data Command = +-- Commands in Edit mode. + CmdQuit -- Quit the simulator. + | CmdRun -- Run simulation. + | CmdCreateObst ObjClass -- Create an object <: ClsObst + | CmdCreateRobot ObjClass -- Create a robot <: ClsRobot. + | CmdCreateBall -- Create a ball. + | CmdDelete -- Delete an object. + | CmdSelectNext -- Select (next) object. + | CmdSelectPrev -- Select (previous) object. + | CmdSelect ObjClass -- Select all matching objects. + | CmdUnselectAll -- Unselect all objects. + | CmdTurnLeft -- Turn robot left. + | CmdTurnRight -- Turn robot right. + | CmdTurnTo Angle -- Turn to specified dir. (radians). + | CmdSave String -- Save the world to file path. + | CmdLoad String -- Load a world from file path. +-- Commands in Frozen mode. + | CmdResume -- Resume simulation. + | CmdEdit -- Terminate sim., go into edit mode. +-- Commands in Running mode. + | CmdFreeze -- Freeze simulation. diff --git a/src/FRP/YFrob/RobotSim/IO.hs b/src/FRP/YFrob/RobotSim/IO.hs new file mode 100644 --- /dev/null +++ b/src/FRP/YFrob/RobotSim/IO.hs @@ -0,0 +1,190 @@ +{- +****************************************************************************** +* Y F R O B / R O B O T S I M * +* * +* Module: IO * +* Purpose: RobotSim I/O types and instances. * +* Author: Henrik Nilsson * +* * +****************************************************************************** +-} + +-- RobotSim-specific I/O types and I/O class instances. +-- +-- Comment from (old) PioneerIO: applies to RobotSim as well: +-- Not quite sure any more it is such a great idea to make individual robot +-- types, even specific configurations of such robots, part of the YFRob +-- framework. +-- The alternative would be to make YFrob quite self contained (including +-- what's now in Common, and possibly a simulator), and moving all this stuff +-- to the application level since it is quite unlikely that this stuff is +-- suficiently general to merit being put into a library. +-- +-- "Simbot" refers to a specific kind of simulated robot, intended to be +-- a fairly generic platform providing aspects common to many typical, real +-- platforms. One could imagine having other types of simulated robots as +-- well, e.g. simulated Pioneers, in which case this module would define +-- PioneerInput/PioneerOutput etc. +-- +-- SimbotInput does not currently instantiate HasTextualConsoleInput. +-- One could imagine providing a GUI mechanism to select the simulated robot +-- which is to receive the current console input. + +module FRP.YFrob.RobotSim.IO ( + SimbotProperties(..), + SimbotInput(..), + SimbotOutput(..), + DriveMode(..), + rfIndex +) where + +import Data.Ix (rangeSize) +import Data.Array (Array, bounds, (!)) + +import FRP.Yampa (Time, Event, noEvent, mergeBy) +import FRP.Yampa.MergeableRecord (MergeableRecord(..), MR, mrMake) +import FRP.YFrob.Common.PhysicalDimensions +import FRP.YFrob.Common.RobotIO + + +------------------------------------------------------------------------------ +-- Property type and instances +------------------------------------------------------------------------------ + +data SimbotProperties = SP { + -- Fields for HasRobotProperties + spRType :: RobotType, -- "SimbotA" and "SimbotB" + spRId :: RobotId, + spDiameter :: Length, + spAccMax :: Acceleration, + spWSMax :: Speed +} + + +instance HasRobotProperties SimbotProperties where + rpType = spRType + rpId = spRId + rpDiameter = spDiameter + rpAccMax = spAccMax + rpWSMax = spWSMax + + +------------------------------------------------------------------------------ +-- Input type and instances +------------------------------------------------------------------------------ + +-- !!! Should the fields be strict? +-- !!! But it is nice that potentially expensive computations are not +-- !!! carried out unless needed. + + +-- !!! Should some of the fields, like the field for stuck, really be +-- an event? + +data SimbotInput = SI { + -- Fields for HasSystemTime + siSystemTime :: Time, + + -- Fields for HasRobotStatus + siBattStat :: BatteryStatus, + siIsStuck :: Bool, -- !!! Should be an event? + + -- Fields for HasOdometry + siPosition :: Position2, + siHeading :: Heading, + + -- Fields for HasRangeFinder + siRanges :: Array Int Distance, -- n equispaced range finders, CCW. + siMaxRange :: Distance, + + -- Fields for HasAnimateObjectTracker + siOtherRobots :: [(RobotType, RobotId, Angle, Distance)], + siBalls :: [(Angle, Distance)] +} + + +instance HasSystemTime SimbotInput where + stSystemTime = siSystemTime + + +instance HasRobotStatus SimbotInput where + rsBattStat = siBattStat + rsIsStuck = siIsStuck + + +instance HasOdometry SimbotInput where + odometryPosition = siPosition + odometryHeading = siHeading + + +instance HasRangeFinder SimbotInput where + rfRange si phi = + case n of + 0 -> rfOutOfRange + _ -> rs ! rfIndex n phi + where + rs = siRanges si + n = rangeSize (bounds rs) + + rfMaxRange = siMaxRange + + +-- Utility function for computing index into range-finder array. +rfIndex :: Int -> Angle -> Int +rfIndex n phi = round (fromIntegral n * phi / (2 * pi)) `mod` n + + +instance HasAnimateObjectTracker SimbotInput where + aotOtherRobots = siOtherRobots + aotBalls = siBalls + + +------------------------------------------------------------------------------ +-- Output type with instances and subordinate types +------------------------------------------------------------------------------ + +-- !!! Could be extended with other kinds of control modes. + +data DriveMode = + DMBrake -- Brake both wheels. + | DMDiff { + dmdLWV :: Velocity, -- Left Wheel Velocity. + dmdRWV :: Velocity -- Right Wheel Velocity. + } + | DMTR { + dmtrTV :: Velocity, -- Translational Velocity. + dmtrRV :: RotVel -- Rotational Velocity. + } + + +data SimbotOutput = SO { + soDM :: DriveMode, -- Drive mode output. + soTCO :: Event [String] -- Text console output. +-- soFVGO :: SimpleGraphic -- FVision GUI output. +} + + +instance MergeableRecord SimbotOutput where + mrDefault = SO {soDM = DMBrake, soTCO = noEvent} + + +instance HasDiffDrive SimbotOutput where + ddBrake = mrMake (\o -> o {soDM = DMBrake}) + + ddVelDiff lwv rwv = + mrMake (\o -> o {soDM = DMDiff {dmdLWV = lwv, dmdRWV = rwv}}) + + ddVelTR v rv = + mrMake (\o -> o {soDM = DMTR {dmtrTV = v, dmtrRV = rv}}) + + +instance HasTextConsoleOutput SimbotOutput where + tcoPrintMessage em = + mrMake (\o -> o {soTCO = mergeBy (++) (soTCO o) (fmap (:[]) em)}) + + +{- +instance HasFVisionGUIOutput SimbotOutput where + fvgoOverlaySimpleGraphic sg = + mrMake (\o -> o {soFVGO = sg `SGOver` soFVGO o}) +-} diff --git a/src/FRP/YFrob/RobotSim/IdentityList.hs b/src/FRP/YFrob/RobotSim/IdentityList.hs new file mode 100644 --- /dev/null +++ b/src/FRP/YFrob/RobotSim/IdentityList.hs @@ -0,0 +1,162 @@ +{- +****************************************************************************** +* Y F R O B / R O B O T S I M * +* * +* Module: IdentityList * +* Purpose: Association list with automatic key assignment and * +* identity-preserving map and filter operations. * +* Author: Henrik Nilsson * +* * +****************************************************************************** +-} + +-- ToDo: +-- * Change names of ilKeys, ilElems, ilAssocs to keysIL, elemsIL, assocsIL. +-- (Keep name of fields in IL type, thus assocsIL = ilAssocs.) + +module FRP.YFrob.RobotSim.IdentityList ( + ILKey, -- Identity-list key type + IL, -- Identity-list, abstract. Instance of functor. + emptyIL, -- :: IL a + insertIL_, -- :: a -> IL a -> IL a + insertIL, -- :: a -> IL a -> (ILKey, IL a) + listToIL, -- :: [a] -> IL a + ilKeys, -- :: IL a -> [ILKey] + ilElems, -- :: IL a -> [a] + ilAssocs, -- :: IL a -> [(ILKey, a)] + deleteIL, -- :: ILKey -> IL a -> IL a + mapIL, -- :: ((ILKey, a) -> b) -> IL a -> IL b + filterIL, -- :: ((ILKey, a) -> Bool) -> IL a -> IL a + mapFilterIL, -- :: ((ILKey, a) -> Maybe b) -> IL a -> IL b + lookupIL, -- :: ILKey -> IL a -> Maybe a + findIL, -- :: ((ILKey, a) -> Bool) -> IL a -> Maybe a + mapFindIL, -- :: ((ILKey, a) -> Maybe b) -> IL a -> Maybe b + findAllIL, -- :: ((ILKey, a) -> Bool) -> IL a -> [a] + mapFindAllIL -- :: ((ILKey, a) -> Maybe b) -> IL a -> [b] +) where + +------------------------------------------------------------------------------ +-- Data type definitions +------------------------------------------------------------------------------ + +type ILKey = Int + +-- Invariants: +-- * List sorted in descending key order. +-- * Keys never reused. +data IL a = IL { ilNextKey :: ILKey, ilAssocs :: [(ILKey, a)] } + + +------------------------------------------------------------------------------ +-- Class instances +------------------------------------------------------------------------------ + +instance Functor IL where + fmap f (IL {ilNextKey = nk, ilAssocs = kas}) = + IL {ilNextKey = nk, ilAssocs = [ (i, f a) | (i, a) <- kas ]} + + +------------------------------------------------------------------------------ +-- Constructors +------------------------------------------------------------------------------ + +emptyIL :: IL a +emptyIL = IL {ilNextKey = 0, ilAssocs = []} + + +insertIL_ :: a -> IL a -> IL a +insertIL_ a il = snd (insertIL a il) + + +insertIL :: a -> IL a -> (ILKey, IL a) +insertIL a (IL {ilNextKey = k, ilAssocs = kas}) = (k, il') where + il' = IL {ilNextKey = k + 1, ilAssocs = (k, a) : kas} + + +listToIL :: [a] -> IL a +listToIL as = IL {ilNextKey = length as, + ilAssocs = reverse (zip [0..] as)} -- Maintain invariant! + + +------------------------------------------------------------------------------ +-- Additional selectors +------------------------------------------------------------------------------ + +ilKeys :: IL a -> [ILKey] +ilKeys = map fst . ilAssocs + + +ilElems :: IL a -> [a] +ilElems = map snd . ilAssocs + + +------------------------------------------------------------------------------ +-- Mutators +------------------------------------------------------------------------------ + +deleteIL :: ILKey -> IL a -> IL a +deleteIL k (IL {ilNextKey = nk, ilAssocs = kas}) = + IL {ilNextKey = nk, ilAssocs = deleteHlp kas} + where + deleteHlp [] = [] + deleteHlp kakas@(ka@(k', _) : kas) | k > k' = kakas + | k == k' = kas + | otherwise = ka : deleteHlp kas + + +------------------------------------------------------------------------------ +-- Filter and map operations +------------------------------------------------------------------------------ + +-- These are "identity-preserving", i.e. the key associated with an element +-- in the result is the same as the key of the element from which the +-- result element was derived. + +mapIL :: ((ILKey, a) -> b) -> IL a -> IL b +mapIL f (IL {ilNextKey = nk, ilAssocs = kas}) = + IL {ilNextKey = nk, ilAssocs = [(k, f ka) | ka@(k,_) <- kas]} + + +filterIL :: ((ILKey, a) -> Bool) -> IL a -> IL a +filterIL p (IL {ilNextKey = nk, ilAssocs = kas}) = + IL {ilNextKey = nk, ilAssocs = filter p kas} + + +mapFilterIL :: ((ILKey, a) -> Maybe b) -> IL a -> IL b +mapFilterIL p (IL {ilNextKey = nk, ilAssocs = kas}) = + IL { + ilNextKey = nk, + ilAssocs = [(k, b) | ka@(k, _) <- kas, Just b <- [p ka]] + } + + +------------------------------------------------------------------------------ +-- Lookup operations +------------------------------------------------------------------------------ + +lookupIL :: ILKey -> IL a -> Maybe a +lookupIL k il = lookup k (ilAssocs il) + + +findIL :: ((ILKey, a) -> Bool) -> IL a -> Maybe a +findIL p (IL {ilAssocs = kas}) = findHlp kas + where + findHlp [] = Nothing + findHlp (ka@(_, a) : kas) = if p ka then Just a else findHlp kas + + +mapFindIL :: ((ILKey, a) -> Maybe b) -> IL a -> Maybe b +mapFindIL p (IL {ilAssocs = kas}) = mapFindHlp kas + where + mapFindHlp [] = Nothing + mapFindHlp (ka : kas) = case p ka of + Nothing -> mapFindHlp kas + jb@(Just _) -> jb + + +findAllIL :: ((ILKey, a) -> Bool) -> IL a -> [a] +findAllIL p (IL {ilAssocs = kas}) = [ a | ka@(_, a) <- kas, p ka ] + + +mapFindAllIL:: ((ILKey, a) -> Maybe b) -> IL a -> [b] +mapFindAllIL p (IL {ilAssocs = kas}) = [ b | ka <- kas, Just b <- [p ka] ] diff --git a/src/FRP/YFrob/RobotSim/Object.hs b/src/FRP/YFrob/RobotSim/Object.hs new file mode 100644 --- /dev/null +++ b/src/FRP/YFrob/RobotSim/Object.hs @@ -0,0 +1,557 @@ +{- +****************************************************************************** +* Y F R O B / R O B O T S I M * +* * +* Module: Object * +* Purpose: Definition of objects in the world and their static * +* properties. * +* Author: Henrik Nilsson * +* * +****************************************************************************** +-} + +-- ToDo: +-- * Re-organize the object type. +-- - Single constructor for all robots probably a good start. +-- - Single constructor for all fixed objects too? +-- - Fields like robot id and robot type should be moved into +-- physical properties (stated purpose of RPP should be extended). + +-- To think about: +-- Maybe "Object" is a misnomer, since the true objects are the signal +-- functions. Change to "ObjectState"? Or, since this is how it influences +-- the rest or the world, ObjectInfluence? ObjectOutput? + +-- ToDo: +-- Rethink the object creation interface. Drag object templates into the +-- picture? + +-- Note on names. For fields and other selectors, the "selector convention" +-- is used, i.e. using a type-based prefix. This is less natural when +-- one wants to emphasize an (possibly n-ary) operation, or a test +-- (predicate or relation), having a suggestive name such as "hit" or +-- "filter" or something. A prefix could still work, especially if it's +-- clear that it is a prefix (i.e. it is indecipherable!) and thus easily +-- can be classified as "noise" when reading. But when disambiguation is +-- needed, a suffix (i.e. the type name) may well be the better option. +-- Especially if the operation conceptually could be understood as +-- "overloaded". E.g. showXXX, filterXXX, ... + +module FRP.YFrob.RobotSim.Object ( + Object(..), + RobotPhysicalProperties(..), + BallPhysicalProperties(..), + BBox (..), + objRadius, -- :: Object -> Length + block, -- :: Bool -> Position2 -> Object + nsWall, -- :: Bool -> Position2 -> Object + ewWall, -- :: Bool -> Position2 -> Object + simbotA, -- :: RobotType -> RobotId -> Bool -> Position2 + -- -> Heading -> Velocity2 -> Object + simbotB, -- :: RobotType -> RobotId -> Bool -> Position2 + -- -> Heading -> Velocity2 -> Object + ball, -- :: Bool -> Position2 -> Velocity2 -> Object + objSetSel, -- :: Object -> Bool -> Object + objSetPos, -- :: Object -> Position2 -> Object + objSetPosRel, -- :: Object -> Distance2 -> Object + objSetHdng, -- :: Object -> Heading -> Object + touchesFixedWall, -- :: Object -> Bool + touches, -- :: Object -> Object -> Bool + intersects, -- :: BoundingBox -> BoundingBox -> Bool + within, -- :: Position2 -> BoundingBox -> Bool + blockSide, -- :: Length + nsWallXSide, -- :: Length + nsWallYSide, -- :: Length + ewWallXSide, -- :: Length + ewWallYSide, -- :: Length + simbotARType, -- :: RobotType + simbotADiam, -- :: Length + simbotARadius, -- :: Length + simbotAAccMax, -- :: Acceleration + simbotAWSMax, -- :: Speed + simbotARFN, -- :: Int + simbotARFMaxRange, -- :: Distance + simbotBRType, -- :: RobotType + simbotBDiam, -- :: Length + simbotBRadius, -- :: Length + simbotBAccMax, -- :: Acceleration + simbotBWSMax, -- :: Speed + simbotBRFN, -- :: Int + simbotBRFMaxRange, -- :: Distance + ballDiam, -- :: Length + ballRadius, -- :: Length + ObjClass(..), + (<:), -- :: ObjClass -> ObjClass -> Bool + objClass, -- :: Object -> ObjClass + inClass -- :: Object -> ObjClass -> Bool +) where + +import FRP.Yampa.Geometry +import FRP.Yampa.Forceable + +import FRP.YFrob.Common.Diagnostics (intErr) +import FRP.YFrob.Common.PhysicalDimensions +import FRP.YFrob.Common.RobotIO (RobotId, RobotType) + +import FRP.YFrob.RobotSim.WorldGeometry + + +------------------------------------------------------------------------------ +-- Object type with constructors and selectors +------------------------------------------------------------------------------ + +-- Note: Objects should only be constructed/updated through the provided +-- constructors/update functions since some of the fields (e.g. oBBox) +-- are interdependent. The reason Object is not exported abstractly is that +-- it is convenient to inspect object by pattern matching. +-- +-- To avoid space leaks, all fields of Object, *except* objBBox!, are strict. +-- Also, the special type BBox was introduced to ensure the bounding box was +-- completely evaluated once the enclosing object was evaluated. +-- +-- !!! The Robot/Ball physical properties fields are currently rather +-- !!! pointless. The idea is that there in the furure will be a larger +-- !!! degree of parameterization, at least for robots and balls. E.g. +-- !!! balls with different masses, user-specifiable physical robot properties +-- !!! such as max velocity. Maybe even visual aspects such as colour. Or +-- !!! maybe even a rendering function could be part of the object +-- !!! representation. +-- +-- !!! Maybe objects ought to be parameterized to a larger extent. +-- !!! E.g. a single robot type parameterized on shape (enumeration type +-- !!! used both in ObjectTemplate and in Object), colour, size, inertia, +-- !!! and so on. Similarly, walls and blocks could be instances of a single +-- !!! object parameterized on shape and colour. Or maybe the latter is +-- !!! fairly pointless, and only adds overhead? People are only interested +-- !!! in defining robots anyway. +-- +-- !!! The fields objMsg and objPic has been removed. The current theory is +-- !!! that they don't belong in the object, although it would make the +-- !!! the output type for robot simulation signal function less verbose. +-- !!! But these fields are completely useless e.g. when editing, so the +-- !!! current choise is probably right. Maybe a message field would be +-- !!! a simple hack to enable robot to robot communication through +-- !!! their perception system, though? + +data Object = + ObjBlock { + objSel :: !Bool, + objPos :: !Position2, + objBBox :: BBox -- Not strict! + } + | ObjNSWall { + objSel :: !Bool, + objPos :: !Position2, + objBBox :: BBox + } + | ObjEWWall { + objSel :: !Bool, + objPos :: !Position2, + objBBox :: BBox + } + | ObjSimbotA { + objRType :: !RobotType, + objRId :: !RobotId, + objRPP :: !RobotPhysicalProperties, + objSel :: !Bool, + objPos :: !Position2, + objHdng :: !Heading, + objVel :: !Velocity2, + objBBox :: BBox + } + | ObjSimbotB { + objRType :: !RobotType, + objRId :: !RobotId, + objRPP :: !RobotPhysicalProperties, + objSel :: !Bool, + objPos :: !Position2, + objHdng :: !Heading, + objVel :: !Velocity2, + objBBox :: BBox + } + | ObjBall { + objBPP :: !BallPhysicalProperties, + objSel :: !Bool, + objPos :: !Position2, + objVel :: !Velocity2, + objBBox :: BBox + } + + +-- Properties pertaining to perception and physical interaction. +-- !!! Needs to include things like mass in order to do collisions etc. +-- !!! properly. +-- !!! Could also include function for computing (some) aspects of e.g. sonar +-- !!! echo, like perceived size and distance from certain direction. +data RobotPhysicalProperties = RPP { + rppRadius :: !Length, -- Robot radius. + rppRFN :: !Int, -- Number of range finders. + rppRFMaxRange :: !Distance -- Maximal range finder distance. +} + + +data BallPhysicalProperties = BPP { + bppRadius :: !Length -- Ball radius. +} + + +data BBox = BBox !Position2 !Position2 + deriving Eq + + +objRadius :: Object -> Length +objRadius (ObjSimbotA {objRPP = RPP {rppRadius = r}}) = r +objRadius (ObjSimbotB {objRPP = RPP {rppRadius = r}}) = r +objRadius (ObjBall {objBPP = BPP {bppRadius = r}}) = r +objRadius _ = intErrObj "objRadius" "Object does not have a radius." + + +------------------------------------------------------------------------------ +-- Constants defining various object properties +------------------------------------------------------------------------------ + +-- Obstacles are square shaped. + +blockSide :: Length +blockSide = 0.5 + + +-- Walls are rectangular. + +nsWallXSide, nsWallYSide :: Length +nsWallXSide = 0.1 +nsWallYSide = 1.0 + +ewWallXSide, ewWallYSide :: Length +ewWallXSide = 1.0 +ewWallYSide = 0.1 + + +-- Robots of type Simbot A are round. + +simbotARType :: RobotType +simbotARType = "SimbotA" + +simbotADiam, simbotARadius :: Length +simbotADiam = 0.5 +simbotARadius = simbotADiam / 2 + +simbotAAccMax :: Acceleration +simbotAAccMax = 0.2 -- Maximal translational acceleration. + +simbotAWSMax :: Speed +simbotAWSMax = 1.0 -- Maximal (peripheral) wheel speed. + +simbotARFN :: Int -- Number of range finders. +simbotARFN = 8 + +simbotARFMaxRange :: Distance -- Maximal range finder distance +simbotARFMaxRange = 2 + + +-- Robots of type Simbot B currently behaves as a round robot too (but is drawn +-- triangular). + +simbotBRType :: RobotType +simbotBRType = "SimbotB" + +simbotBDiam, simbotBRadius :: Length +simbotBDiam = 0.5 +simbotBRadius = simbotBDiam / 2 + +simbotBAccMax :: Acceleration +simbotBAccMax = 0.5 -- Maximal translational acceleration. + +simbotBWSMax :: Speed +simbotBWSMax = 2.0 -- Maximal (peripheral) wheel speed. + +simbotBRFN :: Int -- Number of range finders. +simbotBRFN = 8 + +simbotBRFMaxRange :: Distance -- Maximal range finder distance +simbotBRFMaxRange = 2 + + +-- The ball is, well, round + +ballDiam, ballRadius :: Length +ballDiam = 0.3 +ballRadius = ballDiam / 2 + + +------------------------------------------------------------------------------ +-- Instances +------------------------------------------------------------------------------ + +instance Forceable Object where + force obj = objBBox obj `seq` obj + + +------------------------------------------------------------------------------ +-- Smart constructors +------------------------------------------------------------------------------ + +block :: Bool -> Position2 -> Object +block sel p = obj + where + obj = ObjBlock { + objSel = sel, + objPos = p, + objBBox = computeObjBBox obj + } + + +nsWall :: Bool -> Position2 -> Object +nsWall sel p = obj + where + obj = ObjNSWall { + objSel = sel, + objPos = p, + objBBox = computeObjBBox obj + } + + +ewWall :: Bool -> Position2 -> Object +ewWall sel p = obj + where + obj = ObjEWWall { + objSel = sel, + objPos = p, + objBBox = computeObjBBox obj + } + + +simbotA :: RobotType -> RobotId -> Bool -> Position2 -> Heading -> Velocity2 + -> Object +simbotA rtp rid sel p h v = obj + where + obj = ObjSimbotA { + objRType = rtp, + objRId = rid, + objRPP = rpp, + objSel = sel, + objPos = p, + objHdng = normalizeHeading h, + objVel = v, + objBBox = computeObjBBox obj + } + + rpp = RPP { + rppRadius = simbotARadius, + rppRFN = simbotARFN, + rppRFMaxRange = simbotARFMaxRange + } + + +simbotB :: RobotType -> RobotId -> Bool -> Position2 -> Heading -> Velocity2 + ->Object +simbotB rtp rid sel p h v = obj + where + obj = ObjSimbotB { + objRType = rtp, + objRId = rid, + objRPP = rpp, + objSel = sel, + objPos = p, + objHdng = normalizeHeading h, + objVel = v, + objBBox = computeObjBBox obj + } + + rpp = RPP { + rppRadius = simbotBRadius, + rppRFN = simbotBRFN, + rppRFMaxRange = simbotBRFMaxRange + } + + +ball :: Bool -> Position2 -> Velocity2 -> Object +ball sel p v = obj + where + obj = ObjBall { + objBPP = bpp, + objSel = sel, + objPos = p, + objVel = v, + objBBox = computeObjBBox obj + } + + bpp = BPP { + bppRadius = ballRadius + } + + +------------------------------------------------------------------------------ +-- Object updating +------------------------------------------------------------------------------ + +objSetSel :: Object -> Bool -> Object +objSetSel obj sel = obj {objSel = sel} + + +objSetPos :: Object -> Position2 -> Object +objSetPos obj pos = obj' + where + obj' = obj {objPos = pos, objBBox = computeObjBBox obj'} + + +objSetPosRel :: Object -> Distance2 -> Object +objSetPosRel obj d = obj' + where + obj' = obj {objPos = objPos obj .+^ d, objBBox = computeObjBBox obj'} + + +objSetHdng :: Object -> Heading -> Object +objSetHdng obj h = obj' + where + -- The bounding box is affected if the shape of the robot is not round. + obj' = obj {objHdng = normalizeHeading h, objBBox=computeObjBBox obj'} + + +------------------------------------------------------------------------------ +-- Object and bounding box geometrical predicates +------------------------------------------------------------------------------ + +-- Check if object touches any of the fixed walls. +-- Note: The geometry of the current set of objects and of the current fixed +-- walls are such that a simple bounding box test is enough. + +touchesFixedWall :: Object -> Bool +touchesFixedWall obj = + x1 <= worldWestWall + || x2 >= worldEastWall + || y1 <= worldSouthWall + || y2 >= worldNorthWall + where + BBox (Point2 x1 y1) (Point2 x2 y2) = objBBox obj + + +-- Check if two objects touch each other. +-- Currently only a bounding box test. +-- ToDo: if bounding box tests succeeds, then carry out precise test. + +touches :: Object -> Object -> Bool +obj1 `touches` obj2 = (objBBox obj1) `intersects` (objBBox obj2) + + +intersects :: BBox -> BBox -> Bool +bb1 `intersects` bb2 = + x11 <= x22 && x12 >= x21 + && y11 <= y22 && y12 >= y21 + where + BBox (Point2 x11 y11) (Point2 x12 y12) = bb1 + BBox (Point2 x21 y21) (Point2 x22 y22) = bb2 + + +within :: Position2 -> BBox -> Bool +(Point2 x y) `within` bb = + x1 <= x && x <= x2 && y1 <= y && y <= y2 + where + BBox (Point2 x1 y1) (Point2 x2 y2) = bb + + +------------------------------------------------------------------------------ +-- Hierarchical object classification +------------------------------------------------------------------------------ + +-- Currently, all things which can move or be moved are considered to be +-- "animate". This seems convenient, even if it is not quite right. +-- We might want to have a more refined object hierarchy. +-- E.g. ClsAnimate for all "living" things (but currently only robots) +-- and ClsMovable for objects which can be moved (currently only balls). +-- Of course, the distinction between obstacles and movable objects might +-- be bad. A robot is also an obstacle, and so is a ball stuck between two +-- robots from the perspective of either robot! + +data ObjClass = + ClsObj -- Top. + | ClsInanimate -- Superclass for all inanimate objects. + | ClsBlock + | ClsWall -- Superclass for all movable walls. + | ClsNSWall + | ClsEWWall + | ClsAnimate -- Superclass for all animate objects. + | ClsRobot -- Superclass for all robot types. + | ClsSimbotA + | ClsSimbotB + | ClsBall -- Superclass for all balls. Currently only one type. + deriving (Eq) + + +(<:) :: ObjClass -> ObjClass -> Bool +_ <: ClsObj = True +ClsInanimate <: ClsInanimate = True +ClsBlock <: ClsInanimate = True +ClsWall <: ClsInanimate = True +ClsNSWall <: ClsInanimate = True +ClsEWWall <: ClsInanimate = True +ClsBlock <: ClsBlock = True +ClsWall <: ClsWall = True +ClsNSWall <: ClsWall = True +ClsEWWall <: ClsWall = True +ClsNSWall <: ClsNSWall = True +ClsEWWall <: ClsEWWall = True +ClsAnimate <: ClsAnimate = True +ClsRobot <: ClsAnimate = True +ClsSimbotA <: ClsAnimate = True +ClsSimbotB <: ClsAnimate = True +ClsBall <: ClsAnimate = True +ClsRobot <: ClsRobot = True +ClsSimbotA <: ClsRobot = True +ClsSimbotA <: ClsSimbotA = True +ClsSimbotB <: ClsRobot = True +ClsSimbotB <: ClsSimbotB = True +ClsBall <: ClsBall = True +_ <: _ = False + + +objClass :: Object -> ObjClass +objClass (ObjBlock {}) = ClsBlock +objClass (ObjNSWall {}) = ClsNSWall +objClass (ObjEWWall {}) = ClsEWWall +objClass (ObjSimbotA {}) = ClsSimbotA +objClass (ObjSimbotB {}) = ClsSimbotB +objClass (ObjBall {}) = ClsBall + + +inClass :: Object -> ObjClass -> Bool +obj `inClass` cls = objClass obj <: cls + + +------------------------------------------------------------------------------ +-- Support functions +------------------------------------------------------------------------------ + +-- Bounding box is calculated once and cached inside object. + +computeObjBBox :: Object -> BBox +computeObjBBox (ObjBlock {objPos = p}) = BBox (p .-^ d) (p .+^ d) + where + d = vector2 (blockSide / 2) (blockSide / 2) +computeObjBBox (ObjNSWall {objPos = p}) = BBox (p .-^ d) (p .+^ d) + where + d = vector2 (nsWallXSide / 2) (nsWallYSide / 2) +computeObjBBox (ObjEWWall {objPos = p}) = BBox (p .-^ d) (p .+^ d) + where + d = vector2 (ewWallXSide / 2) (ewWallYSide / 2) +computeObjBBox (ObjSimbotA {objPos = p}) = BBox (p .-^ d) (p .+^ d) + where + d = vector2 simbotARadius simbotARadius +computeObjBBox (ObjSimbotB {objPos = p, objHdng = d}) = BBox p1 p2 + where + Point2 x1 y1 = p .+^ (vector2Polar simbotBRadius d) -- Nose + Point2 x2 y2 = p .+^ (vector2Polar simbotBRadius (d + 2*pi/3)) + Point2 x3 y3 = p .+^ (vector2Polar simbotBRadius (d - 2*pi/3)) + + p1 = Point2 (minimum [x1,x2,x3]) (minimum [y1,y2,y3]) + p2 = Point2 (maximum [x1,x2,x3]) (maximum [y1,y2,y3]) +computeObjBBox (ObjBall {objPos = p}) = BBox (p .-^ d) (p .+^ d) + where + d = vector2 ballRadius ballRadius + +------------------------------------------------------------------------------ +-- Utilities +------------------------------------------------------------------------------ + +intErrObj :: String -> String -> a +intErrObj = intErr "RobotSim/Object" diff --git a/src/FRP/YFrob/RobotSim/ObjectPhysics.hs b/src/FRP/YFrob/RobotSim/ObjectPhysics.hs new file mode 100644 --- /dev/null +++ b/src/FRP/YFrob/RobotSim/ObjectPhysics.hs @@ -0,0 +1,616 @@ +{-# LANGUAGE Arrows #-} + +{- +****************************************************************************** +* Y F R O B / R O B O T S I M * +* * +* Module: ObjectPhysics * +* Purpose: Physics for objects and their interactions. * +* Author: Henrik Nilsson * +* * +****************************************************************************** +-} + +module FRP.YFrob.RobotSim.ObjectPhysics ( + SimbotDynamics, + BallDynamics, + simbotDynamics, -- :: Length -> Acceleration -> Speed -> Position2 + -- -> Heading -> Velocity -> SimbotDynamics + ballDynamics, -- :: YFrobReal -> YFrobReal -> Position2 -> Velocity2 + -- -> BallDynamics + hitFixedWall, -- :: Object -> Event Velocity + hit, -- :: Object -> Object -> Event Velocity + sonarEchoFixedWall, -- :: Position2 -> Length -> Heading -> Angle + -- -> Distance + sonarEcho, -- :: Position2 -> Heading -> Length -> Angle -> Object + -- -> (Heading, Distance) +) where + +import FRP.Yampa +import FRP.Yampa.Utilities (impulseIntegral) +import FRP.Yampa.Geometry +-- Actually, publicly Yampa 0.9.2.3 exports Event non-abstractly. +import FRP.Yampa.Internals (Event(..)) -- Breach of abstraction! + +import FRP.YFrob.Common.Diagnostics +import FRP.YFrob.Common.PhysicalDimensions + +import FRP.YFrob.RobotSim.WorldGeometry (worldEastWall, worldNorthWall, + worldWestWall, worldSouthWall) +import FRP.YFrob.RobotSim.Object + + +------------------------------------------------------------------------------ +-- Simbot dynamics +------------------------------------------------------------------------------ + +{- +General structure +----------------- + +This is a very simple physical robot model for the Simbots, especially +w.r.t. interaction which is internalized in the model. Ideally, the +physics model would have inputs for both control signals and external +influences, such as impulses (events) for collisions, or external forces +due to someone pushing the robot, etc. + +The Simbot physical robot model +------------------------------- + +We adopt a simplified robot model which has translational inertia but no +rotational inertia (i.e. all mass is located at the center of gravity). + +The robots have two individually controllable wheels which do not slip. +One wheel is located on the left side of the robot, the other wheel on the +right side. The distance between the wheels is known as the robot diameter, +and denoted by d in the following. The true peripheral speed of the wheels +are denoted by v_l and v_r respectively. + +The heading h of the robot is given by + + h = h_0 + integral (v_r - v_l) / d + +The speed v (in direction h) is given by + + v = (v_r + v_l) / 2 (1) + +Thus the position p is given by + + p = p_0 + integral (v@h) + +where v@h is the velocity _vector_. + +The robot model receives the desired peripheral wheel speeds v_ld and v_rd +from the controller. Of course, these can change instantaneously. + +v_rd - v_ld is proportinal to the desired instantaneous change in direction. +Since there is no rotational inertia, this should also be the true +instantaneous change in direction. Thus we have + + v_rd - v_ld = v_r - v_l (2) + +v_rd + v_ld is proportional to the desired translational speed. The +translational speed is subject to inertia and thus cannot change instantan- +eously. We assume a maximal translational acceleration a_max (which is +a function of the mass of the robot, the strengths of the motors etc.), +and that changes in translational speed always is done at that acceleration. + +Note that our assumptions unrealisticly imply that the _heading_ can be +changed almost discontinuously. E.g. suppose that v_l = v_r = x. Then by +setting v_ld = -N*x, v_rd = (N+2)*x briefly, where N is a very large number, +the robot could almost turn on the spot despite the inertia. However, by +limiting the peripheral wheel speeds to some maximal value, this should +not be too much of a problem. + +Also note that the the assumptions imply that the peripheral speed of +individual wheels does change discontinuously. Thus we cannot compute +any individual acceleration for the wheels. + +Instead we proceed as follows. Let a be the instantaneous translational +acceleration (in direction h): + + a = signum (v_rd + v_ld - 2 * v) * a_max + +Then the translational speed v is given by: + + v = v_0 + integral a + +Now solve (1) and (2) for the individaul true wheel speeds: + + v = (v_r + v_l) / 2 (1) + v_rd - v_ld = v_r - v_l (2) + +<=> + + v_rd - v_ld + 2 * v = 2 * v_r + -(v_rd - v_ld) + 2 * v = 2 * v_l + +<=> + + v_r = v + (v_rd - v_ld)/2 + v_l = v - (v_rd - v_ld)/2 + +Thus we get the following control equations: + + a = signum (v_rd + v_ld - 2 * v) * a_max + v = v_0 + integral a + v_r = v + (v_rd - v_ld)/2 + v_l = v - (v_rd - v_ld)/2 + h = h_0 + integral (v_r - v_l) / d + p = p_0 + integral (v@h) + +On rotational inertia +--------------------- + +One probably can regard rotation around a non-centered axis as translation +combined with rotation around the centered axis. In that case, adding +rotational inertia should be simple. One might want to compute both +translational and rotational inertia from a common mass, assuming some +simple mass distribution, but maybe separate macc acc. and max rotational +acc. will do just as well. + +On collision handling +--------------------- + +Currently thr robot models receives a "collision event". This causes it +to stop and bounce back to where it was before. This has potential problems, +especially when many moving objects interact closely. To counter this, the +event source should generate events not only on an the start of an +overlap condition, but as long as it remains and the relative velocities +would make it worse. "Unphysical"? + +In a more sophisticated implementation, the event could carry an impulse +(modelled as a velocity difference since an event as 0 duration). An +interaction resolution algorithm at a higher level could try to figure out +the overall impulse on each object by iteratively consider object-object +interactions until no more collision is found. It is important that the +effect of each found interaction is ADDED BACK to representation of the +state before the checking continues. Effectively this will result in all +interactions being serialised. + +The potential drawback of this approach is modularity. It would seem as if +the interaction resolver would have to know a lot about the physics of the +involved objects. Maybe one has to split the physical model into two, one +for continuous dynamics (signal functions), and one for describing how +an object reacts to impulses (ordinary functions, invoked repeatedly during +interaction resolution). + +We also have to ensure termination of the resolution algorithm ... + +On the inadequacy of the current physical model +----------------------------------------------- + +To model object interaction properly, we probably have to restructure the +simulator substantially. On epossibility might be to add extra "ports" +to models, allowing external forces to be taken into account. Alternatively, +one could expose a lot of the internal state (velocity, acceleration) to +allow a generalized "resolve" procedure to compute a suitable new state +after an interaction. + +A problem with the latter though, is that it assumes momentary interactions. +This is just not true. E.g. consider pushing (as opposed to "kicking") a ball. +A problem with the former is that it assumes that the external forces are +computed separately for interacting objects. This might be diffifult/ +impossible. In any event it is wasteful and unsafe since that would prevent +us to take advantage of the law of action/reaction. + +Maybe this boils down to solving systems of differential equations after all? +And having different models possibly with different causality for different +interaction cases? + +On the assumption that the overall velocity and heading coincide +---------------------------------------------------------------- + +Note: In a more advanced model, the initial translational velocity +and the output velocity should really be *vectors* since the heading +and where the robot is heading would not necessarily agree, e.g. if +the robot was being pushed. +-} + + +type SimbotDynamics = SF ((Velocity, Velocity), Event ()) + (Position2, Heading, Velocity) + +type SimbotDynamics' = SF (Velocity, Velocity) (Position2, Heading, Velocity) + +-- Simbot dynamics. +-- Arguments: +-- d .......... Robot diameter. +-- a_max ...... Maximal translational acceleration. +-- ws_max ..... Maximal (peripheral) wheel speed. +-- p_0 ........ Initial position. +-- h_0 ........ Initial heading. +-- v_0 ........ Initial translational velocity. +-- +-- Signal inputs: +-- wvs_d....... Desired wheel velocities. +-- ce ......... Collision event. +-- +-- Signal outputs: +-- p .......... Current position. +-- h .......... Current heading. +-- v .......... Current translational velocity. + +simbotDynamics :: + Length -> Acceleration -> Speed -> Position2 -> Heading -> Velocity + -> SimbotDynamics +simbotDynamics d a_max ws_max p_0 h_0 v_0 = proc (wvs_d, ce) -> do + -- !!! Should be phv@(p, h, v) <- ... but Arrowp refuse. + rec (p,h,v) <- drSwitch (sd p_0 h_0 v_0) -< (wvs_d, ce') + p_pre <- iPre p_0 -< p + let ce' = ce `tag` sd p_pre h 0.0 + returnA -< (p, h, v) + where + sd = simbotDynamics' d a_max ws_max + + +-- Interaction-free simbot dynamics. +-- Arguments: +-- d .......... Robot diameter. +-- a_max ...... Maximal translational acceleration. +-- ws_max ..... Maximal (peripheral) wheel speed. +-- p_0 ........ Initial position. +-- h_0 ........ Initial heading. +-- v_0 ........ Initial translational velocity. +-- +-- Signal inputs: +-- v_ld ....... Desired left wheel velocity. +-- v_rd ....... Desired right wheel velocity. +-- +-- Signal outputs: +-- p .......... Current position. +-- h .......... Current heading. +-- v .......... Current translational velocity. + +simbotDynamics' :: + Length -> Acceleration -> Speed -> Position2 -> Heading -> Velocity + -> SimbotDynamics' +simbotDynamics' d a_max ws_max p_0 h_0 v_0 = proc (v_ld, v_rd) -> do + rec let v_ld' = symLimit ws_max v_ld + v_rd' = symLimit ws_max v_rd + a = signum (v_rd' + v_ld' - 2 * v) * a_max + v_l = v - (v_rd' - v_ld') / 2 + v_r = v + (v_rd' - v_ld') / 2 + v <- (v_0 +) ^<< integral -< a + h <- hdngIgrl h_0 -< (v_r - v_l) / d + p <- (p_0 .+^) ^<< integral -< vector2Polar v h + returnA -< (p, h, v) -- Note: h is normalized! + where + limit ll ul x = if x < ll then ll else if x > ul then ul else x + symLimit l = let absl = abs l in limit (-absl) absl + + -- Ensures the heading remains normalized (and the integral bounded). + hdngIgrl :: Heading -> SF RotVel Heading + hdngIgrl h_0 = + switch + (proc rv -> do + h <- (h_0 +) ^<< integral -< rv + e <- edge -< h < (-pi) || h >= pi + returnA -< (h, e `tag` normalizeHeading h)) + hdngIgrl + + +------------------------------------------------------------------------------ +-- Ball dynamics +------------------------------------------------------------------------------ + +{- +General structure +----------------- + +Simple physical model with friction that slows a ball down and where +collisions are represented by (dirac) impulses. modelled as Events, which, +since all balls are assumed to be equally heavy and essentially weigthless +w.r.t. everything else carries instantaneous velocity changes. +-} + +type BallDynamics = SF (Event Velocity2) (Position2, Velocity2) + +-- Physical dynamics for a simple ball. +-- Arguments: +-- fc ......... Friction coefficient (constant). +-- dc ......... Aerodynamic drag coefficient (drag is prop. to v^2). +-- p_0 ........ Initial position. +-- v_0 ........ Initial velocity. +-- +-- Signal inputs: +-- iv ......... Velocity impulses due to collisions: causes instantaneous +-- change in velocity. +-- +-- Signal outputs: +-- p .......... Current position. +-- v .......... Current velocity. + +ballDynamics :: + YFrobReal -> YFrobReal -> Position2 -> Velocity2 -> BallDynamics +ballDynamics fc dc p_0 v_0 = proc iv -> do + rec let nv = norm v + a = if nv > 0 then + (fc/nv + dc*nv) *^ negateVector v + else + zeroVector + v <- (v_0 ^+^) ^<< impulseIntegral -< (a, iv) + p <- (p_0 .+^) ^<< integral -< v + returnA -< (p, v) + + +------------------------------------------------------------------------------ +-- Collisions +------------------------------------------------------------------------------ + +{- +1-dimensional Collisions +------------------------ + +Given an object with mass m1 moving at velocity v1 and an object with mass +m2 moving at velocity v2, assume that the collide fully elastiacally and +instantaneously. Then the following hold in general for the velocities +v1' and v2' after the collision: + + v1' = (m1 * v1 + m2 * v2 +/- m2 * abs (v1 - v2)) / (m1 + m2) +or + v1' - v1 = m2 * (v2 - v1 +/- abs (v1 - v2)) / (m1 + m2) + +and + v2' = (m1 * v2 + m2 * v2 -/+ m1 * abs (v1 - v2)) / (m1 + m2) +or + v2' - v2 = m1 * (v1 - v2 -/+ abs (v1 - v2)) / (m1 + m2) + +The signs are given by geometrical constraints, i.e. assuing that the +objects cannot move through each other. + +Given the assumptions that all balls weigh the same and that balls are much +lighter than everything else, we get two special cases. + +1. Two balls collide, i.e. m1 = m2 = m. + +2. A ball and a heavy object (or wall) collide. E.g. m1 >> m2. + +For case 1 we get: + + v1' = (v1 + v2 +/- abs (v1 - v2)) / 2 +or + v1' - v1 = (v2 - v1 +/- abs (v1 - v2)) / 2 + +and + v2' = (v2 + v2 -/+ abs (v1 - v2)) / 2 +or + v2' - v2 = (v1 - v2 -/+ abs (v1 - v2)) / 2 + +For case 2 we get: + + v1' ~= v1 +or + v1' - v1 = 0 +and + + v2' ~= v1 + abs (v1 - v2) +or + v1' - v1 = v1 - v2 +/- abs (v1 - v2) + +-} + +-- Impulses are modelled as events. +-- !!! This currently constitute a breach of abstraction! +impulse :: VectorSpace v k => v -> Event v +impulse v = Event v + + +noImpulse :: VectorSpace v k => Event v +noImpulse = noEvent + + +-- Check if an animate object has hit any of the fixed walls. +-- All animate objects are currently considered to be circles as far as +-- modelling of collisions go. +hitFixedWall :: Object -> Event Velocity2 +hitFixedWall obj = + if (x - r < worldWestWall && vx < 0) + || (x + r > worldEastWall && vx > 0) then + impulse (vector2 (-(2 * vx)) 0) + else if (y - r < worldSouthWall && vy < 0) + || (y + r > worldNorthWall && vy > 0) then + impulse (vector2 0 (-(2 * vy))) + else + noImpulse + where + Point2 x y = objPos obj + r = objRadius obj + (vx, vy) = vector2XY (objVel obj) + + +-- Check if animate object has hit another object. +-- Animate objects are assumed to be round, inanimate objects are assumed +-- to be rectangular and fixed (infinitely heavy). +hit :: Object -> Object -> Event Velocity2 +obj1 `hit` obj2 | objBBox obj1 `intersects` objBBox obj2 = hit' + | otherwise = noImpulse + where + hit' = if obj2 `inClass` ClsInanimate then + hitInanimate p r v obj2 + else + case obj1 of + ObjSimbotA {} -> robotHit p r v obj2 + ObjSimbotB {} -> robotHit p r v obj2 + ObjBall {} -> ballHit p r v obj2 + _ -> intErrObjPhys "hit" + "Unknown animate object" + p = objPos obj1 + r = objRadius obj1 + v = objVel obj1 + + +-- Check for animate object (round) hitting inanimate (rectangular) object. +hitInanimate :: Position2 -> Length -> Velocity2 -> Object -> Event Velocity2 +hitInanimate p r v obj + -- Hit straight from left or right (west/east)? + | x < x1 && y1 < y && y < y2 && x + r > x1 && vx > 0 + || x > x2 && y1 < y && y < y2 && x - r < x2 && vx < 0 + = impulse (vector2 (-(2 * vx)) 0) + -- Hit straight from top or bottom (nort/south)? + | x1 < x && x < x2 && y < y1 && y + r > y1 && vy > 0 + || x1 < x && x < x2 && y > y2 && y - r < y2 && vy < 0 + = impulse (vector2 0 (-(2 * vy))) + -- Hit lower left corner? + | x <= x1 && y <= y1 + = hitInanimateCorner r (p11 .-. p) v + -- Hit upper left corner? + | x <= x1 && y >= y2 + = hitInanimateCorner r (p12 .-. p) v + -- Hit upper right corner? + | x >= x2 && y >= y2 + = hitInanimateCorner r (p22 .-. p) v + -- Hit lower right corner? + | x >= x2 && y <= y1 + = hitInanimateCorner r (p21 .-. p) v + | otherwise + = noImpulse + where + Point2 x y = p + (vx, vy) = vector2XY v + BBox p11@(Point2 x1 y1) p22@(Point2 x2 y2) = objBBox obj + p12 = Point2 x1 y2 + p21 = Point2 x2 y1 + + hitInanimateCorner r d v + -- Closer than r and approaching? + | nd < r && v_approach > 0 + = impulse ((-(2*v_approach)) *^ d_hat) + | otherwise + = noImpulse + where + nd = norm d + d_hat = d ^/ nd + v_approach = v `dot` d_hat + + +-- Check for robot hitting animate object. +robotHit :: Position2 -> Length -> Velocity2 -> Object -> Event Velocity2 +robotHit _ _ _ (ObjBall {}) = noImpulse -- Robots are not affected by balls. +robotHit p r v robot + | nd < r + objRadius robot && v_approach > 0 + -- Robots are equally heavy. + = impulse ((-v_approach) *^ d_hat) + | otherwise + = noImpulse + where + d = objPos robot .-. p + nd = norm d + d_hat = d ^/ nd + v_rel = v ^-^ objVel robot + v_approach = v_rel `dot` d_hat + + +-- Check for ball hitting animate object. +ballHit :: Position2 -> Length -> Velocity2 -> Object -> Event Velocity2 +ballHit p r v obj + | nd < r + objRadius obj && v_approach > 0 + = if obj `inClass` ClsBall then + -- Balls are equally heavy. + impulse ((-v_approach) *^ d_hat) + else + -- Robots are infinitely heavy compared to balls. + impulse (((-2) * v_approach) *^ d_hat) + | otherwise + = noImpulse + where + d = objPos obj .-. p + nd = norm d + d_hat = d ^/ nd + v_rel = v ^-^ objVel obj + v_approach = v_rel `dot` d_hat + + +{- + x11 <= x22 && x12 >= x21 + && y11 <= y22 && y12 >= y21 + && approaching + where + BBox (Point2 x11 y11) (Point2 x12 y12) = objBBox obj1 + BBox (Point2 x21 y21) (Point2 x22 y22) = objBBox obj2 + approaching = ((objVel obj2 ^-^ objVel obj1) + `dot` (objPos obj2 .-. objPos obj1)) + < 0 +-} + +------------------------------------------------------------------------------ +-- Sonar +------------------------------------------------------------------------------ + +-- Sonar echo from fixed walls. Currently simplified, more like laser ranger. +-- Arguments: +-- p .......... Robot position. +-- r .......... Robot radius. Sonar devices assumed to be mounted on perimeter. +-- h .......... Heading for sonar device. +-- phi ........ Lobe width. +-- smr ........ Maximal sonar range. +-- +-- Returns: distance to wall in given direction. + +-- !!! Could make this better by also computing distances for h +/- (phi/2). + +sonarEchoFixedWall :: + Position2 -> Length -> Heading -> Angle -> Distance +sonarEchoFixedWall p r h phi = + distanceFixedWall p (normalizeHeading h) - r + where + -- Distance to fixed wall in given direction. The position is assumed + -- to be inside the fixed walls, the heading is assumed to be + -- normalized. + distanceFixedWall :: Position2 -> Heading -> Distance + distanceFixedWall p h = wd + where + Point2 x y = p + + -- East wall + wd1 = if (-pi/2) < h && h < pi/2 then + ((worldEastWall - x) / cos h) + else + 1.0e100 -- Hack! Works as long as world not too big. + + -- North wall + wd2 = if 0 < h && h < pi then + min wd1 ((worldNorthWall - y) / sin h) + else + wd1 + + -- West wall + wd3 = if h < (-pi/2) || h > pi/2 then + min wd2 ((worldWestWall - x) / cos h) + else + wd2 + + -- South wall + wd = if (-pi) < h && h < 0 then + min wd3 ((worldSouthWall - y) / sin h) + else + wd3 + + +-- Sonar echo for an object. Currently very simplified: object is treated +-- as if it did not have any physical size. Some form of projection for +-- computing the visible surface area and scaling to take the distance into +-- account would be a good idea. But then the interface may need to change. +-- Sonar eche from fixed walls. Currently simplified, more like laser ranger. +-- Arguments: +-- p .......... Robot position. +-- h .......... Robot heading. +-- rr.......... Robot radius. Sonar devices assumed to be mounted on perimeter. +-- phi ........ Lobe width. +-- obj ........ Object to compute echo for. +-- +-- Returns: angle relative own heading (not normalized) and distance to +-- object. + +sonarEcho :: Position2 -> Heading -> Length -> Angle -> Object + -> (Angle, Distance) +sonarEcho p h rr phi obj = (h' - h, rho - rr) + where + (rho, h') = vector2RhoTheta (objPos obj .-. p) + +------------------------------------------------------------------------------ +-- Utilities +------------------------------------------------------------------------------ + +intErrObjPhys :: String -> String -> a +intErrObjPhys = intErr "RobotSim.ObjectPhysics" diff --git a/src/FRP/YFrob/RobotSim/ObjectTemplate.hs b/src/FRP/YFrob/RobotSim/ObjectTemplate.hs new file mode 100644 --- /dev/null +++ b/src/FRP/YFrob/RobotSim/ObjectTemplate.hs @@ -0,0 +1,148 @@ +{- +****************************************************************************** +* Y F R O B / R O B O T S I M * +* * +* Module: ObjectTemplate * +* Purpose: Interface types and functions for creating objects * +* and worlds. * +* Author: Henrik Nilsson * +* * +****************************************************************************** +-} + +module FRP.YFrob.RobotSim.ObjectTemplate ( + WorldTemplate, + ObjectTemplate(..), + objectToOT, -- :: Object -> ObjectTemplate + BoundingBox, + boundingBox -- :: ObjectTemplate -> BoundingBox +) where + +import FRP.YFrob.Common.PhysicalDimensions +import FRP.YFrob.Common.RobotIO (RobotId) + +import FRP.YFrob.RobotSim.Object (Object(..)) + + +type WorldTemplate = [ObjectTemplate] + + +-- !!! Maybe objects ought to be parameterized to a larger extent. +-- !!! E.g. a single robot type parameterized on shape (enumeration type +-- !!! used both in ObjectTemplate and in Object), colour, size, inertia, +-- !!! and so on. Similarly, walls and blocks could be instances of a single +-- !!! object parameterized on shape and colour. + +data ObjectTemplate = + OTBlock { -- Square-shaped obstacle + otPos :: Position2 + } + | OTNSWall { -- North-South wall segment + otPos :: Position2 + } + | OTEWWall { -- East-west wall segment + otPos :: Position2 + } + | OTVWall { -- "Vertical" wall segment, same as North-South + otPos :: Position2 + } + | OTHWall { -- "Horizontal" wall segment, same as east-west + otPos :: Position2 + } + | OTSimbotA { -- Robot of type Simbot A + otRId :: RobotId, + otPos :: Position2, + otHdng :: Heading + } + | OTSimbotB { -- Robot of type Simbot B + otRId :: RobotId, + otPos :: Position2, + otHdng :: Heading + } + | OTBall { -- Ball + otPos :: Position2 + } + + +objectToOT :: Object -> ObjectTemplate +objectToOT (ObjBlock {objPos = p}) = OTBlock {otPos = p} +objectToOT (ObjNSWall {objPos = p}) = OTNSWall {otPos = p} +objectToOT (ObjEWWall {objPos = p}) = OTEWWall {otPos = p} +objectToOT (ObjSimbotA {objRId = rid, objPos = p, objHdng = h}) = + OTSimbotA {otRId = rid, otPos = p, otHdng = h} +objectToOT (ObjSimbotB {objRId = rid, objPos = p, objHdng = h}) = + OTSimbotB {otRId = rid, otPos = p, otHdng = h} +objectToOT (ObjBall {objPos = p}) = OTBall {otPos = p} + + +type BoundingBox = (Position2, Position2) + +-- This is probably not such a great way of finding out the size of objects? +boundingBox :: ObjectTemplate -> BoundingBox +boundingBox = undefined + + +{- +-- This should be redone. +-- Define a simple textual format for the world (which equally well could be +-- created in a text editor) and write a parser/printer for that. +-- For example, one object per line, attributes separated by spaces. + +data ObjectForShowRead = ObjectForShowRead + { osrType :: String, + osrId :: ObjId, + --osrSel :: Bool, + osrPos :: Point2, + --osrBBox :: BBox, + osrHdng :: Maybe Heading + } + deriving (Show, Read) + +objToObjSR :: Object -> ObjectForShowRead +objToObjSR obj = + ObjectForShowRead { + osrType = typeOf obj, + osrId = oId obj, + --osrSel = oSel obj, + osrPos = oPos obj, + --osrBBox = oBBox obj, + osrHdng = hdngOf obj + } where + typeOf (ObjBlock {}) = "Block" + typeOf (ObjNSWall {}) = "NSWall" + typeOf (ObjEWWall {}) = "EWWall" + typeOf (ObjSimbotA {}) = "SimbotA" + typeOf (ObjSimbotB {}) = "SimbotB" + typeOf _ = intErr "RSObjectTemplate" + "typeOf" + "unknown object type" + + hdngOf (ObjSimbotA { orHdng = hdng }) = Just hdng + hdngOf (ObjSimbotB { orHdng = hdng }) = Just hdng + hdngOf _ = Nothing + +objSRToObj :: ObjectForShowRead -> Object +objSRToObj osr = obj where + obj = initObj { + oId = osrId osr, + oSel = False, -- osrSel osr, + oPos = osrPos osr, + oBBox = objBBox obj -- osrBBox osr + } + + initObj = case osrType osr of + "Block" -> ObjBlock {} + "NSWall" -> ObjNSWall {} + "EWWall" -> ObjEWWall {} + "SimbotA" -> defaultSimbotA { orHdng = hdng } + "SimbotB" -> defaultSimbotB { orHdng = hdng } + + Just hdng = osrHdng osr + +instance Show Object where + show obj = show (objToObjSR obj) + +instance Read Object where + readsPrec _ = readParen False $ + \r -> [(objSRToObj osr, s) | (osr, s) <- reads r] +-} diff --git a/src/FRP/YFrob/RobotSim/Parser.hs b/src/FRP/YFrob/RobotSim/Parser.hs new file mode 100644 --- /dev/null +++ b/src/FRP/YFrob/RobotSim/Parser.hs @@ -0,0 +1,632 @@ +{- +****************************************************************************** +* Y F R O B / R O B O T S I M * +* * +* Module: Parser * +* Purpose: Parsing (mainly lexical analysis) of window event * +* stream. * +* Author: Henrik Nilsson * +* * +****************************************************************************** +-} + +-- To do: +-- * Quick 'n dirty adaptation from old simulator. Could probably be done +-- better in the new Yampa framework. +-- * It is questionable to what extent the scanner should know about +-- simulator modes! Maybe one should just have one single set of commands. +-- * Use e.g ':' as a general prefix for long commands? +-- * Re-evaluate the stateful (mode dependent) scanning strategy. +-- E.g. suppose we'd like a single common character to abandon the +-- entire command. This is simple if we can goto a single, globally +-- known state. With the current approach, we'd have to add failure +-- continuations all over the place. + +{- + +The lexical scanner for character input in effect implements a simple, +interactive, command parser. The following general rules apply: + +* A command consists of a sequence of one or more fields. +* The first field is the command keword. +* The number of fields and the length of each fields are known, but not + necessarily fixed; i.e., they can be computed from the command input + seen thus far. +* The scanner will start reading the next field as soon as a field is + complete and the next command as soon as a command is complete + (i.e. the last field is complete). +* The return key can be used to terminate a field early. If the input + in the field thus far is valid (e.g. a valid command or a valid integer), + then the field is considered to be complete and the scanner starts + reading the next field (or next command, if this was the last field). + Otherwise nothing happens (the return character is discarded). +* Any invalid character (backspace and delete should always be invalid) + will cause the reading of a field to be restarted. +* The character '.' is used to complete command keywords. (Not much use + currently since all commands have very short names.) + +The simulator can be in one the the three modes: Edit, Run and Frozon. +Different commands are available in these three modes. + +Edit Mode: + +The simulator is in this mode initially. Commands in this mode are: + + cr + creates a robot, where can be: + a -- a robot of type Simbot A (round) + b -- a robot of type Simbot B (triangular) + + co + creates an obstacle, where can be: + block -- a quare block + nswall -- a north-south wall + ewwall -- an east-west wall + + cb + creates a ball + + select + adds zero or more objects to the current selection, where + can be: + block -- all block obstacles + nswall -- all north-south walls + ewwall -- all east-west walls + wall -- all walls + obst -- all obstacles + simbota -- all robots of type Simbot A + simbotb -- all robots of type Simbot B + robot -- all robots + all -- all robots and obstacles + Besides, you can also left click to select a single object. + + p selects the previous object + + n selects the next object + + u unselects all + + tl turns all robots selected left + + tr turns all robots selected right + + tn turns all robots selected north + + te turns all robots selected east + + ts turns all robots selected south + + tw turns all robots selected west + + tt + turns all robots selected to a specific angle (in degrees), where + 0 is heading north, 90 is east, and so on + + delete + deletes all objects selected + + go lets all robots run + + quit + quits the simulator (unimplemented yet, you'll have to close the + window instead) + +Run Mode: + +The simulator enters the run mode once the "go" command is issued. +There are only two commands in this mode: + + f freezes the robots and enters the frozen mode; this is not fully + implemented yet -- the mode is switched but the robots are not + stopping + + edit + freezes the robots and returns to the edit mode + +Frozen Mode: + +This mode is entered by the "f" command in the run mode. + + r resumes the robots and goes back to the frozen mode + + edit + swtiches to the edit mode +-} + + +module FRP.YFrob.RobotSim.Parser ( + SimInput, -- Abstract + parseWinInput, -- :: SF WinInput SimInput + command, -- :: SF SimInput (Event Command) + cmdString, -- :: SF SimInput (Event String) + ptrPos, -- :: SF SimInput Position2 + lbp, -- :: SF SimInput (Event ()) + lbpPos, -- :: SF SimInput (Event Position2) + lbDown, -- :: SF SimInput Bool + rbp, -- :: SF SimInput (Event ()) + rbpPos, -- :: SF SimInput (Event Position2) + rbDown, -- :: SF SimInput Bool + dragStart, -- :: SF SimInput (Event ()) + dragStop, -- :: SF SimInput (Event Distance2) + dragStartPos, -- :: SF SimInput Position2 + dragVec, -- :: SF SimInput Distance2 + dragging -- :: SF SimInput Bool +) where + +import Data.Maybe (isJust) +import Data.Char (ord, isSpace, isDigit) + +import qualified Graphics.HGL as HGL (Event(..)) + +import FRP.Yampa +import FRP.Yampa.Geometry + +import FRP.YFrob.Common.PhysicalDimensions + +import FRP.YFrob.RobotSim.WorldGeometry (gPointToPosition2) +import FRP.YFrob.RobotSim.Command +import FRP.YFrob.RobotSim.Object (ObjClass(..)) +import FRP.YFrob.RobotSim.Animate (WinInput) + +------------------------------------------------------------------------------ +-- Exported entities +------------------------------------------------------------------------------ + +data SimInput = SimInput { + siCmdStr :: String, + siCmd :: Event Command, + siPDS :: PDState +} + + +parseWinInput :: SF WinInput SimInput +parseWinInput = wiToCmd &&& wiToPDS + >>^ \((cmdStr, cmd), pds) -> + SimInput {siCmdStr = cmdStr, siCmd = cmd, siPDS = pds} + + +-- All event sources below are defined such that they will NOT occur at local +-- time 0 (immediately after a switch). Sometimes explicitly using a "notYet". +-- Sometimes using through careful use of "edge" and relatives. Is this the +-- right approach? + +-- A valid command has been read. +command :: SF SimInput (Event Command) +command = siCmd ^>> notYet + + +-- Continuous parser feed back. +cmdString :: SF SimInput String +cmdString = arr siCmdStr + + +{- +-- Probably not needed. Old FRP code! +-- New command scanner state, represented as the current command prefix. +-- Empty string once a valid command has been scanned! +cmdStateChange :: SF SimInput (Event String) +siCmdStateChange :: Event SI String +siCmdStateChange = whileByE (fmap curPfx) (fmap siCmd inputB) + where + curPfx (pfx, mcmd) = maybe pfx (const "") mcmd +-} + + +ptrPos :: SF SimInput Position2 +ptrPos = arr (pdsPos . siPDS) + + +lbp :: SF SimInput (Event ()) +lbp = lbpPos >>^ (`tag` ()) + + +lbpPos :: SF SimInput (Event Position2) +lbpPos = siPDS # pdsLeft ^>> edgeJust + + +lbDown :: SF SimInput Bool +lbDown = arr (siPDS # pdsLeft # isJust) + + +rbp :: SF SimInput (Event ()) +rbp = rbpPos >>^ (`tag` ()) + + +rbpPos :: SF SimInput (Event Position2) +rbpPos = siPDS # pdsRight ^>> edgeJust + + +rbDown :: SF SimInput Bool +rbDown = arr (siPDS # pdsRight # isJust) + + +dragStart :: SF SimInput (Event ()) +dragStart = siPDS # pdsDrag ^>> edgeBy detectStart (Just undefined) + where + detectStart Nothing (Just _) = Just () + detectStart _ _ = Nothing + + +dragStop :: SF SimInput (Event Distance2) +dragStop = (siPDS # pdsDrag ^>> edgeBy detectStop Nothing) &&& dragVec + >>^ \(e, dv) -> e `tag` dv + where + detectStop (Just _) Nothing = Just () + detectStop _ _ = Nothing + + +-- (Last) drag start position. +dragStartPos :: SF SimInput Position2 +dragStartPos = arr (siPDS # pdsDragStartPos) + + +-- (Last) drag vector. +dragVec :: SF SimInput Distance2 +dragVec = arr (siPDS # pdsDragVec) + + +dragging :: SF SimInput Bool +dragging = arr (siPDS # pdsDrag # isJust) + + +------------------------------------------------------------------------------ +-- Lexical analysis of character input +------------------------------------------------------------------------------ + +wiToCmd :: SF WinInput (String, Event Command) +wiToCmd = arr (mapFilterE selChar) + >>> (accumBy scanChar (undefined,scanCmdsEdit) + >>^ fmap fst >>^ splitE) + >>> hold "" *** arr (mapFilterE id) + where + scanChar (_, S cont) c = cont c + + selChar (HGL.Char {HGL.char=c}) = Just c + selChar _ = Nothing + + +-- This ought to be redone. Kont should probably be called Tranition or +-- somethinig. + +-- We define a continuation to be the command recognized thus far (a String +-- and maybe a complete Command), and a scanner to be applied to the rest +-- of the input. (I.e., there's output at every step.) + +type Kont = ((String, Maybe Command), Scanner) +type Cont a = a -> Kont + +-- Since a scanner is applied to one character at a time (typically, on +-- Char events), we recursively define a scanner to be a character +-- continuation. + +newtype Scanner = S (Cont Char) + +-- *** Note! It is questionable to what extent the scanner should know about +-- simulator modes! Maybe one should just have one single set of commands. + +-- Can only read lowercase and some symbols currently. In particular, cannot +-- read '!', so the commands "q!", "r!", and "e!" are out. + + +-- Scan commands in Edit mode. + +scanCmdsEdit :: Scanner +scanCmdsEdit = scanCmd editCmds + where + editCmds = + [ ("quit", emitCmd scanCmdsEdit CmdQuit), -- Discard inp.? + ("run", emitCmd scanCmdsRun CmdRun), + ("co", readObstClass), + ("cr", readRobotClass), + ("cb", emitCmd scanCmdsEdit CmdCreateBall), + ("delete", emitCmd scanCmdsEdit CmdDelete), + ("n", emitCmd scanCmdsEdit CmdSelectNext), + ("p", emitCmd scanCmdsEdit CmdSelectPrev), + ("select", readObjClass), + ("u", emitCmd scanCmdsEdit CmdUnselectAll), + ("tl", emitCmd scanCmdsEdit CmdTurnLeft), + ("tr", emitCmd scanCmdsEdit CmdTurnRight), + ("tn", emitCmd scanCmdsEdit + (CmdTurnTo (bearingToHeading 0))), + ("te", emitCmd scanCmdsEdit + (CmdTurnTo (bearingToHeading 90))), + ("ts", emitCmd scanCmdsEdit + (CmdTurnTo (bearingToHeading 180))), + ("tw", emitCmd scanCmdsEdit + (CmdTurnTo (bearingToHeading 270))), + ("tt", readAngle), + ("save", readFilePathForSave), + ("open", readFilePathForLoad) + ] + + readObstClass pfx = emitPfx (scanSubCmd (pfx ++ " ") coSubCmds) pfx + + coSubCmds = + [ ("block", emitCmd scanCmdsEdit (CmdCreateObst ClsBlock)), + ("nswall", emitCmd scanCmdsEdit (CmdCreateObst ClsNSWall)), + ("ewwall", emitCmd scanCmdsEdit (CmdCreateObst ClsEWWall)) + ] + + -- We will have to read further parameters here. + readRobotClass pfx = emitPfx (scanSubCmd (pfx ++ " ") crSubCmds) pfx + + crSubCmds = [("a", emitCmd scanCmdsEdit (CmdCreateRobot ClsSimbotA)), + ("b", emitCmd scanCmdsEdit (CmdCreateRobot ClsSimbotB))] + + readObjClass pfx = emitPfx (scanSubCmd (pfx ++ " ") selectSubCmds) pfx + + selectSubCmds = + [ ("all", emitCmd scanCmdsEdit (CmdSelect ClsObj)), + ("obst", emitCmd scanCmdsEdit (CmdSelect ClsInanimate)), + ("block", emitCmd scanCmdsEdit (CmdSelect ClsBlock)), + ("wall", emitCmd scanCmdsEdit (CmdSelect ClsWall)), + ("nswall", emitCmd scanCmdsEdit (CmdSelect ClsNSWall)), + ("ewwall", emitCmd scanCmdsEdit (CmdSelect ClsEWWall)), + ("robot", emitCmd scanCmdsEdit (CmdSelect ClsRobot)), + ("simbota", emitCmd scanCmdsEdit (CmdSelect ClsSimbotA)), + ("simbotb", emitCmd scanCmdsEdit (CmdSelect ClsSimbotB)) + ] + + readAngle pfx = + emitPfx (scanIntegerArg pfx + 3 + (\(cmdStr, ang) -> + emitCmd scanCmdsEdit + (CmdTurnTo + (bearingToHeading + (fromInteger ang))) + cmdStr)) + pfx + + readFilePathForSave pfx = + emitPfx (scanStringArg pfx + (\(cmdStr, path) -> + emitCmd scanCmdsEdit + (CmdSave path) + cmdStr)) + pfx + + readFilePathForLoad pfx = + emitPfx (scanStringArg pfx + (\(cmdStr, path) -> + emitCmd scanCmdsEdit + (CmdLoad path) + cmdStr)) + pfx + + +-- Scan commands in Run mode. + +scanCmdsRun :: Scanner +scanCmdsRun = scanCmd runCmds + where + runCmds = + [ ("f", emitCmd scanCmdsFrozen CmdFreeze), + ("edit", emitCmd scanCmdsEdit CmdEdit) + ] + + +-- Scan commands in Frozen mode. + +scanCmdsFrozen :: Scanner +scanCmdsFrozen = scanCmd frozenCmds + where + frozenCmds = + [ ("r", emitCmd scanCmdsRun CmdResume), + ("edit", emitCmd scanCmdsEdit CmdEdit) + ] + + +-- Scan one command. +-- Looks for a valid command. Outputs prefix as long as the current +-- prefix is valid. Starts over on first invalid character. Invokes success +-- continuation on success. +-- cmds ....... List of pairs of valid command and corresponding success +-- continuation. + +scanCmd :: [(String, Cont String)] -> Scanner +scanCmd cmds = scanSubCmd "" cmds + + +-- Scan one subcommand/keyword argument. +-- Looks for a valid command. Outputs prefix as long as the current +-- prefix is valid. Starts over on first invalid character. Invokes success +-- continuation on success. +-- pfx0 ....... Initial prefix. +-- cmds ....... List of pairs of valid command and corresponding success +-- continuation. + +scanSubCmd :: String -> [(String, Cont String)] -> Scanner +scanSubCmd pfx0 cmds = S (scHlp pfx0 cmds) + where + -- pfx ........ Command prefix. + -- sfxconts ... Command suffixes paired with success continuations. + -- c .......... Input character. + scHlp pfx sfxconts c = + case c of + '\r' -> + case [ cont | ("", cont) <- sfxconts ] of + [] -> emitPfx (S (scHlp pfx sfxconts)) pfx + (cont : _) -> cont pfx + '.' -> + case sfxconts of + [] -> emitPfx (S (scHlp pfx0 cmds)) pfx0 + [(sfx, cont)] -> cont (pfx ++ sfx) + _ -> + let + (sfxs, conts) = unzip sfxconts + cpfx = foldr1 lcp sfxs + sfxs' = map (drop (length cpfx)) sfxs + pfx' = pfx ++ cpfx + sfxconts' = zip sfxs' conts + in + emitPfx (S (scHlp pfx' sfxconts')) pfx' + _ -> + let + pfx' = pfx ++ [c] + sfxconts' = [ (tail sfx, cont) + | (sfx, cont) <- sfxconts, + not (null sfx) && head sfx == c + ] + in + case sfxconts' of + [] -> emitPfx (S (scHlp pfx0 cmds)) + pfx0 + -- ("Invalid: " ++ [c]) + [("", cont)] -> cont pfx' + _ -> emitPfx (S (scHlp pfx' sfxconts')) + pfx' + + +-- Scan fixed-length integer argument. +-- pfx0 ....... Initial prefix (command scanned thus far). +-- n0 ......... Maximal number of digits. +-- cont ....... Continuation: will be passed the new prefix and the +-- integer value of the scanned argument. + +scanIntegerArg :: String -> Int -> Cont (String,Integer) -> Scanner +scanIntegerArg pfx0 n0 cont | n0 > 0 = S (siaHlp (pfx0 ++ " ") n0 0) + where + siaHlp pfx n a c = + if c == '\r' then + cont (pfx, a) + else if isDigit c then + let a' = a * 10 + fromIntegral (ord c - ord '0') + pfx' = pfx ++ [c] + in + if n > 1 then + emitPfx (S (siaHlp pfx' (n - 1) a')) pfx' + else + cont (pfx', a') + else + emitPfx (S (siaHlp (pfx0 ++ " ") n0 0)) pfx0 + + +-- Scan variable-length string argument. +-- pfx0 ....... Initial prefix (command scanned thus far). +-- cont ....... Continuation: will be passed the new prefix and the +-- string value of the scanned argument. + +scanStringArg :: String -> Cont (String,String) -> Scanner +scanStringArg pfx0 cont = S (ssaHlp (pfx0 ++ " ") "") + where + ssaHlp pfx a c = + if c == '\r' then + cont (pfx, a) + else + let a' = dropWhile isSpace $ a ++ [c] + pfx' = pfx ++ [c] + in + emitPfx (S (ssaHlp pfx' a')) pfx' + + +-- Emit command (and command string), then continue scanning. +emitCmd :: Scanner -> Command -> String -> Kont +emitCmd scanner cmd cmdStr = ((cmdStr, Just cmd), scanner) + + +-- Emit current prefix, then scan next character. +emitPfx :: Scanner -> String -> Kont +emitPfx scanner pfx = ((pfx, Nothing), scanner) + + +------------------------------------------------------------------------------ +-- Pointing device processing +------------------------------------------------------------------------------ + +-- State of the pointing device. +-- The points for pdsLeft, pdsRight, and pdsDrag reflect where the button +-- was initially pressed. + + +data PDState = PDState { + pdsPos :: Position2, -- Current position. + pdsDragStartPos :: Position2, -- (Last) drag start position. + pdsDragVec :: Distance2, -- (Latest) drag vector. + pdsLeft :: Maybe Position2, -- Left button currently down. + pdsRight :: Maybe Position2, -- Right button currently down. + pdsDrag :: Maybe Position2 -- Currently dragging. +-- pdsPrevLeft :: Maybe Position2, -- Previous left button state. +-- pdsPrevRight:: Maybe Position2, -- Previous right button state. +-- pdsPrevDrag :: Maybe Position2 -- Previous drag state. +} + + +-- Initial state. +initPDS :: PDState +initPDS = PDState { + pdsPos = origin, + pdsDragStartPos = origin, + pdsDragVec = zeroVector, + pdsLeft = Nothing, + pdsRight = Nothing, + pdsDrag = Nothing +-- pdsPrevLeft = Nothing, +-- pdsPrevRight = Nothing, +-- pdsPrevDrag = Nothing + } + + +wiToPDS :: SF WinInput PDState +wiToPDS = accumHoldBy nextPDS initPDS + + +{- +-- Left-over from the "prev" mechanism that hopefully will not be needed. +updPrev pds (PDState {pdsLeft = pl, pdsRight = pr, pdsDrag = pd}) = + pds {pdsPrevLeft = pl, pdsPrevRight = pr, pdsPrevDrag = pd} +-} + + +-- Compute next pointing device state. +nextPDS :: PDState -> HGL.Event -> PDState +nextPDS pds (HGL.Key {}) = pds -- Currently we ignore keys. +nextPDS pds (HGL.Button {HGL.pt = p, HGL.isLeft = True, HGL.isDown = True}) = + -- Left button pressed. + pds {pdsPos = p', pdsDragVec = dv, pdsLeft = Just p'} + where + p' = gPointToPosition2 p + dv = maybe (pdsDragVec pds) (\dspos -> p' .-. dspos) (pdsDrag pds) +nextPDS pds (HGL.Button {HGL.pt = p, HGL.isLeft = True, HGL.isDown = False}) = + -- Left button released. + pds {pdsPos = p', pdsDragVec = dv, pdsLeft = Nothing, pdsDrag = md} + where + p' = gPointToPosition2 p + md = maybe Nothing (const (pdsDrag pds)) (pdsRight pds) + dv = maybe (pdsDragVec pds) (\dspos -> p' .-. dspos) md +nextPDS pds (HGL.Button {HGL.pt = p, HGL.isLeft = False, HGL.isDown = True}) = + -- Right button pressed. + pds {pdsPos = p', pdsDragVec = dv, pdsRight = Just p'} + where + p' = gPointToPosition2 p + dv = maybe (pdsDragVec pds) (\dspos -> p' .-. dspos) (pdsDrag pds) +nextPDS pds (HGL.Button {HGL.pt = p, HGL.isLeft = False, HGL.isDown = False}) = + -- Right button released. + pds {pdsPos = p', pdsDragVec = dv, pdsRight = Nothing, pdsDrag = md} + where + p' = gPointToPosition2 p + md = maybe Nothing (const (pdsDrag pds)) (pdsLeft pds) + dv = maybe (pdsDragVec pds) (\dspos -> p' .-. dspos) md +nextPDS pds (HGL.MouseMove {HGL.pt = p}) = + -- Mouse move. + pds {pdsPos = p', pdsDragStartPos = dsp, pdsDragVec = dv, pdsDrag = md} + where + p' = gPointToPosition2 p + md = case pdsLeft pds of + mlp@(Just _) -> mlp + Nothing -> pdsRight pds + dsp = maybe (pdsDragStartPos pds) id md + dv = maybe (pdsDragVec pds) (\dspos -> p' .-. dspos) md +nextPDS pds _ = pds -- Ignore unknown events. + + +------------------------------------------------------------------------------ +-- General utilities +------------------------------------------------------------------------------ + +-- Longest common prefix. +lcp :: Eq a => [a] -> [a] -> [a] +lcp [] _ = [] +lcp _ [] = [] +lcp (x:xs) (y:ys) | x == y = x : lcp xs ys + | otherwise = [] diff --git a/src/FRP/YFrob/RobotSim/RenderFixedWalls.hs b/src/FRP/YFrob/RobotSim/RenderFixedWalls.hs new file mode 100644 --- /dev/null +++ b/src/FRP/YFrob/RobotSim/RenderFixedWalls.hs @@ -0,0 +1,80 @@ +{- +****************************************************************************** +* Y F R O B / R O B O T S I M * +* * +* Module: RenderFixedWalls * +* Purpose: Rendering of the fixed walls. * +* Author: Henrik Nilsson * +* * +****************************************************************************** +-} + +module FRP.YFrob.RobotSim.RenderFixedWalls ( + fixedWalls -- :: HGL.Graphic +) where + +import Data.Array +import qualified Graphics.HGL as HGL + +import FRP.Yampa.Point2 (Point2(..)) + +import FRP.YFrob.RobotSim.WorldGeometry +import FRP.YFrob.RobotSim.Colors +import FRP.YFrob.RobotSim.ColorBindings + + +------------------------------------------------------------------------------ +-- Fixed wall rendering +------------------------------------------------------------------------------ + +-- Currently, the only fixed walls are the outer ones. + +fixedWalls :: HGL.Graphic +fixedWalls = + HGL.mkBrush (colorTable ! outerWallColor) $ \brush -> + HGL.withBrush brush $ + -- Drawing rectangles seems slightly quicker than a complex polygon. + -- HGL.polygon [pN1, pN2, pN3, pS4, pS1, pW2, pW3, pW4, pE1, pE2] + HGL.overGraphics + [ HGL.polygon [pN1, pN2, pN3, pN4], + HGL.polygon [pE1, pE2, pE3, pE4], + HGL.polygon [pS1, pS2, pS3, pS4], + HGL.polygon [pW1, pW2, pW3, pW4] + ] + where + pN1 = position2ToGPoint (Point2 worldXMin worldNorthWall) + pN2 = position2ToGPoint (Point2 worldXMin worldYMax) + pN3 = position2ToGPoint (Point2 worldXMax worldYMax) + pN4 = position2ToGPoint (Point2 worldXMax worldNorthWall) + + pE1 = position2ToGPoint (Point2 worldEastWall worldSouthWall) + pE2 = position2ToGPoint (Point2 worldEastWall worldNorthWall) + pE3 = position2ToGPoint (Point2 worldXMax worldNorthWall) + pE4 = position2ToGPoint (Point2 worldXMax worldSouthWall) + + pS1 = position2ToGPoint (Point2 worldXMin worldYMin) + pS2 = position2ToGPoint (Point2 worldXMin worldSouthWall) + pS3 = position2ToGPoint (Point2 worldXMax worldSouthWall) + pS4 = position2ToGPoint (Point2 worldXMax worldYMin) + + pW1 = position2ToGPoint (Point2 worldXMin worldSouthWall) + pW2 = position2ToGPoint (Point2 worldXMin worldNorthWall) + pW3 = position2ToGPoint (Point2 worldWestWall worldNorthWall) + pW4 = position2ToGPoint (Point2 worldWestWall worldSouthWall) + + +-- For some reason, using regions does not seem to work (ONLY walls visible.) +{- +walls :: Graphic +walls = + HGL.mkBrush (colorTable ! DimGrey) $ \brush -> + HGL.withBrush brush $ + HGL.regionToGraphic $ + HGL.subtractRegion (HGL.rectangleRegion p1 p2) + (HGL.rectangleRegion p3 p4) + where + p1 = position2ToGPoint (Point2 worldXMin worldYMin) + p2 = position2ToGPoint (Point2 worldXMax worldYMax) + p3 = position2ToGPoint (Point2 worldWestWall worldSouthWall) + p4 = position2ToGPoint (Point2 worldEastWall worldNorthWall) +-} diff --git a/src/FRP/YFrob/RobotSim/RenderObject.hs b/src/FRP/YFrob/RobotSim/RenderObject.hs new file mode 100644 --- /dev/null +++ b/src/FRP/YFrob/RobotSim/RenderObject.hs @@ -0,0 +1,169 @@ +{- +****************************************************************************** +* Y F R O B / R O B O T S I M * +* * +* Module: RenderObject * +* Purpose: Object rendering. * +* Author: Henrik Nilsson * +* * +****************************************************************************** +-} + +-- ToDo: +-- Add display of RobotId to the robots. + +-- Note: Working on the level of signal functions would obviously allow the +-- display of a robot to be animated easily (e.g. cycling colours, flashing +-- bounding boxes). However, the current approach is to map a list of objects +-- directly to a graphic because that, in principle, allows some optimzations +-- that might be important. +-- +-- In the future, one might want to work on a higher abstract level than +-- Graphic to facilitate e.g. scaling and rotations. In particular if a +-- library like Haven replaces HGL. + + +module FRP.YFrob.RobotSim.RenderObject ( + renderObjects -- :: [Object] -> HGL.Graphic +) where + +import Data.Array +import qualified Graphics.HGL as HGL +import FRP.Yampa.Geometry + +import FRP.YFrob.Common.PhysicalDimensions + +import FRP.YFrob.RobotSim.WorldGeometry +import FRP.YFrob.RobotSim.Object +import FRP.YFrob.RobotSim.Colors +import FRP.YFrob.RobotSim.ColorBindings + +------------------------------------------------------------------------------ +-- Object rendering +------------------------------------------------------------------------------ + +-- This interface allows optimization. E.g. pen/brush creation can be +-- lifted to the top level. + +renderObjects :: [Object] -> HGL.Graphic +renderObjects objs = HGL.overGraphics (map renderObject objs) + + +renderObject :: Object -> HGL.Graphic +renderObject (ObjBlock {objSel = s, objBBox = BBox p1 p2}) = + rectObst s blockColor p1 p2 +renderObject (ObjNSWall {objSel = s, objBBox = BBox p1 p2}) = + rectObst s nsWallColor p1 p2 +renderObject (ObjEWWall {objSel = s, objBBox = BBox p1 p2}) = + rectObst s ewWallColor p1 p2 +renderObject (ObjSimbotA {objRId=rid, objSel=s, objPos=p, objHdng=h, + objBBox=BBox p1 p2}) = + -- drawPicUnscaled (positionToPointT %$ pic) `HGL.overGraphic` + if s then + (bbox p1 p2) `HGL.overGraphic` simbot + else + simbot + where + simbot = centeredText p (show rid) + `HGL.overGraphic` (circle simbotANoseColor pn + (simbotARadius/3)) + `HGL.overGraphic` (circle color p simbotARadius) + pn = p .+^ (vector2Polar simbotARadius h) + color = if rid < 10 then simbotAColor else simbotAAltColor +renderObject (ObjSimbotB {objRId = rid, objSel=s, objPos=p, objHdng=h, + objBBox=BBox p1 p2}) = + -- drawPicUnscaled (positionToPointT %$ pic) `HGL.overGraphic` + if s then + (bbox p1 p2) `HGL.overGraphic` simbot + else + simbot + where + simbot = centeredText p (show rid) + `HGL.overGraphic` (circle simbotBNoseColor pn + (simbotBRadius/3)) + `HGL.overGraphic` (triangle color pb1 pn pb2) + pn = p .+^ (vector2Polar simbotBRadius h) + pb1 = p .+^ (vector2Polar simbotBRadius (h + 2*pi/3)) + pb2 = p .+^ (vector2Polar simbotBRadius (h - 2*pi/3)) + color = if rid < 10 then simbotBColor else simbotBAltColor +renderObject (ObjBall {objSel = s, objPos = p, objBBox = BBox p1 p2}) = + if s then + (bbox p1 p2) `HGL.overGraphic` ball + else + ball + where + ball = circle ballColor p ballRadius + + +rectObst :: Bool -> Color -> Position2 -> Position2 -> HGL.Graphic +rectObst s c p1 p2 = + if s then + (bbox p1 p2) `HGL.overGraphic` (rectangle c p1 p2) + else + (rectangle c p1 p2) + + +triangle :: Color -> Position2 -> Position2 -> Position2 -> HGL.Graphic +triangle c p1 p2 p3 = + HGL.mkBrush (colorTable ! c) $ \brush -> + HGL.withBrush brush $ + HGL.polygon [gp1, gp2, gp3] + where + gp1 = position2ToGPoint p1 + gp2 = position2ToGPoint p2 + gp3 = position2ToGPoint p3 + + +rectangle :: Color -> Position2 -> Position2 -> HGL.Graphic +rectangle c p1 p2 = + HGL.mkBrush (colorTable ! c) $ \brush -> + HGL.withBrush brush $ + HGL.polygon [gp11, gp12, gp22, gp21] + where + gp11@(x1,y1) = position2ToGPoint p1 + gp12 = (x1, y2) + gp22@(x2,y2) = position2ToGPoint p2 + gp21 = (x2, y1) + + +circle :: Color -> Position2 -> Length -> HGL.Graphic +circle c p r = + HGL.mkBrush (colorTable ! c) $ \brush -> + HGL.withBrush brush $ + HGL.ellipse gp11 gp22 + where + d = vector2 r r + gp11 = position2ToGPoint (p .-^ d) + gp22 = position2ToGPoint (p .+^ d) + + +bbox :: Position2 -> Position2 -> HGL.Graphic +bbox p1 p2 = + -- Line style and thiknes seems to be ignored completely? + HGL.mkPen HGL.Dash 2 (colorTable ! bboxColor) $ \pen -> + HGL.withPen pen $ + HGL.polyline [gp11, gp12, gp22, gp21, gp11] + where + gp11@(x1,y1) = position2ToGPoint p1 + gp12 = (x1, y2) + gp22@(x2,y2) = position2ToGPoint p2 + gp21 = (x2, y1) + + +-- Centering pretty ad hoc. +centeredText :: Position2 -> String -> HGL.Graphic +centeredText p s = HGL.text (x', y') s + where + (x,y) = position2ToGPoint p + x' = x - (445 * length s) `div` 100 + y' = y - 7 + + +{- +-- Old stuff. Revisit if picture output is re-introduced. + +-- The drawPic routine in Graphics.hs adds an extra bit of scaling. +-- This undoes it. + +drawPicUnscaled pic = FG.drawPic (uscale2 0.01 %$ pic) +-} diff --git a/src/FRP/YFrob/RobotSim/Simulator.hs b/src/FRP/YFrob/RobotSim/Simulator.hs new file mode 100644 --- /dev/null +++ b/src/FRP/YFrob/RobotSim/Simulator.hs @@ -0,0 +1,698 @@ +{-# LANGUAGE Arrows #-} + +{- +****************************************************************************** +* Y F R O B / R O B O T S I M * +* * +* Module: Simulator * +* Purpose: The actual robot simulator * +* Author: Henrik Nilsson * +* * +****************************************************************************** +-} + +-- !!! We are breaking the event abstraction here in order to facilitate +-- !!! programming of collision detectino and handling. One reason is that +-- !!! it currently relies on "dense events" in an attempt to ensure that +-- !!! robots don't get any where when a collision state persists. +-- !!! Another reason is that some of the routing functions for "par" need +-- !!! to compute events, and they are SCALAR functions. (One could potentially +-- !!! return something else and post-process it by pre-composing a signal +-- !!! function on each signal function in the collections, but this is +-- !!! undesirable for a number of reasons.) +-- !!! Maybe this indicates that event should not be abstract, or at least +-- !!! that there should be a constructor for Event. Or maybe it indicates +-- !!! that we do collisions in the wrong way. A reasonable argument *for* +-- !!! being able to construct events in scalar code is that they are a +-- !!! natural representation of impulses. Impulses may have to be computed +-- !!! by complicated interaction functions *within* a time step. + +module FRP.YFrob.RobotSim.Simulator ( + SimbotController, + simWorld, + simWorld' -- Temporary! Need to rethink interface. +) where + +import Data.Array (Array, array, accumArray) + +import FRP.Yampa +import FRP.Yampa.Geometry + +import FRP.YFrob.Common.Diagnostics +import FRP.YFrob.Common.PhysicalDimensions +import FRP.YFrob.Common.RobotIO (RobotType, RobotId, BatteryStatus(..), + rfOutOfRange) + +import FRP.YFrob.RobotSim.IdentityList +import FRP.YFrob.RobotSim.Parser (SimInput) +import FRP.YFrob.RobotSim.IO +import FRP.YFrob.RobotSim.ObjectTemplate +import FRP.YFrob.RobotSim.Object +import FRP.YFrob.RobotSim.ObjectPhysics + + +type SimbotController = SimbotProperties -> SF SimbotInput SimbotOutput +-- !!! Maybe SimbotController should be redefined as follows: +-- type SimbotController = SimbotProperties -> SF SimbotInput (MR SimbotOutput) + + +------------------------------------------------------------------------------ +-- Simulation of a given world +------------------------------------------------------------------------------ + +-- Have to reconsider this at a later point. +-- * Interaction with editor: maybe we rally want to keep all objects +-- alive across switches using our continuation machinery. +-- * Probably want to employ some form of embedding at some level, +-- (if nothing else so for allowing varying simulation speed), but +-- I still want to allow some interaction, obviously. The latter must +-- go outside the embed. And then, how could one possibly recover the +-- continations across an embed? +-- * At least we should use a continuation switch for implementing pause. +-- * It seems as if the signal functions representing different kinds of +-- objects are going to have different output types. Makes sense, I suppose, +-- but then we have to partition the world! Makes recovering continuations +-- even worse. +-- * One could move e.g. Event RobotTCO inside an object. +-- Not very nice, but would give all signal functions the same type. +-- * Otherwise, why have a single object type in the first place? +-- maybe it should be split? +-- * Regarding passing continuations to the edit mode: that assumes that the +-- signal function types are the same in both edit mode and in simulation +-- mode. That's arguable not modular. E.g. why would one want collision +-- and perception input in edit mode??? +-- * For now, assume that the state of the world is passed on as a +-- WorldTemplate. + + +-- Representation of the static part or the world (less fixed walls). +type StaticWorld = [Object] + + +-- Simulation of a given world. +-- Arguments: +-- wt ......... Description of the world. +-- sca ........ Simbot controller for simbots of type A. +-- scb ........ Simbot controller for simbots of type B. +-- +-- Signal inputs: +-- si ......... Simulator input. +-- +-- Signal outputs: +-- #1.1 ....... The simulated animate and inanimate objects. +-- #1.2 ....... Text console output from the robots. +-- #2 ......... Termination event carrying the final state of the world. + +simWorld :: WorldTemplate -> SimbotController -> SimbotController + -> SF SimInput (([Object], Event [String]), Event WorldTemplate) +simWorld wt sca scb = proc si -> do + -- !!! This structure allows for embedding, cont. switching for pause ... + -- (objs, ems) <- embedSynch (simWorld' wt sca scb) dtis -< 4.0 + (objs, ems) <- simWorld' wt sca scb -< () + done <- never -< () -- !!! Termination event. Fix! + returnA -< ((objs, ems), done `tag` map objectToOT objs) + -- where + -- dtis = ((), repeat (0.01, Nothing)) + + +simWorld' :: WorldTemplate -> SimbotController -> SimbotController + -> SF a ([Object], Event [String]) +simWorld' wt sca scb = proc _ -> do + rec (rs, ertcos) <- simRobots wt sca scb sw -< (rs, bs) + bs <- simBalls wt sw -< (rs, bs) + let objs = ilElems rs ++ ilElems bs ++ sw + ems = fmap (concat . map formatRobotTCO) (catEvents ertcos) + returnA -< (objs, ems) + where + sw = inanimateObjects wt + + formatRobotTCO :: (RobotType, RobotId, [String]) -> [String] + formatRobotTCO (rtp, rid, ms) = map (pfx++) ms + where + pfx = rtp ++ "." ++ show rid ++ ": " + + +inanimateObjects :: WorldTemplate -> StaticWorld +inanimateObjects wt = [ o | Just o <- map inanimObj wt] + where + inanimObj (OTBlock {otPos = p}) = Just (block False p) + inanimObj (OTNSWall {otPos = p}) = Just (nsWall False p) + inanimObj (OTEWWall {otPos = p}) = Just (ewWall False p) + inanimObj (OTVWall {otPos = p}) = inanimObj (OTNSWall {otPos = p}) + inanimObj (OTHWall {otPos = p}) = inanimObj (OTEWWall {otPos = p}) + inanimObj _ = Nothing + + +------------------------------------------------------------------------------ +-- Simulation of robots +------------------------------------------------------------------------------ + +{- +-- Shows how one can leverage IL to make a new collection type that +-- associates extra information with the objects, if necessary. +-- But didn't turn out to be a great idea in this case. Also, there is +-- an overhead for ALL uses of the data type, whereas the extra info might +-- only be needed for certain applications. + +newtype ILR a = ILR (IL (RobotPhysicalProperties, a)) +unILR (ILR il) = il + + +instance Functor ILR where + fmap f = ILR . fmap (\(rpp, a) -> (rpp, f a)) . unILR + + +ilrElems :: ILR a -> [a] +ilrElems ilr = map (\(_, (_, a)) -> a) (ilAssocs (unILR ilr)) + + +mapILR :: (ILKey -> RobotPhysicalProperties -> a -> b) -> ILR a -> ILR b +mapILR f ilr = ILR (mapIL (\(k,(rpp,a)) -> (rpp, f k rpp a)) (unILR ilr)) +-} + +data RobotPerception = RP { + rpRanges :: Array Int Distance, + rpMaxRange :: Distance, + rpOtherRobots :: [(RobotType, RobotId, Angle, Distance)], + rpBalls :: [(Angle, Distance)] +} + + +-- A suitable default value when waiting for true sensor input (e.g. iPre). +-- Note that actual controller code should NOT assume that all other robots +-- are visible at all times, nor that rpMaxRange is not changing. +-- (Getting hold of rpMaxRange at the initial time step, e.g. through a +-- snapshot and then using it as a static value is a bad idea.) + +rp_init :: RobotPerception +rp_init = RP { + rpRanges = array (0,-1) [], + rpMaxRange = 0, -- rpRanges = [] => rfRange = rfOutOfRange + rpOtherRobots = [], + rpBalls = [] + } + + +type RobotWorld = IL Object + +type BallWorld = IL Object + +type RobotTCO = (RobotType, RobotId, [String]) + + +-- Simulation of robots. +-- Arguments: +-- wt ......... World description. Non-robots are ignored. +-- sca ........ Simbot controller for simbots of type A. +-- scb ........ Simbot controller for simbots of type B. +-- sw ......... Static part of the world. +-- +-- Signal inputs: +-- rs ......... The simulated robots. +-- bs ......... The simulated balls. +-- +-- Signal outputs: +-- #1 ......... The simulated robots (really the visible part of their state). +-- #2 ......... Text console output from the robots. + +simRobots :: WorldTemplate + -> SimbotController -> SimbotController + -> StaticWorld + -> SF (RobotWorld, BallWorld) (RobotWorld, [Event RobotTCO]) +simRobots wt sca scb sw = proc rsbs -> do + res <- par interactions (listToIL [sf | Just sf<-map simRobot wt]) -< rsbs + returnA -< (fmap fst res, map snd (ilElems res)) + where + simRobot (OTSimbotA {otRId = rid, otPos = p, otHdng = h}) = + Just (simSimbotA rid p h sca) + simRobot (OTSimbotB {otRId = rid, otPos = p, otHdng = h}) = + Just (simSimbotB rid p h scb) + simRobot _ = Nothing + + -- Computes the interaction with the rest of the world for each robot. + interactions :: (RobotWorld, BallWorld) -> IL sf + -> IL ((RobotPerception, Event ()), sf) + interactions (rs, bs) sfs = mapIL interaction sfs + where + interaction (k, sf) = ((rp, ce), sf) + where + rp = robotPerception k r rs bs sw + ce = robotCollision k r rs sw + r = case lookupIL k rs of + Just r -> r + Nothing -> intErrSim + "simRobots" + "Can't find self in world." + + +-- Robot's percpetion of the world. +-- Arguments: +-- k .......... The key of the robot for which to compute perception. +-- r .......... The robot for which to compute perception. +-- rs ......... The simulated robots. +-- bs ......... The simulated balls. +-- sw ......... Static part of the world. +-- +-- Returns: RobotPerception representing the robot's view of the world. + +robotPerception :: ILKey -> Object -> RobotWorld -> BallWorld -> StaticWorld + -> RobotPerception +robotPerception k r rs bs sw = + RP { + rpRanges = sonarEchoes, + rpMaxRange = smr, + rpOtherRobots = otherRobots (ilAssocs rs), + rpBalls = otherBalls (ilAssocs bs) + } + where + p = objPos r + h = objHdng r + rpp = objRPP r + rr = rppRadius rpp + smr = rppRFMaxRange rpp + n = rppRFN rpp + phi = 2 * pi / fromIntegral n + + -- Balls currently do not show up on the sonar. Too "small". + sonarEchoes :: Array Int Distance + sonarEchoes = accumArray min rfOutOfRange (0, n-1) + (sonarFixedWallEchoes + ++ sonarRobotEchoes + ++ sonarStaticWorldEchoes) + + sonarFixedWallEchoes :: [(Int, Distance)] + sonarFixedWallEchoes = + -- !!! Compiler bug!?! [0.0, phi ..] yields a list of length 2. + -- !!! But it works in GHCi ... + [ (i, d) + | i <- [0 .. n-1], let theta = (fromIntegral i) * phi, + let d = sonarEchoFixedWall p rr (h + theta) phi, d <= smr ] + + sonarRobotEchoes :: [(Int, Distance)] + sonarRobotEchoes = + [ (rfIndex n theta, d) + | (k', r') <- ilAssocs rs, k' /= k, + let (theta, d) = sonarEcho p h rr phi r', d <= smr ] + + sonarStaticWorldEchoes :: [(Int, Distance)] + sonarStaticWorldEchoes = + [ (rfIndex n theta, d) + | obj <- sw, let (theta, d) = sonarEcho p h rr phi obj, + d <= smr ] + + otherRobots :: + [(ILKey, Object)] -> [(RobotType, RobotId, Angle, Distance)] + otherRobots [] = [] + otherRobots ((k', r') : krs) | k == k' = otherRobots krs + | otherwise = (rtp', rid', phi, d) + : otherRobots krs + where + rtp' = objRType r' + rid' = objRId r' + (d, h') = vector2RhoTheta (objPos r' .-. p) + phi = normalizeAngle (h' - h) + + otherBalls :: [(ILKey, Object)] -> [(Angle, Distance)] + otherBalls [] = [] + otherBalls ((_, b) : kbs) = (phi, d) : otherBalls kbs + where + (d, h') = vector2RhoTheta (objPos b .-. p) + phi = normalizeAngle (h' - h) + + +-- Robot's physical interaction with the world. Robots are currently not +-- affected by balls (they are considered "light"). +-- Arguments: +-- k .......... The key of the robot for which to compute interaction. +-- r .......... The robot for which to compute interaction. +-- rs ......... The simulated robots. +-- sw ......... Static part of the world. +-- +-- Returns: Event indicating collision. + +-- !!! Simultaneous hits currently ignored (mergeEvents). +-- !!! We currently do not make use of the impulse from the hit event +-- !!! since robots currently just stops. + +robotCollision :: ILKey -> Object -> RobotWorld -> StaticWorld -> Event () +robotCollision k r rs sw = mergeEvents collisions `tag` () + where + collisions = hitFixedWall r + : [ r `hit` obj | obj <- sw] + ++ [ r `hit` r' | (k', r') <- ilAssocs rs, k /= k' ] + + +------------------------------------------------------------------------------ +-- Simulation of balls +------------------------------------------------------------------------------ + +-- Simulation of balls. +-- Arguments: +-- wt ......... World description. Non-balls are ignored. +-- sw ......... Static part of the world. +-- +-- Signal inputs: +-- rs ......... The simulated robots. +-- bs ......... The simulated balls. +-- +-- Signal outputs: +-- #1 ......... The simulated balls. + +simBalls :: WorldTemplate -> StaticWorld + -> SF (RobotWorld, BallWorld) BallWorld +simBalls wt sw = + par interactions (listToIL [ simBall p | OTBall {otPos = p} <- wt ]) + where + -- Computes the interaction with the rest of the world for each ball. + interactions :: (RobotWorld, BallWorld) ->IL sf + ->IL (Event Velocity2,sf) + interactions (rs, bs) sfs = mapIL interaction sfs + where + interaction (k, sf) = (ballCollision k rs bs sw, sf) + + +-- Robot's physical interaction with the world. Robots are currently not +-- affected by balls (they are considered "light"). +-- Arguments: +-- k .......... The key of the ball for which to compute interaction. +-- rs ......... The simulated robots. +-- bs ......... The simulated balls. +-- sw ......... Static part of the world. +-- +-- Returns: Event carrying collision impulse. + +ballCollision :: ILKey -> RobotWorld -> BallWorld -> StaticWorld + -> Event Velocity2 +ballCollision k rs bs sw = mergeEvents collisions + where + b = case lookupIL k bs of + Just b -> b + Nothing -> intErrSim "ballCollision" + "Can't find ball in world." + + collisions = hitFixedWall b + : [ b `hit` obj | obj <- sw] + ++ [ b `hit` b' | (k', b') <- ilAssocs bs, k /= k' ] + ++ [ b `hit` r | r <- ilElems rs ] + + +------------------------------------------------------------------------------ +-- Simbot simulation +------------------------------------------------------------------------------ + +-- (Mainly) physical properties of a simbot. +data SimbotSpec = SS { + ssRType :: RobotType, -- Robot type identification string. + ssDiameter :: Length, -- Distance between the wheels. + ssAccMax :: Acceleration, -- Maximal translational acceleration. + ssWSMax :: Speed -- Maximal peripheral wheel speed. +} + + +-- Simulation of simbots of type A. +-- Arguments: +-- rid ........ Robot identity +-- p_0 ........ Initial position. +-- h_0 ........ Initial heading. +-- sc ......... Simbot controller. +-- +-- Signal inputs: +-- rp ......... The simbot's perception of the world. +-- ce ......... Collision event. +-- +-- Signal outputs: +-- #1 ......... Simbot object (really the visible part of the simbot state). +-- #2 ......... Text console output. + +simSimbotA :: RobotId -> Position2 -> Heading -> SimbotController + -> SF (RobotPerception, Event ()) (Object, Event RobotTCO) +simSimbotA rid p_0 h_0 sc = proc (rp, ce) -> do + (p, h, v, ems) <- simSimbot rid p_0 h_0 ss sc -< (rp, ce) + returnA -< (simbotA rtp rid False p h v, fmap (\ms -> (rtp,rid,ms)) ems) + where + rtp = simbotARType + ss = SS { + ssRType = rtp, + ssDiameter = simbotADiam, + ssAccMax = simbotAAccMax, + ssWSMax = simbotAWSMax + } + + +-- Simulation of simbots of type B. +-- Arguments: +-- rid ........ Robot identity +-- p_0 ........ Initial position. +-- h_0 ........ Initial heading. +-- sc ......... Simbot controller. +-- +-- Signal inputs: +-- rp ......... The simbot's perception of the world. +-- ce ......... Collision event. +-- +-- Signal outputs: +-- #1 ......... Simbot object (really the visible part of the simbot state). +-- #2 ......... Text console output. + +simSimbotB :: RobotId -> Position2 -> Heading -> SimbotController + -> SF (RobotPerception, Event ()) (Object, Event RobotTCO) +simSimbotB rid p_0 h_0 sc = proc (rp, ce) -> do + (p, h, v, ems) <- simSimbot rid p_0 h_0 ss sc -< (rp, ce) + returnA -< (simbotB rtp rid False p h v, fmap (\ms -> (rtp,rid,ms)) ems) + where + rtp = simbotBRType + ss = SS { + ssRType = rtp, + ssDiameter = simbotBDiam, + ssAccMax = simbotBAccMax, + ssWSMax = simbotBWSMax + } + + +-- Simbot simulation. +-- Arguments: +-- d .......... Robot diameter. +-- a_max ...... Maximal translational acceleration. +-- ws_max ..... Maximal (peripheral) wheel speed. +-- rid ........ Robot identity +-- p_0 ........ Initial position. +-- h_0 ........ Initial heading. +-- ss ......... Simbot specification (physical properties). +-- sc ......... Simbot controller. +-- +-- Signal inputs: +-- rp ......... The simbot's perception of the world. +-- ce ......... Collision event. +-- +-- Signal outputs: +-- #1 ......... Current position. +-- #2 ......... Current heading. +-- #3 ......... Current translational velocity. +-- #4 ......... Text console output. + +-- !!! Maybe "simbot" would be a more apt name in a sense since this really is +-- !!! a constructor for the true Simbots. The object on the output is just +-- !!! the "state". + +-- !!! If simbot controllers return mergeable output, then change to +-- !!! so <- mrFinalize ^<< sc sp <- si () p h + +simSimbot :: RobotId -> Position2 -> Heading -> SimbotSpec -> SimbotController + -> SF (RobotPerception, Event ()) + (Position2, Heading, Velocity2, Event [String]) +simSimbot rid p_0 h_0 ss sc = proc (rp, ce) -> do + -- Note the delay on the sensor inputs. We don't want to allow + -- a controller to be control dependent on its output by just looking + -- at sensor input. I.e. sensor input should always be well defined. + st <- localTime -< () + rec s_pre <- iPre False -< s + p_pre <- iPre p_0 -< p + h_pre <- iPre h_0 -< h + rp_pre <- iPre rp_init -< rp + so <- sc sp -< si st s_pre p_pre h_pre rp_pre + wvs <- wheelVelocities d -< soDM so + (p,h,v) <- simbotDynamics d a_max ws_max p_0 h_0 v_0 -< (wvs, ce) + s <- isStuck -< (ce, v) + returnA -< (p, h, vector2Polar v h, soTCO so) + where + d = ssDiameter ss + a_max = ssAccMax ss + ws_max = ssWSMax ss + v_0 = 0.0 + sp = SP { + spRType = ssRType ss, + spRId = rid, + spDiameter = d, + spAccMax = a_max, + spWSMax = ws_max + } + si st s p h rp = SI { + siSystemTime = st, + siBattStat = BSHigh, + siIsStuck = s, + siPosition = p, + siHeading = h, + siRanges = rpRanges rp, + siMaxRange = rpMaxRange rp, + siOtherRobots = rpOtherRobots rp, + siBalls = rpBalls rp + } + + -- Somewhat complicated ... Note how isNoEvent is used to ensure + -- that the edge detector is guaranteed to see a raising edge even + -- if the velocity never got down to 0. (Using iEdge False would + -- probably lead to a loop.) + isStuck :: SF (Event (), Velocity) Bool + isStuck = + switch (constant False &&& arr fst) $ \_ -> + switch (constant True + &&& (arr (\(ce, v) -> isNoEvent ce && abs v > 0.01) + >>> edge)) $ \_ -> + isStuck + + +-- Potentially stateful conversion from high-level drive mode to low-level +-- control signals, i.e. desired wheel velocities. Makes it possible +-- to use signal functions for implementing high-level control algorithms for +-- high-level control modes, such as position control. See the old simulator +-- for some ideas on different kinds of control modes (although all state +-- less). For advaned modes, it is likely that more input signals are +-- needed, e.g. odometry. + +wheelVelocities :: Length -> SF DriveMode (Velocity, Velocity) +wheelVelocities d = proc ddm -> do + ec <- edgeBy newDM DMBrake -< ddm + wvs <- rSwitch wvBrake -< (ddm, ec) + returnA -< wvs + where + newDM DMBrake DMBrake = Nothing + newDM _ DMBrake = Just wvBrake + newDM (DMDiff {}) (DMDiff {}) = Nothing + newDM _ (DMDiff {}) = Just wvDiff + newDM (DMTR {}) (DMTR {}) = Nothing + newDM _ (DMTR {}) = Just (wvTR d) + + + wvBrake :: SF DriveMode (Velocity, Velocity) + wvBrake = constant (0.0, 0.0) + + -- Differential control, essentially the identity signal function. + wvDiff :: SF DriveMode (Velocity, Velocity) + wvDiff = proc (DMDiff {dmdLWV = v_ld, dmdRWV = v_rd}) -> do + returnA -< (v_ld, v_rd) + + -- Translational and rotational velocity control. + -- d .......... Robot diameter. + wvTR :: Length -> SF DriveMode (Velocity, Velocity) + wvTR d = proc (DMTR {dmtrTV = tv_d, dmtrRV = rv_d}) -> do + returnA -< (tv_d - r * rv_d, tv_d + r * rv_d) + where + r = d / 2 + + +------------------------------------------------------------------------------ +-- Ball simulation +------------------------------------------------------------------------------ + +-- Simulation of simbots of type A. +-- Arguments: +-- p_0 ........ Initial position. +-- +-- Signal inputs: +-- ei ......... Impulse event. Instantaneous change in momentum. Since +-- the mass is constant but implicit, the impulse is represented +-- by an instantaneous change in velocity. +-- +-- Signal outputs: +-- #1 ......... Ball (really the visible part of the ball state). + +simBall :: Position2 -> SF (Event Velocity2) Object +simBall p_0 = proc ei -> do + -- Why delay needed? impulseIntegral too strict? Or should objVel field + -- be non-strict? But the latter seems to lead to a loop ... + -- !!! 2003-01-25: OK, one could argue that impulseIntegral is too + -- !!! strict, but not being strict here would lead to unwanted + -- !!! delays. Another way of looking at the problem is that we + -- !!! probably are trying to compute the size of the impulse + -- !!! which directly will affect the velocity in terms of the + -- !!! very same velocity at the very same point in time. + -- !!! So whoever computes the strength of the impulse, should perhaps + -- !!! use the previous velocity. A simple bouncing ball model shows this. + eid <- iPre noEvent -< ei + (p, v) <- ballDynamics 0.4 0.1 p_0 zeroVector -< eid + returnA -< ball False p v + + +------------------------------------------------------------------------------ +-- Old stuff +------------------------------------------------------------------------------ + +{- +type WCont i a = SigBRef i World -> Cont i a + +sWorld :: SimInput i => SimbotController -> SimbotController -> Cont i World +sWorld rcA rcB w = loopB $ \self -> zListToListZ (map (sObj rsA rsB self) w) + where + rsA = RobotSpec { + rsDiameter = robotADiam, + rsAccMax = robotAAccMax, + rsWSMax = robotAWSMax, + rsAngAccMax = robotAAngAccMax, + rsRDAngles = robotARDAngles, + rsModel = robotModel, + rsController = rcA + } + rsB = RobotSpec { + rsDiameter = robotBDiam, + rsAccMax = robotBAccMax, + rsWSMax = robotBWSMax, + rsAngAccMax = robotBAngAccMax, + rsRDAngles = robotBRDAngles, + rsModel = robotModel, + rsController = rcB + } + + +sWorld2 :: SimInput i => + SimbotModel -> SimbotController -> SimbotModel -> SimbotController -> + Cont i World +sWorld2 rmA rcA rmB rcB w = + loopB $ \self -> zListToListZ (map (sObj rsA rsB self) w) + where + rsA = RobotSpec { + rsDiameter = robotADiam, + rsAccMax = robotAAccMax, + rsWSMax = robotAWSMax, + rsAngAccMax = robotAAngAccMax, + rsRDAngles = robotARDAngles, + rsModel = rmA, + rsController = rcA + } + rsB = RobotSpec { + rsDiameter = robotBDiam, + rsAccMax = robotBAccMax, + rsWSMax = robotBWSMax, + rsAngAccMax = robotBAngAccMax, + rsRDAngles = robotBRDAngles, + rsModel = rmB, + rsController = rcB + } + + +-- Object simulation +sObj :: SimInput i => RobotSpec -> RobotSpec -> WCont i Object +sObj rsA rsB wr obj + | obj `oInClass` ClsObst = lift0 obj -- Obstacles don't move. + | obj `oInClass` ClsRobotA = sRobot rsA wr obj + | obj `oInClass` ClsRobotB = sRobot rsB wr obj + | obj `oInClass` ClsBall = sBall wr obj + | otherwise = simulatorErr "sObj" "Unknown object class." +-} + + +------------------------------------------------------------------------------ +-- Utilities +------------------------------------------------------------------------------ + +intErrSim :: String -> String -> a +intErrSim = intErr "RobotSim.Simulator" diff --git a/src/FRP/YFrob/RobotSim/World.hs b/src/FRP/YFrob/RobotSim/World.hs new file mode 100644 --- /dev/null +++ b/src/FRP/YFrob/RobotSim/World.hs @@ -0,0 +1,51 @@ +{- +****************************************************************************** +* Y F R O B / R O B O T S I M * +* * +* Module: World * +* Purpose: The world representation and related definitions. * +* Author: Henrik Nilsson * +* * +****************************************************************************** +-} + +-- !!! This module should probably go away. It does not seem to make much +-- !!! sense to share "world definitions" between the simulator and the +-- !!! editor. Potentially they'll have different types, and the simulator +-- !!! already contains a type defintion for a "static world". "ObjId"s +-- !!! are currently generated locally for in simulator and at some point in +-- !!! the editor. Thus they are not shared either. Interaction between +-- !!! simulator and editor are in terms of "WorldTemplate"s. Moreover, +-- !!! "newRobotId" presumably belongs squarely in the editor. + +module FRP.YFrob.RobotSim.World ( + ObjId, + World, + newRobotId -- :: ObjClass -> World -> RobotId +) where + +import Data.List ((\\)) + +import FRP.YFrob.Common.Diagnostics (intErr) +import FRP.YFrob.Common.RobotIO (RobotId) + +import FRP.YFrob.RobotSim.IdentityList +import FRP.YFrob.RobotSim.Object + +type ObjId = ILKey + +type World = IL Object + + +newRobotId :: ObjClass -> World -> RobotId +newRobotId objCls world | objCls <: ClsRobot = head ([0..] \\ allRIdsForClass) + | otherwise = intErr "RSWorld" + "newRobotId" + "Bad object class." + where + allRIdsForClass = + mapFindAllIL + (\(_, obj) -> if obj `inClass` objCls then + Just (objRId obj) + else Nothing) + world diff --git a/src/FRP/YFrob/RobotSim/WorldGeometry.hs b/src/FRP/YFrob/RobotSim/WorldGeometry.hs new file mode 100644 --- /dev/null +++ b/src/FRP/YFrob/RobotSim/WorldGeometry.hs @@ -0,0 +1,90 @@ +{- +****************************************************************************** +* Y F R O B / R O B O T S I M * +* * +* Module: WorldGeometry * +* Purpose: Constants and functions defining the geometry of * +* the world. * +* Author: Henrik Nilsson * +* * +****************************************************************************** +-} + +module FRP.YFrob.RobotSim.WorldGeometry where + +import FRP.Yampa.Geometry (Point2(..)) +-- import AFRPTransform2 -- Not yet written +import FRP.YFrob.Common.PhysicalDimensions +import qualified Graphics.HGL as HGL (Point) + + +-- Everything in the world is measured in meters. + +pixelsPerMeter :: YFrobReal +pixelsPerMeter = 60.0 + +pixelsToMeters :: Int -> Length +pixelsToMeters p = (fromIntegral p) / pixelsPerMeter + +metersToPixels :: Length -> Int +metersToPixels m = round (m * pixelsPerMeter) + + +-- The world is assumed to be rectangular. + +worldXMin, worldXMax :: Position +worldYMin, worldYMax :: Position +worldXMin = -5.0 +worldYMin = -5.0 +worldXMax = 5.0 +worldYMax = 5.0 + + +-- World size in pixels. + +worldSizeX, worldSizeY :: Int +worldSizeX = metersToPixels (worldXMax - worldXMin) +worldSizeY = metersToPixels (worldYMax - worldYMin) + + +-- Positions of the walls. + +worldNorthWall, worldSouthWall :: Position +worldEastWall, worldWestWall :: Position +worldNorthWall = worldYMax - 0.2 +worldEastWall = worldXMax - 0.2 +worldSouthWall = worldYMin + 0.2 +worldWestWall = worldXMin + 0.2 + + +-- Co-ordinate translations + +-- Re-visit these definitions if/once affine transforms are introduced in +-- Yampa. +-- Maybe use affine transformations also for the basic conversions HGL.Point +-- <-> Position2? + +{- +pointToPositionT :: Transform2 +pointToPositionT = translate2 (vector2XY worldXMin worldYMax) `compose2` + uscale2 (1 / pixelsPerMeter) `compose2` + mirrorY2 +-} + + +gPointToPosition2 :: HGL.Point -> Position2 +gPointToPosition2 (x, y) = (Point2 (pixelsToMeters x + worldXMin) + (worldYMax - pixelsToMeters y)) + + +{- +positionToPointT :: Transform2 +positionToPointT = uscale2 pixelsPerMeter `compose2` + translate2 (vector2XY (-worldXMin) worldYMax) `compose2` + mirrorY2 +-} + + +position2ToGPoint :: Position2 -> HGL.Point +position2ToGPoint (Point2 x y) = + (metersToPixels (x - worldXMin), metersToPixels (worldYMax - y))