def AddMovingObstacles(system):
    # Create contact material, of appropriate type. Use default properties
    material = None
    if (NSC_SMC == chrono.ChContactMethod_NSC):
        matNSC = chrono.ChMaterialSurfaceNSC()
        #Change NSC material properties as desired
        material = matNSC
    elif (NSC_SMC == chrono.ChContactMethod_SMC):
        matSMC = chrono.ChMaterialSurfaceSMC()
        # Change SMC material properties as desired
        material = matSMC
    else:
        raise ("Unvalid Contact Method")
    sizeX = 300
    sizeY = 300
    height = 0
    numObstacles = 10

    for i in range(numObstacles):
        o_sizeX = 1.0 + 3.0 * chrono.ChRandom()
        o_sizeY = 0.3 + 0.2 * chrono.ChRandom()
        o_sizeZ = 0.05 + 0.1 * chrono.ChRandom()
        obstacle = chrono.ChBodyEasyBox(o_sizeX, o_sizeY, o_sizeZ, 2000.0,
                                        True, True, material)

        o_posX = (chrono.ChRandom() - 0.5) * 0.4 * sizeX
        o_posY = (chrono.ChRandom() - 0.5) * 0.4 * sizeY
        o_posZ = height + 4
        rot = chrono.ChQuaternionD(chrono.ChRandom(), chrono.ChRandom(),
                                   chrono.ChRandom(), chrono.ChRandom())
        rot.Normalize()
        obstacle.SetPos(chrono.ChVectorD(o_posX, o_posY, o_posZ))
        obstacle.SetRot(rot)

        system.AddBody(obstacle)
    def __init__(self, filename, scale_range):
        self.filename = filename
        self.scale_range = scale_range
        self.ready = False

        self.mesh = chrono.ChTriangleMeshConnected()
        self.mesh.LoadWavefrontMesh(chrono.GetChronoDataFile(self.filename), True, True)

        self.shape = chrono.ChTriangleMeshShape()
        self.shape.SetMesh(self.mesh)
        self.shape.SetStatic(True)

        self.body = chrono.ChBody()
        self.body.AddAsset(self.shape)
        self.body.SetCollide(True)
        self.body.SetBodyFixed(True)

        surface_mat = chrono.ChMaterialSurfaceNSC()
        surface_mat.SetFriction(0.9)
        surface_mat.SetRestitution(0.01)

        self.body.GetCollisionModel().ClearModel()
        self.body.GetCollisionModel().AddTriangleMesh(surface_mat, self.mesh, False, False)
        self.body.GetCollisionModel().BuildModel()

        self.scale = 1
def AddFixedObstacles(system):
    # Create contact material, of appropriate type. Use default properties
    material = None
    if (NSC_SMC == chrono.ChContactMethod_NSC):
        matNSC = chrono.ChMaterialSurfaceNSC()
        #Change NSC material properties as desired
        material = matNSC
    elif (NSC_SMC == chrono.ChContactMethod_SMC):
        matSMC = chrono.ChMaterialSurfaceSMC()
        # Change SMC material properties as desired
        material = matSMC
    else:
        raise ("Unvalid Contact Method")
    radius = 3
    length = 10
    obstacle = chrono.ChBodyEasyCylinder(radius, length, 2000, True, True,
                                         material)

    obstacle.SetPos(chrono.ChVectorD(-20, 0, -2.7))
    obstacle.SetBodyFixed(True)

    system.AddBody(obstacle)

    for i in range(8):
        stoneslab = chrono.ChBodyEasyBox(0.5, 2.5, 0.25, 2000, True, True,
                                         material)
        stoneslab.SetPos(chrono.ChVectorD(-1.2 * i + 22, -1.5, -0.05))

        stoneslab.SetRot(
            chrono.Q_from_AngAxis(15 * chrono.CH_C_DEG_TO_RAD, chrono.VECT_Y))
        stoneslab.SetBodyFixed(True)
        system.AddBody(stoneslab)
Beispiel #4
0
    def __init__(self):
        ChronoBaseEnv.__init__(self)
        #self._set_observation_space(np.ndarray([4,]))
        #self.action_space = np.zeros([4,])
        low = np.full(4, -1000)
        high = np.full(4, 1000)
        self.observation_space = spaces.Box(low, high, dtype=np.float32)
        self.action_space = spaces.Box(low=-1.0,
                                       high=1.0,
                                       shape=(1, ),
                                       dtype=np.float32)
        self.info = {"timeout": 100000}
        self.timestep = 0.01
        # ---------------------------------------------------------------------
        #
        #  Create the simulation system and add items
        #

        self.rev_pend_sys = chrono.ChSystemNSC()

        chrono.ChCollisionModel.SetDefaultSuggestedEnvelope(0.001)
        chrono.ChCollisionModel.SetDefaultSuggestedMargin(0.001)

        #rev_pend_sys.SetSolverType(chrono.ChSolver.Type_BARZILAIBORWEIN) # precise, more slow
        self.rev_pend_sys.SetMaxItersSolverSpeed(70)

        # Create a contact material (surface property)to share between all objects.
        self.rod_material = chrono.ChMaterialSurfaceNSC()
        self.rod_material.SetFriction(0.5)
        self.rod_material.SetDampingF(0.2)
        self.rod_material.SetCompliance(0.0000001)
        self.rod_material.SetComplianceT(0.0000001)

        # Create the set of rods in a vertical stack, along Y axis

        self.size_rod_y = 2.0
        self.radius_rod = 0.05
        self.density_rod = 50  # kg/m^3

        self.mass_rod = self.density_rod * self.size_rod_y * chrono.CH_C_PI * (
            self.radius_rod**2)
        self.inertia_rod_y = (self.radius_rod**2) * self.mass_rod / 2
        self.inertia_rod_x = (self.mass_rod / 12) * ((self.size_rod_y**2) + 3 *
                                                     (self.radius_rod**2))

        self.size_table_x = 0.3
        self.size_table_y = 0.3
        self.size_table_z = 0.3
        self.reset()
Beispiel #5
0
# Visualization shape
body_1_2_shape = chrono.ChObjShapeFile()
body_1_2_shape.SetFilename(shapes_dir + 'body_1_2.obj')
body_1_2_level = chrono.ChAssetLevel()
body_1_2_level.GetFrame().SetPos(
    chrono.ChVectorD(0.0654391949504455, 0.053691361648147,
                     -0.187114016856069))
body_1_2_level.GetFrame().SetRot(
    chrono.ChQuaternionD(0.667465028254667, -0.66746502825467,
                         -0.233431865984476, -0.233431865984475))
body_1_2_level.GetAssets().push_back(body_1_2_shape)
body_1.GetAssets().push_back(body_1_2_level)

# Collision material
mat_1 = chrono.ChMaterialSurfaceNSC()

