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()
Esempio n. 3
0
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
Esempio n. 4
0

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),
Esempio n. 5
0
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)
Esempio n. 6
0
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)
Esempio n. 7
0
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
Esempio n. 8
0
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])
Esempio n. 11
0
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)