YFrob (empty) → 0.4
raw patch · 33 files changed
+5162/−0 lines, 33 filesdep +HGLdep +Yampadep +arraysetup-changed
Dependencies added: HGL, Yampa, array, base
Files
- LICENSE +29/−0
- Setup.hs +2/−0
- YFrob.cabal +59/−0
- afp-demos/AFPDemos.hs +127/−0
- afp-demos/ITest.hs +103/−0
- afp-demos/Main.hs +83/−0
- afp-demos/Makefile +22/−0
- afp-demos/Setup.hs +2/−0
- afp-demos/YFrobAFPDemos.cabal +26/−0
- robotsim-test/Main.hs +221/−0
- robotsim-test/Makefile +22/−0
- robotsim-test/Setup.hs +2/−0
- robotsim-test/YFrobRobotSimTest.cabal +21/−0
- src/FRP/YFrob/Common.hs +34/−0
- src/FRP/YFrob/Common/Diagnostics.hs +19/−0
- src/FRP/YFrob/Common/PhysicalDimensions.hs +129/−0
- src/FRP/YFrob/Common/RobotIO.hs +246/−0
- src/FRP/YFrob/RobotSim.hs +185/−0
- src/FRP/YFrob/RobotSim/Animate.hs +208/−0
- src/FRP/YFrob/RobotSim/ColorBindings.hs +38/−0
- src/FRP/YFrob/RobotSim/Colors.hs +150/−0
- src/FRP/YFrob/RobotSim/Command.hs +41/−0
- src/FRP/YFrob/RobotSim/IO.hs +190/−0
- src/FRP/YFrob/RobotSim/IdentityList.hs +162/−0
- src/FRP/YFrob/RobotSim/Object.hs +557/−0
- src/FRP/YFrob/RobotSim/ObjectPhysics.hs +616/−0
- src/FRP/YFrob/RobotSim/ObjectTemplate.hs +148/−0
- src/FRP/YFrob/RobotSim/Parser.hs +632/−0
- src/FRP/YFrob/RobotSim/RenderFixedWalls.hs +80/−0
- src/FRP/YFrob/RobotSim/RenderObject.hs +169/−0
- src/FRP/YFrob/RobotSim/Simulator.hs +698/−0
- src/FRP/YFrob/RobotSim/World.hs +51/−0
- src/FRP/YFrob/RobotSim/WorldGeometry.hs +90/−0
+ LICENSE view
@@ -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.
+ Setup.hs view
@@ -0,0 +1,2 @@+import Distribution.Simple+main = defaultMain
+ YFrob.cabal view
@@ -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
+ afp-demos/AFPDemos.hs view
@@ -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)))))+
+ afp-demos/ITest.hs view
@@ -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+ +
+ afp-demos/Main.hs view
@@ -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++++
+ afp-demos/Makefile view
@@ -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}
+ afp-demos/Setup.hs view
@@ -0,0 +1,2 @@+import Distribution.Simple+main = defaultMain
+ afp-demos/YFrobAFPDemos.cabal view
@@ -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
+ robotsim-test/Main.hs view
@@ -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<he) || ((-pi<he) && (he<0)) + then -rMax else rMax+ returnA -< mrFinalize (ddVelTR (vMax/2) {- he -} rot)++rcHeading1 :: Velocity -> 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<he) || ((-pi<he) && (he<0)) + then -rMax else rMax+ returnA -< mrFinalize (ddVelTR (vMax/2) rot)++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)++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)
+ robotsim-test/Makefile view
@@ -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}
+ robotsim-test/Setup.hs view
@@ -0,0 +1,2 @@+import Distribution.Simple+main = defaultMain
+ robotsim-test/YFrobRobotSimTest.cabal view
@@ -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
+ src/FRP/YFrob/Common.hs view
@@ -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
+ src/FRP/YFrob/Common/Diagnostics.hs view
@@ -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)
+ src/FRP/YFrob/Common/PhysicalDimensions.hs view
@@ -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
+ src/FRP/YFrob/Common/RobotIO.hs view
@@ -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
+ src/FRP/YFrob/RobotSim.hs view
@@ -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
+ src/FRP/YFrob/RobotSim/Animate.hs view
@@ -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))
+ src/FRP/YFrob/RobotSim/ColorBindings.hs view
@@ -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
+ src/FRP/YFrob/RobotSim/Colors.hs view
@@ -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
+ src/FRP/YFrob/RobotSim/Command.hs view
@@ -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.
+ src/FRP/YFrob/RobotSim/IO.hs view
@@ -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})+-}
+ src/FRP/YFrob/RobotSim/IdentityList.hs view
@@ -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] ]
+ src/FRP/YFrob/RobotSim/Object.hs view
@@ -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"
+ src/FRP/YFrob/RobotSim/ObjectPhysics.hs view
@@ -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"
+ src/FRP/YFrob/RobotSim/ObjectTemplate.hs view
@@ -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]+-}
+ src/FRP/YFrob/RobotSim/Parser.hs view
@@ -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 <robot>+ creates a robot, where <robot> can be:+ a -- a robot of type Simbot A (round)+ b -- a robot of type Simbot B (triangular)++ co <obstacle>+ creates an obstacle, where <obstacle> can be:+ block -- a quare block+ nswall -- a north-south wall+ ewwall -- an east-west wall++ cb+ creates a ball++ select <objects>+ adds zero or more objects to the current selection, where+ <objects> 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 <angle-in-degrees>+ 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 = []
+ src/FRP/YFrob/RobotSim/RenderFixedWalls.hs view
@@ -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)+-}
+ src/FRP/YFrob/RobotSim/RenderObject.hs view
@@ -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)+-}
+ src/FRP/YFrob/RobotSim/Simulator.hs view
@@ -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"
+ src/FRP/YFrob/RobotSim/World.hs view
@@ -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
+ src/FRP/YFrob/RobotSim/WorldGeometry.hs view
@@ -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))