diff --git a/simpylc/simulations/car_manual_flex_track/control_client/manual_client.py b/simpylc/simulations/car_manual_flex_track/control_client/manual_client.py new file mode 100644 index 0000000..6cc40b9 --- /dev/null +++ b/simpylc/simulations/car_manual_flex_track/control_client/manual_client.py @@ -0,0 +1,123 @@ +''' +====== Legal notices + +Copyright (C) 2013 - 2021 GEATEC engineering + +This program is free software. +You can use, redistribute and/or modify it, but only under the terms stated in the QQuickLicense. + +This program is distributed in the hope that it will be useful, +but WITHOUT ANY WARRANTY, without even the implied warranty of +MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. +See the QQuickLicense for details. + +The QQuickLicense can be accessed at: http://www.qquick.org/license.html + +__________________________________________________________________________ + + + THIS PROGRAM IS FUNDAMENTALLY UNSUITABLE FOR CONTROLLING REAL SYSTEMS !! + +__________________________________________________________________________ + +It is meant for training purposes only. + +Removing this header ends your license. +''' + +import time as tm +import traceback as tb +import math as mt +import sys as ss +import os +import socket as sc +import curses as cs + +ss.path += [os.path.abspath (relPath) for relPath in ('..',)] + +import socket_wrapper as sw +import parameters as pm + +class ManualClient: + def __init__ (self): + self.steeringAngle = 0 + self.targetVelocity = 0 + + self.io = cs.initscr () # take over terminal + cs.noecho () # do not update terminal + cs.cbreak () # set direct mode (no enter needed) + self.io.keypad (True) # convert to key codes + + with open (pm.sampleFileName, 'w') as self.sampleFile: + with sc.socket (*sw.socketType) as self.clientSocket: + self.clientSocket.connect (sw.address) + self.socketWrapper = sw.SocketWrapper (self.clientSocket) + self.halfApertureAngle = False + + while True: + self.input () + self.process () + self.output () + self.logTraining () + tm.sleep (0.02) + + def input (self): + sensors = self.socketWrapper.recv () + + if not self.halfApertureAngle: + self.halfApertureAngle = sensors ['halfApertureAngle'] + self.sectorAngle = 2 * self.halfApertureAngle / pm.lidarInputDim + self.halfMiddleApertureAngle = sensors ['halfMiddleApertureAngle'] + + if 'lidarDistances' in sensors: + self.lidarDistances = sensors ['lidarDistances'] + else: + self.sonarDistances = sensors ['sonarDistances'] + + def process (self): + key = self.io.getch () + if key == cs.KEY_UP: + self.targetVelocity += pm.speedDelta + elif key == cs.KEY_DOWN: + self.targetVelocity -= pm.speedDelta + elif key == cs.KEY_RIGHT: + self.steeringAngle += pm.steerDelta + elif key == cs.KEY_LEFT: + self.steeringAngle -= pm.steerDelta + + def output (self): + actuators = { + 'steeringAngle': self.steeringAngle, + 'targetVelocity': self.targetVelocity + } + + self.socketWrapper.send (actuators) + + def logLidarTraining (self): + sample = [pm.finity for entryIndex in range (pm.lidarInputDim + 1)] + + for lidarAngle in range (-self.halfApertureAngle, self.halfApertureAngle): + sectorIndex = round (lidarAngle / self.sectorAngle) + sample [sectorIndex] = min (sample [sectorIndex], self.lidarDistances [lidarAngle]) + + sample [-2] = self.steeringAngle + sample [-1] = self.targetVelocity + print (*sample, file = self.sampleFile) + + def logSonarTraining (self): + sample = [pm.finity for entryIndex in range (pm.sonarInputDim + 1)] + + for entryIndex, sectorIndex in ((2, -1), (0, 0), (1, 1)): + sample [entryIndex] = self.sonarDistances [sectorIndex] + + sample [-2] = self.steeringAngle + sample [-1] = self.targetVelocity + print (*sample, file = self.sampleFile) + + def logTraining (self): + if hasattr (self, 'lidarDistances'): + self.logLidarTraining () + else: + self.logSonarTraining () + +ManualClient () diff --git a/simpylc/simulations/car_manual_flex_track/control_client/parameters.py b/simpylc/simulations/car_manual_flex_track/control_client/parameters.py new file mode 100644 index 0000000..ac56eaa --- /dev/null +++ b/simpylc/simulations/car_manual_flex_track/control_client/parameters.py @@ -0,0 +1,39 @@ +''' +====== Legal notices + +Copyright (C) 2013 - 2021 GEATEC engineering + +This program is free software. +You can use, redistribute and/or modify it, but only under the terms stated in the QQuickLicense. + +This program is distributed in the hope that it will be useful, +but WITHOUT ANY WARRANTY, without even the implied warranty of +MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. +See the QQuickLicense for details. + +The QQuickLicense can be accessed at: http://www.qquick.org/license.html + +__________________________________________________________________________ + + + THIS PROGRAM IS FUNDAMENTALLY UNSUITABLE FOR CONTROLLING REAL SYSTEMS !! + +__________________________________________________________________________ + +It is meant for training purposes only. + +Removing this header ends your license. +''' + +finity = 20.0 # Needs to be float to obtain ditto numpy array + +lidarInputDim = 16 +sonarInputDim = 3 + +speedDelta = +0.2 +steerDelta = -5.0 + +sampleFileName = 'default.samples' + +def getTargetVelocity (steeringAngle): + return (90 - abs (steeringAngle)) / 60 diff --git a/simpylc/simulations/car_manual_flex_track/control_server.py b/simpylc/simulations/car_manual_flex_track/control_server.py new file mode 100644 index 0000000..d302e1c --- /dev/null +++ b/simpylc/simulations/car_manual_flex_track/control_server.py @@ -0,0 +1,62 @@ +''' +====== Legal notices + +Copyright (C) 2013 - 2021 GEATEC engineering + +This program is free software. +You can use, redistribute and/or modify it, but only under the terms stated in the QQuickLicense. + +This program is distributed in the hope that it will be useful, +but WITHOUT ANY WARRANTY, without even the implied warranty of +MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. +See the QQuickLicense for details. + +The QQuickLicense can be accessed at: http://www.qquick.org/license.html + +__________________________________________________________________________ + + + THIS PROGRAM IS FUNDAMENTALLY UNSUITABLE FOR CONTROLLING REAL SYSTEMS !! + +__________________________________________________________________________ + +It is meant for training purposes only. + +Removing this header ends your license. +''' + +import socket as sc +import time as tm + +import simpylc as sp +import socket_wrapper as sw + +class ControlServer: + def __init__ (self): + with sc.socket (*sw.socketType) as serverSocket: + serverSocket.bind (sw.address) + serverSocket.listen (sw.maxNrOfConnectionRequests) + + while True: + self.clientSocket = serverSocket.accept ()[0] + self.socketWrapper = sw.SocketWrapper (self.clientSocket) + + with self.clientSocket: + while True: + sensors = { + 'halfApertureAngle': sp.world.visualisation.scanner.halfApertureAngle, + 'halfMiddleApertureAngle': sp.world.visualisation.scanner.halfMiddleApertureAngle + } | ( + {'lidarDistances': sp.world.visualisation.scanner.lidarDistances} + if hasattr (sp.world.visualisation.scanner, 'lidarDistances') else + {'sonarDistances': sp.world.visualisation.scanner.sonarDistances} + ) + + self.socketWrapper.send (sensors) + tm.sleep (0.02) + actuators = self.socketWrapper.recv () + + sp.world.physics.steeringAngle.set (actuators ['steeringAngle']) + sp.world.physics.targetVelocity.set (actuators ['targetVelocity']) + + \ No newline at end of file diff --git a/simpylc/simulations/car_manual_flex_track/dimensions.py b/simpylc/simulations/car_manual_flex_track/dimensions.py new file mode 100644 index 0000000..2758d42 --- /dev/null +++ b/simpylc/simulations/car_manual_flex_track/dimensions.py @@ -0,0 +1,39 @@ +''' +====== Legal notices + +Copyright (C) 2013 - 2021 GEATEC engineering + +This program is free software. +You can use, redistribute and/or modify it, but only under the terms stated in the QQuickLicense. + +This program is distributed in the hope that it will be useful, +but WITHOUT ANY WARRANTY, without even the implied warranty of +MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. +See the QQuickLicense for details. + +The QQuickLicense can be accessed at: http://www.qquick.org/license.html + +__________________________________________________________________________ + + + + THIS PROGRAM IS FUNDAMENTALLY UNSUITABLE FOR CONTROLLING REAL SYSTEMS !! + +__________________________________________________________________________ + +It is meant for training purposes only. + +Removing this header ends your license. +''' + +import simpylc as sp + +wheelDiameter = 0.10 +wheelRadius = wheelDiameter / 2 +displacementPerWheelAngle = sp.radiansPerDegree * wheelRadius + +wheelBase = 0.40 +wheelShift = wheelBase / 2 + +apertureAngle = 120 +middleApertureAngle = 45 \ No newline at end of file diff --git a/simpylc/simulations/car_manual_flex_track/easy.track b/simpylc/simulations/car_manual_flex_track/easy.track new file mode 100644 index 0000000..951880c --- /dev/null +++ b/simpylc/simulations/car_manual_flex_track/easy.track @@ -0,0 +1,32 @@ +- - - - - - - - - - - - - * * * - - - - - - - - - - - - - - - - +- - - - - - - * * * * * * - - - * * * - - - - - - - - - - - - - +- - - - - - * - - - - - - - - - - - - * * * * - - - - - - - - - +- - - - - * - - - - - - - - - - - - - - - - - * - - - - - - - - +- - - - * - - - - - - - - * * * - - - - @ - - - * - - - - - - - +- - - * - - - - * * * * * - - - * * * - - - - - - * - - - - - - +- - * - - - - * - - - - - - - - - - - * * * - - - - * - - - - - +- * - - - - * - - - - - - - - - - - - - - - * - - - - * - - - - +* - - - - * - - - - - - - - - - - - - - - - - * - - - - * - - - +* - - - * - - - - - - - - - - - - - - - - - - - * - - - - * - - +* - - - * - - - - - - - - - - - - - - - - - - - - * - - - - * - +* - - - * - - - - - - - - - - - - - - - - - - - - - * - - - * - +- * - - - * - - - - - - - - - - - - - - - - - - - - * - - - * - +- * - - - * - - - - - - - - - - - - - - - - - - - - - * - - - * +- * - - - * - - - - - - - - - - - - - - - - - - - - - * - - - * +- - * - - - * - - - - - - - - - - - - - - - - - - - - * - - - * +- - * - - - * - - - - - - - - - - - - - - - - - - - - * - - - * +- - * - - - * - - - - - - - - - - - - - - - - - - - - * - - - * +- * - - - * - - - - - - - - - - - - - - - - - - - - * - - - * - +- * - - - * - - - - - - - - - - - - - - - - - - - * - - - * - - +- * - - - * - - - - - - - - - - - - - - - - - - * - - - -*- - - +* - - - * - - - - - - - - - - - - - - - - - - * - - - - * - - - +* - - - * - - - - - - - - - - - - - - - - - * - - - - * - - - - +* - - - * - - - - - - - - - - * * * * * * * - - - - * - - - - - +- * - - - * - - - - - - - - * - - - - - - - - - - * - - - - - - +- * - - - * - - - - - - - * - - - - - - - - - - * - - - - - - - +- * - - - - * - - - - - * - - - - - - - - - - * - - - - - - - - +- - * - - - - * - - - * - - - - * * * * * * * - - - - - - - - - +- - - * - - - - * * * - - - - * - - - - - - - - - - - - - - - - +- - - - * - - - - - - - - - * - - - - - - - - - - - - - - - - - +- - - - - * - - - - - - - * - - - - - - - - - - - - - - - - - - +- - - - - - * * * * * * * - - - - - - - - - - - - - - - - - - - diff --git a/simpylc/simulations/car_manual_flex_track/hard.track b/simpylc/simulations/car_manual_flex_track/hard.track new file mode 100644 index 0000000..3912850 --- /dev/null +++ b/simpylc/simulations/car_manual_flex_track/hard.track @@ -0,0 +1,32 @@ +- - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - +- - - - - - - * - - - * - - - - - - - - - - - - - - - - - - - - +- - - - - - - - - - - - - - * - - - - - - - - - - - * - - - - - +- - - * - - - * - - * - - - - - - - * - - - * - - - - - - - * - +- - - - - * - - - - - - - * - - - - - - @ - - - - * - - - - - - +- - - - - - - - - - - - - - - - - * - - - - * - - - - - * - - - +- - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - +- * - * - - - - - - - - - - - - - - - - - - - - - - - - - - * - +- - - - - - - - - - - - - - - - - - - - - - - - - - - - * - - - +- - - - - - - - - - * - - * - - - - - - - - - - - - - - - - - - +- - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - +- - * - * - - * - - - - * - - - - * - - - - - - - - - - * - * - +- - - - - - - - - * - - - - - * - - - - - - - - - - - - - - - - +- - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - +- * - * - - - - - - - - - - - - - - - - - - - - - - - - - - - - +- - - - - - - * - * - - - - - - - * - * - - - - - - - * - * - - +- - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - +- - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - +- * - * - - - - * - * - - - - - - - - - - - - - - - - - - - - - +- - - - - - - - - - - - - - - - - * - * - - - - - - * - * - - - +- - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - +- - - - - - - - - * - * - - - - - - - - - - - - - - - - - - - - +- - * - * - - - - - - - - - - - - - - - - - - - - - - - - - - - +- - - - - - - - - - - - - - - - - * - * - - - - - - * - * - - - +- - - - - - - - - * - * - - - - - - - - - - - - - - - - - - - - +- - - * - - - - - - - - - - - - - - - - - - - - - - - - - - - - +- * - - - - - - - - - - - - - - - - - - * - - - - - - - - - - - +- - - - * - - - - * - - - - - - - - * - - - - - - - - * - * - - +- - - - - - * - * - - * - - - - - - - - - * - - - - - - - - - - +- - * - - - - - - - - - - - - - - - - - - - - - * - * - - - - - +- - - - - - * - - * - - - - - - - - - - * - - - - - - - * - - - +- - - - - - - - - - - - - - - - - - - - - - - - * - - - - - - - diff --git a/simpylc/simulations/car_manual_flex_track/physics.py b/simpylc/simulations/car_manual_flex_track/physics.py new file mode 100755 index 0000000..da39e4d --- /dev/null +++ b/simpylc/simulations/car_manual_flex_track/physics.py @@ -0,0 +1,141 @@ +''' +====== Legal notices + +Copyright (C) 2013 - 2021 GEATEC engineering + +This program is free software. +You can use, redistribute and/or modify it, but only under the terms stated in the QQuickLicense. + +This program is distributed in the hope that it will be useful, +but WITHOUT ANY WARRANTY, without even the implied warranty of +MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. +See the QQuickLicense for details. + +The QQuickLicense can be accessed at: http://www.qquick.org/license.html + +__________________________________________________________________________ + + + THIS PROGRAM IS FUNDAMENTALLY UNSUITABLE FOR CONTROLLING REAL SYSTEMS !! + +__________________________________________________________________________ + +It is meant for training purposes only. + +Removing this header ends your license. +''' + +import simpylc as sp + +import dimensions as dm + +class Physics (sp.Module): + def __init__ (self): + sp.Module.__init__ (self) + + self.page ('car physics dashboard') + + self.group ('wheels', True) + + self.acceleration = sp.Register (2) + + self.targetVelocity= sp.Register () + self.velocity = sp.Register () + + self.steeringAngle = sp.Register () + + self.group ('position', True) + + self.attitudeAngle = sp.Register (50) + self.courseAngle = sp.Register () + + self.positionX = sp.Register () + self.positionY = sp.Register () + + self.group('slip', True) + + self.radialAcceleration = sp.Register () + self.slipping = sp.Marker () + + self.group ('camera') + + self.soccerView = sp.Latch (True) + self.heliView = sp.Latch () + self.driverView = sp.Latch () + + self.driverFocusDist = sp.Register (2) + + self.page ('car physics internals') + + self.group ('wheels', True) + + self.midWheelAngularVelocity = sp.Register () + self.midWheelAngle = sp.Register (30) + + self.midSteeringAngle = sp.Register () + self.inverseMidCurveRadius = sp.Register (20) + self.midAngularVelocity = sp.Register () + + self.group ('position', True) + + self.tangentialVelocity = sp.Register () + self.velocityX = sp.Register () + self.velocityY = sp.Register () + + self.group('slip', True) + self.radialVelocity = sp.Register () + + self.group ('camera') + + self.soccerViewOneshot = sp.Oneshot () + self.heliViewOneshot = sp.Oneshot () + self.driverViewOneshot = sp.Oneshot () + + self.driverFocusX = sp.Register () + self.driverFocusY = sp.Register () + + def sweep (self): + self.page ('car physics') + + self.group ('speed') + + self.velocity.set (self.velocity + self.acceleration * sp.world.period, self.velocity < self.targetVelocity, self.velocity - self.acceleration * sp.world.period) + self.midWheelAngularVelocity.set (self.velocity / dm.displacementPerWheelAngle) + self.midWheelAngle.set (self.midWheelAngle + self.midWheelAngularVelocity * sp.world.period) + self.tangentialVelocity.set (self.midWheelAngularVelocity * dm.displacementPerWheelAngle) + + self.group ('direction') + + self.midSteeringAngle.set (sp.atan (0.5 * sp.tan (self.steeringAngle))) + + self.inverseMidCurveRadius.set (sp.sin (self.midSteeringAngle) / dm.wheelShift) + self.midAngularVelocity.set (sp.degreesPerRadian * self.tangentialVelocity * self.inverseMidCurveRadius) + + self.attitudeAngle.set (self.attitudeAngle + self.midAngularVelocity * sp.world.period) + self.courseAngle.set (self.attitudeAngle + self.midSteeringAngle) + + self.radialAcceleration.set (sp.max (abs (self.tangentialVelocity * self.tangentialVelocity * self.inverseMidCurveRadius) - 0.5, 0)) + self.slipping.mark (sp.abs (self.radialAcceleration) > 0.55) + self.radialVelocity.set (self.radialVelocity + self.radialAcceleration * sp.world.period, self.slipping, 0) + + self.velocityX.set (self.tangentialVelocity * sp.cos (self.courseAngle) + self.radialVelocity * sp.sin (self.courseAngle)) + self.velocityY.set (self.tangentialVelocity * sp.sin (self.courseAngle) + self.radialVelocity * sp.cos (self.courseAngle)) + + self.group ('position') + + self.positionX.set (self.positionX + self.velocityX * sp.world.period) + self.positionY.set (self.positionY + self.velocityY * sp.world.period) + + self.group ('camera') + + self.soccerView.unlatch (self.heliViewOneshot or self.driverViewOneshot) + self.heliView.unlatch (self.soccerViewOneshot or self.driverViewOneshot) + self.driverView.unlatch (self.soccerViewOneshot or self.heliViewOneshot) + + self.soccerViewOneshot.trigger (self.soccerView) + self.heliViewOneshot.trigger (self.heliView) + self.driverViewOneshot.trigger (self.driverView) + + self.driverFocusX.set (self.positionX + self.driverFocusDist * sp.cos (self.attitudeAngle)) + self.driverFocusY.set (self.positionY + self.driverFocusDist * sp.sin (self.attitudeAngle)) + \ No newline at end of file diff --git a/simpylc/simulations/car_manual_flex_track/socket_wrapper.py b/simpylc/simulations/car_manual_flex_track/socket_wrapper.py new file mode 100644 index 0000000..be0049a --- /dev/null +++ b/simpylc/simulations/car_manual_flex_track/socket_wrapper.py @@ -0,0 +1,69 @@ +''' +====== Legal notices + +Copyright (C) 2013 - 2021 GEATEC engineering + +This program is free software. +You can use, redistribute and/or modify it, but only under the terms stated in the QQuickLicense. + +This program is distributed in the hope that it will be useful, +but WITHOUT ANY WARRANTY, without even the implied warranty of +MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. +See the QQuickLicense for details. + +The QQuickLicense can be accessed at: http://www.qquick.org/license.html + +__________________________________________________________________________ + + + THIS PROGRAM IS FUNDAMENTALLY UNSUITABLE FOR CONTROLLING REAL SYSTEMS !! + +__________________________________________________________________________ + +It is meant for training purposes only. + +Removing this header ends your license. +''' + +import socket as sc +import json as js + +address = 'localhost', 50012 +socketType = sc.AF_INET, sc.SOCK_STREAM +maxNrOfConnectionRequests = 5 +maxMessageLength = 2048 + +class SocketWrapper: + def __init__ (self, clientSocket): + self.clientSocket = clientSocket + + def send (self, anObject): + buffer = bytes (f'{js.dumps (anObject):<{maxMessageLength}}', 'ascii') + + totalNrOfSentBytes = 0 + + while totalNrOfSentBytes < maxMessageLength: + nrOfSentBytes = self.clientSocket.send (buffer [totalNrOfSentBytes:]) + + if not nrOfSentBytes: + self.raiseConnectionError () + + totalNrOfSentBytes += nrOfSentBytes + + def recv (self): + totalNrOfReceivedBytes = 0 + receivedChunks = [] + + while totalNrOfReceivedBytes < maxMessageLength: + receivedChunk = self.clientSocket.recv (maxMessageLength - totalNrOfReceivedBytes) + + if not receivedChunk: + self.raiseConnectionError () + + receivedChunks.append (receivedChunk) + totalNrOfReceivedBytes += len (receivedChunk) + + return js.loads (b''.join (receivedChunks) .decode ('ascii')) + + def raiseConnectionError (self): + raise RuntimeError ('Socket connection broken') diff --git a/simpylc/simulations/car_manual_flex_track/visualisation.py b/simpylc/simulations/car_manual_flex_track/visualisation.py new file mode 100644 index 0000000..6e9f680 --- /dev/null +++ b/simpylc/simulations/car_manual_flex_track/visualisation.py @@ -0,0 +1,238 @@ +''' +====== Legal notices + +Copyright (C) 2013 - 2021 GEATEC engineering + +This program is free software. +You can use, redistribute and/or modify it, but only under the terms stated in the QQuickLicense. + +This program is distributed in the hope that it will be useful, +but WITHOUT ANY WARRANTY, without even the implied warranty of +MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. +See the QQuickLicense for details. + +The QQuickLicense can be accessed at: http://www.qquick.org/license.html + +__________________________________________________________________________ + + + THIS PROGRAM IS FUNDAMENTALLY UNSUITABLE FOR CONTROLLING REAL SYSTEMS !! + +__________________________________________________________________________ + +It is meant for training purposes only. + +Removing this header ends your license. +''' + +''' + + z + | + o -- y + / + x + +''' + +import random as rd +import os + +import simpylc as sp + +import dimensions as dm + +normalFloorColor = (0, 0.001, 0) +collisionFloorColor = (1, 0, 0.3) +normalTireColor = (0.03, 0.03, 0.03) + +class Scanner (sp.Cylinder): + def __init__ (self, apertureAngle, middleApertureAngle, obstacles, **arguments): + super () .__init__ (color = (0, 0, 0), **arguments) + + self.apertureAngle = apertureAngle + self.halfApertureAngle = self.apertureAngle // 2 + + self.middleApertureAngle = middleApertureAngle + self.halfMiddleApertureAngle = self.middleApertureAngle // 2 + + self.obstacles = obstacles + + if scannerType == 'lidar': + self.lidarDistances = [sp.finity for angle in range (self.apertureAngle)] # 0, ..., halfApertureAngle - 1, -halfApertureAngle, ..., -1 + else: + self.sonarDistances = [sp.finity for sectorIndex in range (3)] + + def scan (self, mountPosition, mountAngle): + if scannerType == 'lidar': + self.lidarDistances = [sp.finity for angle in range (self.apertureAngle)] + else: + self.sonarDistances = [sp.finity for sectorIndex in range (3)] + + for obstacle in self.obstacles: + relativePosition = sp.tSub (obstacle.center, mountPosition) + distance = sp.tNor (relativePosition) + absoluteAngle = sp.atan2 (relativePosition [1], relativePosition [0]) + relativeAngle = (round (absoluteAngle - mountAngle) + 180) % 360 - 180 + + if -self.halfApertureAngle <= relativeAngle < self.halfApertureAngle - 1: # In case of coincidence, favor nearby obstacle + if scannerType == 'lidar': + self.lidarDistances [relativeAngle] = round (min (distance, self.lidarDistances [relativeAngle]), 4) + else: + sectorIndex = ( + -1 + if relativeAngle < -self.halfMiddleApertureAngle else + 0 + if relativeAngle < self.halfMiddleApertureAngle else + 1 + ) + + self.sonarDistances [sectorIndex] = round (min (distance, self.sonarDistances [sectorIndex]), 4) + +class Line (sp.Cylinder): + def __init__ (self, **arguments): + super () .__init__ (size = (0.01, 0.01, 0), axis = (1, 0, 0), angle = 90, color = (0, 1, 1), **arguments) + +class BodyPart (sp.Beam): + def __init__ (self, **arguments): + super () .__init__ (color = (0.7, 0, 0), **arguments) + +class Wheel: + def __init__ (self, **arguments): + self.suspension = sp.Cylinder (size = (0.01, 0.01, 0.001), axis = (1, 0, 0), angle = 90, pivot = (0, 0, 1), **arguments) + self.rim = sp.Beam (size = (0.08, 0.06, 0.02), pivot = (0, 1, 0), color = (0.2, 0, 0)) + self.tire = sp.Cylinder (size = (dm.wheelDiameter, dm.wheelDiameter, 0.04), axis = (1, 0, 0), angle = 90, color = normalTireColor) + self.line = Line () + + def __call__ (self, wheelAngle, slipping, steeringAngle = 0): + return self.suspension (rotation = steeringAngle, parts = lambda: + self.rim (rotation = wheelAngle, parts = lambda: + self.tire (color = (rd.random (), 0.5 * rd.random (), 0.5 * rd.random ()) if slipping else normalTireColor) + + self.line () + ) ) + +class Window (sp.Beam): + def __init__ (self, **arguments): + super () .__init__ (axis = (0, 1, 0), color = (0, 0, 0.2), **arguments) + +class Floor (sp.Beam): + side = 16 + spacing = 0.5 + halfSteps = round (0.5 * side / spacing) + + class Stripe (sp.Beam): + def __init__ (self, **arguments): + super () .__init__ (size = (0.004, Floor.side, 0.001), **arguments) + + def __init__ (self, **arguments): + super () .__init__ (size = (self.side, self.side, 0.0005), color = normalFloorColor) + self.xStripes = [self.Stripe (center = (0, nr * self.spacing, 0.0001), angle = 90, color = (0, 0.004, 0)) for nr in range (-self.halfSteps, self.halfSteps)] + self.yStripes = [self.Stripe (center = (nr * self.spacing, 0, 0), color = (0, 0.004, 0)) for nr in range (-self.halfSteps, self.halfSteps)] + + def __call__ (self, parts): + return super () .__call__ (color = collisionFloorColor if self.scene.collided else normalFloorColor, parts = lambda: + parts () + + sum (xStripe () for xStripe in self.xStripes) + + sum (yStripe () for yStripe in self.yStripes) + ) + +class Visualisation (sp.Scene): + def __init__ (self): + super () .__init__ () + self.roadCones = [] + trackFileName = 'easy.track' if trackType == 'easy' else 'hard.track' + + with open (f'{os.path.dirname (os.path.abspath (__file__))}/{trackFileName}') as trackFile: + track = trackFile.readlines () + + for rowIndex, row in enumerate (track): + for columnIndex, column in enumerate (row): + if column == '*': + self.roadCones.append (sp.Cone ( + size = (0.07, 0.07, 0.15), + center = (columnIndex / 4 - 7.75, rowIndex / 2 - 7.75, 0.15), + color = (1, 0.3, 0), + group = 1 + )) + elif column == "@": + self.startX = columnIndex / 4 - 8 + self.startY = rowIndex / 2 - 8 + self.init = True + + self.camera = sp.Camera () + + self.floor = Floor (scene = self) + + self.fuselage = BodyPart (size = (0.65, 0.165, 0.09), center = (0, 0, 0.07), pivot = (0, 0, 1), group = 0) + self.fuselageLine = Line () + + self.wheelFrontLeft = Wheel (center = (dm.wheelShift, 0.08, -0.02)) + self.wheelFrontRight = Wheel (center = (dm.wheelShift, -0.08, -0.02)) + + self.wheelRearLeft = Wheel (center = (-dm.wheelShift, 0.08, -0.02)) + self.wheelRearRight = Wheel (center = (-dm.wheelShift, -0.08, -0.02)) + + self.cabin = BodyPart (size = (0.20, 0.16, 0.06), center = (-0.06, 0, 0.07)) + self.windowFront = Window (size = (0.045, 0.158, 0.14), center = (0.15, 0, -0.025), angle = -56) + self.windowRear = Window (size = (0.042, 0.158, 0.18), center = (-0.18, 0, -0.025),angle = 72) + + self.scanner = Scanner (dm.apertureAngle, dm.middleApertureAngle, self.roadCones, size = (0.02, 0.02, 0.03), center = (0.05, 0, 0.03)) + + def display (self): + if self.init: + self.init = False + sp.world.physics.positionX.set (self.startX) + sp.world.physics.positionY.set (self.startY) + + if sp.world.physics.soccerView: + self.camera ( + position = sp.tEva ((sp.world.physics.positionX + 2, sp.world.physics.positionY, 2)), + focus = sp.tEva ((sp.world.physics.positionX + 0.001, sp.world.physics.positionY, 0)) + ) + elif sp.world.physics.heliView: + self.camera ( + position = sp.tEva ((0.0000001, 0, 20)), + focus = sp.tEva ((0, 0, 0)) + ) + elif sp.world.physics.driverView: + self.camera ( + position = sp.tEva ((sp.world.physics.positionX, sp.world.physics.positionY, 1)), + focus = sp.tEva ((sp.world.physics.driverFocusX, sp.world.physics.driverFocusY, 0)) + ) + + self.floor (parts = lambda: + self.fuselage (position = (sp.world.physics.positionX, sp.world.physics.positionY, 0), rotation = sp.world.physics.attitudeAngle, parts = lambda: + self.cabin (parts = lambda: + self.windowFront () + + self.windowRear () + + self.scanner () + ) + + + self.wheelFrontLeft ( + wheelAngle = sp.world.physics.midWheelAngle, + slipping = sp.world.physics.slipping, + steeringAngle = sp.world.physics.steeringAngle + ) + + self.wheelFrontRight ( + wheelAngle = sp.world.physics.midWheelAngle, + slipping = sp.world.physics.slipping, + steeringAngle = sp.world.physics.steeringAngle + ) + + + self.wheelRearLeft ( + wheelAngle = sp.world.physics.midWheelAngle, + slipping = sp.world.physics.slipping + ) + + self.wheelRearRight ( + wheelAngle = sp.world.physics.midWheelAngle, + slipping = sp.world.physics.slipping + ) + + + self.fuselageLine () + ) + + + sum (roadCone () for roadCone in self.roadCones) + ) + + if hasattr (self.fuselage, 'position'): + self.scanner.scan (self.fuselage.position, self.fuselage.rotation) diff --git a/simpylc/simulations/car_manual_flex_track/world.py b/simpylc/simulations/car_manual_flex_track/world.py new file mode 100644 index 0000000..10a9367 --- /dev/null +++ b/simpylc/simulations/car_manual_flex_track/world.py @@ -0,0 +1,49 @@ +''' +====== Legal notices + +Copyright (C) 2013 - 2021 GEATEC engineering + +This program is free software. +You can use, redistribute and/or modify it, but only under the terms stated in the QQuickLicense. + +This program is distributed in the hope that it will be useful, +but WITHOUT ANY WARRANTY, without even the implied warranty of +MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. +See the QQuickLicense for details. + +The QQuickLicense can be accessed at: http://www.qquick.org/license.html + +__________________________________________________________________________ + + + THIS PROGRAM IS FUNDAMENTALLY UNSUITABLE FOR CONTROLLING REAL SYSTEMS !! + +__________________________________________________________________________ + +It is meant for training purposes only. + +Removing this header ends your license. +''' + +import os +import sys as ss +import time as tm + +ss.path += [os.path.abspath (relPath) for relPath in ('../../..', '..')] # If you want to store your simulations somewhere else, put SimPyLC in your PYTHONPATH environment variable +scannerType = 'lidar' if input ('Lidar or sonar : ') == 'l' else 'sonar' # Should be done prior to any SimPyLC related imports due to concurrency +trackType = 'easy' if input ('Easy or hard track : ') == 'e' else 'hard' # Should be done prior to any SimPyLC related imports due to concurrency + +import simpylc as sp + +import control_server as cs +import physics as ps +import visualisation as vs + +vs.scannerType = scannerType +vs.trackType = trackType + +sp.World ( + cs.ControlServer, + ps.Physics, + vs.Visualisation +)