def SetupSystem():
ChSystem = chrono.ChSystemNSC()
ChSimulation = chronoirr.ChIrrApp(ChSystem, 'MiroSimulation', chronoirr.dimension2du(1720, 920))
# Set the default outward/inward shape margins for collision detection,
# this is epecially important for very large or very small objects.
chrono.ChCollisionModel.SetDefaultSuggestedEnvelope(0.0000001)
chrono.ChCollisionModel.SetDefaultSuggestedMargin(0.0001)
# Maybe you want to change some settings for the solver. For example you
# might want to use SetSolverMaxIterations to set the number of iterations
# per timestep, etc.
#MiroSystem.ChSystem.SetSolverType(chrono.ChSolver.Type_BARZILAIBORWEIN) # precise, more slow
ChSystem.SetSolverMaxIterations(70)
return [ChSystem, ChSimulation]
def __init__(self):
self.amountOfSimulations = 0
self.maxSpeed = 4
self.score = 0
self.previousScore = 0
self.highscore = 0
self.highscoreTime = 0
#self.delayCounter = 0
#self.delay = randint(5,10)
self.keepRunning = True
self.agent = DQNAgent()
self.mysystem = chrono.ChSystemNSC()
self.ground = theBattleground.theBattleground(self.mysystem)
self.createRobot(self.mysystem)
self.createApplication()
self.run()
def restart(self):
print("restart called")
del self.myapplication
del self.mysystem
self.mysystem = chrono.ChSystemNSC()
self.ground = theBattleground.theBattleground(self.mysystem)
self.createRobot(self.mysystem)
self.createApplication()
self.mysystem.SetChTime(0)
self.keepRunning = True
if self.score > self.highscore:
self.highscore = self.score
self.highscoreTime = self.amountOfSimulations
self.previousScore = self.score
self.score = 0
def __init__(self):
self.Kp = 1
self.Ki = 0.7
self.Kd = 1
self.maxspeed = 3
self.TimestepCtr = 0
self.errorArray = []
self.errorIntArray = []
self.errorDerArray = []
self.outputArray = []
self.angleArray = []
self.mysystem = chrono.ChSystemNSC()
self.ground = theBattleground.theBattleground(self.mysystem)
self.robot = theRobot.theRobot(self.mysystem)
self.createApplication()
self.run()
def initialise_system(self, use_set_params=False):
self.system = chrono.ChSystemNSC()
# Modify some setting of the physical system for the simulation, if you want
self.system.SetSolverType(chrono.ChSolver.Type_BARZILAIBORWEIN)
# mysystem.SetSolverMaxIterations(20)
self.add_container()
self.dice = self.add_dice(use_set_params)
# adding start up facing side to past_start_params
start_most_up_side_idx, how_much = self.find_up_face_idx()
if round(how_much, 1) == 1.0:
how_much = 0
else:
how_much = 1
try:
how_much += self.past_start_params[-2][-1]
except IndexError:
pass
self.past_start_params[-1] += (start_most_up_side_idx, how_much)
import pychrono.irrlicht as chronoirr
import math
print("Example: demonstration of a universal joint")
# The path to the Chrono data directory containing various assets (meshes, textures, data files)
# is automatically set, relative to the default location of this demo.
# If running from a different directory, you must change the path to the data directory with:
#chrono.SetChronoDataPath('relative/path/to/data/directory/')
# ---------------------------------------------------------------------
#
# Create the simulation system and add items
#
mysystem = chrono.ChSystemNSC()
mysystem.Set_G_acc(chrono.ChVectorD(0, 0, 0))
# Set the half-length of the two shafts
hl = 2
# Set the bend angle between the two shafts (positive rotation
# about the global X axis)
angle = math.pi / 6.
cosa = math.cos(angle)
sina = math.sin(angle)
rot = chrono.Q_from_AngX(angle)
# Create the ground body
# ----------------------
ground = chrono.ChBody()
# ---------------------------------------------------------------------
#
# Create the simulation system and add items
#
# Change use_NSC to specify different contact method
use_NSC = 0
ball_radius = 0.5
obst_radius = 2.0
obst_center = chrono.ChVectorD(2.9, 0, 2.9)
obstacle = MyObstacle(obst_radius, obst_center)
# Create the system and the various contact materials
if use_NSC:
sys = chrono.ChSystemNSC()
g_mat = chrono.ChMaterialSurfaceNSC()
g_mat.SetRestitution(0.9)
g_mat.SetFriction(0.4)
b_mat = chrono.ChMaterialSurfaceNSC()
b_mat.SetRestitution(0.9)
b_mat.SetFriction(0.5)
o_mat = chrono.ChMaterialSurfaceNSC()
o_mat.SetRestitution(0.9)
o_mat.SetFriction(0.4)
ground_mat = g_mat
ball_mat = b_mat
obst_mat = o_mat
time_step = 1e-3
def main():
# -----------------
# Create the system
# -----------------
mphysicalSystem = chrono.ChSystemNSC()
mphysicalSystem.Set_G_acc(chrono.ChVectorD(0, 0, -9.81))
# ----------------------------------------
# add a floor, box and sphere to the scene
# ----------------------------------------
phys_mat = chrono.ChMaterialSurfaceNSC()
phys_mat.SetFriction(0.5)
phys_mat.SetDampingF(0.00000)
phys_mat.SetCompliance(1e-9)
phys_mat.SetComplianceT(1e-9)
floor = chrono.ChBodyEasyBox(10, 10, 1, 1000, True, True, phys_mat)
floor.SetPos(chrono.ChVectorD(0, 0, -1))
floor.SetBodyFixed(True)
mphysicalSystem.Add(floor)
box = chrono.ChBodyEasyBox(1, 1, 1, 1000, True, True, phys_mat)
box.SetPos(chrono.ChVectorD(0, 0, 5))
box.SetRot(chrono.Q_from_AngAxis(.2, chrono.ChVectorD(1, 0, 0)))
mphysicalSystem.Add(box)
sphere = chrono.ChBodyEasySphere(.5, 1000, True, True, phys_mat)
sphere.SetPos(chrono.ChVectorD(0, 0, 8))
sphere.SetRot(chrono.Q_from_AngAxis(.2, chrono.ChVectorD(1, 0, 0)))
mphysicalSystem.Add(sphere)
sphere_asset = sphere.GetAssets()[0]
visual_asset = chrono.CastToChVisualization(sphere_asset)
vis_mat = chrono.ChVisualMaterial()
vis_mat.SetAmbientColor(chrono.ChVectorF(0, 0, 0))
vis_mat.SetDiffuseColor(chrono.ChVectorF(.2, .2, .9))
vis_mat.SetSpecularColor(chrono.ChVectorF(.9, .9, .9))
visual_asset.material_list.append(vis_mat)
# -----------------------
# Create a sensor manager
# -----------------------
manager = sens.ChSensorManager(mphysicalSystem)
manager.scene.AddPointLight(chrono.ChVectorF(100, 100, 100),
chrono.ChVectorF(1, 1, 1), 1000.0)
manager.scene.AddPointLight(chrono.ChVectorF(-100, -100, 100),
chrono.ChVectorF(1, 1, 1), 1000.0)
# ------------------------------------------------
# Create a camera 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)))
cam = sens.ChCameraSensor(
floor, # body camera is attached to
cam_update_rate, # update rate in Hz
offset_pose, # offset pose
image_width, # number of horizontal samples
image_height, # number of vertical channels
cam_fov # vertical field of view
)
cam.SetName("Camera Sensor")
cam.SetLag(cam_lag)
cam.SetCollectionWindow(cam_collection_time)
# ------------------------------------------------------------------
# Create a filter graph for post-processing the data from the camera
# ------------------------------------------------------------------
# Visualizes the image
if vis:
cam.PushFilter(
sens.ChFilterVisualize(image_width, image_height, "RGB Image"))
# Save the current image to a png file at the specified path
if (save):
cam.PushFilter(sens.ChFilterSave(out_dir + "/rgb/"))
# Provides the host access to this RGBA8 buffer
cam.PushFilter(sens.ChFilterRGBA8Access())
# 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/"))
# Access the grayscaled buffer as R8 pixels
cam.PushFilter(sens.ChFilterR8Access())
# Add a camera to a sensor manager
manager.AddSensor(cam)
# ------------------------------------------------
# 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(
floor, # body lidar is attached to
lidar_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,
min_vert_angle, # vertical field of view
100 #max lidar range
)
lidar.SetName("Lidar Sensor")
lidar.SetLag(lidar_lag)
lidar.SetCollectionWindow(lidar_collection_time)
# -----------------------------------------------------------------
# Create a filter graph for post-processing the data from the lidar
# -----------------------------------------------------------------
if vis:
# Randers 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)
# ----------------------------------------------
# Create an IMU sensor and add it to the manager
# ----------------------------------------------
offset_pose = chrono.ChFrameD(
chrono.ChVectorD(-8, 0, 1),
chrono.Q_from_AngAxis(0, chrono.ChVectorD(0, 1, 0)))
imu = sens.ChIMUSensor(
box, # body imu is attached to
imu_update_rate, # update rate in Hz
offset_pose, # offset pose
imu_noise_none, # noise model
)
imu.SetName("IMU Sensor")
imu.SetLag(imu_lag)
imu.SetCollectionWindow(imu_collection_time)
# Provides the host access to the imu data
imu.PushFilter(sens.ChFilterIMUAccess())
# Add the imu to the sensor manager
manager.AddSensor(imu)
# ----------------------------------------------
# Create an GPS sensor and add it to the manager
# ----------------------------------------------
offset_pose = chrono.ChFrameD(
chrono.ChVectorD(-8, 0, 1),
chrono.Q_from_AngAxis(0, chrono.ChVectorD(0, 1, 0)))
gps = sens.ChGPSSensor(
box, # body imu is attached to
gps_update_rate, # update rate in Hz
offset_pose, # offset pose
gps_reference,
gps_noise_none # noise model
)
gps.SetName("GPS Sensor")
gps.SetLag(gps_lag)
gps.SetCollectionWindow(gps_collection_time)
# Provides the host access to the gps data
gps.PushFilter(sens.ChFilterGPSAccess())
# Add the gps to the sensor manager
manager.AddSensor(gps)
# ---------------
# Simulate system
# ---------------
t1 = time.time()
ch_time = 0
while (ch_time < end_time):
# Access the sensor data
camera_data_RGBA8 = cam.GetMostRecentRGBA8Buffer()
camera_data_R8 = cam.GetMostRecentR8Buffer()
lidar_data_DI = lidar.GetMostRecentDIBuffer()
lidar_data_XYZI = lidar.GetMostRecentXYZIBuffer()
gps_data = gps.GetMostRecentGPSBuffer()
imu_data = imu.GetMostRecentIMUBuffer()
# Check data is present
# If so, print out the max value
if camera_data_RGBA8.HasData():
print("Camera RGBA8:",
camera_data_RGBA8.GetRGBA8Data().shape, "max:",
np.max(camera_data_RGBA8.GetRGBA8Data()))
if camera_data_R8.HasData():
print("Camera R8:",
camera_data_R8.GetChar8Data().shape, "max:",
np.max(camera_data_R8.GetChar8Data()))
if lidar_data_DI.HasData():
print("Lidar DI:",
lidar_data_DI.GetDIData().shape, "max:",
np.max(lidar_data_DI.GetDIData()))
if lidar_data_XYZI.HasData():
print("Lidar XYZI:",
lidar_data_XYZI.GetXYZIData().shape, "max:",
np.max(lidar_data_XYZI.GetXYZIData()))
if gps_data.HasData():
print("GPS:",
gps_data.GetGPSData().shape, "max:",
np.max(gps_data.GetGPSData()))
if imu_data.HasData():
print("IMU:",
imu_data.GetIMUData().shape, "max:",
np.max(imu_data.GetIMUData()))
# 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 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)
""" test that you can replicate the results of the 2DOF script, with functional approach """ #---------------------------- Imports ----------------------------------------- import pychrono.core as chrono import pychrono.irrlicht as chronoirr import numpy as np import SimTools as st #---------------------------- run --------------------------------------------- #%% effects under gravity sys = chrono.ChSystemNSC() #initialize the system st.models.buildALEXR(sys) #add ALEXR robot st.drivers.addRevJoints(sys) #add passive revolute joints st.animateSystem(sys) #visualize the system #%% source of vibration - example of a situation in which the assembly cannot solve sys = chrono.ChSystemNSC() #initialize the system st.models.buildALEXR(sys) #add ALEXR robot st.drivers.addRotationAngleDrivers(sys) #add rotational drivers st.animateSystem(sys) #visualize the system #%% ----------------------------- NOTES ---------------------------------------
import pychrono.core as chrono
import pychrono.irrlicht as chronoirr
print("Example: demonstration of using friction models")
# The path to the Chrono data directory containing various assets (meshes, textures, data files)
# is automatically set, relative to the default location of this demo.
# If running from a different directory, you must change the path to the data directory with:
# chrono.SetChronoDataPath('relative/path/to/data/directory/')
# ---------------------------------------------------------------------
#
# Create the simulation system and add items
#
mphysicalSystem = chrono.ChSystemNSC()
# Create all the rigid bodies.
mradius = 0.5
density = 1000
# Create a texture asset. It can be shared between bodies.
textureasset = chrono.ChTexture(
chrono.GetChronoDataFile("textures/bluewhite.png"))
# Create some spheres that roll horizontally, with increasing rolling friction values
for bi in range(10):
mat = chrono.ChMaterialSurfaceNSC()
mat.SetFriction(0.4)
mat.SetRollingFriction((float(bi) / 10.) * 0.05)
def run_sim(traits, trial_num, gen_num, difficulty_level):
my_system = chrono.ChSystemNSC()
# Set the default outward/inward shape margins for collision detection
chrono.ChCollisionModel.SetDefaultSuggestedEnvelope(0.001)
chrono.ChCollisionModel.SetDefaultSuggestedMargin(0.001)
# Sets simulation precision
my_system.SetMaxItersSolverSpeed(70)
# Create a contact material (surface property)to share between all objects.
rollfrict_param = 0.5 / 10.0 * 0.05
brick_material = chrono.ChMaterialSurfaceNSC()
brick_material.SetFriction(0.5)
brick_material.SetDampingF(0.2)
brick_material.SetCompliance(0.0000001)
brick_material.SetComplianceT(0.0000001)
brick_material.SetRollingFriction(rollfrict_param)
brick_material.SetSpinningFriction(0.00000001)
brick_material.SetComplianceRolling(0.0000001)
brick_material.SetComplianceSpinning(0.0000001)
# Create the set of bricks in a vertical stack, along Y axis
block_bodies = [] # visualizes bodies
block_shapes = [] # geometry purposes
current_y = 0
for block_index in range(0, 4):
size_brick_x = traits[block_index][0]
size_brick_z = traits[block_index][1]
size_brick_y = traits[block_index][2]
if size_brick_y < settings.MIN_DIMENSIONS_THRESHOLD or size_brick_x < settings.MIN_DIMENSIONS_THRESHOLD or size_brick_z < settings.MIN_DIMENSIONS_THRESHOLD:
return [-50, traits]
mass_brick = settings.BLOCK_MASS
inertia_brick_xx = 1 / 12 * mass_brick * (pow(size_brick_z, 2) + pow(size_brick_y, 2))
inertia_brick_yy = 1 / 12 * mass_brick * (pow(size_brick_x, 2) + pow(size_brick_z, 2))
inertia_brick_zz = 1 / 12 * mass_brick * (pow(size_brick_x, 2) + pow(size_brick_y, 2))
body_brick = chrono.ChBody()
body_brick.SetPos(chrono.ChVectorD(0, current_y + 0.5 * size_brick_y, 0)) # set initial position
current_y += size_brick_y # set tower block positions
# setting mass properties
body_brick.SetMass(mass_brick)
body_brick.SetInertiaXX(chrono.ChVectorD(inertia_brick_xx, inertia_brick_yy, inertia_brick_zz))
# set collision surface properties
body_brick.SetMaterialSurface(brick_material)
# Collision shape
body_brick.GetCollisionModel().ClearModel()
body_brick.GetCollisionModel().AddBox(size_brick_x / 2, size_brick_y / 2,
size_brick_z / 2) # must set half sizes
body_brick.GetCollisionModel().BuildModel()
body_brick.SetCollide(True)
# Visualization shape, for rendering animation
body_brick_shape = chrono.ChBoxShape()
body_brick_shape.GetBoxGeometry().Size = chrono.ChVectorD(size_brick_x / 2, size_brick_y / 2,
size_brick_z / 2)
if block_index % 2 == 0:
body_brick_shape.SetColor(chrono.ChColor(0.65, 0.65, 0.6)) # set gray color only for odd bricks
body_brick.GetAssets().push_back(body_brick_shape)
my_system.Add(body_brick)
block_bodies.append(body_brick)
block_shapes.append(body_brick_shape);
# Create the room floor
body_floor = chrono.ChBody()
body_floor.SetBodyFixed(True)
body_floor.SetPos(chrono.ChVectorD(0, -2, 0))
body_floor.SetMaterialSurface(brick_material)
# Floor's collision shape
body_floor.GetCollisionModel().ClearModel()
body_floor.GetCollisionModel().AddBox(3, 1, 3) # hemi sizes default: 3,1,3
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.GetChronoDataPath() + '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
body_table.GetCollisionModel().ClearModel()
body_table.GetCollisionModel().AddBox(size_table_x / 2, size_table_y / 2, size_table_z / 2) # hemi sizes
body_table.GetCollisionModel().BuildModel()
body_table.SetCollide(True)
# Visualization shape
body_table_shape = chrono.ChBoxShape()
body_table_shape.GetBoxGeometry().Size = chrono.ChVectorD(size_table_x / 2, size_table_y / 2, size_table_z / 2)
body_table_shape.SetColor(chrono.ChColor(0.4, 0.4, 0.5))
body_table.GetAssets().push_back(body_table_shape)
body_table_texture = chrono.ChTexture()
# body_table_texture.SetTextureFilename(chrono.GetChronoDataPath() + 'concrete.jpg')
body_table.GetAssets().push_back(body_table_texture)
my_system.Add(body_table)
# Makes the table shake
link_shaker = chrono.ChLinkLockLock()
# link_shaker.SetMotion_X()
link_shaker.Initialize(body_table, body_floor, chrono.CSYSNORM)
my_system.Add(link_shaker)
# ..create the function for imposed y vertical motion, etc.
mfunY = chrono.ChFunction_Sine(0, settings.TABLE_FREQ_Y, settings.TABLE_AMP_Y) # phase, frequency, amplitude
# ..create the function for imposed z horizontal motion, etc.
mfunZ = chrono.ChFunction_Sine(0, settings.TABLE_FREQ_Z, settings.TABLE_AMP_Z) # phase, frequency, amplitude
# ..create the function for imposed x horizontal motion, etc.
mfunX = chrono.ChFunction_Sine(2, 0, 0) # phase, frequency, amplitude
print("Sim env_level " + str(difficulty_level))
if difficulty_level == settings.SHAKE_IN_X_AXIS_LEVEL:
mfunX = chrono.ChFunction_Sine(2, settings.TABLE_FREQ_X, settings.TABLE_AMP_X) # phase, frequency, amplitude
elif difficulty_level >= settings.SHAKE_IN_X_AND_Z_AXIS_LEVEL:
increment = 0.25 * difficulty_level
mfunX = chrono.ChFunction_Sine(2, settings.TABLE_FREQ_X + increment, settings.TABLE_AMP_X) # phase, frequency, amplitude
mfunZ = chrono.ChFunction_Sine(0, settings.TABLE_FREQ_Z + increment, settings.TABLE_AMP_Z) # phase, frequency, amplitude
link_shaker.SetMotion_Y(mfunY)
link_shaker.SetMotion_Z(mfunZ)
link_shaker.SetMotion_X(mfunX)
# ---------------------------------------------------------------------
#
# Create an Irrlicht application to visualize the system
window_name = "Tower Trial: " + str(trial_num) + " Gen: " + str(gen_num)
if trial_num == -1:
window_name = "Initializing Population..."
app = chronoirr.ChIrrApp(my_system, window_name, chronoirr.dimension2du(settings.SCREEN_WIDTH, settings.SCREEN_HEIGHT))
app.AddTypicalSky()
app.AddTypicalLogo(chrono.GetChronoDataPath() + 'logo_pychrono_alpha.png')
app.AddTypicalCamera(chronoirr.vector3df(settings.CAMERA_X, settings.CAMERA_Y, settings.CAMERA_Z))
app.AddLightWithShadow(chronoirr.vector3df(2, 4, 2), # point
chronoirr.vector3df(0, 0, 0), # aimpoint
9, # radius (power)
1, 9, # near, far
30)
# Committing visualization
app.AssetBindAll()
app.AssetUpdateAll();
app.AddShadowAll();
# ---------------------------------------------------------------------
#
# Run the simulation. This is where all of the constraints are set
#
app.SetTimestep(settings.SPEED)
app.SetTryRealtime(True)
app.GetDevice().run()
fitness = 0
brick1_init = block_bodies[0].GetPos().y
brick2_init = block_bodies[1].GetPos().y
brick3_init = block_bodies[2].GetPos().y
brick4_init = block_bodies[3].GetPos().y # Highest
while True:
brick1_curr_height = block_bodies[0].GetPos().y
brick2_curr_height = block_bodies[1].GetPos().y
brick3_curr_height = block_bodies[2].GetPos().y
brick4_curr_height = block_bodies[3].GetPos().y # Highest
# Break conditions
if my_system.GetChTime() > settings.SIMULATION_RUNTIME:
break
if my_system.GetChTime() > settings.SIMULATION_RUNTIME / 2:
mfunX = chrono.ChFunction_Sine(2, 0, 0)
mfunZ = chrono.ChFunction_Sine(2, 0, 0)
link_shaker.SetMotion_Z(mfunZ)
link_shaker.SetMotion_X(mfunX)
# If the blocks fall out of line
if brick1_init - brick1_curr_height > settings.CANCEL_SIM_THRESHOLD or \
brick2_init - brick2_curr_height > settings.CANCEL_SIM_THRESHOLD or \
brick3_init - brick3_curr_height > settings.CANCEL_SIM_THRESHOLD or \
brick4_init - brick4_curr_height > settings.CANCEL_SIM_THRESHOLD:
break
# Record fitness every 1/1000 of the runtime
if 0.01 > my_system.GetChTime() % ((1 / 1000) * settings.SIMULATION_RUNTIME) > 0:
if settings.FITNESS_FUNCTION == settings.Fitness.SumLengths: # Sum of size_y
fitness += block_shapes[0].GetBoxGeometry().GetLengths().y + \
block_shapes[1].GetBoxGeometry().GetLengths().y + \
block_shapes[2].GetBoxGeometry().GetLengths().y + \
block_shapes[3].GetBoxGeometry().GetLengths().y
elif settings.FITNESS_FUNCTION == settings.Fitness.MaxPosition: # Max of y positions
fitness += max(block_bodies[0].GetPos().y,
block_bodies[1].GetPos().y,
block_bodies[2].GetPos().y,
block_bodies[3].GetPos().y)
elif settings.FITNESS_FUNCTION == settings.Fitness.MaxPositionSumLengths: # Max * sum of sizes
fitness += max(block_bodies[0].GetPos().y,
block_bodies[1].GetPos().y,
block_bodies[2].GetPos().y,
block_bodies[3].GetPos().y) * \
block_shapes[0].GetBoxGeometry().GetLengths().y + \
block_shapes[1].GetBoxGeometry().GetLengths().y + \
block_shapes[2].GetBoxGeometry().GetLengths().y + \
block_shapes[3].GetBoxGeometry().GetLengths().y
app.BeginScene()
app.DrawAll()
for substep in range(0, 5):
app.DoStep()
app.EndScene()
app.GetDevice().closeDevice()
print("Fitness: " + str(fitness) + " Gen: " + str(gen_num))
return [fitness, traits]
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)