"""Tutorial - Tire model This script demonstrates how to: * Select a simple tire model for a vehicle. Note that the car, motor setup and control setup in this simple example is very basic and is not recommendable, the main instructional focus here is on the tire setup. """ ######################################## # First we need to load script plugins ######################################## # AGX Dynamics imports import agx import agxModel import agxPython import agxCollide import agxOSG # We want graphics, so agxOSG is our choice. import agxIO import agxSDK import agxUtil # Python modules import sys from tutorial_utils import createHelpText # Check if agxIO is built with OIS supprt so we can find a gamepad or joystick ENABLE_STICK = hasattr(agxIO, "JoystickListener") ######################################## # Class for creating a two-body wheel, consisting of tire and hub. # The class inherits from agxModel::TwoBodyTire, which in turn is an agxSDK::Assembly. # innerRadius - the radius of the hub # outerRadius - the outer radius of the tire # breadth - the breadth of the tire # material - the material of the tire # hubMass - the mass of the hub # tireMass - the mass of the hub # rb1 - The second rigid body. # compliance - The compliance for the hinge ######################################## class TwoBodyWheel(agxModel.TwoBodyTire): def __init__( self, innerRadius, outerRadius, breadth, material, hubMass, tireMass, root ): # Create hub body. hubGeo = agxCollide.Geometry(agxCollide.Cylinder(innerRadius, breadth)) hubRb = agx.RigidBody(hubGeo) hubRb.setName("hubRb") hubRb.getMassProperties().setMass(hubMass) agxOSG.createVisual(hubRb, root) hubGeo.setMaterial(material) # Create tire body. tireGeo = agxCollide.Geometry(agxCollide.Cylinder(outerRadius, breadth)) tireRb = agx.RigidBody(tireGeo) tireRb.getMassProperties().setMass(tireMass) tireRb.setName("tireRb") agxOSG.createVisual(tireRb, root) tireGeo.setMaterial(material) # Create the tire model. super().__init__(tireRb, outerRadius, hubRb, innerRadius) # Add RigidBody's to the Assembly to make sure they are added to the simulation assert self.add(hubRb) assert self.add(tireRb) # Set tire parameters. # Note that the values here are not from any real-world data sheet, # but purely example data. # First, friction. # We want the friction in forward direction to be higher than sideways. # For explicit contact materials, we have full control (see also "function createMaterials(sim)") # For implicit friction materials which are auto-computed from the involved materials, we can add # a multiplier. self.setImplicitFrictionMultiplier( agx.Vec2(1.2, 0.8) ) # More forward than sideways. # Then, tire stiffness. In the tire model agxModel.TwoBodyTire, # we can set linear stiffness in 4 directions (unit is Newton). # RADIAL, // Translation orthogonal to rotation axis. # LATERAL, // Translation in rotation axis. # BENDING, // Rotation orthogonal to rotation axis. # TORSIONAL // Rotation in rotation axis. # We use a common multiplier. # The unit for translational stiffness is force/displacement (if using SI: N/m) # The unit for rotational stiffness is torque/angular displacement (if using SI: Nm/rad) stiffness = 5e5 self.setStiffness(stiffness * 0.5, agxModel.TwoBodyTire.RADIAL) self.setStiffness(stiffness, agxModel.TwoBodyTire.LATERAL) self.setStiffness(stiffness * 0.5, agxModel.TwoBodyTire.BENDING) self.setStiffness(stiffness * 0.1, agxModel.TwoBodyTire.TORSIONAL) # Damping can be set in the same dimensions as stiffness. Again, we use a common multiplioer dampingCoefficient = 5000 # The unit for the translational damping coefficient is force * time/displacement (if using SI: Ns/m) # The unit for the rotational damping coefficient is torque * time/angular displacement (if using SI: Nms/rad) self.setDampingCoefficient(dampingCoefficient * 2, agxModel.TwoBodyTire.RADIAL) self.setDampingCoefficient(dampingCoefficient, agxModel.TwoBodyTire.LATERAL) self.setDampingCoefficient(dampingCoefficient * 2, agxModel.TwoBodyTire.BENDING) self.setDampingCoefficient( dampingCoefficient * 10, agxModel.TwoBodyTire.TORSIONAL ) ######################################## # Utility method for creating hinges. # rb0 - The first rigid body. # frame - Relative frame to the first body for the attachment # rb1 - The second rigid body. # compliance - The compliance for the hinge # sim - an agxSDK::Simulation ######################################## def addHinge(rb0, pointRb0, axisRb0, rb1, compliance): assert rb0 assert rb1 frameRb0 = agx.Frame() frameRb1 = agx.Frame() assert agx.Constraint.calculateFramesFromBody( pointRb0, axisRb0, rb0, frameRb0, rb1, frameRb1 ) hinge = agx.Hinge(rb0, frameRb0, rb1, frameRb1) hinge.setCompliance(compliance) hinge.setSolveType(agx.Constraint.DIRECT_AND_ITERATIVE) return hinge ######################################## # Utility method for creating a heightfield for the ground. # sim - an agxSDK::Simulation # root - an agxOSG::Group for graphics # groundMaterial - the physics material of the ground ######################################## def createGroundHeightField(sim, root, groundMaterial): ground = agx.RigidBody("ground") # Create a height field given a texture for relative heights, a size in x and y, and absolute min and max heights. hf = agxUtil.HeightFieldGenerator.createHeightFieldFromFile( "textures/terrain_test/terrain_height.png", 90, 90, -10, 20 ) assert hf groundGeo = agxCollide.Geometry(hf) assert ground.add(groundGeo) groundGeo.setMaterial(groundMaterial) groundNode = agxOSG.createVisual(ground, root) # Also add a graphical texture. agxOSG.setTexture(groundNode, "textures/terrain_test/terrain_detail.png") ground.setMotionControl(agx.RigidBody.STATIC) assert sim.add(ground) # Return the geometry itself, as well as a suggested starting transformation for the vehicle. return ground, agx.AffineMatrix4x4.translate(0, 0, 2.8) # Create a simple "car" for testing. Only an example, no real-world data used. class SimpleCar(agxSDK.Assembly): def __init__(self, root, vehicleStartTransformation, tireMaterial): super().__init__() # Note that we check the return values of calls (using assert), # to make sure they succeeded as intended. innerRadius = 0.3 outerRadius = 0.5 breadth = 0.3 chassisSize = agx.Vec3(3, 1, 0.4) tireMass = 15 hubMass = 10 # Create wheels and position them. wheelFrontLeft = TwoBodyWheel( innerRadius, outerRadius, breadth, tireMaterial, hubMass, tireMass, root ) wheelFrontLeft.setPosition(1, 1, outerRadius) wheelFrontRight = TwoBodyWheel( innerRadius, outerRadius, breadth, tireMaterial, hubMass, tireMass, root ) wheelFrontRight.setPosition(1, -1, outerRadius) wheelBackLeft = TwoBodyWheel( innerRadius, outerRadius, breadth, tireMaterial, hubMass, tireMass, root ) wheelBackLeft.setPosition(-1, 1, outerRadius) wheelBackRight = TwoBodyWheel( innerRadius, outerRadius, breadth, tireMaterial, hubMass, tireMass, root ) wheelBackRight.setPosition(-1, -1, outerRadius) assert self.add(wheelFrontLeft) assert self.add(wheelFrontRight) assert self.add(wheelBackLeft) assert self.add(wheelBackRight) # Create chassis. chassisGeo = agxCollide.Geometry(agxCollide.Box(chassisSize * 0.5)) self.chassisGeo = chassisGeo chassis = agx.RigidBody(chassisGeo) chassis.setName("chassis") agxOSG.createVisual(chassis, root) chassis.setPosition(0, 0, chassisSize.z() * 0.5 + outerRadius * 0.75) # By adding the chassis to the assembly, it will be directly added to # the simulation, since the assembly has been added to the simulation. assert self.add(chassis) chassisGeo.setMaterial(tireMaterial) # Add simple suspension for front axis wheels - bodies with a separate constraint to the chassis. hingeCompliance = 1e-8 frontLeftSuspension = agx.RigidBody( agxCollide.Geometry(agxCollide.Cylinder(0.1, 0.1)) ) assert self.add(frontLeftSuspension) agxOSG.createVisual(frontLeftSuspension, root) frontLeftSuspension.setPosition(1, 0.8, outerRadius) frontRightSuspension = agx.RigidBody( agxCollide.Geometry(agxCollide.Cylinder(0.1, 0.1)) ) assert self.add(frontRightSuspension) agxOSG.createVisual(frontRightSuspension, root) frontRightSuspension.setPosition(1, -0.8, outerRadius) # Disable collisions between suspensions on the one hand and wheel and chassis on the other. agxUtil.setEnableCollisions(frontLeftSuspension, chassis, False) agxUtil.setEnableCollisions(frontRightSuspension, chassis, False) agxUtil.setEnableCollisions(wheelFrontRight, frontRightSuspension, False) agxUtil.setEnableCollisions(wheelFrontLeft, frontLeftSuspension, False) # Set up suspension hinges, allowing for steering. frontLeftSteeringHinge = addHinge( frontLeftSuspension, agx.Vec3(), agx.Vec3(0, 0, 1), chassis, hingeCompliance ) frontRightSteeringHinge = addHinge( frontRightSuspension, agx.Vec3(), agx.Vec3(0, 0, 1), chassis, hingeCompliance, ) self.add(frontLeftSteeringHinge) self.add(frontRightSteeringHinge) frontLeftSteeringHinge.setName("FrontLeftSteeringHinge") frontRightSteeringHinge.setName("FrontRightSteeringHinge") frontLeftSteeringHinge.getLock1D().setEnable(True) frontRightSteeringHinge.getLock1D().setEnable(True) frontLeftSteeringHinge.getRange1D().setEnable(True) frontRightSteeringHinge.getRange1D().setEnable(True) frontLeftSteeringHinge.getRange1D().setRange(-0.5, 0.5) frontRightSteeringHinge.getRange1D().setRange(-0.5, 0.5) # Attach all wheels to chassis/suspension bodies. wheelPoint = agx.Vec3() wheelAxis = agx.Vec3(0, -1, 0) frontLeftHinge = addHinge( wheelFrontLeft.getHubRigidBody(), wheelPoint, wheelAxis, frontLeftSuspension, hingeCompliance, ) frontRightHinge = addHinge( wheelFrontRight.getHubRigidBody(), wheelPoint, wheelAxis, frontRightSuspension, hingeCompliance, ) rearLeftHinge = addHinge( wheelBackLeft.getHubRigidBody(), wheelPoint, wheelAxis, chassis, hingeCompliance, ) rearRightHinge = addHinge( wheelBackRight.getHubRigidBody(), wheelPoint, wheelAxis, chassis, hingeCompliance, ) self.add(frontLeftHinge) self.add(frontRightHinge) self.add(rearLeftHinge) self.add(rearRightHinge) frontLeftHinge.setName("FrontLeftHinge") frontRightHinge.setName("FrontRightHinge") rearLeftHinge.setName("RearLeftHinge") rearRightHinge.setName("RearRightHinge") # Set max motor torque. Constant for this simple model. maxMotorTorque = 5e2 frontRightHinge.getMotor1D().setForceRange(-maxMotorTorque, maxMotorTorque) frontLeftHinge.getMotor1D().setForceRange(-maxMotorTorque, maxMotorTorque) rearRightHinge.getMotor1D().setForceRange(-maxMotorTorque, maxMotorTorque) rearLeftHinge.getMotor1D().setForceRange(-maxMotorTorque, maxMotorTorque) maxSteeringTorque = 1e4 frontRightSteeringHinge.getMotor1D().setForceRange( -maxSteeringTorque, maxSteeringTorque ) frontLeftSteeringHinge.getMotor1D().setForceRange( -maxSteeringTorque, maxSteeringTorque ) self.setTransform(vehicleStartTransformation) # Set up very simple controls. speed = 30 # in m/s self.constraints = [ { "hinge": frontLeftHinge, "keyUp": agxSDK.GuiEventListener.KEY_Up, "keyDown": agxSDK.GuiEventListener.KEY_Down, "axis": 0, "speed": -speed, "locking": False, }, { "hinge": frontRightHinge, "keyUp": agxSDK.GuiEventListener.KEY_Up, "keyDown": agxSDK.GuiEventListener.KEY_Down, "axis": 0, "speed": -speed, "locking": False, }, { "hinge": rearLeftHinge, "keyUp": agxSDK.GuiEventListener.KEY_Up, "keyDown": agxSDK.GuiEventListener.KEY_Down, "axis": 0, "speed": -speed, "locking": False, }, { "hinge": rearRightHinge, "keyUp": agxSDK.GuiEventListener.KEY_Up, "keyDown": agxSDK.GuiEventListener.KEY_Down, "axis": 0, "speed": -speed, "locking": False, }, { "hinge": frontRightSteeringHinge, "keyUp": agxSDK.GuiEventListener.KEY_Left, "keyDown": agxSDK.GuiEventListener.KEY_Right, "axis": 1, "speed": 1, "locking": True, }, { "hinge": frontLeftSteeringHinge, "keyUp": agxSDK.GuiEventListener.KEY_Left, "keyDown": agxSDK.GuiEventListener.KEY_Right, "axis": 1, "speed": 1, "locking": True, }, ] # Create controllers for the constraints in the table, using keyboard as input class ConstraintController: def __init__(self, constraint, key, speed, locking): self.key = key self.constraint = constraint self.speed = speed self.motor = constraint.getMotor1D() self.lock = constraint.getLock1D() self.locking = locking # Called by KeyListener when a keyboard key is pressed/released def keyboard(self, keydown): self.motor.setSpeed(self.speed) self.motor.setEnable(keydown) if self.locking: self.lock.setEnable(not keydown) if not keydown: # stop it self.motor.setSpeed(0) self.lock.setPosition(self.constraint.getAngle()) # Manually called by StickListener when an axis stick is moved def axisMoved(self, scale): stop = abs(scale) < 0.1 if stop: self.motor.setEnable(False) self.lock.setEnable(True) self.motor.setSpeed(0) self.lock.setPosition(self.constraint.getAngle()) else: self.motor.setSpeed(-self.speed * scale) self.motor.setEnable(True) self.lock.setEnable(False) # Create a keyboard listener and bind keyboard keys to KeyControllers class KeyListener(agxSDK.GuiEventListener): def __init__(self, constraints): super().__init__(agxSDK.GuiEventListener.KEYBOARD) # Create a dictionary, where each key corresponds to a keyboard key, # and each value is a list of controllers that should be activated by that key self.keyDict = {} # Loop over all the constraints for c in constraints: hinge = c["hinge"] # Create a list if it does not already exist if c["keyUp"] not in self.keyDict: self.keyDict[c["keyUp"]] = [] if c["keyDown"] not in self.keyDict: self.keyDict[c["keyDown"]] = [] # Assign controllers, one for keyUp and another for keyDown self.keyDict[c["keyUp"]].append( ConstraintController(hinge, c["keyUp"], c["speed"], c["locking"]) ) self.keyDict[c["keyDown"]].append( ConstraintController(hinge, c["keyDown"], -c["speed"], c["locking"]) ) # This method is called each time a keyboard key is pressed def keyboard(self, key, modMask, x, y, keydown): # Is there a controller assigned to this key? if key in self.keyDict: for controller in self.keyDict[key]: controller.keyboard(keydown) # call the controller return True # Return True if keyboard event has been handled return False # Return False if keyboard event should be passed on to AGX # Utility function for attaching a camera to a geometry and following it. def setCamera(geometry, relativeEye, app, root): geometryNode = agxOSG.findGeometryNode(geometry, root) if geometryNode: geoTrans = geometry.getTransform() geoPos = geoTrans.getTranslate() eye = geoPos - relativeEye center = geoPos up = agx.Vec3.Z_AXIS() app.setOrbitCamera(geometryNode, center, eye, up, 2) return True return False ######################################## # Function for setting up materials. # sim - A pointer to an instance of a agxSDK::Simulation ######################################## def createMaterials(sim): # Materials tireMaterial = agx.Material("tireMaterial") groundMaterial = agx.Material("groundMaterial") assert sim.add(tireMaterial) assert sim.add(groundMaterial) tireGroundContactMaterial = agx.ContactMaterial(tireMaterial, groundMaterial) assert sim.add(tireGroundContactMaterial) tireGroundContactMaterial.setFrictionCoefficient( 1.0, agx.ContactMaterial.PRIMARY_DIRECTION ) tireGroundContactMaterial.setFrictionCoefficient( 0.8, agx.ContactMaterial.SECONDARY_DIRECTION ) tireGroundContactMaterial.setRestitution(0) tireGroundFriction = agx.ScaleBoxFrictionModel() tireGroundFriction.setSolveType(agx.FrictionModel.DIRECT_AND_ITERATIVE) tireGroundContactMaterial.setFrictionModel(tireGroundFriction) return tireMaterial, groundMaterial def buildScene1(): # Create simulation context # sim - A pointer to an instance of a agxSDK::Simulation # app - A pointer to an instance of a agxOSG::ExampleApplication # root - A pointer to an instance of agxOSG::Group sim = agxPython.getContext().environment.getSimulation() app = agxPython.getContext().environment.getApplication() root = agxPython.getContext().environment.getSceneRoot() # add text to screen createHelpText(sim, app) global ENABLE_STICK global StickListener try: if ENABLE_STICK: assert agxIO.DeviceManager.instance().init(app.getHWND()) except AssertionError: print( "Disabling joystick/gamepad: Could not detect joystick or gamepad hardware" ) ENABLE_STICK = False if ENABLE_STICK: # Creates a listener for a gamepad/joystick print("Creating joystick/gamepad listener") class StickListener(agxIO.JoystickListener): __instances = list() def __init__(self, constraints): try: self.keyDict = dict() # Keep a reference to this class by having the class reference this instance, # otherwise the class may get garbage collected when its last instance is collected. # This is because of the odd and dangerous locality in which this class is # declared. self.__instances.append(self) # Loop over all the constraints for c in constraints: hinge = c["hinge"] # Create a list if it does not already exist if c["axis"] not in self.keyDict: self.keyDict[c["axis"]] = list() # Assign controllers, one for keyUp and another for keyDown self.keyDict[c["axis"]].append( ConstraintController( hinge, c["axis"], c["speed"], c["locking"] ) ) super().__init__() except Exception as e: print("StickListener failed to init: {}".format(e)) def __del__(self): if self in self.__instances: self.__instances.remove(self) def axisUpdate(self, state, axis): # One axis is moved, but we loop over all of them and set the speed on the controllers for i in range(0, len(state.axes)): if i in self.keyDict: val = state.axes[i] # Ignore small values, and normalize otherwise be between -1 and 1 for controller in self.keyDict[i]: if abs(val) > 100: controller.axisMoved(val / 32768) else: controller.axisMoved(0) return True # Materials tireMaterial, groundMaterial = createMaterials(sim) # Ground ground, vehicleStartTransformation = createGroundHeightField( sim, root, groundMaterial ) # Vehicle car = SimpleCar(root, vehicleStartTransformation, tireMaterial) sim.add(car) # Controls sim.add(KeyListener(car.constraints)) if ENABLE_STICK: try: joystickManager = agxIO.JoystickManager(app.getHWND()) assert sim.add(joystickManager) if not joystickManager.valid(): raise RuntimeError("JoystickManager failed to initialize") stickListener = StickListener(car.constraints) assert joystickManager.add(stickListener) except Exception as e: print("No joystick or gamepad found: {}".format(e)) # Set camera to follow vehicle. setCamera(car.chassisGeo, agx.Vec3(-6, 0, 1), app, root) return root # Entry point if script is launched using agxViewer def buildScene(): app = agxPython.getContext().environment.getApplication() # We just call buildScene1 to create a scene # Multiple scenes can be added with app.addScene, now we add only this one fileName = app.getArguments().getArgumentName(1) app.addScene(fileName, "buildScene1", ord("1"), True) buildScene1() # Main function to be loaded when script is executed using python3 def main(args): # Create an application with graphics etc. app = agxOSG.ExampleApplication() # Create a command line parser. sys.executable will point to python executable # in this case, because getArgumentName(0) needs to match the C argv[0] which # is the name of the program running argParser = agxIO.ArgumentParser([sys.executable] + args) app.addScene(argParser.getArgumentName(1), "buildScene", ord("1"), True) # Call the init method of ExampleApplication # It will setup the viewer, windows etc. if app.init(argParser): app.run() else: print("An error occurred while initializing ExampleApplication.") # Entry point when this script is loaded with python if agxPython.getContext() is None: init = agx.AutoInit() main(sys.argv)