def init(visual, timestep):
""" Initialize ROS2 and various other things common to all agx simulations """
try:
rclpy.init()
except Exception:
if not rclpy.ok():
print("Init error, not because of multiple init")
if visual:
app = agxPython.getContext().environment.getApplication()
sim = agxPython.getContext().environment.getSimulation()
root = agxPython.getContext().environment.getSceneRoot()
else:
app = []
sim = agxSDK.Simulation()
root = []
sim.setTimeStep(timestep)
if visual:
setup_camera(app)
sim.add(GuiListener(app, timestep))
sim.getRenderManager().setScaleFactor(0.03)
app.getSceneDecorator().setBackgroundColor(agxRender.Color.SkyBlue(), agxRender.Color.DodgerBlue())
agx.setNumThreads(2)
return sim, root
def AddObjectByAPI(system_list, Object):
sim = agxPython.getContext().environment.getSimulation()
app = agxPython.getContext().environment.getApplication()
root = agxPython.getContext().environment.getSceneRoot()
sim.add(Object)
return
def buildArena(arena_pos):
sim = agxPython.getContext().environment.getSimulation()
app = agxPython.getContext().environment.getApplication()
root = agxPython.getContext().environment.getSceneRoot()
arena_size = [width, width, 0.2]
h = 0.35
floor = agx.RigidBody(
agxCollide.Geometry(
agxCollide.Box(arena_size[0] / 2, arena_size[1] / 2,
arena_size[2] / 2)))
floor.setPosition(arena_pos[0], arena_pos[1],
arena_pos[2] - arena_size[2] / 2)
floor.setMotionControl(1)
sim.add(floor)
agxOSG.setDiffuseColor(agxOSG.createVisual(floor, root),
agxRender.Color.Gray())
# Octagon sides
sides = 8
skip_sides = [9]
side_len = width / (1 + np.sqrt(2)) + arena_size[2] / 2 / np.sqrt(2)
base_pos = agx.Vec3(arena_pos[0], arena_pos[1],
arena_pos[2] - arena_size[2] / 2 + h / 2)
for w in range(sides):
if w not in skip_sides:
theta = -w * np.pi / 4
rot = agx.Quat(theta, agx.Vec3(0, 0, 1))
rot_pos = agx.Vec3(
np.sin(theta) * width / 2, -np.cos(theta) * width / 2, 0)
wall = agx.RigidBody(
agxCollide.Geometry(
agxCollide.Box(side_len / 2, arena_size[2] / 2, h / 2)))
wall.setPosition(base_pos + rot_pos)
wall.setMotionControl(1)
wall.setRotation(rot)
sim.add(wall)
agxOSG.setDiffuseColor(agxOSG.createVisual(wall, root),
agxRender.Color.DarkGray())
# Ramp up to the course
ramp_dim = [1.4, side_len, 0.2] # *np.cos(np.pi/4)
ramp = agx.RigidBody(
agxCollide.Geometry(
agxCollide.Box(ramp_dim[0] / 2, ramp_dim[1] / 2, ramp_dim[2] / 2)))
theta = -np.arcsin(ramp_dim[2] / ramp_dim[0]) / 2
ramp.setPosition(
arena_pos[0] - arena_size[0] / 2 - ramp_dim[0] / 2 * np.cos(theta) -
ramp_dim[2] / 2 * np.sin(theta), arena_pos[1],
arena_pos[2] - arena_size[2] * 3 / 4) # +arena_size[1]/2-ramp_dim[1]/2
ramp.setRotation(agx.Quat(theta, agx.Vec3(0, 1, 0)))
ramp.setMotionControl(1)
sim.add(ramp)
agxOSG.setDiffuseColor(agxOSG.createVisual(ramp, root),
agxRender.Color.Gray())
obstacles(sim, root, arena_pos)
def add_cylinderShape(MiroSystem, radius, height, density, pos, texture='test.jpg', scale=[1,1], Collide=True, Fixed=True, rotX=0, rotY=0, rotZ=0, rotOrder=['x','y','z'], rotAngle=0, rotAxis=[1,0,0], rotDegrees=True, color=[0.5, 0.5, 0.5]):
'''system, size_x, size_y, size_z, pos, texture, scale = [5,5], hitbox = True/False'''
# Convert position to chrono vector, supports using chvector as input as well
agxSim = agxPython.getContext().environment.getSimulation()
agxApp = agxPython.getContext().environment.getApplication()
agxRoot = agxPython.getContext().environment.getSceneRoot()
agxPos = agxVecify(pos)
agxRotAxis = agxVecify(rotAxis)
scale = scaleLimit(scale)
# Create a cylinder
body_geo = agxCollide.Geometry(agxCollide.Cylinder(radius, height))
body_geo.setName("body")
body_geo.setEnableCollisions(Collide)
body_cylinder = agx.RigidBody(body_geo)
body_cylinder.getMassProperties().setMass(body_geo.calculateVolume()*density)
if Fixed:
body_cylinder.setMotionControl(1)
body_cylinder.setPosition(agxPos)
rotateBody(body_cylinder, rotX=-90)
rotateBody(body_cylinder, rotX, rotY, rotZ, rotOrder, rotAngle, rotAxis, rotDegrees)
# Collision shape
# if(Collide):
# Visualization shape
body_shape = agxOSG.createVisual(body_cylinder, agxRoot)
# Body texture
if texture:
# Filter 'textures/' out of the texture name, it's added later
if len(texture) > len('textures/'):
if texture[0:len('textures/')] == 'textures/':
texture = texture[len('textures/'):]
if TEXTURES_ON:
if texture not in LOADED_TEXTURES.keys():
agxTex = agxOSG.createTexture(TEXTURE_PATH+texture)
LOADED_TEXTURES.update({texture: agxTex})
agxOSG.setTexture(body_shape, LOADED_TEXTURES[texture], True, agxOSG.DIFFUSE_TEXTURE, -scale[0], scale[1])
else:
color = backupColor(texture, color)
texture = False
if not texture:
agxColor = agxRender.Color(color[0], color[1], color[2])
agxOSG.setDiffuseColor(body_shape, agxColor)
if len(color) > 3:
agxOSG.setAlpha(body_shape, color[3])
agxSim.add(body_cylinder)
return body_cylinder
def add_OneBodyTire(MiroSystem, radius_tire, width, pos, density_tire=500, texture='test.jpg', scale=[1,1], Collide=True, Fixed=False, rotX=90, rotY=0, rotZ=0, rotOrder=['x','y','z'], rotAngle=0, rotAxis=[1,0,0], rotDegrees=True, color=[0.5, 0.5, 0.5]):
tire_body = add_cylinderShape(False, radius_tire, width, density_tire, pos, texture, scale, Collide, Fixed, rotX, rotY, rotZ, rotOrder, rotAngle, rotAxis, rotDegrees, color)
tire = agxModel.OneBodyTire(tire_body, radius_tire)
tire.setImplicitFrictionMultiplier(agx.Vec2(1.5, 0.5))
agxSim = agxPython.getContext().environment.getSimulation()
agxApp = agxPython.getContext().environment.getApplication()
agxRoot = agxPython.getContext().environment.getSceneRoot()
agxSim.add(tire)
return tire_body
def __init__(self, MiroSystem, useRealTime=True):
super().__init__(agxSDK.StepEventListener.POST_STEP)
self.system = MiroSystem
self.app = agxPython.getContext().environment.getApplication()
self.lasers = []
self.checks = []
self.complete = False
self.started = False
self.useRealTime = useRealTime
self.bestTime = False
self.lapNr = 0
self.timeLog = []
agxPython.getContext().environment.getSimulation().add(self)
agxPython.getContext().environment.getSimulation().add(TimerReset(self))
def PreSetup(args, SetupFunction):
if agxPython.getContext():
return
init = agx.AutoInit()
## Create an application with graphics etc.
app = agxOSG.ExampleApplication()
dec = app.getSceneDecorator()
dec.setBackgroundColor(agx.Vec3(1,1,1))
dec.setLogoFile('textures/TF-loader.png')
dec.setLogoLocation(agxOSG.SceneDecorator.FREE)
width = 0.25
dec.setLogoPosition(0.5-width/2, 0.3)
dec.setMaximumLogoDimension(width, 1.0)
## 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.")
def stop_video_capture():
""" Stop the video capture process (FFMPEG). This allows a new video to be created. """
app = agxPython.getContext().environment.getApplication()
vc = app.getVideoServerCapture()
vc.stopCapture()
vc.stopProcess()
app.setAllowWindowResizing(True)
def CreateLidar1D(lidar_body, nr_of_beams, angle, reach):
sim = agxPython.getContext().environment.getSimulation()
# lidar = LidarSensor1D(lidar_body, nr_of_beams, np.deg2rad(angle), range, draw_lines=True)
lidar = LidarSensor1D(sim, lidar_body.getPosition(), agxVecify([-1,0,0]), nr_of_beams, np.deg2rad(angle), reach, lidar_body, True)
# sim, lidar_body.getPosition(), agxVecify([0,1,0]), nr_of_beams, angle, reach, lidar_body, True
# (sim, world_position, world_direction, num_side_rayst, rad_range_side, max_length, rb_origin, draw_lines):
sim.add(lidar)
return lidar
def SetupSystem():
if agxPython.getContext():
sim = agxPython.getContext().environment.getSimulation()
app = agxPython.getContext().environment.getApplication()
root = agxPython.getContext().environment.getSceneRoot()
# Run on maximum threads, pasted from ice_floe example
agx.setNumThreads(0)
n = agx.getNumThreads()*0.5-1
agx.setNumThreads(int(n))
dec = app.getSceneDecorator()
dec.setEnableLogo(False)
skybox = agxOSG.SkyBox('Campus','skybox/sky_','.jpg')
root.addChild(skybox)
return [sim, app, root]
return []
def start_video_capture():
""" Start capturing video """
app = agxPython.getContext().environment.getApplication()
app.setupVideoCaptureRenderTotexture()
app.setAllowWindowResizing(False)
vc = app.getVideoServerCapture()
vc.setFilename("agx_movie")
vc.setEnableSyncWithSimulation(True)
vc.setVideoFPS(30)
vc.setImageCaptureFPS(30)
vc.startCapture()
def buildScene():
sim = agxPython.getContext().environment.getSimulation()
app = agxPython.getContext().environment.getApplication()
root = agxPython.getContext().environment.getSceneRoot()
dec = app.getSceneDecorator()
dec.setEnableLogo(False)
arena.buildArena(sim, root)
bot_pos = [-6, 0, -0.2]
if players == 2:
bot1_pos = [bot_pos[0] - 0.35, bot_pos[1], bot_pos[2]]
bot2_pos = [bot_pos[0] + 0.35, bot_pos[1], bot_pos[2]]
bot1 = robot.buildBot(sim,
root,
bot1_pos,
controller='Arrows',
drivetrain=drivetrain)
bot2 = robot.buildBot(sim,
root,
bot2_pos,
controller='Numpad',
drivetrain=drivetrain,
color=agxRender.Color.Cyan())
cameraData = app.getCameraData()
cameraData.eye = agx.Vec3(15, 0, 12)
cameraData.center = agx.Vec3(2.25, 0, 0)
cameraData.up = agx.Vec3(0, 0, 1)
cameraData.nearClippingPlane = 0.1
cameraData.farClippingPlane = 5000
app.applyCameraData(cameraData)
else:
botBody = robot.buildBot(sim,
root,
bot_pos,
controller='Arrows',
drivetrain=drivetrain)
sim.add(camera.FollowCam(app, botBody))
def addGround(MiroSystem, size_x, size_y, size_z, pos, heightmap, texture='test.jpg', scale=[4,3], Collide=True, Fixed=True, rotX=0, rotY=0, rotZ=0, rotOrder=['x','y','z'], rotAngle=0, rotAxis=[1,0,0], rotDegrees=True, mass=False, density=1000, dynamic=False, color=[0.5, 0.5, 0.5]):
agxSim = agxPython.getContext().environment.getSimulation()
agxApp = agxPython.getContext().environment.getApplication()
agxRoot = agxPython.getContext().environment.getSceneRoot()
# Create the ground
ground_material = agx.Material("Ground")
# Create the height field from a heightmap
hf = agxCollide.HeightField.createFromFile("textures/"+heightmap, size_x, size_z, 0, size_y)
ground_geometry = agxCollide.Geometry(hf)
ground = agx.RigidBody(ground_geometry)
ground.setPosition(agxVecify(pos))
ground.setMotionControl(agx.RigidBody.STATIC)
node = agxOSG.createVisual( ground, agxRoot )
agxOSG.setShininess(node, 5)
# Add a visual texture.
agxOSG.setTexture(node, "textures/"+texture, True, agxOSG.DIFFUSE_TEXTURE, 100, 100)
agxSim.add(ground)
def createBeam(self, pos, length, rotY):
agxSim = agxPython.getContext().environment.getSimulation()
agxApp = agxPython.getContext().environment.getApplication()
agxRoot = agxPython.getContext().environment.getSceneRoot()
agxPos = agxVecify(pos)
self.laser_geo = agxCollide.Geometry(agxCollide.Cylinder(0.004, length))
self.laser_geo.setName("body")
# self.laser_geo.setEnableCollisions(False)
self.laser_body = agx.RigidBody(self.laser_geo)
self.laser_body.setMotionControl(1)
self.laser_body.setPosition(agxPos)
rotateBody(self.laser_body, rotY=rotY, rotDegrees=False)
# Visualization shape
self.laser_vis = agxOSG.createVisual(self.laser_body, agxRoot)
agxOSG.setAlpha(self.laser_vis, 0.5)
agxSim.add(self.laser_body)
agxSim.add(self)
def add_TwoBodyTire(MiroSystem, radius_rim, radius_tire, width, pos, density_rim=1000, density_tire= 500, stiffness=10^5, damping=10^3, material = None, texture='test.jpg', scale=[1,1], Collide=True, Fixed=False, rotX=90, rotY=0, rotZ=0, rotOrder=['x','y','z'], rotAngle=0, rotAxis=[1,0,0], rotDegrees=True, color=[0.5, 0.5, 0.5]):
agxSim = agxPython.getContext().environment.getSimulation()
agxApp = agxPython.getContext().environment.getApplication()
agxRoot = agxPython.getContext().environment.getSceneRoot()
rim_body = add_cylinderShape(False, radius_rim, width, density_rim, pos, texture, scale, Collide, Fixed, rotX, rotY, rotZ, rotOrder, rotAngle, rotAxis, rotDegrees, color)
tire_body = add_cylinderShape(False, radius_tire, width, density_tire, pos, texture, scale, Collide, Fixed, rotX, rotY, rotZ, rotOrder, rotAngle, rotAxis, rotDegrees, color)
# if material == None:
# tire_material = agx.Material('TireMaterial')
# rim_body.getGeometry("").setMaterial(tire_material)
# tire_body.getGeometry("").setMaterial(tire_material)
# agxSim.add(tire_material)
# else:
# rim_body.getGeometry().setMaterial(material)
# tire_body.getGeometry().setMaterial(material)
# agxSim.add(tire_material)
tire = agxModel.TwoBodyTire(tire_body, radius_tire, rim_body, radius_rim)
tire.setStiffness(stiffness*2, agxModel.TwoBodyTire.RADIAL)
tire.setStiffness(stiffness*10, agxModel.TwoBodyTire.LATERAL)
tire.setStiffness(stiffness*4, agxModel.TwoBodyTire.BENDING)
tire.setStiffness(stiffness*0.1, agxModel.TwoBodyTire.TORSIONAL)
tire.setDampingCoefficient(damping*2, agxModel.TwoBodyTire.RADIAL)
tire.setDampingCoefficient(damping*10, agxModel.TwoBodyTire.LATERAL)
tire.setDampingCoefficient(damping*4, agxModel.TwoBodyTire.BENDING)
tire.setDampingCoefficient(damping*0.1, agxModel.TwoBodyTire.TORSIONAL)
tire.setImplicitFrictionMultiplier(agx.Vec2(1.5, 0.5))
agxSim.add(tire)
return [rim_body, tire_body]
def post(self, time):
# Check if all checkpoints have been reached
isComplete = True
self.time = time
self.app.getSceneDecorator().setText(0, 'Current time: '+self.getTimeStr())
N = len(self.checks)
for i in range(N):
if not self.checks[i]:
if self.lasers[i].getTriggered():
if i == 0:
self.startTime = self.getTime()
self.started = True
times = [-1]*N
times[0] = 0
self.timeLog.append(times) # append list of -1s
if self.started:
self.checks[i] = True
app = agxPython.getContext().environment.getApplication()
if i > 0:
self.timeLog[self.lapNr][i] = self.getTime() - self.startTime
self.printLap(self.lapNr)
# self.app.getSceneDecorator().setText(i, str(i)+': '+self.getTimeStr(self.timeLog[self.lapNr][i]))
if i < N-1:
self.lasers[i+1].setActive(True)
else:
self.app.getSceneDecorator().setText(0, 'Clock has not been started.')
if not self.checks[i]:
isComplete = False
if isComplete and not self.complete:
self.complete = True
lapTime = self.getTime() - self.startTime
if not self.bestTime or lapTime < self.bestTime:
self.bestTime = lapTime
# print(self.getTimeStr(self.timeLog[self.lapNr][4]))
self.app.getSceneDecorator().setText(len(self.checks)+1, 'Last lap: '+self.getTimeStr(lapTime))
self.printLap(self.lapNr, len(self.checks)+1)
self.app.getSceneDecorator().setText(2*(len(self.checks)+1), 'Best lap: '+self.getTimeStr(self.bestTime))
self.lapNr = self.lapNr + 1
self.reset()
return
def update(self, x, y):
# pylint: disable=line-too-long
sim = agxPython.getContext().environment.getSimulation()
agx.Statistics.instance().setEnable(True)
step_forward = agx.Statistics.instance().getTimingInfo("Simulation", "Step forward time")
interstep = agx.Statistics.instance().getTimingInfo("Simulation", "Inter-step time")
self.avg_time.update(step_forward.current)
self.avg_interstep.update(interstep.current)
num_islands = agx.Statistics.instance().getTimingInfo("DynamicsSystem", "Num solve islands")
if num_islands.current > -1:
self.app.getSceneDecorator().setText(0, "Simulation time : {0:1.2f}".format(self.avg_time.get()))
self.app.getSceneDecorator().setText(1, "Interstep time : {0:1.2f}".format(self.avg_interstep.get()))
self.app.getSceneDecorator().setText(2, "Total time : {0:1.2f}".format(self.avg_interstep.get()+self.avg_time.get()))
self.app.getSceneDecorator().setText(3, "Timestep : {0:1.4f}".format(self.timestep))
self.app.getSceneDecorator().setText(4, "Number of simulation islands: {0:1.0f}".format(num_islands.current))
self.app.getSceneDecorator().setText(5, "Number of threads : {}".format(agx.getNumThreads()))
self.app.getSceneDecorator().setText(6, "% of real time : {0:1.0f}".format((self.avg_interstep.get()+self.avg_time.get()) / self.timestep / 10))
self.app.getSceneDecorator().setText(7, "AMOR: : {}".format(sim.getMergeSplitHandler().getEnable()))
def init_app(**kwargs):
"""Initialize AGX and ExampleApplication.
Template:
init = init_app( name = __name__,
scenes = [ ( 'buildScene', '1' ) ] )
IMPORTANT:
- agx.AutoInit is returned at MUST be captured! Otherwise AGX will crash during exit.
- The name has to be __name__ in the script you're calling this function.
Examples:
from agxPythonModules.utils.environment import init_app
init = init_app( name = __name__,
scenes = [ ( buildScene, '1' ),
( 'anotherScene', agxSDK.GuiEventListener.KEY_F1 ) ],
autoStepping = True, # Default: False
onInitialized = lambda app: print( 'App successfully initialized.' ),
onShutdown = lambda app: print( 'App successfully shut down.' ) )
Arguments:
name: str -- __name__ of the script this function is called from
scenes: [] -- List of scene tuples (scene name or function, string key or int), e.g.,
[ (buildScene1, '1'), 'buildScene2', ord( '2' ) ]
onInitialized: callable -- Callback when ExampleApplication has been initialized. This callback
is only fired once - i.e., not when reloading the script.
autoStepping: bool -- False to start paused. Default False.
Returns:
agx.AutoInit -- Instance of agx.AutoInit that MUST be captured.
"""
## Entry point when this script is loaded with python
if kwargs['name'] == "__main__":
if agxPython.getContext() is None:
init = agx.AutoInit()
_applicationMain(**kwargs)
return init
return None
def reset(self):
""" Resets simulation to make it possible to rerun simulation without having to restart agx and ROS """
if self.scene_settings.visual:
self.scene_settings.sim.remove(self.step_listener)
del self.step_listener
self.scene_settings.sim.cleanup(self.scene_settings.sim.CLEANUP_ALL, True)
self.scene_settings.listener_pool.reset()
#Rebuild everything
self.scene = Scene(self.scene_settings)
self.scene_ref = None
self.ticks = 0.0
if self.scene_settings.visual:
self.step_listener = StepListener(self.step)
self.scene_settings.sim.add(self.step_listener)
self.scene_settings.sim.add(GuiListener(
agxPython.getContext().environment.getApplication(),
self.scene_settings.timestep))
self.reset_ack = 1.0
def createLink(self, simulation, mesh, color):
filename = "data/models/robots/Generic/" + mesh + ".obj"
linkRb = agx.RigidBody(filename)
mesh = agxUtil.createTrimeshFromWavefrontOBJ(
filename, agxCollide.Trimesh.NO_WARNINGS, agx.Matrix3x3(),
agx.Vec3())
if mesh is None:
print("Unable to find file: " + filename)
return None
renderData = agxUtil.createRenderDataFromWavefrontOBJ(
filename, agx.Matrix3x3(), agx.Vec3())
mesh.setRenderData(renderData)
render_material = agxCollide.RenderMaterial()
render_material.setDiffuseColor(color)
renderData.setRenderMaterial(render_material)
meshGeom = agxCollide.Geometry(mesh)
linkRb.add(meshGeom)
agxOSG.createVisual(meshGeom,
agxPython.getContext().environment.getSceneRoot())
simulation.add(linkRb)
agxUtil.addGroup(linkRb, self.robotGroupId)
return linkRb
def app() -> agxOSG.ExampleApplication:
return agxPython.getContext().environment.getApplication()
def sim() -> agxSDK.Simulation:
return agxPython.getContext().environment.getSimulation()
bot_pos,
controller='Arrows',
drivetrain=drivetrain)
sim.add(camera.FollowCam(app, botBody))
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() == None:
init = agx.AutoInit()
main(sys.argv)
def MiniCam(MiroSystem):
sim = agxPython.getContext().environment.getSimulation()
app = agxPython.getContext().environment.getApplication()
sim.add(SideViewer(MiroSystem, sim, app))
def root() -> osg.Group:
return agxPython.getContext().environment.getSceneRoot()
plt.show()
exit()
fig = plt.figure(figsize=[6.4, 2.8])
ax = fig.add_subplot(1, 1, 1)
# ax.plot(np.array([-8,5]),np.array([0,0]), color = 'black')
ax.plot(x_gt_mpc_1[0, :, 0], x_gt_mpc_1[0, :, 1], color = 'lightsteelblue')
ax = agx_sim.mpcc.draw_soil(ax,-5, 5)
ax = agx_sim.mpcc.draw_path(ax, -10, -5)
ax.set_ylabel(r'$\mathit{y}$ $(m)$')
ax.set_xlabel(r'$\mathit{x}$ $(m)$')
ax.axis('equal')
plt.tight_layout()
ax.set_xlim(-4.5,0.0)
ax.set_ylim(np.amin(x_gt_mpc_1[0, :, 1])-0.5, np.amax(x_gt_mpc_1[0, :, 1]) + 0.5)
plt.show()
# Entry point when this script is loaded with python
if agxPython.getContext() is None:
init = agx.AutoInit()
main(sys.argv)
def add_boxShape(MiroSystem, size_x, size_y, size_z, pos, texture=False, scale=[4,3], Collide=True, Fixed=True, rotX=0, rotY=0, rotZ=0, rotOrder=['x','y','z'], rotAngle=0, rotAxis=[1,0,0], rotDegrees=True, mass=False, density=1000, dynamic=False, color=[0.5, 0.5, 0.5], friction=False):
'''system, size_x, size_y, size_z, pos, texture, scale = [5,5], hitbox = True/False'''
# Convert position to chrono vector, supports using chvector as input as well
agxSim = agxPython.getContext().environment.getSimulation()
agxApp = agxPython.getContext().environment.getApplication()
agxRoot = agxPython.getContext().environment.getSceneRoot()
agxPos = agxVecify(pos)
agxRotAxis = agxVecify(rotAxis)
[size_x, size_y, size_z] = xyzTransform([size_x, size_y, size_z], True)
scale = [scale[0]/4, scale[1]/3]
# Create a box
body_geo = agxCollide.Geometry( agxCollide.Box(size_x/2, size_y/2, size_z/2))
body_geo.setName("body")
if friction:
high_friction_tires = agx.Material('Tires', 0.05, friction)
body_geo.setMaterial(high_friction_tires)
body_geo.setEnableCollisions(Collide)
body_box = agx.RigidBody(body_geo)
if mass:
body_box.getMassProperties().setMass(mass)
else:
body_box.getMassProperties().setMass(body_geo.calculateVolume()*density)
if Fixed:
body_box.setMotionControl(1)
body_box.setPosition(agxPos)
rotateBody(body_box, rotX, rotY, rotZ, rotOrder, rotAngle, rotAxis, rotDegrees)
# Collision shape
# if(Collide):
# Visualization shape
body_shape = agxOSG.createVisual(body_box, agxRoot)
# Body texture
if texture:
# Filter 'textures/' out of the texture name, it's added later
if len(texture) > len('textures/'):
if texture[0:len('textures/')] == 'textures/':
texture = texture[len('textures/'):]
if TEXTURES_ON or texture in important_textures:
if texture not in LOADED_TEXTURES.keys():
agxTex = agxOSG.createTexture(TEXTURE_PATH+texture)
LOADED_TEXTURES.update({texture: agxTex})
if TEXTURE_PATH == 'textures_lowres/' and texture=='yellow_brick.jpg':
scale[0] = 11*scale[0]
scale[1] = 8*scale[1]
agxOSG.setTexture(body_shape, TEXTURE_PATH+texture, True, agxOSG.DIFFUSE_TEXTURE, scale[0], scale[1])
else:
color = backupColor(texture, color)
texture = False
if not texture:
agxColor = agxRender.Color(color[0], color[1], color[2])
agxOSG.setDiffuseColor(body_shape, agxColor)
if len(color) > 3:
agxOSG.setAlpha(body_shape, color[3])
agxSim.add(body_box)
return body_box