# Collision shapes
body_1.GetCollisionModel().ClearModel()
mr = chrono.ChMatrix33D()
mr[0, 0] = 0.563277959889745
mr[1, 0] = -3.62094996255649E-15
mr[2, 0] = -0.826267474793996
mr[0, 1] = 0.826267474793996
mr[1, 1] = 1.45159525356894E-15
mr[2, 1] = 0.563277959889746
mr[0, 2] = -8.40195363182309E-16
mr[1, 2] = -1
mr[2, 2] = 3.80952479493291E-15
body_1.GetCollisionModel().AddBox(
    mat_1, 0.00250923730353971, 0.00239999309720335, 0.00280857758415823,
Beispiel #6
0
def main():
    #print("Copyright (c) 2017 projectchrono.org\nChrono version: ", CHRONO_VERSION , "\n\n")

    #  Create the HMMWV vehicle, set parameters, and initialize
    my_hmmwv = veh.HMMWV_Full()
    my_hmmwv.SetContactMethod(contact_method)
    my_hmmwv.SetChassisFixed(False)
    my_hmmwv.SetInitPosition(
        chrono.ChCoordsysD(initLoc, chrono.ChQuaternionD(1, 0, 0, 0)))
    my_hmmwv.SetPowertrainType(powertrain_model)
    my_hmmwv.SetDriveType(drive_type)
    my_hmmwv.SetSteeringType(steering_type)
    my_hmmwv.SetTireType(tire_model)
    my_hmmwv.SetTireStepSize(tire_step_size)
    my_hmmwv.Initialize()

    my_hmmwv.SetChassisVisualizationType(chassis_vis_type)
    my_hmmwv.SetSuspensionVisualizationType(suspension_vis_type)
    my_hmmwv.SetSteeringVisualizationType(steering_vis_type)
    my_hmmwv.SetWheelVisualizationType(wheel_vis_type)
    my_hmmwv.SetTireVisualizationType(tire_vis_type)

    # Create the terrain

    terrain = veh.RigidTerrain(my_hmmwv.GetSystem())
    if (contact_method == chrono.ChContactMethod_NSC):
        patch_mat = chrono.ChMaterialSurfaceNSC()
        patch_mat.SetFriction(0.9)
        patch_mat.SetRestitution(0.01)
    elif (contact_method == chrono.ChContactMethod_SMC):
        patch_mat = chrono.ChMaterialSurfaceSMC()
        patch_mat.SetFriction(0.9)
        patch_mat.SetRestitution(0.01)
        patch_mat.SetYoungModulus(2e7)
    patch = terrain.AddPatch(patch_mat, chrono.ChVectorD(0, 0, 0),
                             chrono.ChVectorD(0, 0, 1), 300, 50)
    patch.SetTexture(veh.GetDataFile("terrain/textures/tile4.jpg"), 200, 200)
    patch.SetColor(chrono.ChColor(0.8, 0.8, 0.5))
    terrain.Initialize()

    # Create the path-follower, cruise-control driver
    # Use a parameterized ISO double lane change (to left)
    path = veh.DoubleLaneChangePath(initLoc, 13.5, 4.0, 11.0, 50.0, True)
    driver = veh.ChPathFollowerDriver(my_hmmwv.GetVehicle(), path, "my_path",
                                      target_speed)
    driver.GetSteeringController().SetLookAheadDistance(5)
    driver.GetSteeringController().SetGains(0.8, 0, 0)
    driver.GetSpeedController().SetGains(0.4, 0, 0)
    driver.Initialize()

    # Create the vehicle Irrlicht interface
    app = veh.ChWheeledVehicleIrrApp(my_hmmwv.GetVehicle(), 'HMMWV',
                                     irr.dimension2du(1000, 800))
    app.SetSkyBox()
    app.AddTypicalLights(irr.vector3df(-60, -30, 100),
                         irr.vector3df(60, 30, 100), 250, 130)
    app.AddTypicalLogo(chrono.GetChronoDataFile('logo_pychrono_alpha.png'))
    app.SetChaseCamera(chrono.ChVectorD(0.0, 0.0, 1.75), 6.0, 0.5)
    app.SetTimestep(step_size)
    app.AssetBindAll()
    app.AssetUpdateAll()

    # Visualization of controller points (sentinel & target)
    ballS = app.GetSceneManager().addSphereSceneNode(0.1)
    ballT = app.GetSceneManager().addSphereSceneNode(0.1)
    ballS.getMaterial(0).EmissiveColor = irr.SColor(0, 255, 0, 0)
    ballT.getMaterial(0).EmissiveColor = irr.SColor(0, 0, 255, 0)

    # Simulation loop
    realtime_timer = chrono.ChRealtimeStepTimer()
    while (app.GetDevice().run()):
        time = my_hmmwv.GetSystem().GetChTime()

        # End simulation
        if (time >= t_end):
            break

        # Update sentinel and target location markers for the path-follower controller.
        pS = driver.GetSteeringController().GetSentinelLocation()
        pT = driver.GetSteeringController().GetTargetLocation()
        ballS.setPosition(irr.vector3df(pS.x, pS.y, pS.z))
        ballT.setPosition(irr.vector3df(pT.x, pT.y, pT.z))

        # Draw scene
        app.BeginScene(True, True, irr.SColor(255, 140, 161, 192))
        app.DrawAll()
        app.EndScene()

        # Get driver inputs
        driver_inputs = driver.GetInputs()

        # Update modules (process inputs from other modules)
        driver.Synchronize(time)
        terrain.Synchronize(time)
        my_hmmwv.Synchronize(time, driver_inputs, terrain)
        app.Synchronize("", driver_inputs)

        # Advance simulation for one timestep for all modules
        driver.Advance(step_size)
        terrain.Advance(step_size)
        my_hmmwv.Advance(step_size)
        app.Advance(step_size)

        # Spin in place for real time to catch up
        realtime_timer.Spin(step_size)

    return 0
Beispiel #7
0
chrono.ChCollisionModel.SetDefaultSuggestedEnvelope(0.01)
chrono.ChCollisionModel.SetDefaultSuggestedMargin(0.005)
#chrono.ChCollisionSystemBullet.SetContactBreakingThreshold(0.01)

# Create a fixed rigid body

phi = 40 * chrono.CH_C_DEG_TO_RAD
rad_sph = 0.1
alpha = 45 * chrono.CH_C_DEG_TO_RAD
beta = alpha - 0 * chrono.CH_C_DEG_TO_RAD
thick = 0.2
fixed_L = True

friction = math.tan(phi)
print(friction)
brick_material = chrono.ChMaterialSurfaceNSC()
brick_material.SetFriction(friction)
brick_material.SetDampingF(0.00000)
brick_material.SetCompliance(1e-9)
brick_material.SetComplianceT(1e-9)
# brick_material.SetRollingFriction(rollfrict_param)
# brick_material.SetSpinningFriction(0)
# brick_material.SetComplianceRolling(0.0000001)
# brick_material.SetComplianceSpinning(0.0000001)

L_material = chrono.ChMaterialSurfaceNSC()
L_material.SetFriction(0)
# brick_material.SetDampingF(0.2)
# brick_material.SetCompliance (0.0000001)
# brick_material.SetComplianceT(0.0000001)
# brick_material.SetRollingFriction(rollfrict_param)
my_shbodyB.SetName('BodyB')
my_shbodyB.SetPos(chrono.ChVectorD(0,2,0))
my_shbodyB.GetCollisionModel().AddBox(1,1,1)
my_shbodyB.SetCollide(True)

my_shmarker = chrono.ChMarker()
my_funct = chrono.ChFunction_Sine(0,0.5,3)
my_shmarker.SetMotion_X(my_funct)
my_shmarker.SetPos(chrono.ChVectorD(1,2,3))
my_shbodyB.AddMarker(my_shmarker)

my_system.Add(my_shbodyA)
my_system.Add(my_shbodyB)

# Define surface material(s)
my_shmaterial = chrono.ChMaterialSurfaceNSC()
my_shmaterial.SetFriction(0.3)
my_shmaterial.SetCompliance(0)
my_shbodyA.SetMaterialSurface(my_shmaterial)
my_shbodyB.SetMaterialSurface(my_shmaterial)


# Add Contact callback (TO FIX!!)
##class MyContactCallback(chrono.ChCustomCollisionPointCallbackP):
##    def __init__(self):
##         chrono.ChCustomCollisionPointCallbackP.__init__(self)
##    def ContactCallback(self,collinfo,matcouple):
##         print (' add contact: ' , collinfo.distance, matcouple.static_friction)
##
##my_call = MyContactCallback()
##my_system.SetCustomCollisionPointCallback(my_call)
Beispiel #9
0
# Create a Chrono::Engine physical system
mphysicalSystem = chrono.ChSystemNSC()

# Create the Irrlicht visualization (open the Irrlicht device,
# bind a simple user interface, etc, etc.)
application = chronoirr.ChIrrApp(mphysicalSystem, "Gears annd pulleys",
                                 chronoirr.dimension2du(800, 600), False)

# Easy shortcuts to add camera, lights, logo, and sky in Irrlicht scene:
application.AddTypicalLogo()
application.AddTypicalSky()
application.AddTypicalLights()
application.AddTypicalCamera(chronoirr.vector3df(12, 15, -20))

# Contact material shared among all bodies
mat = chrono.ChMaterialSurfaceNSC()

# Create all rigid bodies.

radA = 2
radB = 4

# ...the truss
mbody_truss = chrono.ChBodyEasyBox(20, 10, 2, 1000, True, False, mat)
mphysicalSystem.Add(mbody_truss)
mbody_truss.SetBodyFixed(True)
mbody_truss.SetPos(chrono.ChVectorD(0, 0, 3))

# ...a texture asset that will be shared among the four wheels
cylinder_texture = chrono.ChTexture(
    chrono.GetChronoDataFile("textures/pinkwhite.png"))
    def reset(self):
        del self.manager
        material = chrono.ChMaterialSurfaceNSC()
        self.vehicle = veh.HMMWV_Reduced()
        self.vehicle.SetContactMethod(chrono.ChContactMethod_NSC)
        self.vehicle.SetChassisCollisionType(
            veh.ChassisCollisionType_PRIMITIVES)

        self.vehicle.SetChassisFixed(False)
        self.vehicle.SetInitPosition(
            chrono.ChCoordsysD(self.initLoc, self.initRot))
        self.vehicle.SetPowertrainType(veh.PowertrainModelType_SHAFTS)
        self.vehicle.SetDriveType(veh.DrivelineType_AWD)
        #self.vehicle.SetSteeringType(veh.SteeringType_PITMAN_ARM)
        self.vehicle.SetTireType(veh.TireModelType_TMEASY)
        self.vehicle.SetTireStepSize(self.timestep)
        self.vehicle.Initialize()

        #self.vehicle.SetStepsize(self.timestep)
        if self.play_mode == True:
            self.vehicle.SetChassisVisualizationType(
                veh.VisualizationType_MESH)
            self.vehicle.SetWheelVisualizationType(veh.VisualizationType_MESH)
            self.vehicle.SetTireVisualizationType(veh.VisualizationType_MESH)
        else:
            self.vehicle.SetChassisVisualizationType(
                veh.VisualizationType_PRIMITIVES)
            self.vehicle.SetWheelVisualizationType(
                veh.VisualizationType_PRIMITIVES)
        self.vehicle.SetSuspensionVisualizationType(
            veh.VisualizationType_PRIMITIVES)
        self.vehicle.SetSteeringVisualizationType(
            veh.VisualizationType_PRIMITIVES)
        self.chassis_body = self.vehicle.GetChassisBody()
        self.chassis_body.GetCollisionModel().ClearModel()
        size = chrono.ChVectorD(3, 2, 0.2)
        self.chassis_body.GetCollisionModel().AddBox(material, 0.5 * size.x,
                                                     0.5 * size.y,
                                                     0.5 * size.z)
        self.chassis_body.GetCollisionModel().BuildModel()

        # Driver
        self.driver = veh.ChDriver(self.vehicle.GetVehicle())

        # Rigid terrain
        self.system = self.vehicle.GetSystem()
        self.terrain = veh.RigidTerrain(self.system)
        patch = self.terrain.AddPatch(
            material, chrono.ChVectorD(0, 0, self.terrainHeight - .5),
            chrono.VECT_Z, self.terrainLength, self.terrainWidth, 10)
        material.SetFriction(0.9)
        material.SetRestitution(0.01)
        #patch.SetContactMaterialProperties(2e7, 0.3)
        patch.SetTexture(veh.GetDataFile("terrain/textures/tile4.jpg"), 200,
                         200)
        patch.SetColor(chrono.ChColor(0.8, 0.8, 0.5))
        self.terrain.Initialize()

        ground_body = patch.GetGroundBody()
        ground_asset = ground_body.GetAssets()[0]
        visual_asset = chrono.CastToChVisualization(ground_asset)
        vis_mat = chrono.ChVisualMaterial()
        vis_mat.SetKdTexture(chrono.GetChronoDataFile("concrete.jpg"))
        visual_asset.material_list.append(vis_mat)

        # create obstacles
        self.boxes = []
        for i in range(3):
            box = chrono.ChBodyEasyBox(2, 2, 10, 1000, True, True)
            box.SetPos(
                chrono.ChVectorD(25 + 25 * i,
                                 (np.random.rand(1)[0] - 0.5) * 10, .05))
            box.SetBodyFixed(True)
            box_asset = box.GetAssets()[0]
            visual_asset = chrono.CastToChVisualization(box_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)
            visual_asset.SetStatic(True)
            self.boxes.append(box)
            self.system.Add(box)

        # Set the time response for steering and throttle inputs.
        # NOTE: this is not exact, since we do not render quite at the specified FPS.
        steering_time = 1.0
        # time to go from 0 to +1 (or from 0 to -1)
        throttle_time = .5
        # time to go from 0 to +1
        braking_time = 0.3
        # time to go from 0 to +1
        self.SteeringDelta = (self.timestep / steering_time)
        self.ThrottleDelta = (self.timestep / throttle_time)
        self.BrakingDelta = (self.timestep / braking_time)

        self.manager = sens.ChSensorManager(self.system)
        self.manager.scene.AddPointLight(chrono.ChVectorF(100, 100, 100),
                                         chrono.ChVectorF(1, 1, 1), 4000.0)
        self.manager.scene.AddPointLight(chrono.ChVectorF(-100, -100, 100),
                                         chrono.ChVectorF(1, 1, 1), 4000.0)
        # ------------------------------------------------
        # Create a self.camera and add it to the sensor manager
        # ------------------------------------------------
        self.camera = sens.ChCameraSensor(
            self.chassis_body,  # body camera is attached to
            50,  # scanning rate in Hz
            chrono.ChFrameD(chrono.ChVectorD(1.5, 0, .875)),
            # offset pose
            self.camera_width,  # number of horizontal samples
            self.camera_height,  # number of vertical channels
            chrono.CH_C_PI / 3,  # horizontal field of view
            #(self.camera_height / self.camera_width) * chrono.CH_C_PI / 3.  # vertical field of view
        )
        self.camera.SetName("Camera Sensor")
        self.camera.PushFilter(sens.ChFilterRGBA8Access())
        self.manager.AddSensor(self.camera)
        # -----------------------------------------------------------------
        # Create a filter graph for post-processing the data from the lidar
        # -----------------------------------------------------------------

        self.d_old = np.linalg.norm(self.Xtarg + self.Ytarg)
        self.step_number = 0
        self.c_f = 0
        self.isdone = False
        self.render_setup = False
        if self.play_mode:
            self.render()

        return self.get_ob()
Beispiel #11
0

# Add items to the physical system
my_system = chrono.ChSystemNSC()
for my_item in exported_items:
    my_system.Add(my_item)






# Create a contact material (surface property)to share between all objects.
# The rolling and spinning parameters are optional - if enabled they double
# the computational time.
pebble_material = chrono.ChMaterialSurfaceNSC()
pebble_material.SetFriction(0.6)
#pebble_material.SetDampingF(0.1)
#pebble_material.SetCompliance (0.0000000001)
#pebble_material.SetComplianceT(0.0000000001)
pebble_material.SetRollingFriction(0.005)
pebble_material.SetSpinningFriction(0.005)
# pebble_material.SetComplianceRolling(0.0000001)
# pebble_material.SetComplianceSpinning(0.0000001)


# Assign the surface material also to items made with CAD:
my_bodyfloor = my_system.SearchBody('floor_box^assembly_conveyor-1')
if not my_bodyfloor :
    sys.exit('Error: cannot find floor_box from its name in the C::E system!')
Beispiel #12
0
def main():
    print("Copyright (c) 2017 projectchrono.org" + "\n\n")

    # Create systems

    #  Create the HMMWV vehicle, set parameters, and initialize
    my_hmmwv = veh.HMMWV_Full()
    my_hmmwv.SetContactMethod(contact_method)
    my_hmmwv.SetChassisCollisionType(chassis_collision_type)
    my_hmmwv.SetChassisFixed(False)
    my_hmmwv.SetInitPosition(chrono.ChCoordsysD(initLoc, initRot))
    my_hmmwv.SetPowertrainType(powertrain_model)
    my_hmmwv.SetDriveType(drive_type)
    my_hmmwv.SetSteeringType(steering_type)
    my_hmmwv.SetTireType(tire_model)
    my_hmmwv.SetTireStepSize(tire_step_size)
    my_hmmwv.Initialize()

    my_hmmwv.SetChassisVisualizationType(chassis_vis_type)
    my_hmmwv.SetSuspensionVisualizationType(suspension_vis_type)
    my_hmmwv.SetSteeringVisualizationType(steering_vis_type)
    my_hmmwv.SetWheelVisualizationType(wheel_vis_type)
    my_hmmwv.SetTireVisualizationType(tire_vis_type)

    # Create the terrain

    terrain = veh.RigidTerrain(my_hmmwv.GetSystem())
    if (contact_method == chrono.ChContactMethod_NSC):
        patch_mat = chrono.ChMaterialSurfaceNSC()
        patch_mat.SetFriction(0.9)
        patch_mat.SetRestitution(0.01)
    elif (contact_method == chrono.ChContactMethod_SMC):
        patch_mat = chrono.ChMaterialSurfaceSMC()
        patch_mat.SetFriction(0.9)
        patch_mat.SetRestitution(0.01)
        patch_mat.SetYoungModulus(2e7)
    patch = terrain.AddPatch(patch_mat, chrono.ChVectorD(0, 0, 0),
                             chrono.ChVectorD(0, 0, 1), terrainLength,
                             terrainWidth)
    patch.SetTexture(veh.GetDataFile("terrain/textures/tile4.jpg"), 200, 200)
    patch.SetColor(chrono.ChColor(0.8, 0.8, 0.5))
    terrain.Initialize()

    # Create the vehicle Irrlicht interface
    app = veh.ChWheeledVehicleIrrApp(my_hmmwv.GetVehicle(), 'HMMWV',
                                     irr.dimension2du(1000, 800))

    app.SetSkyBox()
    app.AddTypicalLights(irr.vector3df(30, -30, 100),
                         irr.vector3df(30, 50, 100), 250, 130)
    app.AddTypicalLogo(chrono.GetChronoDataFile('logo_pychrono_alpha.png'))
    app.SetChaseCamera(trackPoint, 6.0, 0.5)
    app.SetTimestep(step_size)
    app.AssetBindAll()
    app.AssetUpdateAll()

    # Initialize output

    try:
        os.mkdir(out_dir)
    except:
        print("Error creating directory ")

    # Set up vehicle output
    my_hmmwv.GetVehicle().SetChassisOutput(True)
    my_hmmwv.GetVehicle().SetSuspensionOutput(0, True)
    my_hmmwv.GetVehicle().SetSteeringOutput(0, True)
    my_hmmwv.GetVehicle().SetOutput(veh.ChVehicleOutput.ASCII, out_dir,
                                    "output", 0.1)

    # Generate JSON information with available output channels
    my_hmmwv.GetVehicle().ExportComponentList(out_dir + "/component_list.json")

    # Create the interactive driver system
    driver = veh.ChIrrGuiDriver(app)

    # Set the time response for steering and throttle keyboard inputs.
    steering_time = 1.0  # time to go from 0 to +1 (or from 0 to -1)
    throttle_time = 1.0  # time to go from 0 to +1
    braking_time = 0.3  # time to go from 0 to +1
    driver.SetSteeringDelta(render_step_size / steering_time)
    driver.SetThrottleDelta(render_step_size / throttle_time)
    driver.SetBrakingDelta(render_step_size / braking_time)

    driver.Initialize()

    # Simulation loop

    # Number of simulation steps between miscellaneous events
    render_steps = m.ceil(render_step_size / step_size)
    debug_steps = m.ceil(debug_step_size / step_size)

    # Initialize simulation frame counter and simulation time
    step_number = 0
    render_frame = 0

    if (contact_vis):
        app.SetSymbolscale(1e-4)
        # app.SetContactsDrawMode(irr.eCh_ContactsDrawMode::CONTACT_FORCES);

    # ---------------------------------------------
    # Create a sensor manager and add a point light
    # ---------------------------------------------
    manager = sens.ChSensorManager(my_hmmwv.GetSystem())
    manager.scene.AddPointLight(chrono.ChVectorF(0, 0, 100),
                                chrono.ChVectorF(2, 2, 2), 5000)
    manager.SetKeyframeSizeFromTimeStep(.001, 1 / 5)

    # ------------------------------------------------
    # Create a camera and add it to the sensor manager
    # ------------------------------------------------
    fov = 1.408
    lag = 0
    update_rate = 5
    exposure_time = 1 / update_rate
    offset_pose = chrono.ChFrameD(chrono.ChVectorD(-5, 0, 2))
    cam = sens.ChCameraSensor(
        my_hmmwv.GetChassisBody(),  # 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(0);
    # cam.SetCollectionWindow(0);

    # Visualizes the image
    if vis:
        cam.PushFilter(
            sens.ChFilterVisualize(image_width, image_height, "HMMWV Camera"))

    # Save the current image to a png file at the specified path
    if save:
        cam.PushFilter(sens.ChFilterSave(out_dir + "cam/"))

    # Add a camera to a sensor manager
    manager.AddSensor(cam)

    # ----------------------------------------------
    # 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(
        my_hmmwv.GetChassisBody(),  # 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(
        my_hmmwv.GetChassisBody(),  # 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)

    realtime_timer = chrono.ChRealtimeStepTimer()
    while (app.GetDevice().run()):
        time = my_hmmwv.GetSystem().GetChTime()

        #End simulation
        if (time >= t_end):
            break

        if (step_number % render_steps == 0):
            app.BeginScene(True, True, irr.SColor(255, 140, 161, 192))
            app.DrawAll()
            app.EndScene()

        #Debug logging
        if (debug_output and step_number % debug_steps == 0):
            print("\n\n============ System Information ============\n")
            print("Time = " << time << "\n\n")
            #my_hmmwv.DebugLog(OUT_SPRINGS | OUT_SHOCKS | OUT_CONSTRAINTS)

            marker_driver = my_hmmwv.GetChassis().GetMarkers()[0].GetAbsCoord(
            ).pos
            marker_com = my_hmmwv.GetChassis().GetMarkers()[1].GetAbsCoord(
            ).pos
            print("Markers\n")
            print("  Driver loc:      ", marker_driver.x, " ", marker_driver.y,
                  " ", marker_driver.z)
            print("  Chassis COM loc: ", marker_com.x, " ", marker_com.y, " ",
                  marker_com.z)

        # Get driver inputs
        driver_inputs = driver.GetInputs()

        # Update modules (process inputs from other modules)
        driver.Synchronize(time)
        terrain.Synchronize(time)
        my_hmmwv.Synchronize(time, driver_inputs, terrain)
        app.Synchronize(driver.GetInputModeAsString(), driver_inputs)

        # Advance simulation for one timestep for all modules
        driver.Advance(step_size)
        terrain.Advance(step_size)
        my_hmmwv.Advance(step_size)
        app.Advance(step_size)

        # Update sensor manager
        # Will render/save/filter automatically
        manager.Update()

        # Increment frame number
        step_number += 1

        # Spin in place for real time to catch up
        realtime_timer.Spin(step_size)

    return 0
Beispiel #13
0
# Set the default margins for collision detection, this is epecially
# important for very large or very small objects.
chrono.ChCollisionModel.SetDefaultSuggestedEnvelope(0.001)
chrono.ChCollisionModel.SetDefaultSuggestedMargin(0.001)

# Create the set of the particle clones (many rigid bodies that
# share the same mass and collision shape, so they are memory efficient
# in case you want to simulate granular material)

body_particles = chrono.ChParticlesClones()
body_particles.SetMass(0.01)
inertia = 2 / 5 * (pow(0.005, 2)) * 0.01
body_particles.SetInertiaXX(chrono.ChVectorD(inertia, inertia, inertia))

# Collision shape (shared by all particle clones) Must be defined BEFORE adding particles
particle_material = chrono.ChMaterialSurfaceNSC()

body_particles.GetCollisionModel().ClearModel()
body_particles.GetCollisionModel().AddSphere(particle_material, 0.005)
body_particles.GetCollisionModel().BuildModel()
body_particles.SetCollide(True)

# add particles
for ix in range(0, 5):
    for iy in range(0, 5):
        for iz in range(0, 3):
            body_particles.AddParticle(
                chrono.ChCoordsysD(
                    chrono.ChVectorD(ix / 100, 0.1 + iy / 100, iz / 100)))

# Visualization shape (shared by all particle clones)
Beispiel #14
0
def main():
    #print("Copyright (c) 2017 projectchrono.org\nChrono version: ", CHRONO_VERSION , "\n\n")

    # Create systems

    #  Create the HMMWV vehicle, set parameters, and initialize
    my_hmmwv = veh.HMMWV_Full()
    my_hmmwv.SetContactMethod(contact_method)
    my_hmmwv.SetChassisCollisionType(chassis_collision_type)
    my_hmmwv.SetChassisFixed(False)
    my_hmmwv.SetInitPosition(chrono.ChCoordsysD(initLoc, initRot))
    my_hmmwv.SetPowertrainType(powertrain_model)
    my_hmmwv.SetDriveType(drive_type)
    my_hmmwv.SetSteeringType(steering_type)
    my_hmmwv.SetTireType(tire_model)
    my_hmmwv.SetTireStepSize(tire_step_size)
    my_hmmwv.Initialize()

    my_hmmwv.SetChassisVisualizationType(chassis_vis_type)
    my_hmmwv.SetSuspensionVisualizationType(suspension_vis_type)
    my_hmmwv.SetSteeringVisualizationType(steering_vis_type)
    my_hmmwv.SetWheelVisualizationType(wheel_vis_type)
    my_hmmwv.SetTireVisualizationType(tire_vis_type)

    # Create the terrain

    terrain = veh.RigidTerrain(my_hmmwv.GetSystem())
    if (contact_method == chrono.ChContactMethod_NSC):
        patch_mat = chrono.ChMaterialSurfaceNSC()
        patch_mat.SetFriction(0.9)
        patch_mat.SetRestitution(0.01)
    elif (contact_method == chrono.ChContactMethod_SMC):
        patch_mat = chrono.ChMaterialSurfaceSMC()
        patch_mat.SetFriction(0.9)
        patch_mat.SetRestitution(0.01)
        patch_mat.SetYoungModulus(2e7)
    patch = terrain.AddPatch(patch_mat, chrono.ChVectorD(0, 0, 0),
                             chrono.ChVectorD(0, 0, 1), terrainLength,
                             terrainWidth)
    patch.SetTexture(veh.GetDataFile("terrain/textures/tile4.jpg"), 200, 200)
    patch.SetColor(chrono.ChColor(0.8, 0.8, 0.5))
    terrain.Initialize()

    # Create the vehicle Irrlicht interface
    app = veh.ChWheeledVehicleIrrApp(my_hmmwv.GetVehicle(), 'HMMWV',
                                     irr.dimension2du(1000, 800))

    app.SetSkyBox()
    app.AddTypicalLights(irr.vector3df(30, -30, 100),
                         irr.vector3df(30, 50, 100), 250, 130)
    app.AddTypicalLogo(chrono.GetChronoDataFile('logo_pychrono_alpha.png'))
    app.SetChaseCamera(trackPoint, 6.0, 0.5)
    app.SetTimestep(step_size)
    app.AssetBindAll()
    app.AssetUpdateAll()

    # Initialize output

    try:
        os.mkdir(out_dir)
    except:
        print("Error creating directory ")

    # Set up vehicle output
    my_hmmwv.GetVehicle().SetChassisOutput(True)
    my_hmmwv.GetVehicle().SetSuspensionOutput(0, True)
    my_hmmwv.GetVehicle().SetSteeringOutput(0, True)
    my_hmmwv.GetVehicle().SetOutput(veh.ChVehicleOutput.ASCII, out_dir,
                                    "output", 0.1)

    # Generate JSON information with available output channels
    my_hmmwv.GetVehicle().ExportComponentList(out_dir + "/component_list.json")

    # Create the interactive driver system
    driver = veh.ChIrrGuiDriver(app)

    # Set the time response for steering and throttle keyboard inputs.
    steering_time = 1.0  # time to go from 0 to +1 (or from 0 to -1)
    throttle_time = 1.0  # time to go from 0 to +1
    braking_time = 0.3  # time to go from 0 to +1
    driver.SetSteeringDelta(render_step_size / steering_time)
    driver.SetThrottleDelta(render_step_size / throttle_time)
    driver.SetBrakingDelta(render_step_size / braking_time)

    driver.Initialize()

    # Simulation loop

    # Number of simulation steps between miscellaneous events
    render_steps = m.ceil(render_step_size / step_size)
    debug_steps = m.ceil(debug_step_size / step_size)

    # Initialize simulation frame counter and simulation time
    step_number = 0
    render_frame = 0

    if (contact_vis):
        app.SetSymbolscale(1e-4)
        #app.SetContactsDrawMode(irr.eCh_ContactsDrawMode::CONTACT_FORCES);

    realtime_timer = chrono.ChRealtimeStepTimer()
    while (app.GetDevice().run()):
        time = my_hmmwv.GetSystem().GetChTime()

        #End simulation
        if (time >= t_end):
            break

        app.BeginScene(True, True, irr.SColor(255, 140, 161, 192))
        app.DrawAll()
        app.EndScene()

        #Debug logging
        if (debug_output and step_number % debug_steps == 0):
            print("\n\n============ System Information ============\n")
            print("Time = " << time << "\n\n")
            #my_hmmwv.DebugLog(OUT_SPRINGS | OUT_SHOCKS | OUT_CONSTRAINTS)

            marker_driver = my_hmmwv.GetChassis().GetMarkers()[0].GetAbsCoord(
            ).pos
            marker_com = my_hmmwv.GetChassis().GetMarkers()[1].GetAbsCoord(
            ).pos
            print("Markers\n")
            print("  Driver loc:      ", marker_driver.x, " ", marker_driver.y,
                  " ", marker_driver.z)
            print("  Chassis COM loc: ", marker_com.x, " ", marker_com.y, " ",
                  marker_com.z)

        # Get driver inputs
        driver_inputs = driver.GetInputs()

        # Update modules (process inputs from other modules)
        driver.Synchronize(time)
        terrain.Synchronize(time)
        my_hmmwv.Synchronize(time, driver_inputs, terrain)
        app.Synchronize(driver.GetInputModeAsString(), driver_inputs)

        # Advance simulation for one timestep for all modules
        driver.Advance(step_size)
        terrain.Advance(step_size)
        my_hmmwv.Advance(step_size)
        app.Advance(step_size)

        # Increment frame number
        step_number += 1

        # Spin in place for real time to catch up
        realtime_timer.Spin(step_size)

    return 0
Beispiel #15
0
    def __init__(self, render):
        self.render = render
        #self.size_rod_y = l
        self.observation_space = np.empty([4, 1])
        self.action_space = np.empty([1, 1])
        self.info = {}
        # ---------------------------------------------------------------------
        #
        #  Create the simulation system and add items
        #

        self.rev_pend_sys = chrono.ChSystemNSC()

        # 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.001)
        chrono.ChCollisionModel.SetDefaultSuggestedMargin(0.001)

        #rev_pend_sys.SetSolverType(chrono.ChSolver.Type_BARZILAIBORWEIN) # precise, more slow
        self.rev_pend_sys.SetMaxItersSolverSpeed(70)

        # Create a contact material (surface property)to share between all objects.
        # The rolling and spinning parameters are optional - if enabled they double
        # the computational time.
        # non ho cantatti, da vedere se è necessario tenere tutto il baraccone
        self.rod_material = chrono.ChMaterialSurfaceNSC()
        self.rod_material.SetFriction(0.5)
        self.rod_material.SetDampingF(0.2)
        self.rod_material.SetCompliance(0.0000001)
        self.rod_material.SetComplianceT(0.0000001)
        # rod_material.SetRollingFriction(rollfrict_param)
        # rod_material.SetSpinningFriction(0)
        # rod_material.SetComplianceRolling(0.0000001)
        # rod_material.SetComplianceSpinning(0.0000001)

        # Create the set of rods in a vertical stack, along Y axis

        self.size_rod_y = 2.0
        self.radius_rod = 0.05
        self.density_rod = 50
        # kg/m^3

        self.mass_rod = self.density_rod * self.size_rod_y * chrono.CH_C_PI * (
            self.radius_rod**2)
        self.inertia_rod_y = (self.radius_rod**2) * self.mass_rod / 2
        self.inertia_rod_x = (self.mass_rod / 12) * ((self.size_rod_y**2) + 3 *
                                                     (self.radius_rod**2))

        self.size_table_x = 0.3
        self.size_table_y = 0.3

        if self.render:
            self.size_table_z = 0.3
            self.myapplication = chronoirr.ChIrrApp(self.rev_pend_sys)
            self.myapplication.AddShadowAll()
            self.myapplication.SetStepManage(True)
            self.myapplication.SetTimestep(0.01)
            self.myapplication.SetTryRealtime(True)

            self.myapplication.AddTypicalSky('../data/skybox/')
            self.myapplication.AddTypicalCamera(
                chronoirr.vector3df(0.5, 0.5, 1.0))
            self.myapplication.AddLightWithShadow(
                chronoirr.vector3df(2, 4, 2),  # point
                chronoirr.vector3df(0, 0, 0),  # aimpoint
                9,  # radius (power)
                1,
                9,  # near, far
                30)  # angle of FOV
Beispiel #16
0
    def __init__(self, render):
        self.render = render

        self.observation_space = np.empty([4, 1])
        self.action_space = np.empty([1, 1])
        self.info = {}
        self.timestep = 0.01
        # ---------------------------------------------------------------------
        #
        #  Create the simulation system and add items
        #

        self.rev_pend_sys = chrono.ChSystemNSC()

        chrono.ChCollisionModel.SetDefaultSuggestedEnvelope(0.001)
        chrono.ChCollisionModel.SetDefaultSuggestedMargin(0.001)

        #rev_pend_sys.SetSolverType(chrono.ChSolver.Type_BARZILAIBORWEIN) # precise, more slow
        self.rev_pend_sys.SetMaxItersSolverSpeed(70)

        # Create a contact material (surface property)to share between all objects.
        self.rod_material = chrono.ChMaterialSurfaceNSC()
        self.rod_material.SetFriction(0.5)
        self.rod_material.SetDampingF(0.2)
        self.rod_material.SetCompliance(0.0000001)
        self.rod_material.SetComplianceT(0.0000001)

        # Create the set of rods in a vertical stack, along Y axis

        self.size_rod_y = 2.0
        self.radius_rod = 0.05
        self.density_rod = 50
        # kg/m^3

        self.mass_rod = self.density_rod * self.size_rod_y * chrono.CH_C_PI * (
            self.radius_rod**2)
        self.inertia_rod_y = (self.radius_rod**2) * self.mass_rod / 2
        self.inertia_rod_x = (self.mass_rod / 12) * ((self.size_rod_y**2) + 3 *
                                                     (self.radius_rod**2))

        self.size_table_x = 0.3
        self.size_table_y = 0.3

        if self.render:
            self.size_table_z = 0.3
            self.myapplication = chronoirr.ChIrrApp(self.rev_pend_sys)
            self.myapplication.AddShadowAll()
            self.myapplication.SetStepManage(True)
            self.myapplication.SetTimestep(0.01)
            self.myapplication.SetTryRealtime(True)

            self.myapplication.AddTypicalSky('../data/skybox/')
            self.myapplication.AddTypicalCamera(
                chronoirr.vector3df(0.5, 0.5, 1.0))
            self.myapplication.AddLightWithShadow(
                chronoirr.vector3df(2, 4, 2),  # point
                chronoirr.vector3df(0, 0, 0),  # aimpoint
                9,  # radius (power)
                1,
                9,  # near, far
                30)  # angle of FOV
Beispiel #17
0
   def __init__(self):
      ChronoBaseEnv.__init__(self)
      
      low = np.full(30, -1000)
      high = np.full(30, 1000)
      self.observation_space = spaces.Box(low, high, dtype=np.float32)
      self.action_space = spaces.Box(low=-1.0, high=1.0, shape=(8,), dtype=np.float32)
      
      self.info =  {"timeout": 10000.0}
    # ---------------------------------------------------------------------
    #
    #  Create the simulation system and add items
    #
      self.Xtarg = 1000.0
      self.Ytarg = 0.0
      self.d_old = np.linalg.norm(self.Xtarg + self.Ytarg)
      self.ant_sys = chrono.ChSystemNSC()

    # 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.001)
      chrono.ChCollisionModel.SetDefaultSuggestedMargin(0.001)

    #ant_sys.SetSolverType(chrono.ChSolver.Type_BARZILAIBORWEIN) # precise, more slow
      self.ant_sys.SetSolverMaxIterations(70)
      

      self.ant_material = chrono.ChMaterialSurfaceNSC()
      self.ant_material.SetFriction(0.5)
      self.ant_material.SetDampingF(0.2)
      self.ant_material.SetCompliance (0.0005)
      self.ant_material.SetComplianceT(0.0005)

      self.timestep = 0.01
      self.abdomen_x = 0.25
      self.abdomen_y = 0.25
      self.abdomen_z = 0.25

      self.leg_density = 250    # kg/m^3
      self.abdomen_density = 100
      self.abdomen_y0 = 0.4
      self.leg_length = 0.3
      self.leg_radius = 0.04
      self.ankle_angle = 60*(math.pi/180)
      self.ankle_length = 0.4
      self.ankle_radius = 0.04
      self.gain = 30

      self.abdomen_mass = self.abdomen_density * ((4/3)*chrono.CH_C_PI*self.abdomen_x*self.abdomen_y*self.abdomen_z)
      self.abdomen_inertia = chrono.ChVectorD((1/5)*self.abdomen_mass*(pow(self.abdomen_y,2)+pow(self.abdomen_z,2)),(1/5)*self.abdomen_mass*(pow(self.abdomen_x,2)+pow(self.abdomen_z,2)),(1/5)*self.abdomen_mass*(pow(self.abdomen_y,2)+pow(self.abdomen_x,2)))
      self.leg_mass = self.leg_density * self.leg_length * math.pi* pow (self.leg_radius,2)
      self.leg_inertia = chrono.ChVectorD(0.5*self.leg_mass*pow(self.leg_radius,2), (self.leg_mass/12)*(3*pow(self.leg_radius,2)+pow(self.leg_length,2)),(self.leg_mass/12)*(3*pow(self.leg_radius,2)+pow(self.leg_length,2)))
      self.ankle_mass = self.leg_density * self.ankle_length * math.pi* pow (self.ankle_radius,2)
      self.ankle_inertia = chrono.ChVectorD(0.5*self.ankle_mass*pow(self.ankle_radius,2), (self.ankle_mass/12)*(3*pow(self.ankle_radius,2)+pow(self.ankle_length,2)),(self.ankle_mass/12)*(3*pow(self.ankle_radius,2)+pow(self.ankle_length,2)))
      
      self.leg_limit = chrono.ChLinkLimit()
      self.ankle_limit = chrono.ChLinkLimit()
      self.leg_limit.SetRmax(math.pi/9)
      self.leg_limit.SetRmin(-math.pi/9)
      self.ankle_limit.SetRmax(math.pi/9)
      self.ankle_limit.SetRmin(-math.pi/9)
Beispiel #18
0
    def __init__(self):
        ChronoBaseEnv.__init__(self)
        # Set to False to avoid vis shapes loading. Once you do this, you can no longer render until this is re set to True
        self.animate = True

        low = np.full(53, -1000)
        high = np.full(53, 1000)
        self.observation_space = spaces.Box(low, high, dtype=np.float32)
        self.action_space = spaces.Box(low=-1.0,
                                       high=1.0,
                                       shape=(18, ),
                                       dtype=np.float32)

        self.Xtarg = 0.0
        self.Ztarg = 1000.0
        self.d_old = np.linalg.norm(self.Xtarg + self.Ztarg)

        #self.d_old = np.linalg.norm(self.Xtarg + self.Ytarg)
        self.hexapod_sys = chrono.ChSystemNSC()
        chrono.ChCollisionModel.SetDefaultSuggestedEnvelope(0.0001)
        chrono.ChCollisionModel.SetDefaultSuggestedMargin(0.0001)
        #hexapod_sys.SetSolverType(chrono.ChSolver.Type_BARZILAIBORWEIN) # precise, more slow

        self.info = {"timeout": 3200.0}
        if self.animate:
            m_filename = "hexapod"
        else:
            m_filename = "hexapod_novis"
        self.timestep = 0.005
        m_length = 1.0
        self.my_material = chrono.ChMaterialSurfaceNSC()
        self.my_material.SetFriction(0.5)
        self.my_material.SetDampingF(0.2)
        self.my_material.SetCompliance(0.0000001)
        self.my_material.SetComplianceT(0.0000001)
        #self.my_material.SetCompliance (0.0005)
        #self.my_material.SetComplianceT(0.0005)
        #m_visualization = "irrlicht"
        print("  file to load is ", m_filename)
        print("  timestep is ", self.timestep)
        print("  length is ", m_length)
        print("  data path for fonts etc.: ", chrono.GetChronoDataPath())
        # ---------------------------------------------------------------------
        #
        #  load the file generated by the SolidWorks CAD plugin
        #  and add it to the ChSystem.
        # Remove the trailing .py and add / in case of file without ./
        #m_absfilename = os.path.abspath(m_filename)
        #m_modulename = os.path.splitext(m_absfilename)[0]

        print("Loading C::E scene...")
        dir_path = os.path.dirname(os.path.realpath(__file__))
        self.fpath = os.path.join(dir_path, m_filename)
        #exported_items = chrono.ImportSolidWorksSystem(self.fpath)
        #self.exported_items = chrono.ImportSolidWorksSystem(m_modulename)

        print("...loading done!")
        # Print exported items
        #for my_item in exported_items:
        #print (my_item.GetName())
        # Optionally set some solver parameters.
        #self.hexapod_sys.SetMaxPenetrationRecoverySpeed(1.00)
        solver = chrono.ChSolverBB()
        self.hexapod_sys.SetSolver(solver)
        solver.SetMaxIterations(600)
        solver.EnableWarmStart(True)
        self.hexapod_sys.Set_G_acc(chrono.ChVectorD(0, -9.8, 0))
        """
              $$$$$$$$ FIND THE SW DEFINED CONSTRAINTS, GET THEIR self.frames AND GET RID OF EM $$$$$$$$ 
              """

        self.con_link = []
        self.coi_link = []
        self.hip_names = []
        self.femur_names = []
        self.tibia_names = []
        self.feet_names = []
        self.maxSpeed = []
        self.maxRot = []
        self.minRot = []
        Tmax = []

        for i in range(1, 19):
            self.con_link.append("Concentric" + str(i))
            self.coi_link.append("Coincident" + str(i))
            self.maxSpeed.append(354)  # 59 RPM to deg/s
            self.maxRot.append(90)  #deg
            self.minRot.append(-90)  #deg
            self.minRot.append(-90)  #deg
            Tmax.append(1.5)  #deg

        for i in range(1, 7):
            self.hip_names.append("Hip-" + str(i))
            self.femur_names.append("Femur-" + str(i))
            self.tibia_names.append("Tibia-" + str(i))
            self.feet_names.append("Foot-" + str(i))

        self.maxT = np.asarray(Tmax)
Beispiel #19
0
def main():
    #print("Copyright (c) 2017 projectchrono.org\nChrono version: ", CHRONO_VERSION , "\n\n")

    # Create systems

    #  Create the FEDA vehicle, set parameters, and initialize
    my_feda = veh.FEDA()
    my_feda.SetContactMethod(contact_method)
    my_feda.SetChassisCollisionType(chassis_collision_type)
    my_feda.SetChassisFixed(False)
    my_feda.SetInitPosition(chrono.ChCoordsysD(initLoc, initRot))
    my_feda.SetPowertrainType(powertrain_model)
    my_feda.SetTireType(tire_model)
    my_feda.SetTireStepSize(tire_step_size)
    my_feda.Initialize()

    my_feda.SetChassisVisualizationType(chassis_vis_type)
    my_feda.SetSuspensionVisualizationType(suspension_vis_type)
    my_feda.SetSteeringVisualizationType(steering_vis_type)
    my_feda.SetWheelVisualizationType(wheel_vis_type)
    my_feda.SetTireVisualizationType(tire_vis_type)

    # Create the terrain

    terrain = veh.RigidTerrain(my_feda.GetSystem())
    if (contact_method == chrono.ChContactMethod_NSC):
        patch_mat = chrono.ChMaterialSurfaceNSC()
        patch_mat.SetFriction(0.9)
        patch_mat.SetRestitution(0.01)
    elif (contact_method == chrono.ChContactMethod_SMC):
        patch_mat = chrono.ChMaterialSurfaceSMC()
        patch_mat.SetFriction(0.9)
        patch_mat.SetRestitution(0.01)
        patch_mat.SetYoungModulus(2e7)
    patch = terrain.AddPatch(patch_mat, chrono.ChVectorD(0, 0, 0),
                             chrono.ChVectorD(0, 0, 1), terrainLength,
                             terrainWidth)
    patch.SetTexture(veh.GetDataFile("terrain/textures/tile4.jpg"), 200, 200)
    patch.SetColor(chrono.ChColor(0.8, 0.8, 0.5))
    terrain.Initialize()

    # Create the vehicle Irrlicht interface
    app = veh.ChWheeledVehicleIrrApp(my_feda.GetVehicle(), 'HMMWV',
                                     irr.dimension2du(1000, 800))

    app.SetSkyBox()
    app.AddTypicalLights(irr.vector3df(30, -30, 100),
                         irr.vector3df(30, 50, 100), 250, 130)
    app.AddTypicalLogo(chrono.GetChronoDataFile('logo_pychrono_alpha.png'))
    app.SetChaseCamera(trackPoint, 6.0, 0.5)
    app.SetTimestep(step_size)
    app.AssetBindAll()
    app.AssetUpdateAll()

    # Create the interactive driver system
    driver = veh.ChIrrGuiDriver(app)

    # Set the time response for steering and throttle keyboard inputs.
    steering_time = 1.0  # time to go from 0 to +1 (or from 0 to -1)
    throttle_time = 1.0  # time to go from 0 to +1
    braking_time = 0.3  # time to go from 0 to +1
    driver.SetSteeringDelta(10 * step_size / steering_time)
    driver.SetThrottleDelta(10 * step_size / throttle_time)
    driver.SetBrakingDelta(10 * step_size / braking_time)

    driver.Initialize()

    # Simulation loop
    realtime_timer = chrono.ChRealtimeStepTimer()
    while (app.GetDevice().run()):
        time = my_feda.GetSystem().GetChTime()

        app.BeginScene(True, True, irr.SColor(255, 140, 161, 192))
        app.DrawAll()
        app.EndScene()

        # Get driver inputs
        driver_inputs = driver.GetInputs()

        # Update modules (process inputs from other modules)
        driver.Synchronize(time)
        terrain.Synchronize(time)
        my_feda.Synchronize(time, driver_inputs, terrain)
        app.Synchronize(driver.GetInputModeAsString(), driver_inputs)

        # Advance simulation for one timestep for all modules
        driver.Advance(step_size)
        terrain.Advance(step_size)
        my_feda.Advance(step_size)
        app.Advance(step_size)

        # Spin in place for real time to catch up
        realtime_timer.Spin(step_size)

    return 0
Beispiel #20
0
def main():
    #print("Copyright (c) 2017 projectchrono.org\nChrono version: ", CHRONO_VERSION , "\n\n")

    #  Create the M113 vehicle
    # ------------------------

    vehicle = veh.M113_Vehicle(False, veh.TrackShoeType_SINGLE_PIN,
                               veh.BrakeType_SIMPLE,
                               chrono.ChContactMethod_SMC,
                               veh.CollisionType_NONE)

    vehicle.Initialize(chrono.ChCoordsysD(initLoc, initRot))

    vehicle.SetChassisVisualizationType(veh.VisualizationType_PRIMITIVES)
    vehicle.SetSprocketVisualizationType(veh.VisualizationType_MESH)
    vehicle.SetIdlerVisualizationType(veh.VisualizationType_MESH)
    vehicle.SetRoadWheelAssemblyVisualizationType(veh.VisualizationType_MESH)
    vehicle.SetRoadWheelVisualizationType(veh.VisualizationType_MESH)
    vehicle.SetTrackShoeVisualizationType(veh.VisualizationType_MESH)

    # Create the powertrain system
    # ----------------------------

    powertrain = veh.M113_SimpleCVTPowertrain("Powertrain")
    vehicle.InitializePowertrain(powertrain)

    # Create the terrain
    # ------------------

    terrain = veh.RigidTerrain(vehicle.GetSystem())
    if (contact_method == chrono.ChContactMethod_NSC):
        patch_mat = chrono.ChMaterialSurfaceNSC()
        patch_mat.SetFriction(0.9)
        patch_mat.SetRestitution(0.01)
    elif (contact_method == chrono.ChContactMethod_SMC):
        patch_mat = chrono.ChMaterialSurfaceSMC()
        patch_mat.SetFriction(0.9)
        patch_mat.SetRestitution(0.01)
        patch_mat.SetYoungModulus(2e7)
    patch = terrain.AddPatch(patch_mat, chrono.ChVectorD(0, 0, 0),
                             chrono.ChVectorD(0, 0, 1), terrainLength,
                             terrainWidth)
    patch.SetTexture(veh.GetDataFile("terrain/textures/tile4.jpg"), 200, 200)
    patch.SetColor(chrono.ChColor(0.5, 0.8, 0.5))
    terrain.Initialize()

    # Create the vehicle Irrlicht interface
    # -------------------------------------

    app = veh.ChTrackedVehicleIrrApp(vehicle, 'M113',
                                     irr.dimension2du(1000, 800))

    app.SetSkyBox()
    app.AddTypicalLights(irr.vector3df(30, -30, 100),
                         irr.vector3df(30, 50, 100), 250, 130)
    app.AddTypicalLogo(chrono.GetChronoDataFile('logo_pychrono_alpha.png'))
    app.SetChaseCamera(trackPoint, 6.0, 0.5)
    app.SetTimestep(step_size)
    app.AssetBindAll()
    app.AssetUpdateAll()

    # Create the interactive driver system
    # ------------------------------------

    driver = veh.ChIrrGuiDriver(app)

    # Set the time response for steering and throttle keyboard inputs.
    steering_time = 0.5  # time to go from 0 to +1 (or from 0 to -1)
    throttle_time = 1.0  # time to go from 0 to +1
    braking_time = 0.3  # time to go from 0 to +1
    driver.SetSteeringDelta(render_step_size / steering_time)
    driver.SetThrottleDelta(render_step_size / throttle_time)
    driver.SetBrakingDelta(render_step_size / braking_time)

    driver.Initialize()

    # Simulation loop
    # ---------------

    # Inter-module communication data
    shoe_forces_left = veh.TerrainForces(vehicle.GetNumTrackShoes(veh.LEFT))
    shoe_forces_right = veh.TerrainForces(vehicle.GetNumTrackShoes(veh.RIGHT))

    # Number of simulation steps between miscellaneous events
    render_steps = m.ceil(render_step_size / step_size)

    # Initialize simulation frame counter and simulation time
    step_number = 0

    realtime_timer = chrono.ChRealtimeStepTimer()
    while (app.GetDevice().run()):
        time = vehicle.GetSystem().GetChTime()

        app.BeginScene(True, True, irr.SColor(255, 140, 161, 192))
        app.DrawAll()
        app.EndScene()

        # Get driver inputs
        driver_inputs = driver.GetInputs()

        # Update modules (process inputs from other modules)
        driver.Synchronize(time)
        terrain.Synchronize(time)
        vehicle.Synchronize(time, driver_inputs, shoe_forces_left,
                            shoe_forces_right)
        app.Synchronize("", driver_inputs)

        # Advance simulation for one timestep for all modules
        driver.Advance(step_size)
        terrain.Advance(step_size)
        vehicle.Advance(step_size)
        app.Advance(step_size)

        # Increment frame number
        step_number += 1

        # Spin in place for real time to catch up
        realtime_timer.Spin(step_size)

    return 0
    def __init__(self, render):
        self.animate = render
        self.observation_space = np.empty([
            18,
        ])
        self.action_space = np.zeros([
            6,
        ])
        #TODO: check if targ is reachable

        self.fingerdist = chrono.ChVectorD(0, 0.1117, 0)
        #self.d_old = np.linalg.norm(self.Xtarg + self.Ytarg)
        self.robosystem = chrono.ChSystemNSC()
        chrono.ChCollisionModel.SetDefaultSuggestedEnvelope(0.001)
        chrono.ChCollisionModel.SetDefaultSuggestedMargin(0.001)
        #robosystem.SetSolverType(chrono.ChSolver.Type_BARZILAIBORWEIN) # precise, more slow
        self.timestep = 0.01

        self.info = {}

        m_filename = "ABB_IRB120"
        self.timestep = 0.001
        m_length = 1.0
        self.my_material = chrono.ChMaterialSurfaceNSC()
        self.my_material.SetFriction(0.5)
        self.my_material.SetDampingF(0.2)
        self.my_material.SetCompliance(0.0000001)
        self.my_material.SetComplianceT(0.0000001)
        #m_visualization = "irrlicht"
        self.m_datapath = "../../../../../../codes/Chrono/Chrono_Source/data"
        chrono.SetChronoDataPath(self.m_datapath)
        print("  file to load is ", m_filename)
        print("  timestep is ", self.timestep)
        print("  length is ", m_length)
        print("  data path for fonts etc.: ", self.m_datapath)
        # ---------------------------------------------------------------------
        #
        #  load the file generated by the SolidWorks CAD plugin
        #  and add it to the ChSystem.
        # Remove the trailing .py and add / in case of file without ./
        #m_absfilename = os.path.abspath(m_filename)
        #m_modulename = os.path.splitext(m_absfilename)[0]

        print("Loading C::E scene...")
        dir_path = os.path.dirname(os.path.realpath(__file__))
        self.fpath = os.path.join(dir_path, m_filename)
        #exported_items = chrono.ImportSolidWorksSystem(self.fpath)
        #self.exported_items = chrono.ImportSolidWorksSystem(m_modulename)

        print("...loading done!")
        # Print exported items
        #for my_item in exported_items:
        #print (my_item.GetName())
        # Optionally set some solver parameters.
        #self.robosystem.SetMaxPenetrationRecoverySpeed(1.00)
        self.robosystem.SetSolverType(chrono.ChSolver.Type_BARZILAIBORWEIN)
        self.robosystem.SetMaxItersSolverSpeed(600)
        self.robosystem.SetSolverWarmStarting(True)
        self.robosystem.Set_G_acc(chrono.ChVectorD(0, -9.8, 0))
        """
              $$$$$$$$ FIND THE SW DEFINED CONSTRAINTS, GET THEIR self.frames AND GET RID OF EM $$$$$$$$ 
              """

        self.con_link = [
            "Concentric1", "Concentric2", "Concentric3", "Concentric4",
            "Concentric5", "Concentric6"
        ]
        self.coi_link = [
            "Coincident1", "Coincident2", "Coincident3", "Coincident4",
            "Coincident5", "Coincident6"
        ]
        self.bodiesNames = [
            "Racer3_p01-1", "Racer3_p02-1", "Racer3_p03-1", "Racer3_p04-1",
            "Racer3_p05-1", "Racer3_p06-2", "Hand_base_and_p07-3"
        ]
        #              self.maxSpeed = [430, 450, 500, 600, 600, 900] #°/s -->COMAU
        self.maxSpeed = [250, 250, 250, 320, 320, 420]  #°/s --> ABB
        # TODO: convert to rads (converted in every operation)

        #self.limits:
        # TODO: check signs
        # TODO: peridodicity ignored
        # REAL self.limits
        #self.maxRot = [170, 135, 90, 200, 125, 2700] # °
        #self.minRot = [-170, -95, -155, -200, -125, -2700] # °
        # MODIFIED self.limits
        #              self.maxRot = [170, 135, 90, 179, 50, 79] # ° --> COMAU
        #              self.minRot = [-170, -95, -155, -179, -50, -79] # ° --> COMAU
        self.maxRot = [165, 110, 70, 160, 120, 179]  # ° --> ABB
        self.minRot = [-165, -110, -110, -160, -100, -179]  # ° --> ABB
        #self.maxT = np.asarray([28, 28, 28, 7.36, 7.36, 4.41])
        self.maxT = np.asarray([100, 100, 50, 7.36, 7.36,
                                4.41])  # --> LASCIATI UGUALI A COMAU

        if (self.animate):
            self.myapplication = chronoirr.ChIrrApp(
                self.robosystem, 'Test', chronoirr.dimension2du(1280, 720))
            self.myapplication.AddShadowAll()
            self.myapplication.SetStepManage(True)
            self.myapplication.SetTimestep(self.timestep)
            self.myapplication.AddTypicalSky(chrono.GetChronoDataPath() +
                                             'skybox/')
            self.myapplication.AddTypicalLogo(chrono.GetChronoDataPath() +
                                              'logo_pychrono_alpha.png')
            self.myapplication.AddTypicalCamera(
                chronoirr.vector3df(1, 1, 1),
                chronoirr.vector3df(0.0, 0.0, 0.0))
            self.myapplication.AddTypicalLights()  # angle of FOV
Beispiel #22
0
    def reset(self):
        x_half_length = 90
        y_half_length = 40
        self.path = BezierPath(x_half_length, y_half_length, 0.5,
                               self.interval)
        pos, rot = self.path.getPosRot(self.path.current_t - self.interval)
        self.initLoc = chrono.ChVectorD(pos)
        self.initRot = chrono.ChQuaternionD(rot)

        self.vehicle = veh.HMMWV_Reduced()
        self.vehicle.SetContactMethod(chrono.ChContactMethod_NSC)
        self.surf_material = chrono.ChMaterialSurfaceNSC()
        self.vehicle.SetChassisCollisionType(
            veh.ChassisCollisionType_PRIMITIVES)

        self.vehicle.SetChassisFixed(False)
        self.vehicle.SetInitPosition(
            chrono.ChCoordsysD(self.initLoc, self.initRot))
        self.vehicle.SetPowertrainType(veh.PowertrainModelType_SHAFTS)
        self.vehicle.SetDriveType(veh.DrivelineType_AWD)
        # self.vehicle.SetSteeringType(veh.SteeringType_PITMAN_ARM)
        self.vehicle.SetTireType(veh.TireModelType_TMEASY)
        self.vehicle.SetTireStepSize(self.timestep)
        self.vehicle.Initialize()
        if self.play_mode == True:
            # self.vehicle.SetChassisVisualizationType(veh.VisualizationType_MESH)
            self.vehicle.SetChassisVisualizationType(
                veh.VisualizationType_PRIMITIVES)
            self.vehicle.SetWheelVisualizationType(veh.VisualizationType_MESH)
            self.vehicle.SetTireVisualizationType(veh.VisualizationType_MESH)
        else:
            self.vehicle.SetChassisVisualizationType(
                veh.VisualizationType_PRIMITIVES)
            self.vehicle.SetWheelVisualizationType(
                veh.VisualizationType_PRIMITIVES)
        self.vehicle.SetSuspensionVisualizationType(
            veh.VisualizationType_PRIMITIVES)
        self.vehicle.SetSteeringVisualizationType(
            veh.VisualizationType_PRIMITIVES)
        self.chassis_body = self.vehicle.GetChassisBody()
        self.chassis_body.GetCollisionModel().ClearModel()
        size = chrono.ChVectorD(3, 2, 0.2)
        self.chassis_body.GetCollisionModel().AddBox(self.surf_material,
                                                     0.5 * size.x,
                                                     0.5 * size.y,
                                                     0.5 * size.z)
        self.chassis_body.GetCollisionModel().BuildModel()
        self.m_inputs = veh.Inputs()
        self.system = self.vehicle.GetVehicle().GetSystem()
        self.manager = sens.ChSensorManager(self.system)
        self.manager.scene.AddPointLight(chrono.ChVectorF(100, 100, 100),
                                         chrono.ChVectorF(1, 1, 1), 5000.0)
        self.manager.scene.AddPointLight(chrono.ChVectorF(-100, -100, 100),
                                         chrono.ChVectorF(1, 1, 1), 5000.0)
        # Driver
        #self.driver = veh.ChDriver(self.vehicle.GetVehicle())

        self.terrain = veh.RigidTerrain(self.system)
        patch_mat = chrono.ChMaterialSurfaceNSC()
        patch_mat.SetFriction(0.9)
        patch_mat.SetRestitution(0.01)
        patch = self.terrain.AddPatch(patch_mat, chrono.ChVectorD(0, 0, 0),
                                      chrono.ChVectorD(0, 0, 1),
                                      self.terrainLength * 1.5,
                                      self.terrainWidth * 1.5)
        patch.SetTexture(veh.GetDataFile("terrain/textures/grass.jpg"), 200,
                         200)
        patch.SetColor(chrono.ChColor(0.8, 0.8, 0.5))
        self.terrain.Initialize()
        self.groundBody = patch.GetGroundBody()
        ground_asset = self.groundBody.GetAssets()[0]
        visual_asset = chrono.CastToChVisualization(ground_asset)
        vis_mat = chrono.ChVisualMaterial()
        vis_mat.SetKdTexture(veh.GetDataFile("terrain/textures/grass.jpg"))
        visual_asset.material_list.append(vis_mat)
        self.leaders.addLeaders(self.system, self.path)
        self.leader_box = self.leaders.getBBox()
        self.lead_pos = (self.chassis_body.GetPos() -
                         self.leaders[0].GetPos()).Length()
        # Add obstacles:
        self.obstacles = []
        self.placeObstacle(8)
        # for leader in self.leaders:

        # ------------------------------------------------
        # Create a self.camera and add it to the sensor manager
        # ------------------------------------------------
        self.camera = sens.ChCameraSensor(
            self.chassis_body,  # body camera is attached to
            10,  # scanning rate in Hz
            chrono.ChFrameD(chrono.ChVectorD(1.5, 0, .875)),
            # offset pose
            self.camera_width,  # number of horizontal samples
            self.camera_height,  # number of vertical channels
            chrono.CH_C_PI / 2,  # horizontal field of view
            6)
        self.camera.SetName("Camera Sensor")
        self.camera.PushFilter(sens.ChFilterRGBA8Access())
        self.manager.AddSensor(self.camera)
        # -----------------------------------------------------
        # Create a self.gps and add it to the sensor manager
        # -----------------------------------------------------
        gps_noise_none = sens.ChGPSNoiseNone()
        self.AgentGPS = sens.ChGPSSensor(
            self.chassis_body, 10,
            chrono.ChFrameD(
                chrono.ChVectorD(0, 0, 0),
                chrono.Q_from_AngAxis(0, chrono.ChVectorD(0, 1, 0))),
            self.origin, gps_noise_none)
        self.AgentGPS.SetName("AgentGPS Sensor")
        self.AgentGPS.PushFilter(sens.ChFilterGPSAccess())
        self.manager.AddSensor(self.AgentGPS)
        ### Target GPS
        self.TargetGPS = sens.ChGPSSensor(
            self.leaders[0], 10,
            chrono.ChFrameD(
                chrono.ChVectorD(0, 0, 0),
                chrono.Q_from_AngAxis(0, chrono.ChVectorD(0, 1, 0))),
            self.origin, gps_noise_none)
        self.TargetGPS.SetName("TargetGPS Sensor")
        self.TargetGPS.PushFilter(sens.ChFilterGPSAccess())
        self.manager.AddSensor(self.TargetGPS)

        self.step_number = 0
        self.num_frames = 0
        self.num_updates = 0
        self.c_f = 0
        self.old_ac = [0, 0]
        self.isdone = False
        self.render_setup = False
        if self.play_mode:
            self.render()

        return self.get_ob()
Beispiel #23
0
    def __init__(self):
        ChronoBaseEnv.__init__(self)
        low = np.full(18, -1000)
        high = np.full(18, 1000)
        self.observation_space = spaces.Box(low, high, dtype=np.float32)
        self.action_space = spaces.Box(low=-1.0,
                                       high=1.0,
                                       shape=(6, ),
                                       dtype=np.float32)

        self.fingerdist = chrono.ChVectorD(0, 0.1117, 0)
        self.robosystem = chrono.ChSystemNSC()
        chrono.ChCollisionModel.SetDefaultSuggestedEnvelope(0.001)
        chrono.ChCollisionModel.SetDefaultSuggestedMargin(0.001)
        self.timestep = 0.01

        self.info = {"timeout": 2000.0}

        m_filename = "ComauR3"
        self.timestep = 0.001
        m_length = 1.0
        self.my_material = chrono.ChMaterialSurfaceNSC()
        self.my_material.SetFriction(0.5)
        self.my_material.SetDampingF(0.2)
        self.my_material.SetCompliance(0.0000001)
        self.my_material.SetComplianceT(0.0000001)
        #m_visualization = "irrlicht"
        print("  file to load is ", m_filename)
        print("  timestep is ", self.timestep)
        print("  length is ", m_length)
        print("  data path for fonts etc.: ", chrono.GetChronoDataPath())
        # ---------------------------------------------------------------------
        #
        #  load the file generated by the SolidWorks CAD plugin
        #  and add it to the ChSystem.
        # Remove the trailing .py and add / in case of file without ./
        #m_absfilename = os.path.abspath(m_filename)
        #m_modulename = os.path.splitext(m_absfilename)[0]

        print("Loading C::E scene...")
        dir_path = os.path.dirname(os.path.realpath(__file__))
        self.fpath = os.path.join(dir_path, m_filename)
        #exported_items = chrono.ImportSolidWorksSystem(self.fpath)
        #self.exported_items = chrono.ImportSolidWorksSystem(m_modulename)

        print("...loading done!")
        # Print exported items
        #for my_item in exported_items:
        #print (my_item.GetName())
        # Optionally set some solver parameters.
        #self.robosystem.SetMaxPenetrationRecoverySpeed(1.00)
        solver = chrono.ChSolverBB()
        self.robosystem.SetSolver(solver)
        solver.SetMaxIterations(600)
        solver.EnableWarmStart(True)
        self.robosystem.Set_G_acc(chrono.ChVectorD(0, -9.8, 0))
        """
              $$$$$$$$ FIND THE SW DEFINED CONSTRAINTS, GET THEIR self.frames AND GET RID OF EM $$$$$$$$ 
              """

        self.con_link = [
            "Concentric1", "Concentric2", "Concentric3", "Concentric4",
            "Concentric5", "Concentric6"
        ]
        self.coi_link = [
            "Coincident1", "Coincident2", "Coincident3", "Coincident4",
            "Coincident5", "Coincident6"
        ]
        self.bodiesNames = [
            "Racer3_p01-3", "Racer3_p02-1", "Racer3_p03-1", "Racer3_p04-1",
            "Racer3_p05-1", "Racer3_p06-1", "Hand_base_and_p07-2"
        ]
        self.maxSpeed = [430, 450, 500, 600, 600, 900]  #°/s

        self.maxRot = [170, 135, 90, 179, 100, 179]  # °
        self.minRot = [-170, -95, -155, -179, -100, -179]  # °
        self.maxT = np.asarray([100, 100, 50, 7.36, 7.36, 4.41])
Beispiel #24
0
   def __init__(self, render):

      self.animate = render
      self.observation_space = np.empty([30,])
      self.action_space = np.zeros([8,])
      self.info =  {}
    # ---------------------------------------------------------------------
    #
    #  Create the simulation system and add items
    #
      self.Xtarg = 1000.0
      self.Ytarg = 0.0
      self.d_old = np.linalg.norm(self.Xtarg + self.Ytarg)
      self.ant_sys = chrono.ChSystemNSC()

    # 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.001)
      chrono.ChCollisionModel.SetDefaultSuggestedMargin(0.001)

    #ant_sys.SetSolverType(chrono.ChSolver.Type_BARZILAIBORWEIN) # precise, more slow
      self.ant_sys.SetSolverMaxIterations(70)
      

      self.ant_material = chrono.ChMaterialSurfaceNSC()
      self.ant_material.SetFriction(0.5)
      self.ant_material.SetDampingF(0.2)
      self.ant_material.SetCompliance (0.0005)
      self.ant_material.SetComplianceT(0.0005)

      self.timestep = 0.01
      self.abdomen_x = 0.25
      self.abdomen_y = 0.25
      self.abdomen_z = 0.25

      self.leg_density = 250    # kg/m^3
      self.abdomen_density = 100
      self.abdomen_y0 = 0.4
      self.leg_length = 0.3
      self.leg_radius = 0.04
      self.ankle_angle = 60*(math.pi/180)
      self.ankle_length = 0.4
      self.ankle_radius = 0.04
      self.gain = 30

      self.abdomen_mass = self.abdomen_density * ((4/3)*chrono.CH_C_PI*self.abdomen_x*self.abdomen_y*self.abdomen_z)
      self.abdomen_inertia = chrono.ChVectorD((1/5)*self.abdomen_mass*(pow(self.abdomen_y,2)+pow(self.abdomen_z,2)),(1/5)*self.abdomen_mass*(pow(self.abdomen_x,2)+pow(self.abdomen_z,2)),(1/5)*self.abdomen_mass*(pow(self.abdomen_y,2)+pow(self.abdomen_x,2)))
      self.leg_mass = self.leg_density * self.leg_length * math.pi* pow (self.leg_radius,2)
      self.leg_inertia = chrono.ChVectorD(0.5*self.leg_mass*pow(self.leg_radius,2), (self.leg_mass/12)*(3*pow(self.leg_radius,2)+pow(self.leg_length,2)),(self.leg_mass/12)*(3*pow(self.leg_radius,2)+pow(self.leg_length,2)))
      self.ankle_mass = self.leg_density * self.ankle_length * math.pi* pow (self.ankle_radius,2)
      self.ankle_inertia = chrono.ChVectorD(0.5*self.ankle_mass*pow(self.ankle_radius,2), (self.ankle_mass/12)*(3*pow(self.ankle_radius,2)+pow(self.ankle_length,2)),(self.ankle_mass/12)*(3*pow(self.ankle_radius,2)+pow(self.ankle_length,2)))
      
      self.leg_limit = chrono.ChLinkLimit()
      self.ankle_limit = chrono.ChLinkLimit()
      self.leg_limit.SetRmax(math.pi/9)
      self.leg_limit.SetRmin(-math.pi/9)
      self.ankle_limit.SetRmax(math.pi/9)
      self.ankle_limit.SetRmin(-math.pi/9)
      
      if (self.animate) :
             self.myapplication = chronoirr.ChIrrApp(self.ant_sys)
             self.myapplication.AddShadowAll()
             self.myapplication.SetStepManage(True)
             self.myapplication.SetTimestep(self.timestep)
             self. myapplication.SetTryRealtime(True)  
             self.myapplication.AddTypicalSky()
             self.myapplication.AddTypicalLogo(chrono.GetChronoDataFile('logo_pychrono_alpha.png'))
             self.myapplication.AddTypicalCamera(chronoirr.vector3df(0,1.5,0))
             self.myapplication.AddLightWithShadow(chronoirr.vector3df(4,4,0),    # point
                                            chronoirr.vector3df(0,0,0),    # aimpoint
                                            20,                 # radius (power)
                                            1,9,               # near, far
                                            90)                # angle of FOV
Beispiel #25
0
def main():
    #print("Copyright (c) 2017 projectchrono.org\nChrono version: ", CHRONO_VERSION , "\n\n")

    step_size = 0.005

    sys = chrono.ChSystemNSC()
    sys.Set_G_acc(chrono.ChVectorD(0, 0, -9.81))
    sys.SetSolverType(chrono.ChSolver.Type_BARZILAIBORWEIN)
    sys.SetSolverMaxIterations(150)
    sys.SetMaxPenetrationRecoverySpeed(4.0)

    # Create the terrain
    terrain = veh.RigidTerrain(sys)
    patch_mat = chrono.ChMaterialSurfaceNSC()
    patch_mat.SetFriction(0.9)
    patch_mat.SetRestitution(0.01)
    patch = terrain.AddPatch(patch_mat, chrono.ChVectorD(0, 0, 0),
                             chrono.ChVectorD(0, 0, 1), 200, 100)
    patch.SetColor(chrono.ChColor(0.8, 0.8, 0.5))
    patch.SetTexture(veh.GetDataFile("terrain/textures/tile4.jpg"), 200, 200)
    terrain.Initialize()

    # Create and initialize the first vehicle
    hmmwv_1 = veh.HMMWV_Reduced(sys)
    hmmwv_1.SetInitPosition(
        chrono.ChCoordsysD(chrono.ChVectorD(0, -1.5, 1.0),
                           chrono.ChQuaternionD(1, 0, 0, 0)))
    hmmwv_1.SetPowertrainType(veh.PowertrainModelType_SIMPLE)
    hmmwv_1.SetDriveType(veh.DrivelineType_RWD)
    hmmwv_1.SetTireType(veh.TireModelType_RIGID)
    hmmwv_1.Initialize()
    hmmwv_1.SetChassisVisualizationType(veh.VisualizationType_PRIMITIVES)
    hmmwv_1.SetSuspensionVisualizationType(veh.VisualizationType_PRIMITIVES)
    hmmwv_1.SetSteeringVisualizationType(veh.VisualizationType_PRIMITIVES)
    hmmwv_1.SetWheelVisualizationType(veh.VisualizationType_NONE)
    hmmwv_1.SetTireVisualizationType(veh.VisualizationType_PRIMITIVES)

    driver_data_1 = veh.vector_Entry([
        veh.DataDriverEntry(0.0, 0.0, 0.0, 0.0),
        veh.DataDriverEntry(0.5, 0.0, 0.0, 0.0),
        veh.DataDriverEntry(0.7, 0.3, 0.7, 0.0),
        veh.DataDriverEntry(1.0, 0.3, 0.7, 0.0),
        veh.DataDriverEntry(3.0, 0.5, 0.1, 0.0)
    ])
    driver_1 = veh.ChDataDriver(hmmwv_1.GetVehicle(), driver_data_1)
    driver_1.Initialize()

    # Create and initialize the second vehicle
    hmmwv_2 = veh.HMMWV_Reduced(sys)
    hmmwv_2.SetInitPosition(
        chrono.ChCoordsysD(chrono.ChVectorD(7, 1.5, 1.0),
                           chrono.ChQuaternionD(1, 0, 0, 0)))
    hmmwv_2.SetPowertrainType(veh.PowertrainModelType_SIMPLE)
    hmmwv_2.SetDriveType(veh.DrivelineType_RWD)
    hmmwv_2.SetTireType(veh.TireModelType_RIGID)
    hmmwv_2.Initialize()
    hmmwv_2.SetChassisVisualizationType(veh.VisualizationType_PRIMITIVES)
    hmmwv_2.SetSuspensionVisualizationType(veh.VisualizationType_PRIMITIVES)
    hmmwv_2.SetSteeringVisualizationType(veh.VisualizationType_PRIMITIVES)
    hmmwv_2.SetWheelVisualizationType(veh.VisualizationType_NONE)
    hmmwv_2.SetTireVisualizationType(veh.VisualizationType_PRIMITIVES)

    driver_data_2 = veh.vector_Entry([
        veh.DataDriverEntry(0.0, 0.0, 0.0, 0.0),
        veh.DataDriverEntry(0.5, 0.0, 0.0, 0.0),
        veh.DataDriverEntry(0.7, -0.3, 0.7, 0.0),
        veh.DataDriverEntry(1.0, -0.3, 0.7, 0.0),
        veh.DataDriverEntry(3.0, -0.5, 0.1, 0.0)
    ])
    driver_2 = veh.ChDataDriver(hmmwv_2.GetVehicle(), driver_data_2)
    driver_2.Initialize()

    # Create the vehicle Irrlicht interface
    app = veh.ChWheeledVehicleIrrApp(hmmwv_1.GetVehicle(), 'Two Car Demo',
                                     irr.dimension2du(1000, 800))

    app.SetSkyBox()
    app.AddTypicalLights(irr.vector3df(30, -30, 100),
                         irr.vector3df(30, 50, 100), 250, 130)
    app.AddTypicalLogo(chrono.GetChronoDataFile('logo_pychrono_alpha.png'))
    app.SetChaseCamera(chrono.ChVectorD(0.0, 0.0, 0.75), 6.0, 0.5)
    app.SetChaseCameraState(veh.ChChaseCamera.Track)
    app.SetChaseCameraPosition(chrono.ChVectorD(-15, 0, 2.0))
    app.SetTimestep(step_size)
    app.AssetBindAll()
    app.AssetUpdateAll()

    # Simulation loop
    realtime_timer = chrono.ChRealtimeStepTimer()
    while (app.GetDevice().run()):
        time = hmmwv_1.GetSystem().GetChTime()

        app.BeginScene(True, True, irr.SColor(255, 140, 161, 192))
        app.DrawAll()
        app.EndScene()

        # Get driver inputs
        driver_inputs_1 = driver_1.GetInputs()
        driver_inputs_2 = driver_2.GetInputs()

        # Update modules (process inputs from other modules)
        driver_1.Synchronize(time)
        driver_2.Synchronize(time)
        hmmwv_1.Synchronize(time, driver_inputs_1, terrain)
        hmmwv_2.Synchronize(time, driver_inputs_2, terrain)
        terrain.Synchronize(time)
        app.Synchronize("", driver_inputs_1)

        # Advance simulation for one timestep for all modules
        driver_1.Advance(step_size)
        driver_2.Advance(step_size)
        hmmwv_1.Advance(step_size)
        hmmwv_2.Advance(step_size)
        terrain.Advance(step_size)
        app.Advance(step_size)

        # Advance state of entire system (containing both vehicles)
        sys.DoStepDynamics(step_size)

        # Spin in place for real time to catch up
        realtime_timer.Spin(step_size)
    return 0
    def reset(self):
        n = 2 * np.random.randint(0, 4)
        b1 = 0
        b2 = 0
        r1 = n
        r2 = n
        r3 = n
        r4 = n
        r5 = n
        t1 = 0
        t2 = 0
        t3 = 0
        c = 0
        self.assets = AssetList(b1, b2, r1, r2, r3, r4, r5, t1, t2, t3, c)
        # Create systems
        self.system = chrono.ChSystemNSC()
        self.system.Set_G_acc(chrono.ChVectorD(0, 0, -9.81))
        self.system.SetSolverType(chrono.ChSolver.Type_BARZILAIBORWEIN)
        self.system.SetSolverMaxIterations(150)
        self.system.SetMaxPenetrationRecoverySpeed(4.0)

        # Create the terrain
        rigid_terrain = True
        self.terrain = veh.RigidTerrain(self.system)
        if rigid_terrain:
            patch_mat = chrono.ChMaterialSurfaceNSC()
            patch_mat.SetFriction(0.9)
            patch_mat.SetRestitution(0.01)
            patch = self.terrain.AddPatch(patch_mat, chrono.ChVectorD(0, 0, 0),
                                          chrono.ChVectorD(0, 0, 1),
                                          self.terrain_length * 1.5,
                                          self.terrain_width * 1.5)
        else:
            self.bitmap_file = os.path.dirname(
                os.path.realpath(__file__)) + "/../utils/height_map.bmp"
            self.bitmap_file_backup = os.path.dirname(
                os.path.realpath(__file__)) + "/../utils/height_map_backup.bmp"
            shape = (252, 252)
            generate_random_bitmap(shape=shape,
                                   resolutions=[(2, 2)],
                                   mappings=[(-1.5, 1.5)],
                                   file_name=self.bitmap_file)
            try:
                patch = self.terrain.AddPatch(
                    chrono.CSYSNORM,  # position
                    self.bitmap_file,  # heightmap file (.bmp)
                    "test",  # mesh name
                    self.terrain_length * 1.5,  # sizeX
                    self.terrain_width * 1.5,  # sizeY
                    self.min_terrain_height,  # hMin
                    self.max_terrain_height)  # hMax
            except Exception:
                print('Corrupt Bitmap File')
                patch = self.terrain.AddPatch(
                    chrono.CSYSNORM,  # position
                    self.bitmap_file_backup,  # heightmap file (.bmp)
                    "test",  # mesh name
                    self.terrain_length * 1.5,  # sizeX
                    self.terrain_width * 1.5,  # sizeY
                    self.min_terrain_height,  # hMin
                    self.max_terrain_height)  # hMax
        patch.SetTexture(veh.GetDataFile("terrain/textures/grass.jpg"), 200,
                         200)

        patch.SetColor(chrono.ChColor(0.8, 0.8, 0.5))
        self.terrain.Initialize()

        ground_body = patch.GetGroundBody()
        ground_asset = ground_body.GetAssets()[0]
        visual_asset = chrono.CastToChVisualization(ground_asset)
        visual_asset.SetStatic(True)
        vis_mat = chrono.ChVisualMaterial()
        vis_mat.SetKdTexture(veh.GetDataFile("terrain/textures/grass.jpg"))
        visual_asset.material_list.append(vis_mat)

        theta = random.random() * 2 * np.pi
        x, y = self.terrain_length * 0.5 * np.cos(
            theta), self.terrain_width * 0.5 * np.sin(theta)
        z = self.terrain.GetHeight(chrono.ChVectorD(x, y, 0)) + 0.25
        ang = np.pi + theta
        self.initLoc = chrono.ChVectorD(x, y, z)
        self.initRot = chrono.Q_from_AngZ(ang)

        self.vehicle = veh.Gator(self.system)
        self.vehicle.SetContactMethod(chrono.ChContactMethod_NSC)
        self.vehicle.SetChassisCollisionType(veh.ChassisCollisionType_NONE)
        self.vehicle.SetChassisFixed(False)
        self.m_inputs = veh.Inputs()
        self.vehicle.SetInitPosition(
            chrono.ChCoordsysD(self.initLoc, self.initRot))
        self.vehicle.SetTireType(veh.TireModelType_TMEASY)
        self.vehicle.SetTireStepSize(self.timestep)
        self.vehicle.Initialize()

        if self.play_mode:
            self.vehicle.SetChassisVisualizationType(
                veh.VisualizationType_MESH)
            self.vehicle.SetWheelVisualizationType(veh.VisualizationType_MESH)
            self.vehicle.SetTireVisualizationType(veh.VisualizationType_MESH)
        else:
            self.vehicle.SetChassisVisualizationType(
                veh.VisualizationType_PRIMITIVES)
            self.vehicle.SetWheelVisualizationType(
                veh.VisualizationType_PRIMITIVES)
            self.vehicle.SetTireVisualizationType(
                veh.VisualizationType_PRIMITIVES)
        self.vehicle.SetSuspensionVisualizationType(
            veh.VisualizationType_PRIMITIVES)
        self.vehicle.SetSteeringVisualizationType(
            veh.VisualizationType_PRIMITIVES)
        self.chassis_body = self.vehicle.GetChassisBody()
        # self.chassis_body.GetCollisionModel().ClearModel()
        # size = chrono.ChVectorD(3,2,0.2)
        # self.chassis_body.GetCollisionModel().AddBox(0.5 * size.x, 0.5 * size.y, 0.5 * size.z)
        # self.chassis_body.GetCollisionModel().BuildModel()
        self.chassis_collision_box = chrono.ChVectorD(3, 2, 0.2)

        # Driver
        self.driver = veh.ChDriver(self.vehicle.GetVehicle())

        # create goal
        # pi/4 ang displ
        delta_theta = (random.random() - 0.5) * 1.0 * np.pi
        gx, gy = self.terrain_length * 0.5 * np.cos(
            theta + np.pi +
            delta_theta), self.terrain_width * 0.5 * np.sin(theta + np.pi +
                                                            delta_theta)
        self.goal = chrono.ChVectorD(
            gx, gy,
            self.terrain.GetHeight(chrono.ChVectorD(gx, gy, 0)) + 1.0)

        i = 0
        while (self.goal - self.initLoc).Length() < 15:
            gx = random.random() * self.terrain_length - self.terrain_length / 2
            gy = random.random() * self.terrain_width - self.terrain_width / 2
            self.goal = chrono.ChVectorD(gx, gy, self.max_terrain_height + 1)
            if i > 100:
                print('Break')
                break
            i += 1

        # self.goal = chrono.ChVectorD(75, 0, 0)
        # Origin in Madison WI
        self.origin = chrono.ChVectorD(43.073268, -89.400636, 260.0)
        self.goal_coord = chrono.ChVectorD(self.goal)
        sens.Cartesian2GPS(self.goal_coord, self.origin)

        self.goal_sphere = chrono.ChBodyEasySphere(.55, 1000, True, False)
        self.goal_sphere.SetBodyFixed(True)

        sphere_asset = self.goal_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)
        visual_asset.SetStatic(True)

        self.goal_sphere.SetPos(self.goal)
        if self.play_mode:
            self.system.Add(self.goal_sphere)

        # create obstacles
        # start = t.time()
        self.assets.Clear()
        self.assets.RandomlyPositionAssets(self.system,
                                           self.initLoc,
                                           self.goal,
                                           self.terrain,
                                           self.terrain_length * 1.5,
                                           self.terrain_width * 1.5,
                                           should_scale=False)

        # Set the time response for steering and throttle inputs.
        # NOTE: this is not exact, since we do not render quite at the specified FPS.
        steering_time = 0.75
        # time to go from 0 to +1 (or from 0 to -1)
        throttle_time = .5
        # time to go from 0 to +1
        braking_time = 0.3
        # time to go from 0 to +1
        self.SteeringDelta = (self.timestep / steering_time)
        self.ThrottleDelta = (self.timestep / throttle_time)
        self.BrakingDelta = (self.timestep / braking_time)

        self.manager = sens.ChSensorManager(self.system)
        self.manager.scene.AddPointLight(chrono.ChVectorF(100, 100, 100),
                                         chrono.ChVectorF(1, 1, 1), 5000.0)
        self.manager.scene.AddPointLight(chrono.ChVectorF(-100, -100, 100),
                                         chrono.ChVectorF(1, 1, 1), 5000.0)
        # Let's not, for the moment, give a different scenario during test
        """
        if self.play_mode:
            self.manager.scene.GetBackground().has_texture = True;
            self.manager.scene.GetBackground().env_tex = "sensor/textures/qwantani_8k.hdr"
            self.manager.scene.GetBackground().has_changed = True;
        """
        # -----------------------------------------------------
        # Create a self.camera and add it to the sensor manager
        # -----------------------------------------------------
        #chrono.ChFrameD(chrono.ChVectorD(1.5, 0, .875), chrono.Q_from_AngAxis(0, chrono.ChVectorD(0, 1, 0))),
        self.camera = sens.ChCameraSensor(
            self.chassis_body,  # body camera is attached to
            20,  # scanning rate in Hz
            chrono.ChFrameD(
                chrono.ChVectorD(.65, 0, .75),
                chrono.Q_from_AngAxis(0, chrono.ChVectorD(0, 1, 0))),
            # offset pose
            self.camera_width,  # number of horizontal samples
            self.camera_height,  # number of vertical channels
            chrono.CH_C_PI / 2,  # horizontal field of view
            6  # supersampling factor
        )
        self.camera.SetName("Camera Sensor")
        self.camera.PushFilter(sens.ChFilterRGBA8Access())
        if self.play_mode:
            self.camera.PushFilter(
                sens.ChFilterVisualize(self.camera_width, self.camera_height,
                                       "RGB Camera"))
        self.manager.AddSensor(self.camera)

        # -----------------------------------------------------
        # Create a self.gps and add it to the sensor manager
        # -----------------------------------------------------
        gps_noise_none = sens.ChGPSNoiseNone()
        self.gps = sens.ChGPSSensor(
            self.chassis_body, 15,
            chrono.ChFrameD(
                chrono.ChVectorD(0, 0, 0),
                chrono.Q_from_AngAxis(0, chrono.ChVectorD(0, 1, 0))),
            self.origin, gps_noise_none)
        self.gps.SetName("GPS Sensor")
        self.gps.PushFilter(sens.ChFilterGPSAccess())
        self.manager.AddSensor(self.gps)

        # have to reconstruct scene because sensor loads in meshes separately (ask Asher)
        # start = t.time()
        if self.assets.GetNum() > 0:
            # self.assets.TransformAgain()
            # start = t.time()
            for asset in self.assets.assets:
                if len(asset.frames) > 0:
                    self.manager.AddInstancedStaticSceneMeshes(
                        asset.frames, asset.mesh.shape)
            # self.manager.ReconstructScenes()
            # self.manager.AddInstancedStaticSceneMeshes(self.assets.frames, self.assets.shapes)
            # self.manager.Update()
            # print('Reconstruction :: ', t.time() - start)

        self.old_dist = (self.goal - self.initLoc).Length()

        self.step_number = 0
        self.c_f = 0
        self.isdone = False
        self.render_setup = False
        self.dist0 = (self.goal - self.chassis_body.GetPos()).Length()
        if self.play_mode:
            self.render()

        # print(self.get_ob()[1])
        return self.get_ob()
rod.AddAsset(cyl_r)

col_r = chrono.ChColorAsset()
col_r.SetColor(chrono.ChColor(0.2, 0.6, 0.2))
rod.AddAsset(col_r)

#### -------------------------------------------------------------------------
#### EXERCISE 2.1
#### Enable contact on the slider body and specify contact geometry
#### The contact shape attached to the slider body should be a box with the
#### same dimensions as the visualization asset, centered at the body origin.
#### Use a coefficient of friction of 0.4.
#### -------------------------------------------------------------------------

slider.SetCollide(True)
slider_mat = chrono.ChMaterialSurfaceNSC()
slider_mat.SetFriction(0.4)

slider.GetCollisionModel().ClearModel()
slider.GetCollisionModel().AddBox(slider_mat, 0.2, 0.1, 0.1, chrono.VNULL,
                                  chrono.ChMatrix33D(chrono.QUNIT))
slider.GetCollisionModel().BuildModel()

#### -------------------------------------------------------------------------
#### EXERCISE 2.2
#### Create a new body, with a spherical shape (radius 0.2), used both as
#### visualization asset and contact shape (mu = 0.4). This body should have:
####    mass: 1
####    moments of inertia: I_xx = I_yy = I_zz = 0.02
####    initial location: (5.5, 0, 0)
#### -------------------------------------------------------------------------