def AddFallingItems(sys):
# Shared contact materials for falling objects
mat = chrono.ChMaterialSurfaceSMC()
# Create falling rigid bodies (spheres and boxes etc.)
for ix in range(-2, 3):
for iz in range(-2, 3):
# add spheres
mass = 1
radius = 1.1
body = chrono.ChBody()
comp = (2.0 / 5.0) * mass * radius**2
body.SetInertiaXX(chrono.ChVectorD(comp, comp, comp))
body.SetMass(mass)
body.SetPos(chrono.ChVectorD(4.0 * ix + 0.1, 4.0, 4.0 * iz))
body.GetCollisionModel().ClearModel()
body.GetCollisionModel().AddSphere(mat, radius)
body.GetCollisionModel().BuildModel()
body.SetCollide(True)
sphere = chrono.ChSphereShape()
sphere.GetSphereGeometry().rad = radius
body.AddAsset(sphere)
texture = chrono.ChTexture()
texture.SetTextureFilename(
chrono.GetChronoDataFile("textures/bluewhite.png"))
body.AddAsset(texture)
sys.AddBody(body)
# add boxes
mass = 1
hsize = chrono.ChVectorD(0.75, 0.75, 0.75)
body = chrono.ChBody()
body.SetMass(mass)
body.SetPos(chrono.ChVectorD(4.0 * ix, 6.0, 4.0 * iz))
body.GetCollisionModel().ClearModel()
body.GetCollisionModel().AddBox(mat, hsize.x, hsize.y, hsize.z)
body.GetCollisionModel().BuildModel()
body.SetCollide(True)
box = chrono.ChBoxShape()
box.GetBoxGeometry().Size = hsize
body.AddAsset(box)
texture = chrono.ChTexture()
texture.SetTextureFilename(
chrono.GetChronoDataFile("textures/pinkwhite.png"))
body.AddAsset(texture)
sys.AddBody(body)
def add_ellisoidShape(MiroSystem, radius_x, radius_y, radius_z, pos, texture='test.jpg', density=1000, 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]):
# Convert position to chrono vector, supports using chvector as input as well
ChPos = ChVecify(pos)
ChRotAxis = ChVecify(rotAxis)
# Create a cylinder
body_ball = chrono.ChBodyEasyEllipsoid(chrono.ChVectorD(radius_x, radius_y, radius_z), density)
body_ball.SetBodyFixed(Fixed)
body_ball.SetPos(ChPos)
rotateBody(body_ball, rotX, rotY, rotZ, rotOrder, rotAngle, ChRotAxis, rotDegrees)
# Collision shape
if(Collide):
body_ball.GetCollisionModel().ClearModel()
body_ball.GetCollisionModel().AddEllipsoid(radius_x, radius_y, radius_z) # hemi sizes
body_ball.GetCollisionModel().BuildModel()
body_ball.SetCollide(Collide)
# 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/'):]
body_texture = chrono.ChTexture()
body_texture.SetTextureFilename(chrono.GetChronoDataFile('textures/'+texture))
body_texture.SetTextureScale(scale[0], scale[1])
body_ball.GetAssets().push_back(body_texture)
if MiroSystem:
ChSystem = MiroSystem.Get_APIsystem()[0]
ChSystem.Add(body_ball)
return body_ball
def AddFallingItems(sys):
# Shared contact materials for falling objects
sph_mat = chrono.ChMaterialSurfaceNSC()
sph_mat.SetFriction(0.2)
box_mat = chrono.ChMaterialSurfaceNSC()
cyl_mat = chrono.ChMaterialSurfaceNSC()
# Create falling rigid bodies (spheres and boxes etc.)
for bi in range(29):
msphereBody = chrono.ChBodyEasySphere(1.1, # radius size
1000, # density
True, # visualization?
True, # collision?
sph_mat) # contact material
msphereBody.SetPos(chrono.ChVectorD(-5 + chrono.ChRandom() * 10, 4 + bi * 0.05, -5 + chrono.ChRandom() * 10))
sys.Add(msphereBody)
mtexture = chrono.ChTexture()
mtexture.SetTextureFilename(chrono.GetChronoDataFile("textures/bluewhite.png"))
msphereBody.AddAsset(mtexture)
mboxBody = chrono.ChBodyEasyBox(1.5, 1.5, 1.5, # x,y,z size
100, # density
True, # visualization?
True, # collision?
box_mat) # contact material
mboxBody.SetPos(chrono.ChVectorD(-5 + chrono.ChRandom() * 10, 4 + bi * 0.05, -5 + chrono.ChRandom() * 10))
sys.Add(mboxBody)
mtexturebox = chrono.ChTexture()
mtexturebox.SetTextureFilename(chrono.GetChronoDataFile("textures/cubetexture_bluewhite.png"))
mboxBody.AddAsset(mtexturebox)
mcylBody = chrono.ChBodyEasyCylinder(0.75, 0.5, # radius, height
100, # density
True, # visualization?
True, # collision?
cyl_mat) # contact material
mcylBody.SetPos(chrono.ChVectorD(-5 + chrono.ChRandom() * 10, 4 + bi * 0.05, -5 + chrono.ChRandom() * 10))
sys.Add(mcylBody)
# optional, attach a texture for better visualization
mtexturecyl = chrono.ChTexture()
mtexturecyl.SetTextureFilename(chrono.GetChronoDataFile("textures/pinkwhite.png"))
mcylBody.AddAsset(mtexturecyl)
def add_boxShape(MiroSystem, size_x, size_y, size_z, pos, 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]):
'''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
ChPos = ChVecify(pos)
ChRotAxis = ChVecify(rotAxis)
# Create a box
body_box = chrono.ChBody()
body_box.SetBodyFixed(Fixed)
body_box.SetPos(ChPos)
if not mass:
mass = density * size_x * size_y * size_z
inertia_brick_xx = (size_y**2 + size_z**2)*mass/3
inertia_brick_yy = (size_x**2 + size_z**2)*mass/3
inertia_brick_zz = (size_x**2 + size_y**2)*mass/3
body_box.SetMass(mass)
body_box.SetInertiaXX(chrono.ChVectorD(inertia_brick_xx, inertia_brick_yy, inertia_brick_zz))
# Collision shape
body_box.GetCollisionModel().ClearModel()
body_box.GetCollisionModel().AddBox(size_x/2, size_y/2, size_z/2) # hemi sizes
body_box.GetCollisionModel().BuildModel()
body_box.SetCollide(Collide)
# Visualization shape
body_box_shape = chrono.ChBoxShape()
body_box_shape.GetBoxGeometry().Size = chrono.ChVectorD(size_x/2, size_y/2, size_z/2)
body_box_shape.SetColor(chrono.ChColor(0.4,0.4,0.5))
body_box.GetAssets().push_back(body_box_shape)
# 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/'):]
body_box_texture = chrono.ChTexture()
body_box_texture.SetTextureFilename(chrono.GetChronoDataFile('textures/'+texture))
body_box_texture.SetTextureScale(scale[0], scale[1])
body_box.GetAssets().push_back(body_box_texture)
rotateBody(body_box, rotX, rotY, rotZ, rotOrder, rotAngle, rotAxis, rotDegrees)
if MiroSystem:
ChSystem = MiroSystem.Get_APIsystem()[0]
ChSystem.Add(body_box)
return body_box
patch_mat.SetRestitution(0.01)
patch_mat.SetYoungModulus(2e7)
patch = terrain.AddPatch(patch_mat,
chrono.ChVectorD(0, 0, 0), chrono.ChVectorD(0, 0, 1),
terrainLength, terrainWidth)
patch.SetTexture(veh.GetDataFile("terrain/textures/tile4.jpg"), 200, 200)
patch.SetColor(chrono.ChColor(0.8, 0.8, 0.5))
terrain.Initialize()
patch_asset = patch.GetGroundBody().GetAssets()[0]
patch_visual_asset = chrono.CastToChVisualization(patch_asset)
vis_mat = chrono.ChVisualMaterial()
# vis_mat.SetAmbientColor(chrono.ChVectorF(0, 0, 0))
vis_mat.SetKdTexture(chrono.GetChronoDataFile("vehicle/terrain/textures/dirt.jpg"))
vis_mat.SetSpecularColor(chrono.ChVectorF(.0, .0, .0))
vis_mat.SetFresnelMin(0)
vis_mat.SetFresnelMax(.1)
patch_visual_asset.material_list.append(vis_mat)
# Create the vehicle Irrlicht interface
app = veh.ChWheeledVehicleIrrApp(my_rccar.GetVehicle())
app.SetSkyBox()
app.AddTypicalLights(irr.vector3df(30, -30, 100), irr.vector3df(30, 50, 100), 250, 130)
app.AddTypicalLogo(chrono.GetChronoDataPath() + 'logo_pychrono_alpha.png')
app.SetChaseCamera(trackPoint, 1.5, 0.5)
app.SetTimestep(step_size)
# Create the ground
ground = chrono.ChBody()
ground.SetBodyFixed(True)
mysystem.Add(ground)
# Create the rigid body with contact mesh
body = chrono.ChBody()
mysystem.Add(body)
body.SetMass(500)
body.SetInertiaXX(chrono.ChVectorD(20, 20, 20))
body.SetPos(tire_center + chrono.ChVectorD(0, 0.3, 0))
# Load mesh
mesh = chrono.ChTriangleMeshConnected()
mesh.LoadWavefrontMesh(
chrono.GetChronoDataFile('models/tractor_wheel/tractor_wheel.obj'))
# Set visualization assets
vis_shape = chrono.ChTriangleMeshShape()
vis_shape.SetMesh(mesh)
body.AddAsset(vis_shape)
body.AddAsset(chrono.ChColorAsset(0.3, 0.3, 0.3))
# Set collision shape
material = chrono.ChMaterialSurfaceSMC()
body.GetCollisionModel().ClearModel()
body.GetCollisionModel().AddTriangleMesh(
material, # contact material
mesh, # the mesh
False, # is it static?
def main():
# -----------------
# Create the system
# -----------------
mphysicalSystem = chrono.ChSystemNSC()
# -----------------------------------
# add a mesh to be sensed by a camera
# -----------------------------------
mmesh = chrono.ChTriangleMeshConnected()
mmesh.LoadWavefrontMesh(
chrono.GetChronoDataFile("vehicle/hmmwv/hmmwv_chassis.obj"), False,
True)
# scale to a different size
mmesh.Transform(chrono.ChVectorD(0, 0, 0), chrono.ChMatrix33D(2))
trimesh_shape = chrono.ChTriangleMeshShape()
trimesh_shape.SetMesh(mmesh)
trimesh_shape.SetName("HMMWV Chassis Mesh")
trimesh_shape.SetStatic(True)
mesh_body = chrono.ChBody()
mesh_body.SetPos(chrono.ChVectorD(0, 0, 0))
mesh_body.AddAsset(trimesh_shape)
mesh_body.SetBodyFixed(True)
mphysicalSystem.Add(mesh_body)
# -----------------------
# Create a sensor manager
# -----------------------
manager = sens.ChSensorManager(mphysicalSystem)
intensity = 1.0
manager.scene.AddPointLight(
chrono.ChVectorF(2, 2.5, 100),
chrono.ChVectorF(intensity, intensity, intensity), 500.0)
manager.scene.AddPointLight(
chrono.ChVectorF(9, 2.5, 100),
chrono.ChVectorF(intensity, intensity, intensity), 500.0)
manager.scene.AddPointLight(
chrono.ChVectorF(16, 2.5, 100),
chrono.ChVectorF(intensity, intensity, intensity), 500.0)
manager.scene.AddPointLight(
chrono.ChVectorF(23, 2.5, 100),
chrono.ChVectorF(intensity, intensity, intensity), 500.0)
# manager.SetKeyframeSizeFromTimeStep(.001,1/exposure_time)
# ------------------------------------------------
# Create a camera and add it to the sensor manager
# ------------------------------------------------
offset_pose = chrono.ChFrameD(
chrono.ChVectorD(-5, 0, 2),
chrono.Q_from_AngAxis(2, chrono.ChVectorD(0, 1, 0)))
cam = sens.ChCameraSensor(
mesh_body, # body camera is attached to
update_rate, # update rate in Hz
offset_pose, # offset pose
image_width, # image width
image_height, # image height
fov # camera's horizontal field of view
)
cam.SetName("Camera Sensor")
cam.SetLag(lag)
cam.SetCollectionWindow(exposure_time)
# ------------------------------------------------------------------
# Create a filter graph for post-processing the data from the camera
# ------------------------------------------------------------------
if noise_model == "CONST_NORMAL":
cam.PushFilter(sens.ChFilterCameraNoiseConstNormal(0.0, 0.02))
elif noise_model == "PIXEL_DEPENDENT":
cam.PushFilter(sens.ChFilterCameraNoisePixDep(0, 0.02, 0.03))
elif noise_model == "NONE":
# Don't add any noise models
pass
# Renders the image at current point in the filter graph
if vis:
cam.PushFilter(
sens.ChFilterVisualize(image_width, image_height,
"Before Grayscale Filter"))
# Provides the host access to this RGBA8 buffer
cam.PushFilter(sens.ChFilterRGBA8Access())
# Save the current image to a png file at the specified path
if save:
cam.PushFilter(sens.ChFilterSave(out_dir + "rgb/"))
# Filter the sensor to grayscale
cam.PushFilter(sens.ChFilterGrayscale())
# Render the buffer again to see the new grayscaled image
if vis:
cam.PushFilter(
sens.ChFilterVisualize(int(image_width / 2), int(image_height / 2),
"Grayscale Image"))
# Save the grayscaled image at the specified path
if save:
cam.PushFilter(sens.ChFilterSave(out_dir + "gray/"))
# Resizes the image to the provided width and height
cam.PushFilter(
sens.ChFilterImageResize(int(image_width / 2), int(image_height / 2)))
# Access the grayscaled buffer as R8 pixels
cam.PushFilter(sens.ChFilterR8Access())
# add sensor to manager
manager.AddSensor(cam)
# ---------------
# Simulate system
# ---------------
orbit_radius = 10
orbit_rate = 0.5
ch_time = 0.0
t1 = time.time()
while (ch_time < end_time):
cam.SetOffsetPose(
chrono.ChFrameD(
chrono.ChVectorD(
-orbit_radius * math.cos(ch_time * orbit_rate),
-orbit_radius * math.sin(ch_time * orbit_rate), 1),
chrono.Q_from_AngAxis(ch_time * orbit_rate,
chrono.ChVectorD(0, 0, 1))))
# Access the RGBA8 buffer from the camera
rgba8_buffer = cam.GetMostRecentRGBA8Buffer()
if (rgba8_buffer.HasData()):
rgba8_data = rgba8_buffer.GetRGBA8Data()
print('RGBA8 buffer recieved from cam. Camera resolution: {0}x{1}'\
.format(rgba8_buffer.Width, rgba8_buffer.Height))
print('First Pixel: {0}'.format(rgba8_data[0, 0, :]))
# Update sensor manager
# Will render/save/filter automatically
manager.Update()
# Perform step of dynamics
mphysicalSystem.DoStepDynamics(step_size)
# Get the current time of the simulation
ch_time = mphysicalSystem.GetChTime()
print("Sim time:", end_time, "Wall time:", time.time() - t1)
# you can generate it from 3D modelers such as Blender, Maya, etc.
#
# NOTE: for collision purposes, the .obj mesh must be "watertight", i.e. having
# no gaps in edges, no repeated vertexes, etc.
#
# NOTE: for visualization purposes only, i.e. if you do not use the mesh also for
# collision, the mesh does not need to be watertight.
# Method A:
# - use the ChBodyEasyMesh
# This will automatically create the visualization mesh, the collision mesh,
# and will automatically compute the mass property (COG position respect to REF,
# mass and inertia tensor) given an uniform density.
body_A = chrono.ChBodyEasyMesh(
chrono.GetChronoDataFile('shoe_view.obj'), # mesh filename
7000, # density kg/m^3
True, # use mesh for visualization?
True) # use mesh for collision?
body_A.SetPos(chrono.ChVectorD(0.5, 0.5, 0))
mysystem.Add(body_A)
# Method B:
# - create a ChBodyAuxRef,
# - set mass and inertia tensor as you like
# - set COG center of mass position respect to REF reference as you like
# - attach a visualization shape based on a .obj triangle mesh
# - add contact shape based on a .obj triangle mesh
# This is more complicate than method A, yet this can be still preferred if you
# need deeper control, ex. you want to provide two different meshes, one
# with high level of detail just for the visualization and a coarse one for
print("Copyright (c) 2017 projectchrono.org")
# Create a ChronoENGINE physical system
mphysicalSystem = chrono.ChSystemNSC()
# Contact material shared among all objects
material = chrono.ChMaterialSurfaceNSC()
# Create a floor that is fixed (that is used also to represent the absolute reference)
floorBody = chrono.ChBodyEasyBox(20, 2, 20, 3000, True, True, material)
floorBody.SetPos(chrono.ChVectorD(0, -2, 0))
floorBody.SetBodyFixed(True)
mphysicalSystem.Add(floorBody)
mtexture = chrono.ChTexture()
mtexture.SetTextureFilename(chrono.GetChronoDataFile("textures/blue.png"))
floorBody.AddAsset(mtexture)
# In the following we will create different types of motors
# - rotational motors: examples A.1, A.2, etc.
# - linear motors, examples B.1, B.2 etc.
# EXAMPLE A.1
#
# - class: ChLinkMotorRotationSpeed
# - type: rotational motor
# - control: impose a time-dependent speed=v(t)
#
# This is a simple type of rotational actuator. It assumes that
# you know the exact angular speed of the rotor respect to the stator,
# as a function of time: angular speed = w(t).
# Create the simulation system and add items
#
mysystem = chrono.ChSystemNSC()
# Load a STEP file, containing a mechanism. The demo STEP file has been
# created using a 3D CAD (in this case, SolidEdge v.18).
#
# Create the ChCascadeDoc, a container that loads the STEP model
# and manages its subassembles
mydoc = cascade.ChCascadeDoc()
# load the STEP model using this command:
load_ok = mydoc.Load_STEP(chrono.GetChronoDataFile('cascade/IRB7600_23_500_m2000_rev1_01_decorated.stp')) # or specify abs.path: ("C:\\data\\cascade\\assembly.stp");
if not load_ok:
raise ValueError("Warning. Desired STEP file could not be opened/parsed \n")
CH_C_PI = 3.1456
# In most CADs the Y axis is horizontal, but we want it vertical.
# So define a root transformation for rotating all the imported objects.
rotation1 = chrono.ChQuaternionD()
rotation1.Q_from_AngAxis(-CH_C_PI / 2, chrono.ChVectorD(1, 0, 0)) # 1: rotate 90° on X axis
rotation2 = chrono.ChQuaternionD()
rotation2.Q_from_AngAxis(CH_C_PI, chrono.ChVectorD(0, 1, 0)) # 2: rotate 180° on vertical Y axis
tot_rotation = chrono.ChQuaternionD()
patch_mat.SetRestitution(0.01)
patch_mat.SetYoungModulus(2e7)
patch = terrain.AddPatch(patch_mat, chrono.ChVectorD(0, 0, 0),
chrono.ChVectorD(0, 0, 1), terrainLength,
terrainWidth)
patch.SetTexture(veh.GetDataFile("terrain/textures/tile4.jpg"), 200, 200)
patch.SetColor(chrono.ChColor(0.8, 0.8, 0.5))
terrain.Initialize()
# Create the vehicle Irrlicht interface
app = veh.ChWheeledVehicleIrrApp(my_bus.GetVehicle(), 'Citybus',
irr.dimension2du(1000, 800))
app.SetSkyBox()
app.AddTypicalLights(irr.vector3df(30, -30, 100), irr.vector3df(30, 50, 100),
250, 130)
app.AddTypicalLogo(chrono.GetChronoDataFile('logo_pychrono_alpha.png'))
app.SetChaseCamera(trackPoint, 15.0, 0.5)
app.SetTimestep(step_size)
app.AssetBindAll()
app.AssetUpdateAll()
# Create the driver system
driver = veh.ChIrrGuiDriver(app)
# Set the time response for steering and throttle keyboard inputs.
steering_time = 1.0 # time to go from 0 to +1 (or from 0 to -1)
throttle_time = 1.0 # time to go from 0 to +1
braking_time = 0.3 # time to go from 0 to +1
driver.SetSteeringDelta(render_step_size / steering_time)
driver.SetThrottleDelta(render_step_size / throttle_time)
driver.SetBrakingDelta(render_step_size / braking_time)
def ChangeBodyTexture(body, texture_file, scale=[1,1]):
texture = chrono.ChTexture()
texture.SetTextureFilename(chrono.GetChronoDataFile(texture_file))
texture.SetTextureScale(scale[0], scale[1])
body.AddAsset(texture)
def main():
chrono.SetChronoDataPath("../../../data/")
# -----------------
# Create the system
# -----------------
mphysicalSystem = chrono.ChSystemNSC()
# ----------------------------------
# add a mesh to be sensed by a lidar
# ----------------------------------
mmesh = chrono.ChTriangleMeshConnected()
mmesh.LoadWavefrontMesh(
chrono.GetChronoDataFile("vehicle/hmmwv/hmmwv_chassis.obj"), False,
True)
mmesh.Transform(chrono.ChVectorD(0, 0, 0),
chrono.ChMatrix33D(2)) # scale to a different size
trimesh_shape = chrono.ChTriangleMeshShape()
trimesh_shape.SetMesh(mmesh)
trimesh_shape.SetName("HMMWV Chassis Mesh")
trimesh_shape.SetStatic(True)
mesh_body = chrono.ChBody()
mesh_body.SetPos(chrono.ChVectorD(0, 0, 0))
mesh_body.AddAsset(trimesh_shape)
mesh_body.SetBodyFixed(True)
mphysicalSystem.Add(mesh_body)
# -----------------------
# Create a sensor manager
# -----------------------
manager = sens.ChSensorManager(mphysicalSystem)
manager.SetKeyframeSizeFromTimeStep(.001, 1 / collection_time)
# ------------------------------------------------
# Create a lidar and add it to the sensor manager
# ------------------------------------------------
offset_pose = chrono.ChFrameD(
chrono.ChVectorD(-8, 0, 1),
chrono.Q_from_AngAxis(0, chrono.ChVectorD(0, 1, 0)))
lidar = sens.ChLidarSensor(
mesh_body, # body lidar is attached to
update_rate, # scanning rate in Hz
offset_pose, # offset pose
horizontal_samples, # number of horizontal samples
vertical_samples, # number of vertical channels
horizontal_fov, # horizontal field of view
max_vert_angle, # vertical field of view
min_vert_angle,
100.0, #max lidar range
sample_radius, # sample radius
divergence_angle, # divergence angle
return_mode, # return mode for the lidar
lens_model # method/model to use for generating data
)
lidar.SetName("Lidar Sensor")
lidar.SetLag(lag)
lidar.SetCollectionWindow(collection_time)
# -----------------------------------------------------------------
# Create a filter graph for post-processing the data from the lidar
# -----------------------------------------------------------------
if noise_model == "CONST_NORMAL_XYZI":
lidar.PushFilter(sens.ChFilterLidarNoiseXYZI(0.01, 0.001, 0.001, 0.01))
elif noise_model == "NONE":
# Don't add any noise models
pass
if vis:
# Visualize the raw lidar data
lidar.PushFilter(
sens.ChFilterVisualize(horizontal_samples, vertical_samples,
"Raw Lidar Depth Data"))
# Provides the host access to the Depth,Intensity data
lidar.PushFilter(sens.ChFilterDIAccess())
# Convert Depth,Intensity data to XYZI point cloud data
lidar.PushFilter(sens.ChFilterPCfromDepth())
if vis:
# Visualize the point cloud
lidar.PushFilter(
sens.ChFilterVisualizePointCloud(640, 480, 1.0,
"Lidar Point Cloud"))
# Provides the host access to the XYZI data
lidar.PushFilter(sens.ChFilterXYZIAccess())
# Add the lidar to the sensor manager
manager.AddSensor(lidar)
# ---------------
# Simulate system
# ---------------
orbit_radius = 5
orbit_rate = 0.2
ch_time = 0.0
render_time = 0
t1 = time.time()
while (ch_time < end_time):
lidar.SetOffsetPose(
chrono.ChFrameD(
chrono.ChVectorD(
-orbit_radius * math.cos(ch_time * orbit_rate),
-orbit_radius * math.sin(ch_time * orbit_rate), 1),
chrono.Q_from_AngAxis(ch_time * orbit_rate,
chrono.ChVectorD(0, 0, 1))))
# Access the XYZI buffer from lidar
xyzi_buffer = lidar.GetMostRecentXYZIBuffer()
if xyzi_buffer.HasData():
xyzi_data = xyzi_buffer.GetXYZIData()
print('XYZI buffer recieved from lidar. Lidar resolution: {0}x{1}'\
.format(xyzi_buffer.Width, xyzi_buffer.Height))
print('Max Value: {0}'.format(np.max(xyzi_data)))
# Update sensor manager
# Will render/save/filter automatically
manager.Update()
# Perform step of dynamics
mphysicalSystem.DoStepDynamics(step_size)
# Get the current time of the simulation
ch_time = mphysicalSystem.GetChTime()
print("Sim time:", end_time, "Wall time:", time.time() - t1)
def AddContainer(sys):
# Contact material for container
ground_mat = chrono.ChMaterialSurfaceNSC()
# Create the five walls of the rectangular container, using fixed rigid bodies of 'box' type
floorBody = chrono.ChBodyEasyBox(20, 1, 20, 1000, True, True, ground_mat)
floorBody.SetPos(chrono.ChVectorD(0, -5, 0))
floorBody.SetBodyFixed(True)
sys.Add(floorBody)
wallBody1 = chrono.ChBodyEasyBox(1, 10, 20.99, 1000, True, True, ground_mat)
wallBody1.SetPos(chrono.ChVectorD(-10, 0, 0))
wallBody1.SetBodyFixed(True)
sys.Add(wallBody1)
wallBody2 = chrono.ChBodyEasyBox(1, 10, 20.99, 1000, True, True, ground_mat)
wallBody2.SetPos(chrono.ChVectorD(10, 0, 0))
wallBody2.SetBodyFixed(True)
sys.Add(wallBody2)
wallBody3 = chrono.ChBodyEasyBox(20.99, 10, 1, 1000, False, True, ground_mat)
wallBody3.SetPos(chrono.ChVectorD(0, 0, -10))
wallBody3.SetBodyFixed(True)
sys.Add(wallBody3)
wallBody4 = chrono.ChBodyEasyBox(20.99, 10, 1, 1000, True, True, ground_mat)
wallBody4.SetPos(chrono.ChVectorD(0, 0, 10))
wallBody4.SetBodyFixed(True)
sys.Add(wallBody4)
# optional, attach textures for better visualization
mtexturewall = chrono.ChTexture()
mtexturewall.SetTextureFilename(chrono.GetChronoDataFile("textures/concrete.jpg"))
wallBody1.AddAsset(mtexturewall) # note: most assets can be shared
wallBody2.AddAsset(mtexturewall)
wallBody3.AddAsset(mtexturewall)
wallBody4.AddAsset(mtexturewall)
floorBody.AddAsset(mtexturewall)
# Add the rotating mixer
mixer_mat = chrono.ChMaterialSurfaceNSC()
mixer_mat.SetFriction(0.4)
rotatingBody = chrono.ChBodyEasyBox(10, 5, 1, # x,y,z size
4000, # density
True, # visualization?
True, # collision?
mixer_mat) # contact material
rotatingBody.SetPos(chrono.ChVectorD(0, -1.6, 0))
sys.Add(rotatingBody)
# .. a motor between mixer and truss
my_motor = chrono.ChLinkMotorRotationSpeed()
my_motor.Initialize(rotatingBody,
floorBody,
chrono.ChFrameD(chrono.ChVectorD(0, 0, 0),
chrono.Q_from_AngAxis(chrono.CH_C_PI_2, chrono.VECT_X)))
mfun = chrono.ChFunction_Const(chrono.CH_C_PI / 4.0) # speed 45 deg/s
my_motor.SetSpeedFunction(mfun)
sys.AddLink(my_motor)
# NOTE: Instead of creating five separate 'box' bodies to make
# the walls of the container, you could have used a single body
# made of five box shapes, which build a single collision description,
# as in the alternative approach:
"""
# create a plain ChBody (no colliding shape nor visualization mesh is used yet)
mrigidBody = chrono.ChBody()
# set as fixed body, and turn collision ON, otherwise no collide by default
mrigidBody.SetBodyFixed(True)
mrigidBody.SetCollide(True)
# Clear model. The colliding shape description MUST be between ClearModel() .. BuildModel() pair.
mrigidBody.GetCollisionModel().ClearModel()
# Describe the (invisible) colliding shape by adding five boxes (the walls and floor)
mrigidBody.GetCollisionModel().AddBox(ground_mat, 20, 1, 20, chrono.ChVectorD(0, -10, 0))
mrigidBody.GetCollisionModel().AddBox(ground_mat, 1, 40, 20, chrono.ChVectorD(-11, 0, 0))
mrigidBody.GetCollisionModel().AddBox(ground_mat, 1, 40, 20, chrono.ChVectorD(11, 0, 0))
mrigidBody.GetCollisionModel().AddBox(ground_mat, 20, 40, 1, chrono.ChVectorD(0, 0, -11))
mrigidBody.GetCollisionModel().AddBox(ground_mat, 20, 40, 1, chrono.ChVectorD(0, 0, 11))
# Complete the description of collision shape.
mrigidBody.GetCollisionModel().BuildModel()
# Attach some visualization shapes if needed:
vshape = chrono.ChBoxShape()
vshape.GetBoxGeometry().SetLengths(chrono.ChVectorD(20, 1, 20))
vshape.GetBoxGeometry().Pos = chrono.ChVectorD(0, -5, 0)
this.AddAsset(vshape)
# etc. for other 4 box shapes..
"""
return rotatingBody
body_floor.SetPos(chrono.ChVectorD(0, -2, 0))
# Collision shape
body_floor.GetCollisionModel().ClearModel()
body_floor.GetCollisionModel().AddBox(brick_material, 3, 1, 3) # hemi sizes
body_floor.GetCollisionModel().BuildModel()
body_floor.SetCollide(True)
# Visualization shape
body_floor_shape = chrono.ChBoxShape()
body_floor_shape.GetBoxGeometry().Size = chrono.ChVectorD(3, 1, 3)
body_floor.GetAssets().push_back(body_floor_shape)
body_floor_texture = chrono.ChTexture()
body_floor_texture.SetTextureFilename(
chrono.GetChronoDataFile('textures/concrete.jpg'))
body_floor.GetAssets().push_back(body_floor_texture)
my_system.Add(body_floor)
# Create the shaking table, as a box
size_table_x = 1
size_table_y = 0.2
size_table_z = 1
body_table = chrono.ChBody()
body_table.SetPos(chrono.ChVectorD(0, -size_table_y / 2, 0))
# Collision shape
body_table.GetCollisionModel().ClearModel()
# you can generate it from 3D modelers such as Blender, Maya, etc.
#
# NOTE: for collision purposes, the .obj mesh must be "watertight", i.e. having
# no gaps in edges, no repeated vertexes, etc.
#
# NOTE: for visualization purposes only, i.e. if you do not use the mesh also for
# collision, the mesh does not need to be watertight.
# Method A:
# - use the ChBodyEasyMesh
# This will automatically create the visualization mesh, the collision mesh,
# and will automatically compute the mass property (COG position respect to REF,
# mass and inertia tensor) given an uniform density.
body_A= chrono.ChBodyEasyMesh(chrono.GetChronoDataFile('models/bulldozer/shoe_view.obj'), # mesh filename
7000, # density kg/m^3
True, # automatically compute mass and inertia
True, # visualize?>
True, # collide?
contact_material, # contact material
)
body_A.SetPos(chrono.ChVectorD(0.5,0.5,0))
mysystem.Add(body_A)
# Method B:
# - create a ChBodyAuxRef,
# - set mass and inertia tensor as you like
# - set COG center of mass position respect to REF reference as you like
mboxasset.GetBoxGeometry().Size = chrono.ChVectorD(0.2, 0.5, 0.1)
mbody1.AddAsset(mboxasset)
# Create a swinging rigid body
mbody2 = chrono.ChBody()
mbody2.SetBodyFixed(False)
mysystem.Add(mbody2)
mboxasset = chrono.ChBoxShape()
mboxasset.GetBoxGeometry().Size = chrono.ChVectorD(0.2, 0.5, 0.1)
mbody2.AddAsset(mboxasset)
mboxtexture = chrono.ChTexture()
mboxtexture.SetTextureFilename(
chrono.GetChronoDataFile('textures/concrete.jpg'))
mbody2.GetAssets().push_back(mboxtexture)
# Create a revolute constraint
mlink = chrono.ChLinkRevolute()
# the coordinate system of the constraint reference in abs. space:
mframe = chrono.ChFrameD(chrono.ChVectorD(0.1, 0.5, 0))
# initialize the constraint telling which part must be connected, and where:
mlink.Initialize(mbody1, mbody2, mframe)
mysystem.Add(mlink)
# ---------------------------------------------------------------------
vshape_3.GetBoxGeometry().SetLengths(chrono.ChVectorD(0.2, 2, 10.2))
vshape_3.GetBoxGeometry().Pos = chrono.ChVectorD(5, 0, 0)
ground.AddAsset(vshape_3)
vshape_4 = chrono.ChBoxShape()
vshape_4.GetBoxGeometry().SetLengths(chrono.ChVectorD(10.2, 2, 0.2))
vshape_4.GetBoxGeometry().Pos = chrono.ChVectorD(0, 0, -5)
ground.AddAsset(vshape_4)
vshape_5 = chrono.ChBoxShape()
vshape_5.GetBoxGeometry().SetLengths(chrono.ChVectorD(10.2, 2, 0.2))
vshape_5.GetBoxGeometry().Pos = chrono.ChVectorD(0, 0, 5)
ground.AddAsset(vshape_5)
ground.AddAsset(chrono.ChTexture(
chrono.GetChronoDataFile("textures/blue.png")))
# Add obstacle visualization (in a separate level with a different color).
ground.AddAsset(obstacle.GetVisualization())
# Create the falling ball
ball = chrono.ChBody()
sys.AddBody(ball)
ball.SetMass(10)
comp = 4 * ball_radius * ball_radius
ball.SetInertiaXX(chrono.ChVectorD(comp, comp, comp))
ball.SetPos(chrono.ChVectorD(-3, 1.2 * ball_radius, -3))
ball.SetPos_dt(chrono.ChVectorD(5, 0, 5))
ball.SetCollide(True)
ball.GetCollisionModel().ClearModel()
body_floor.SetPos(chrono.ChVectorD(0, -2, 0))
body_floor.SetMaterialSurface(brick_material)
# Collision shape
body_floor.GetCollisionModel().ClearModel()
body_floor.GetCollisionModel().AddBox(3, 1, 3) # hemi sizes
body_floor.GetCollisionModel().BuildModel()
body_floor.SetCollide(True)
# Visualization shape
body_floor_shape = chrono.ChBoxShape()
body_floor_shape.GetBoxGeometry().Size = chrono.ChVectorD(3, 1, 3)
body_floor.GetAssets().push_back(body_floor_shape)
body_floor_texture = chrono.ChTexture()
body_floor_texture.SetTextureFilename(chrono.GetChronoDataFile('concrete.jpg'))
body_floor.GetAssets().push_back(body_floor_texture)
my_system.Add(body_floor)
# Create the shaking table, as a box
size_table_x = 1
size_table_y = 0.2
size_table_z = 1
body_table = chrono.ChBody()
body_table.SetPos(chrono.ChVectorD(0, -size_table_y / 2, 0))
body_table.SetMaterialSurface(brick_material)
# Collision shape
# Create the ground
ground = chrono.ChBody()
ground.SetBodyFixed(True)
mysystem.Add(ground)
# Create the rigid body with contact mesh
body = chrono.ChBody()
mysystem.Add(body)
body.SetMass(500)
body.SetInertiaXX(chrono.ChVectorD(20, 20, 20))
body.SetPos(tire_center + chrono.ChVectorD(0, 0.3, 0))
# Load mesh
mesh = chrono.ChTriangleMeshConnected()
mesh.LoadWavefrontMesh(chrono.GetChronoDataFile('tractor_wheel.obj'))
# Set visualization assets
vis_shape = chrono.ChTriangleMeshShape()
vis_shape.SetMesh(mesh)
body.AddAsset(vis_shape)
body.AddAsset(chrono.ChColorAsset(0.3, 0.3, 0.3))
# Set collision shape
body.GetCollisionModel().ClearModel()
body.GetCollisionModel().AddTriangleMesh(
mesh, # the mesh
False, # is it static?
False, # is it convex?
chrono.ChVectorD(0, 0, 0), # position on body
chrono.ChMatrix33D(1), # orientation on body
# Create the simulation system and add items
#
mysystem = chrono.ChSystemNSC()
# Load a STEP file, containing a mechanism. The demo STEP file has been
# created using a 3D CAD (in this case, SolidEdge v.18).
#
# Create the ChCascadeDoc, a container that loads the STEP model
# and manages its subassembles
mydoc = cascade.ChCascadeDoc()
# load the STEP model using this command:
load_ok = mydoc.Load_STEP(
chrono.GetChronoDataFile('cascade/assembly.stp')
) # or specify abs.path: ("C:\\data\\cascade\\assembly.stp");
# print the contained shapes
#mydoc.Dump(chrono.GetLog())
# In most CADs the Y axis is horizontal, but we want it vertical.
# So define a root transformation for rotating all the imported objects.
rotation1 = chrono.ChQuaternionD()
rotation1.Q_from_AngAxis(-chrono.CH_C_PI / 2,
chrono.ChVectorD(1, 0, 0)) # 1: rotate 90° on X axis
rotation2 = chrono.ChQuaternionD()
rotation2.Q_from_AngAxis(chrono.CH_C_PI, chrono.ChVectorD(
0, 1, 0)) # 2: rotate 180° on vertical Y axis
tot_rotation = chrono.ChQuaternionD()
tot_rotation = rotation2 % rotation1 # rotate on 1 then on 2, using quaternion product
def AddContainer(sys):
# The fixed body (5 walls)
fixedBody = chrono.ChBody()
fixedBody.SetMass(1.0)
fixedBody.SetBodyFixed(True)
fixedBody.SetPos(chrono.ChVectorD())
fixedBody.SetCollide(True)
# Contact material for container
fixed_mat = chrono.ChMaterialSurfaceSMC()
fixedBody.GetCollisionModel().ClearModel()
AddContainerWall(fixedBody, fixed_mat, chrono.ChVectorD(20, 1, 20),
chrono.ChVectorD(0, -5, 0))
AddContainerWall(fixedBody, fixed_mat, chrono.ChVectorD(1, 10, 20.99),
chrono.ChVectorD(-10, 0, 0))
AddContainerWall(fixedBody, fixed_mat, chrono.ChVectorD(1, 10, 20.99),
chrono.ChVectorD(10, 0, 0))
AddContainerWall(fixedBody, fixed_mat, chrono.ChVectorD(20.99, 10, 1),
chrono.ChVectorD(0, 0, -10), False)
AddContainerWall(fixedBody, fixed_mat, chrono.ChVectorD(20.99, 10, 1),
chrono.ChVectorD(0, 0, 10))
fixedBody.GetCollisionModel().BuildModel()
texture = chrono.ChTexture()
texture.SetTextureFilename(
chrono.GetChronoDataFile("textures/concrete.jpg"))
fixedBody.AddAsset(texture)
sys.AddBody(fixedBody)
# The rotating mixer body
rotatingBody = chrono.ChBody()
rotatingBody.SetMass(10.0)
rotatingBody.SetInertiaXX(chrono.ChVectorD(50, 50, 50))
rotatingBody.SetPos(chrono.ChVectorD(0, -1.6, 0))
rotatingBody.SetCollide(True)
# Contact material for mixer body
rot_mat = chrono.ChMaterialSurfaceSMC()
hsize = chrono.ChVectorD(5, 2.75, 0.5)
rotatingBody.GetCollisionModel().ClearModel()
rotatingBody.GetCollisionModel().AddBox(rot_mat, hsize.x, hsize.y, hsize.z)
rotatingBody.GetCollisionModel().BuildModel()
box = chrono.ChBoxShape()
box.GetBoxGeometry().Size = hsize
rotatingBody.AddAsset(box)
rotatingBody.AddAsset(texture)
sys.AddBody(rotatingBody)
# A motor between the two
my_motor = chrono.ChLinkMotorRotationSpeed()
my_motor.Initialize(
rotatingBody, fixedBody,
chrono.ChFrameD(chrono.ChVectorD(0, 0, 0),
chrono.Q_from_AngAxis(chrono.CH_C_PI_2,
chrono.VECT_X)))
mfun = chrono.ChFunction_Const(chrono.CH_C_PI / 2.0) # speed w=90°/s
my_motor.SetSpeedFunction(mfun)
sys.AddLink(my_motor)
return rotatingBody
#
# Load and create shells from a .obj file containing a triangle mesh surface
#
if (False):
density = 100
E = 6e5
nu = 0.0
thickness = 0.01
melasticity = fea.ChElasticityKirchhoffIsothropic(E, nu)
material = fea.ChMaterialShellKirchhoff(melasticity)
material.SetDensity(density)
fea.ChMeshFileLoader.BSTShellFromObjFile(
my_mesh, chrono.GetChronoDataFile('models/cube.obj'), material,
thickness)
# ==Asset== attach a visualization of the FEM mesh.
# This will automatically update a triangle mesh (a ChTriangleMeshShape
# asset that is internally managed) by setting proper
# coordinates and vertex colors as in the FEM elements.
# Such triangle mesh can be rendered by Irrlicht or POVray or whatever
# postprocessor that can handle a colored ChTriangleMeshShape).
# Do not forget AddAsset() at the end!
mvisualizeshellA = fea.ChVisualizationFEAmesh(my_mesh)
#mvisualizeshellA.SetSmoothFaces(True)
#mvisualizeshellA.SetWireframe(True)
mvisualizeshellA.SetShellResolution(2)
#mvisualizeshellA.SetBackfaceCull(True)
mysystem.AddLink(ujoint)
ujoint.Initialize(shaft_1, shaft_2,
chrono.ChFrameD(chrono.ChVectorD(0, 0, 0), rot))
# ---------------------------------------------------------------------
#
# Create an Irrlicht application to visualize the system
#
myapplication = chronoirr.ChIrrApp(mysystem,
'PyChrono example: universal joint',
chronoirr.dimension2du(1024, 768))
myapplication.AddTypicalSky()
myapplication.AddTypicalLogo(
chrono.GetChronoDataFile('logo_pychrono_alpha.png'))
myapplication.AddTypicalCamera(chronoirr.vector3df(3, 1, -1.5))
myapplication.AddTypicalLights()
# ==IMPORTANT!== Use this function for adding a ChIrrNodeAsset to all items
# in the system. These ChIrrNodeAsset assets are 'proxies' to the Irrlicht meshes.
# If you need a finer control on which item really needs a visualization proxy in
# Irrlicht, just use application.AssetBind(myitem) on a per-item basis.
myapplication.AssetBindAll()
# ==IMPORTANT!== Use this function for 'converting' into Irrlicht meshes the assets
# that you added to the bodies into 3D shapes, they can be visualized by Irrlicht!
myapplication.AssetUpdateAll()
# Create the simulation system and add items
#
mysystem = chrono.ChSystemNSC()
# Load a STEP file, containing a mechanism. The demo STEP file has been
# created using a 3D CAD (in this case, SolidEdge v.18).
#
# Create the ChCascadeDoc, a container that loads the STEP model
# and manages its subassembles
mydoc = cascade.ChCascadeDoc()
# load the STEP model using this command:
load_ok = mydoc.Load_STEP(
chrono.GetChronoDataFile(
'cascade/IRB7600_23_500_m2000_rev1_01_decorated.stp')
) # or specify abs.path: ("C:\\data\\cascade\\assembly.stp");
if not load_ok:
raise ValueError(
"Warning. Desired STEP file could not be opened/parsed \n")
CH_C_PI = 3.1456
# In most CADs the Y axis is horizontal, but we want it vertical.
# So define a root transformation for rotating all the imported objects.
rotation1 = chrono.ChQuaternionD()
rotation1.Q_from_AngAxis(-CH_C_PI / 2,
chrono.ChVectorD(1, 0, 0)) # 1: rotate 90° on X axis
rotation2 = chrono.ChQuaternionD()
rotation2.Q_from_AngAxis(CH_C_PI, chrono.ChVectorD(
