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)