def vect_to_rotation(vector): normal = np.asarray([0, 1, 0]) if (vector == normal).all(): rotation = chrono.Q_from_AngAxis(0, chrono.ChVectorD(*normal)) else: rot_vector = np.cross(normal, vector) rot_vector = rot_vector / np.linalg.norm(rot_vector) angle = np.arccos(np.dot(normal, vector)) rotation = chrono.Q_from_AngAxis(angle, chrono.ChVectorD(*rot_vector)) return rotation
def __init__(self, die_file='die.obj'): self.container_width = 200 self.container_length = 200 self.with_walls = True self.wall_height = 50 self.dice_num = 1 self.polygon = None self.normal_rotation_offset = chrono.Q_from_AngAxis(0, chrono.VECT_Z) if type(die_file) is Polygon: self.polygon = die_file self.die_file = self.polygon.get_chrono_mesh() angle, axis = self.polygon.align_normal_to_vector(0, [0, 1, 0], only_info=True) self.normal_rotation_offset = chrono.Q_from_AngAxis( angle, chrono.ChVectorD(*axis)) else: self.die_file = die_file self.cut_off = 0.0001 # Contact material for container self.ground_mat = chrono.ChMaterialSurfaceNSC() self.ground_mat.SetFriction(0.8) # Shared contact materials for falling objects self.dice_mat = chrono.ChMaterialSurfaceNSC() self.dice_mat.SetFriction(0.5) # initialise start parameters self.dice_position = [0, 5, 0] self.dice_speed = [0, 0, 0] self.dice_rotation = chrono.Q_from_AngAxis(30 * chrono.CH_C_DEG_TO_RAD, chrono.VECT_Z) self.dice_ang_speed = [0, 0, 0] self.past_start_params = [] self.run_results = [] # initialise other variables self.system = None self.dice = list() # init system self.initialise_system()
def CreateStatorRotor(material, msystem, mpos): mstator = chrono.ChBodyEasyCylinder(0.5, 0.1, 1000, True, True, material) mstator.SetPos(mpos) mstator.SetRot(chrono.Q_from_AngAxis(chrono.CH_C_PI_2, chrono.VECT_X)) mstator.SetBodyFixed(True) msystem.Add(mstator) mrotor = chrono.ChBodyEasyBox(1, 0.1, 0.1, 1000, True, True, material) mrotor.SetPos(mpos + chrono.ChVectorD(0.5, 0, -0.15)) msystem.Add(mrotor) mcolor = chrono.ChColorAsset(0.6, 0.6, 0.0) mrotor.AddAsset(mcolor) return mstator, mrotor
manager = sens.ChSensorManager(my_rccar.GetSystem()) manager.scene.AddPointLight(chrono.ChVectorF(100,100,100),chrono.ChVectorF(1,1,1),500.0) manager.scene.AddPointLight(chrono.ChVectorF(-100,100,100),chrono.ChVectorF(1,1,1),500.0) manager.scene.GetBackground().has_texture = True; manager.scene.GetBackground().env_tex = "sensor/textures/cloud_layers_8k.hdr"; manager.scene.GetBackground().has_changed = True; #field of view: fov = 1.408 camera_3rd = sens.ChCameraSensor( my_rccar.GetChassisBody(), # body lidar is attached to 60, # scanning rate in Hz chrono.ChFrameD(chrono.ChVectorD(-2, 0, 1), chrono.Q_from_AngAxis(.3, chrono.ChVectorD(0, 1, 0))), # offset pose 1920*2, # number of horizontal samples 1080*2, # number of vertical channels chrono.CH_C_PI / 4 # horizontal field of view # vertical field of view ) camera_3rd.SetName("Camera Sensor") if(visualize): camera_3rd.PushFilter(sens.ChFilterVisualize(1280,720,"Third Person Camera")) if(save_data): camera_3rd.PushFilter(sens.ChFilterSave(1280,720,"output/iros/third_person_camera/")) manager.AddSensor(camera_3rd) camera_front = sens.ChCameraSensor( my_rccar.GetChassisBody(), # body lidar is attached to 60, # scanning rate in Hz chrono.ChFrameD(chrono.ChVectorD(0, 0, .2),
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)
manager = sens.ChSensorManager(my_rccar.GetSystem()) f = 3 for i in range(8): manager.scene.AddPointLight(chrono.ChVectorF(f, 1.25, 2.3), chrono.ChVectorF(1, 1, 1), 5) manager.scene.AddPointLight(chrono.ChVectorF(f, 3.75, 2.3), chrono.ChVectorF(1, 1, 1), 5) f += 3 factor = 2 cam = sens.ChCameraSensor( my_rccar.GetChassisBody(), # body lidar is attached to 30, # scanning rate in Hz chrono.ChFrameD(chrono.ChVectorD(0, 0, .5), chrono.Q_from_AngAxis(0, chrono.ChVectorD(0, 1, 0))), # offset pose 1920 * factor, # number of horizontal samples 1080 * factor, # number of vertical channels chrono.CH_C_PI / 4, # horizontal field of view (720 / 1280) * chrono.CH_C_PI / 4. # vertical field of view ) cam.SetName("Camera Sensor") # cam.FilterList().append(sens.ChFilterImgAlias(factor)) # cam.FilterList().append(sens.ChFilterVisualize(1920, 1080, "Third Person Camera")) # cam.FilterList().append(sens.ChFilterRGBA8Access()) manager.AddSensor(cam) cam2 = sens.ChCameraSensor( my_rccar.GetChassisBody(), # body lidar is attached to 30, # scanning rate in Hz
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
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
floor_body.SetBodyFixed(True) system.Add(floor_body) die_file = dodeca.get_chrono_mesh() die = chrono.ChBodyEasyMesh( die_file, # obj filename 10000, # density True, # compute mass? True, # visualization? True, # collision? dice_mat) # material system.Add(die) dice_position = [0, 5, 0] dice_speed = [0, 0, 0] dice_rotation = chrono.Q_from_AngAxis( -0 * chrono.CH_C_DEG_TO_RAD, chrono.VECT_Y) * chrono.Q_from_AngAxis( -91 * chrono.CH_C_DEG_TO_RAD, chrono.VECT_Z) print(dice_rotation, chrono.Q_to_Euler123(dice_rotation) * chrono.CH_C_RAD_TO_DEG) normals = dodeca.face_normals for normal in normals: ch_normal = chrono.ChVectorD(*normal) ch_normal = dice_rotation.Rotate(ch_normal) if round(ch_normal ^ chrono.VECT_X, 1) == 1 or round( ch_normal ^ chrono.VECT_Y, 1) == 1 or round( ch_normal ^ chrono.VECT_Z, 1) == 1: print(ch_normal) dice_ang_speed = [0, 0, 0] # self.dice_position = 10 * ([2, 1, 2] * np.random.random(3) + [-1, 0.5, -1])
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)