def __init__(self,
system: 'WAChronoSystem',
vehicle_inputs: 'WAVehicleInputs',
env: 'WAEnvironment',
filename: str,
init_loc: WAVector = WAVector([0, 0, 0.5]),
init_rot: WAQuaternion = WAQuaternion([1, 0, 0, 0]),
init_speed: float = 0.0):
super().__init__(
system, vehicle_inputs,
get_wa_data_file("vehicles/GoKart/GoKart_KinematicBicycle.json"))
# Get the filenames
vehicle_file, powertrain_file, tire_file = read_vehicle_model_file(
filename)
# Create the vehicle
vehicle = veh.WheeledVehicle(system._system, vehicle_file)
# Initialize the vehicle
init_loc = WAVector_to_ChVector(init_loc)
init_rot = WAQuaternion_to_ChQuaternion(init_rot)
vehicle.Initialize(chrono.ChCoordsysD(init_loc, init_rot), init_speed)
# Set the visualization components for the vehicle
vehicle.SetChassisVisualizationType(veh.VisualizationType_MESH)
vehicle.SetSuspensionVisualizationType(veh.VisualizationType_NONE)
vehicle.SetSteeringVisualizationType(veh.VisualizationType_NONE)
vehicle.SetWheelVisualizationType(veh.VisualizationType_MESH)
# Create the powertrain
# Assumes a SimplePowertrain
powertrain = veh.SimplePowertrain(powertrain_file)
vehicle.InitializePowertrain(powertrain)
# Create and initialize the tires
for axle in vehicle.GetAxles():
tireL = create_tire_from_json(tire_file)
vehicle.InitializeTire(tireL, axle.m_wheels[0],
veh.VisualizationType_MESH)
tireR = create_tire_from_json(tire_file)
vehicle.InitializeTire(tireR, axle.m_wheels[1],
veh.VisualizationType_MESH)
self._vehicle = vehicle
self._terrain = env._terrain
def reset(self):
self.isdone = False
self.robosystem.Clear()
#action = (np.random.rand(6,)-0.5)*2
#torques = np.multiply(action, self.maxT)
self.exported_items = chrono.ImportSolidWorksSystem(self.fpath)
self.csys = []
self.frames = []
self.revs = []
self.motors = []
self.limits = []
for con, coi in zip(self.con_link, self.coi_link):
indices = []
for i in range(len(self.exported_items)):
if con == self.exported_items[i].GetName(
) or coi == self.exported_items[i].GetName():
indices.append(i)
rev = self.exported_items[indices[0]]
af0 = rev.GetAssetsFrame()
# Revolute joints and ChLinkMotorRotation are z oriented, while parallel is x oriented.
# Event though this Frame won't be used anymore is good practice to create a copy before editing its value.
af = chrono.ChFrameD(af0)
af.SetRot(af0.GetRot() % chrono.Q_ROTATE_X_TO_Z)
self.frames.append(af)
for i in indices:
del self.exported_items[i]
# ADD IMPORTED ITEMS TO THE SYSTEM
for my_item in self.exported_items:
self.robosystem.Add(my_item)
"""
$$$$$$$$ FIND THE SW DEFINED CONSTRAINTS, GET THEIR MARKERS AND GET RID OF EM $$$$$$$$
"""
self.bodies = [
self.robosystem.SearchBody(name) for name in self.bodiesNames
]
self.hand = self.robosystem.SearchBody('Hand_base_and_p07-2')
self.base = self.robosystem.SearchBody('Racer3_p01-3')
self.biceps = self.robosystem.SearchBody('Racer3_p03-1')
self.forearm = self.robosystem.SearchBody('Racer3_p05-1')
self.finger1 = self.robosystem.SearchBody('HAND_e_finger-1')
self.finger2 = self.robosystem.SearchBody('HAND_e_finger-2')
for i in range(len(self.con_link)):
revolute = chrono.ChLinkLockRevolute()
cs = chrono.ChCoordsysD(self.frames[i].GetPos(),
self.frames[i].GetRot())
self.csys.append(cs)
revolute.Initialize(self.bodies[i], self.bodies[i + 1],
self.csys[i])
self.revs.append(revolute)
self.robosystem.Add(self.revs[i])
lim = self.revs[i].GetLimit_Rz()
self.limits.append(lim)
self.limits[i].SetActive(True)
self.limits[i].SetMin(self.minRot[i] * (math.pi / 180))
self.limits[i].SetMax(self.maxRot[i] * (math.pi / 180))
m = chrono.ChLinkMotorRotationTorque()
m.Initialize(self.bodies[i], self.bodies[i + 1], self.frames[i])
#self.robosystem.Add(m2)
#m2.SetTorqueFunction(chrono.ChFunction_Const(5))
self.motors.append(m)
#self.motors[i].SetTorqueFunction(chrono.ChFunction_Const(float(torques[i])))
self.robosystem.Add(self.motors[i])
self.body_floor = chrono.ChBody()
self.body_floor.SetBodyFixed(True)
self.body_floor.SetPos(chrono.ChVectorD(0, -1, 0))
# Floor Collision.
self.body_floor.GetCollisionModel().ClearModel()
self.body_floor.GetCollisionModel().AddBox(self.my_material, 5, 1, 5,
chrono.ChVectorD(0, 0, 0))
self.body_floor.GetCollisionModel().BuildModel()
self.body_floor.SetCollide(True)
# Visualization shape
body_floor_shape = chrono.ChBoxShape()
body_floor_shape.GetBoxGeometry().Size = chrono.ChVectorD(5, 1, 5)
body_floor_shape.SetColor(chrono.ChColor(0.4, 0.4, 0.5))
self.body_floor.GetAssets().push_back(body_floor_shape)
body_floor_texture = chrono.ChTexture()
texpath = os.path.join(chrono.GetChronoDataPath(), 'concrete.jpg')
body_floor_texture.SetTextureFilename(texpath)
self.body_floor.GetAssets().push_back(body_floor_texture)
self.robosystem.Add(self.body_floor)
r = np.random.rand(2, ) - np.asarray([0.5, 0.5])
self.targ_init_pos = [
-0.52 + 2 * (r[0] * 0.05), 0.015, 2 * r[1] * 0.05
]
self.targ_box = chrono.ChBody()
# UNset to grasp
self.targ_box.SetBodyFixed(True)
self.targ_box.SetPos(
chrono.ChVectorD(self.targ_init_pos[0], self.targ_init_pos[1],
self.targ_init_pos[2]))
# Floor Collision.
self.targ_box.GetCollisionModel().ClearModel()
self.targ_box.GetCollisionModel().AddBox(self.my_material, 0.015,
0.015, 0.015,
chrono.ChVectorD(0, 0, 0))
self.targ_box.GetCollisionModel().BuildModel()
self.targ_box.SetCollide(True)
# Visualization shape
targ_box_shape = chrono.ChBoxShape()
targ_box_shape.GetBoxGeometry().Size = chrono.ChVectorD(
0.015, 0.015, 0.015)
col = chrono.ChColorAsset()
col.SetColor(chrono.ChColor(1.0, 0, 0))
self.targ_box.GetAssets().push_back(targ_box_shape)
self.targ_box.GetAssets().push_back(col)
self.robosystem.Add(self.targ_box)
self.numsteps = 0
if (self.render_setup):
self.myapplication.AssetBindAll()
self.myapplication.AssetUpdateAll()
return self.get_ob()
## Define two quaternions representing:
## - a rotation of -90 degrees around x (z2y)
## - a rotation of +90 degrees around y (z2x)
z2y = chrono.ChQuaternionD()
z2x = chrono.ChQuaternionD()
z2y.Q_from_AngAxis(-chrono.CH_C_PI / 2, chrono.ChVectorD(1, 0, 0))
z2x.Q_from_AngAxis(chrono.CH_C_PI / 2, chrono.ChVectorD(0, 1, 0))
## Revolute joint between ground and crank.
## The rotational axis of a revolute joint is along the Z axis of the
## specified joint coordinate frame. Here, we apply the 'z2y' rotation to
## align it with the Y axis of the global reference frame.
revolute_ground_crank = chrono.ChLinkLockRevolute()
revolute_ground_crank.SetName("revolute_ground_crank")
revolute_ground_crank.Initialize(
ground, crank, chrono.ChCoordsysD(chrono.ChVectorD(0, 0, 0), z2y))
system.AddLink(revolute_ground_crank)
## Prismatic joint between ground and slider.
## The translational axis of a prismatic joint is along the Z axis of the
## specified joint coordinate system. Here, we apply the 'z2x' rotation to
## align it with the X axis of the global reference frame.
prismatic_ground_slider = chrono.ChLinkLockPrismatic()
prismatic_ground_slider.SetName("prismatic_ground_slider")
prismatic_ground_slider.Initialize(
ground, slider, chrono.ChCoordsysD(chrono.ChVectorD(2, 0, 0), z2x))
system.AddLink(prismatic_ground_slider)
## Distance constraint between crank and slider.
## We provide the points on the two bodies in the global reference frame.
## By default the imposed distance is calculated automatically as the distance
def __init__(self, step_size, sys, controller, irrlicht=False, vehicle_type='json', initLoc=chrono.ChVectorD(0,0,0), initRot=chrono.ChQuaternionD(1,0,0,0), vis_balls=False, render_step_size=1.0/60):
# Chrono parameters
self.step_size = step_size
self.irrlicht = irrlicht
self.step_number = 0
# Vehicle controller
self.controller = controller
# Initial vehicle position
self.initLoc = initLoc
# Initial vehicle orientation
self.initRot = initRot
# Point on chassis tracked by the camera (Irrlicht only)
self.trackPoint = chrono.ChVectorD(0.0, 0.0, 1.75)
if vehicle_type == 'json':
# JSON file for vehicle model
self.vehicle_file = veh.GetDataPath() + os.path.join('hmmwv', 'vehicle', 'HMMWV_Vehicle.json')
checkFile(self.vehicle_file)
# JSON file for powertrain (simple)
self.simplepowertrain_file = veh.GetDataPath() + os.path.join('generic', 'powertrain', 'SimplePowertrain.json')
checkFile(self.simplepowertrain_file)
# JSON files tire models (rigid)
self.rigidtire_file = veh.GetDataPath() + os.path.join('hmmwv', 'tire', 'HMMWV_RigidTire.json')
checkFile(self.rigidtire_file)
# --------------------------
# Create the various modules
# --------------------------
if sys == None:
self.wheeled_vehicle = veh.WheeledVehicle(self.vehicle_file)
else:
self.wheeled_vehicle = veh.WheeledVehicle(sys, self.vehicle_file)
self.wheeled_vehicle.Initialize(chrono.ChCoordsysD(self.initLoc, self.initRot))
self.wheeled_vehicle.SetChassisVisualizationType(veh.VisualizationType_PRIMITIVES)
self.wheeled_vehicle.SetSuspensionVisualizationType(veh.VisualizationType_PRIMITIVES)
self.wheeled_vehicle.SetSteeringVisualizationType(veh.VisualizationType_PRIMITIVES)
self.wheeled_vehicle.SetWheelVisualizationType(veh.VisualizationType_NONE)
# Create and initialize the powertrain system
self.powertrain = veh.SimplePowertrain(self.simplepowertrain_file)
self.wheeled_vehicle.InitializePowertrain(self.powertrain)
# Create and initialize the tires
for axle in self.wheeled_vehicle.GetAxles():
tireL = veh.RigidTire(self.rigidtire_file)
self.wheeled_vehicle.InitializeTire(tireL, axle.m_wheels[0], veh.VisualizationType_MESH)
tireR = veh.RigidTire(self.rigidtire_file)
self.wheeled_vehicle.InitializeTire(tireR, axle.m_wheels[1], veh.VisualizationType_MESH)
self.vehicle = self.wheeled_vehicle
self.sys = self.wheeled_vehicle.GetSystem()
elif vehicle_type == 'rccar':
if sys == None:
self.rc_vehicle = veh.RCCar()
self.rc_vehicle.SetContactMethod(chrono.ChMaterialSurface.SMC)
self.rc_vehicle.SetChassisCollisionType(veh.ChassisCollisionType_NONE)
else:
self.rc_vehicle = veh.RCCar(sys)
self.rc_vehicle.SetChassisFixed(False)
self.rc_vehicle.SetInitPosition(chrono.ChCoordsysD(initLoc, initRot))
self.rc_vehicle.SetTireType(veh.TireModelType_RIGID)
self.rc_vehicle.SetTireStepSize(step_size)
self.rc_vehicle.Initialize()
self.rc_vehicle.SetChassisVisualizationType(veh.VisualizationType_PRIMITIVES)
self.rc_vehicle.SetSuspensionVisualizationType(veh.VisualizationType_PRIMITIVES)
self.rc_vehicle.SetSteeringVisualizationType(veh.VisualizationType_PRIMITIVES)
self.rc_vehicle.SetWheelVisualizationType(veh.VisualizationType_PRIMITIVES)
self.rc_vehicle.SetTireVisualizationType(veh.VisualizationType_PRIMITIVES)
self.vehicle = self.rc_vehicle.GetVehicle()
self.sys = self.vehicle.GetSystem()
self.trackPoint = chrono.ChVectorD(4, 0.0, .15)
elif vehicle_type == 'sedan':
if sys == None:
self.sedan = veh.Sedan()
self.sedan.SetContactMethod(chrono.ChMaterialSurface.NSC)
self.sedan.SetChassisCollisionType(veh.ChassisCollisionType_NONE)
else:
self.sedan = veh.Sedan(sys)
self.sedan.SetChassisFixed(False)
self.sedan.SetInitPosition(chrono.ChCoordsysD(initLoc, initRot))
self.sedan.SetTireType(veh.TireModelType_RIGID)
self.sedan.SetTireStepSize(step_size)
self.sedan.Initialize()
self.sedan.SetChassisVisualizationType(veh.VisualizationType_PRIMITIVES)
self.sedan.SetSuspensionVisualizationType(veh.VisualizationType_PRIMITIVES)
self.sedan.SetSteeringVisualizationType(veh.VisualizationType_PRIMITIVES)
self.sedan.SetWheelVisualizationType(veh.VisualizationType_PRIMITIVES)
self.sedan.SetTireVisualizationType(veh.VisualizationType_PRIMITIVES)
self.vehicle = self.sedan.GetVehicle()
self.sys = self.vehicle.GetVehicle().GetSystem()
# -------------
# Create driver
# -------------
self.driver = Driver(self.vehicle)
self.driver.SetStepSize(step_size)
# 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 = 1.0 # time to go from 0 to +1
braking_time = 0.3 # time to go from 0 to +1
self.driver.SetSteeringDelta(render_step_size / steering_time)
self.driver.SetThrottleDelta(render_step_size / throttle_time)
self.driver.SetBrakingDelta(render_step_size / braking_time)
self.vis_balls = vis_balls
if self.vis_balls:
self.sentinel_sphere = chrono.ChBodyEasySphere(.25, 1000, False, True)
self.sentinel_sphere.SetBodyFixed(True)
self.sentinel_sphere.AddAsset(chrono.ChColorAsset(1,0,0))
self.sys.Add(self.sentinel_sphere)
self.sentinel_target = chrono.ChBodyEasySphere(.25, 1000, False, True)
self.sentinel_target.SetBodyFixed(True)
self.sentinel_target.AddAsset(chrono.ChColorAsset(0,1,0));
self.sys.Add(self.sentinel_target)
# Vehicle parameters for matplotlib
self.length = self.vehicle.GetWheelbase() + 2.0 # [m]
self.width = self.vehicle.GetWheeltrack(0) # [m]
self.backtowheel = 1.0 # [m]
self.wheel_len = self.vehicle.GetWheel(0, 1).GetWidth() * 2 # [m]
self.wheel_width = self.vehicle.GetWheel(0, 1).GetWidth() # [m]
self.tread = self.vehicle.GetWheeltrack(0) / 2 # [m]
self.wb = self.vehicle.GetWheelbase() # [m]
self.offset = [-4.0,0] # [m]
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
def reset(self):
self.isdone = False
self.ant_sys.Clear()
self.body_abdomen = chrono.ChBody()
self.body_abdomen.SetPos(chrono.ChVectorD(0, self.abdomen_y0, 0 ))
self.body_abdomen.SetMass(self.abdomen_mass)
self.body_abdomen.SetInertiaXX(self.abdomen_inertia)
# set collision surface properties
self.body_abdomen.SetMaterialSurface(self.ant_material)
abdomen_ellipsoid = chrono.ChEllipsoid(chrono.ChVectorD(0, 0, 0 ), chrono.ChVectorD(self.abdomen_x, self.abdomen_y, self.abdomen_z ))
self.abdomen_shape = chrono.ChEllipsoidShape(abdomen_ellipsoid)
self.body_abdomen.AddAsset(self.abdomen_shape)
self.body_abdomen.SetMaterialSurface(self.ant_material)
self.body_abdomen.SetCollide(True)
self.body_abdomen.GetCollisionModel().ClearModel()
self.body_abdomen.GetCollisionModel().AddEllipsoid(self.abdomen_x, self.abdomen_y, self.abdomen_z, chrono.ChVectorD(0, 0, 0 ) )
self.body_abdomen.GetCollisionModel().BuildModel()
self.ant_sys.Add(self.body_abdomen)
leg_ang = (1/4)*math.pi+(1/2)*math.pi*np.array([0,1,2,3])
Leg_quat = [chrono.ChQuaternionD() for i in range(len(leg_ang))]
self.leg_body = [chrono.ChBody() for i in range(len(leg_ang))]
self.leg_pos= [chrono.ChVectorD() for i in range(len(leg_ang))]
leg_cyl = chrono.ChCylinder(-chrono.ChVectorD( self.leg_length/2, 0 ,0),chrono.ChVectorD( self.leg_length/2, 0 ,0), self.leg_radius)
self.leg_shape = chrono.ChCylinderShape(leg_cyl)
ankle_cyl = chrono.ChCylinder(-chrono.ChVectorD( self.ankle_length/2, 0 ,0),chrono.ChVectorD( self.ankle_length/2, 0 ,0), self.ankle_radius)
self.ankle_shape = chrono.ChCylinderShape(ankle_cyl)
foot_sphere = chrono.ChSphere(chrono.ChVectorD(self.ankle_length/2, 0, 0 ), self.ankle_radius )
self.foot_shape = chrono.ChSphereShape(foot_sphere)
Leg_qa = [ chrono.ChQuaternionD() for i in range(len(leg_ang))]
Leg_q = [ chrono.ChQuaternionD() for i in range(len(leg_ang))]
z2x_leg = [ chrono.ChQuaternionD() for i in range(len(leg_ang))]
Leg_rev_pos=[]
Leg_chordsys = []
self.legjoint_frame = []
x_rel = []
z_rel = []
self.Leg_rev = [chrono.ChLinkLockRevolute() for i in range(len(leg_ang))]
self.leg_motor = [chrono.ChLinkMotorRotationTorque() for i in range(len(leg_ang)) ]
#ankle lists
anklejoint_chordsys = []
self.anklejoint_frame = []
self.ankleCOG_frame = []
q_ankle_zrot = [ chrono.ChQuaternionD() for i in range(len(leg_ang))]
self.ankle_body = [chrono.ChBody() for i in range(len(leg_ang))]
self.Ankle_rev = [chrono.ChLinkLockRevolute() for i in range(len(leg_ang))]
self.ankle_motor = [chrono.ChLinkMotorRotationTorque() for i in range(len(leg_ang)) ]
for i in range(len(leg_ang)):
# Legs
Leg_quat[i].Q_from_AngAxis(-leg_ang[i] , chrono.ChVectorD(0, 1, 0))
self.leg_pos[i] = chrono.ChVectorD( (0.5*self.leg_length+self.abdomen_x)*math.cos(leg_ang[i]) ,self.abdomen_y0, (0.5*self.leg_length+self.abdomen_z)*math.sin(leg_ang[i]))
self.leg_body[i].SetPos(self.leg_pos[i])
self.leg_body[i].SetRot(Leg_quat[i])
self.leg_body[i].AddAsset(self.leg_shape)
self.leg_body[i].SetMass(self.leg_mass)
self.leg_body[i].SetInertiaXX(self.leg_inertia)
self.ant_sys.Add(self.leg_body[i])
x_rel.append( Leg_quat[i].Rotate(chrono.ChVectorD(1, 0, 0)))
z_rel.append( Leg_quat[i].Rotate(chrono.ChVectorD(0, 0, 1)))
Leg_qa[i].Q_from_AngAxis(-leg_ang[i] , chrono.ChVectorD(0, 1, 0))
z2x_leg[i].Q_from_AngAxis(chrono.CH_C_PI / 2 , x_rel[i])
Leg_q[i] = z2x_leg[i] * Leg_qa[i]
Leg_rev_pos.append(chrono.ChVectorD(self.leg_pos[i]-chrono.ChVectorD(math.cos(leg_ang[i])*self.leg_length/2,0,math.sin(leg_ang[i])*self.leg_length/2)))
Leg_chordsys.append(chrono.ChCoordsysD(Leg_rev_pos[i], Leg_q[i]))
self.legjoint_frame.append(chrono.ChFrameD(Leg_chordsys[i]))
self.Leg_rev[i].Initialize(self.body_abdomen, self.leg_body[i],Leg_chordsys[i])
self.ant_sys.Add(self.Leg_rev[i])
self.leg_motor[i].Initialize(self.body_abdomen, self.leg_body[i],self.legjoint_frame[i])
self.ant_sys.Add(self.leg_motor[i])
# Ankles
q_ankle_zrot[i].Q_from_AngAxis(-self.ankle_angle , z_rel[i])
anklejoint_chordsys.append(chrono.ChCoordsysD(self.leg_body[i].GetPos()+ self.leg_body[i].GetRot().Rotate(chrono.ChVectorD(self.leg_length/2, 0, 0)) , q_ankle_zrot[i] * self.leg_body[i].GetRot() ))
self.anklejoint_frame.append(chrono.ChFrameD(anklejoint_chordsys[i]))
self.ankle_body[i].SetPos(self.anklejoint_frame[i].GetPos() + self.anklejoint_frame[i].GetRot().Rotate(chrono.ChVectorD(self.ankle_length/2, 0, 0)))
self.ankle_body[i].SetRot( self.anklejoint_frame[i].GetRot() )
self.ankle_body[i].AddAsset(self.ankle_shape)
self.ankle_body[i].SetMass(self.ankle_mass)
self.ankle_body[i].SetInertiaXX(self.ankle_inertia)
self.ant_sys.Add(self.ankle_body[i])
self.Ankle_rev[i].Initialize(self.leg_body[i], self.ankle_body[i], anklejoint_chordsys[i])
self.ant_sys.Add(self.Ankle_rev[i])
self.ankle_motor[i].Initialize(self.leg_body[i], self.ankle_body[i],self.anklejoint_frame[i])
self.ant_sys.Add(self.ankle_motor[i])
# Feet collisions
self.ankle_body[i].SetMaterialSurface(self.ant_material)
self.ankle_body[i].SetCollide(True)
self.ankle_body[i].GetCollisionModel().ClearModel()
self.ankle_body[i].GetCollisionModel().AddSphere(self.ankle_radius, chrono.ChVectorD(self.ankle_length/2, 0, 0 ) )
self.ankle_body[i].GetCollisionModel().BuildModel()
self.ankle_body[i].AddAsset(self.ankle_shape)
self.ankle_body[i].AddAsset(self.foot_shape)
self.Leg_rev[i].GetLimit_Rz().SetActive(True)
self.Leg_rev[i].GetLimit_Rz().SetMin(-math.pi/3)
self.Leg_rev[i].GetLimit_Rz().SetMax(math.pi/3)
self.Ankle_rev[i].GetLimit_Rz().SetActive(True)
self.Ankle_rev[i].GetLimit_Rz().SetMin(-math.pi/2)
self.Ankle_rev[i].GetLimit_Rz().SetMax(math.pi/4)
# Create the room floor: a simple fixed rigid body with a collision shape
# and a visualization shape
self.body_floor = chrono.ChBody()
self.body_floor.SetBodyFixed(True)
self.body_floor.SetPos(chrono.ChVectorD(0, -1, 0 ))
self.body_floor.SetMaterialSurface(self.ant_material)
# Floor Collision.
self.body_floor.SetMaterialSurface(self.ant_material)
self.body_floor.GetCollisionModel().ClearModel()
self.body_floor.GetCollisionModel().AddBox(50, 1, 50, chrono.ChVectorD(0, 0, 0 ))
self.body_floor.GetCollisionModel().BuildModel()
self.body_floor.SetCollide(True)
# Visualization shape
body_floor_shape = chrono.ChBoxShape()
body_floor_shape.GetBoxGeometry().Size = chrono.ChVectorD(5, 1, 5)
body_floor_shape.SetColor(chrono.ChColor(0.4,0.4,0.5))
self.body_floor.GetAssets().push_back(body_floor_shape)
body_floor_texture = chrono.ChTexture()
body_floor_texture.SetTextureFilename(chrono.GetChronoDataFile('vehicle/terrain/textures/grass.jpg'))
self.body_floor.GetAssets().push_back(body_floor_texture)
self.ant_sys.Add(self.body_floor)
#self.body_abdomen.SetBodyFixed(True)
if (self.animate):
self.myapplication.AssetBindAll()
self.myapplication.AssetUpdateAll()
self.numsteps= 0
self.step(np.zeros(8))
return self.get_ob()
def main() :
#print("Copyright (c) 2017 projectchrono.org\nChrono version: ", CHRONO_VERSION , "\n\n")
# --------------------------
# Create the various modules
# --------------------------
# Create the vehicle system
vehicle = veh.WheeledVehicle(vehicle_file ,chrono.ChMaterialSurface.NSC)
vehicle.Initialize(chrono.ChCoordsysD(initLoc, initRot))
#vehicle.GetChassis().SetFixed(True)
vehicle.SetChassisVisualizationType(veh.VisualizationType_PRIMITIVES)
vehicle.SetSuspensionVisualizationType(veh.VisualizationType_PRIMITIVES)
vehicle.SetSteeringVisualizationType(veh.VisualizationType_PRIMITIVES)
vehicle.SetWheelVisualizationType(veh.VisualizationType_NONE)
# Create the ground
terrain = veh.RigidTerrain(vehicle.GetSystem(), rigidterrain_file)
# Create and initialize the powertrain system
powertrain = veh.SimplePowertrain(simplepowertrain_file)
vehicle.InitializePowertrain(powertrain)
# Create and initialize the tires
for axle in vehicle.GetAxles() :
tireL = veh.RigidTire(rigidtire_file)
vehicle.InitializeTire(tireL, axle.m_wheels[0], veh.VisualizationType_MESH)
tireR = veh.RigidTire(rigidtire_file)
vehicle.InitializeTire(tireR, axle.m_wheels[1], veh.VisualizationType_MESH)
app = veh.ChVehicleIrrApp(vehicle)
app.SetSkyBox()
app.AddTypicalLights(chronoirr.vector3df(30, -30, 100), chronoirr.vector3df(30, 50, 100), 250, 130)
app.AddTypicalLogo(chrono.GetChronoDataPath() + 'logo_pychrono_alpha.png')
app.SetChaseCamera(trackPoint, 6.0, 0.5)
app.SetTimestep(step_size)
app.AssetBindAll()
app.AssetUpdateAll()
driver = veh.ChIrrGuiDriver(app)
# Set the time response for steering and throttle keyboard 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 = 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()
# -----------------
# Initialize output
# -----------------
try:
os.mkdir(out_dir)
except:
print("Error creating directory " )
# Generate JSON information with available output channels
out_json = vehicle.ExportComponentList()
print(out_json)
vehicle.ExportComponentList(out_dir + "/component_list.json")
# ---------------
# Simulation loop
# ---------------
realtime_timer = chrono.ChRealtimeStepTimer()
while (app.GetDevice().run()) :
# Render scene
app.BeginScene(True, True, chronoirr.SColor(255, 140, 161, 192))
app.DrawAll()
app.EndScene()
# Collect output data from modules (for inter-module communication)
driver_inputs = driver.GetInputs()
# Update modules (process inputs from other modules)
time = vehicle.GetSystem().GetChTime()
driver.Synchronize(time)
vehicle.Synchronize(time, driver_inputs, terrain)
terrain.Synchronize(time)
app.Synchronize(driver.GetInputModeAsString(), driver_inputs)
# Advance simulation for one timestep for all modules
driver.Advance(step_size)
vehicle.Advance(step_size)
terrain.Advance(step_size)
app.Advance(step_size)
# Spin in place for real time to catch up
realtime_timer.Spin(step_size)
import pychrono.vehicle as veh import pychrono.irrlicht as chronoirr import numpy as np # ============================================================================= step_size = 2e-3 throttle_value = 0.3 # ============================================================================= # Create the HMMWV vehicle my_hmmwv = veh.HMMWV_Full() my_hmmwv.SetContactMethod(chrono.ChContactMethod_SMC) my_hmmwv.SetChassisFixed(False) my_hmmwv.SetInitPosition(chrono.ChCoordsysD(chrono.ChVectorD(-75, 0, 0.5),chrono.QUNIT)) my_hmmwv.SetPowertrainType(veh.PowertrainModelType_SHAFTS) my_hmmwv.SetDriveType(veh.DrivelineType_RWD) my_hmmwv.SetSteeringType(veh.SteeringType_PITMAN_ARM) my_hmmwv.SetTireType(veh.TireModelType_TMEASY) my_hmmwv.SetTireStepSize(step_size) my_hmmwv.Initialize() my_hmmwv.SetChassisVisualizationType(veh.VisualizationType_PRIMITIVES) my_hmmwv.SetSuspensionVisualizationType(veh.VisualizationType_PRIMITIVES) my_hmmwv.SetSteeringVisualizationType(veh.VisualizationType_PRIMITIVES) my_hmmwv.SetWheelVisualizationType(veh.VisualizationType_NONE) my_hmmwv.SetTireVisualizationType(veh.VisualizationType_PRIMITIVES) # Create the terrain minfo = veh.MaterialInfo()
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
body_particles.GetCollisionModel().ClearModel()
body_particles.GetCollisionModel().AddSphere(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)
body_particles_shape = chrono.ChSphereShape()
body_particles_shape.GetSphereGeometry().rad = 0.005
body_particles.GetAssets().push_back(body_particles_shape)
my_system.Add(body_particles)
# Create the floor: a simple fixed rigid body with a collision shape
# and a visualization shape
body_floor = chrono.ChBody()
body_floor.SetBodyFixed(True)
# Collision shape
def __init__(self, step_size, sys, irrlicht=False, terrain_type='json', height=-0.5, width=300, length=300):
# Chrono parameters
self.step_size = step_size
self.irrlicht = irrlicht
self.step_number = 0
# Rigid terrain dimensions
self.height = height
self.length = length # size in X direction
self.width = width # size in Y direction
self.sys = sys
if terrain_type == 'json':
import os
# JSON files for terrain
self.rigidterrain_file = veh.GetDataPath() + os.path.join('terrain', 'RigidPlane.json')
checkFile(self.rigidterrain_file)
# Create the ground
self.terrain = veh.RigidTerrain(self.sys, self.rigidterrain_file)
elif terrain_type == 'concrete':
# Create the terrain
self.terrain = veh.RigidTerrain(self.sys)
patch = self.terrain.AddPatch(chrono.ChCoordsysD(chrono.ChVectorD(0, 0, self.height - 5), chrono.QUNIT),
chrono.ChVectorD(self.width, self.length, 10))
patch.SetContactFrictionCoefficient(0.9)
patch.SetContactRestitutionCoefficient(0.01)
patch.SetContactMaterialProperties(2e7, 0.3)
patch.SetTexture(chrono.GetChronoDataFile("concrete.jpg"), self.length, self.width)
patch.SetColor(chrono.ChColor(0.8, 0.8, 0.5))
self.terrain.Initialize()
try:
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"))
vis_mat.SetFresnelMax(0);
visual_asset.material_list.append(vis_mat)
except:
print("Not Visual Material")
elif terrain_type == 'hallway':
y_max = 5.65
x_max = 23
offset = chrono.ChVectorD(-x_max/2, -y_max/2, .21)
offsetF = chrono.ChVectorF(offset.x, offset.y, offset.z)
self.terrain = veh.RigidTerrain(self.sys)
coord_sys = chrono.ChCoordsysD(offset, chrono.ChQuaternionD(1,0,0,0))
patch = self.terrain.AddPatch(coord_sys, chrono.GetChronoDataFile("sensor/textures/hallway.obj"), "mesh", 0.01, False)
vis_mesh = chrono.ChTriangleMeshConnected()
vis_mesh.LoadWavefrontMesh(chrono.GetChronoDataFile("sensor/textures/hallway.obj"), True, True)
trimesh_shape = chrono.ChTriangleMeshShape()
trimesh_shape.SetMesh(vis_mesh)
trimesh_shape.SetName("mesh_name")
trimesh_shape.SetStatic(True)
patch.GetGroundBody().AddAsset(trimesh_shape)
patch.SetContactFrictionCoefficient(0.9)
patch.SetContactRestitutionCoefficient(0.01)
patch.SetContactMaterialProperties(2e7, 0.3)
self.terrain.Initialize()
def reset(self):
#print("reset")
self.isdone = False
self.rev_pend_sys.Clear()
# create it
self.body_rod = chrono.ChBody()
# set initial position
self.body_rod.SetPos(chrono.ChVectorD(0, self.size_rod_y / 2, 0))
# set mass properties
self.body_rod.SetMass(self.mass_rod)
self.body_rod.SetInertiaXX(
chrono.ChVectorD(self.inertia_rod_x, self.inertia_rod_y,
self.inertia_rod_x))
# set collision surface properties
self.body_rod.SetMaterialSurface(self.rod_material)
# Visualization shape, for rendering animation
self.cyl_base1 = chrono.ChVectorD(0, -self.size_rod_y / 2, 0)
self.cyl_base2 = chrono.ChVectorD(0, self.size_rod_y / 2, 0)
self.body_rod_shape = chrono.ChCylinderShape()
self.body_rod_shape.GetCylinderGeometry().p1 = self.cyl_base1
self.body_rod_shape.GetCylinderGeometry().p2 = self.cyl_base2
self.body_rod_shape.GetCylinderGeometry().rad = self.radius_rod
self.body_rod.AddAsset(self.body_rod_shape)
self.rev_pend_sys.Add(self.body_rod)
self.body_floor = chrono.ChBody()
self.body_floor.SetBodyFixed(True)
self.body_floor.SetPos(chrono.ChVectorD(0, -5, 0))
self.body_floor.SetMaterialSurface(self.rod_material)
if self.render:
self.body_floor_shape = chrono.ChBoxShape()
self.body_floor_shape.GetBoxGeometry().Size = chrono.ChVectorD(
3, 1, 3)
self.body_floor.GetAssets().push_back(self.body_floor_shape)
self.body_floor_texture = chrono.ChTexture()
self.body_floor_texture.SetTextureFilename(
'../../../data/concrete.jpg')
self.body_floor.GetAssets().push_back(self.body_floor_texture)
self.rev_pend_sys.Add(self.body_floor)
self.body_table = chrono.ChBody()
self.body_table.SetPos(chrono.ChVectorD(0, -self.size_table_y / 2, 0))
self.body_table.SetMaterialSurface(self.rod_material)
if self.render:
self.body_table_shape = chrono.ChBoxShape()
self.body_table_shape.GetBoxGeometry().Size = chrono.ChVectorD(
self.size_table_x / 2, self.size_table_y / 2,
self.size_table_z / 2)
self.body_table_shape.SetColor(chrono.ChColor(0.4, 0.4, 0.5))
self.body_table.GetAssets().push_back(self.body_table_shape)
self.body_table_texture = chrono.ChTexture()
self.body_table_texture.SetTextureFilename(
'../../../data/concrete.jpg')
self.body_table.GetAssets().push_back(self.body_table_texture)
self.body_table.SetMass(0.1)
self.rev_pend_sys.Add(self.body_table)
self.link_slider = chrono.ChLinkLockPrismatic()
z2x = chrono.ChQuaternionD()
z2x.Q_from_AngAxis(-chrono.CH_C_PI / 2, chrono.ChVectorD(0, 1, 0))
self.link_slider.Initialize(
self.body_table, self.body_floor,
chrono.ChCoordsysD(chrono.ChVectorD(0, 0, 0), z2x))
self.rev_pend_sys.Add(self.link_slider)
self.act_initpos = chrono.ChVectorD(0, 0, 0)
self.actuator = chrono.ChLinkMotorLinearForce()
self.actuator.Initialize(self.body_table, self.body_floor,
chrono.ChFrameD(self.act_initpos))
self.rev_pend_sys.Add(self.actuator)
self.rod_pin = chrono.ChMarker()
self.body_rod.AddMarker(self.rod_pin)
self.rod_pin.Impose_Abs_Coord(
chrono.ChCoordsysD(chrono.ChVectorD(0, 0, 0)))
self.table_pin = chrono.ChMarker()
self.body_table.AddMarker(self.table_pin)
self.table_pin.Impose_Abs_Coord(
chrono.ChCoordsysD(chrono.ChVectorD(0, 0, 0)))
self.pin_joint = chrono.ChLinkLockRevolute()
self.pin_joint.Initialize(self.rod_pin, self.table_pin)
self.rev_pend_sys.Add(self.pin_joint)
if self.render:
# ---------------------------------------------------------------------
#
# Create an Irrlicht application to visualize the system
#
# ==IMPORTANT!== Use this function for adding a ChIrrNodeAsset to all items
# in the system. These ChIrrNodeAsset assets are 'proxies' to the Irrlicht meshes.
# If you need a finer control on which item really needs a visualization proxy
# Irrlicht, just use application.AssetBind(myitem); on a per-item basis.
self.myapplication.AssetBindAll()
# ==IMPORTANT!== Use this function for 'converting' into Irrlicht meshes the assets
# that you added to the bodies into 3D shapes, they can be visualized by Irrlicht!
self.myapplication.AssetUpdateAll()
self.isdone = False
self.steps = 0
self.step(np.array([[0]]))
return self.get_ob()
def reset(self):
#print("reset")
self.isdone = False
self.rev_pend_sys.Clear()
# create it
self.body_rod = chrono.ChBody()
# set initial position
self.body_rod.SetPos(chrono.ChVectorD(0, self.size_rod_y / 2, 0))
# set mass properties
self.body_rod.SetMass(self.mass_rod)
self.body_rod.SetInertiaXX(
chrono.ChVectorD(self.inertia_rod_x, self.inertia_rod_y,
self.inertia_rod_x))
# set collision surface properties
self.body_rod.SetMaterialSurface(self.rod_material)
self.cyl_base1 = chrono.ChVectorD(0, -self.size_rod_y / 2, 0)
self.cyl_base2 = chrono.ChVectorD(0, self.size_rod_y / 2, 0)
self.body_rod_shape = chrono.ChCylinderShape()
self.body_rod_shape.GetCylinderGeometry().p1 = self.cyl_base1
self.body_rod_shape.GetCylinderGeometry().p2 = self.cyl_base2
self.body_rod_shape.GetCylinderGeometry().rad = self.radius_rod
self.body_rod.AddAsset(self.body_rod_shape)
self.rev_pend_sys.Add(self.body_rod)
self.body_floor = chrono.ChBody()
self.body_floor.SetBodyFixed(True)
self.body_floor.SetPos(chrono.ChVectorD(0, -5, 0))
self.body_floor.SetMaterialSurface(self.rod_material)
self.body_floor_shape = chrono.ChBoxShape()
self.body_floor_shape.GetBoxGeometry().Size = chrono.ChVectorD(3, 1, 3)
self.body_floor.GetAssets().push_back(self.body_floor_shape)
self.body_floor_texture = chrono.ChTexture()
self.body_floor_texture.SetTextureFilename(chrono.GetChronoDataPath() +
'/concrete.jpg')
self.body_floor.GetAssets().push_back(self.body_floor_texture)
self.rev_pend_sys.Add(self.body_floor)
self.body_table = chrono.ChBody()
self.body_table.SetPos(chrono.ChVectorD(0, -self.size_table_y / 2, 0))
self.body_table.SetMaterialSurface(self.rod_material)
self.body_table.SetMass(0.1)
self.body_table_shape = chrono.ChBoxShape()
self.body_table_shape.GetBoxGeometry().Size = chrono.ChVectorD(
self.size_table_x / 2, self.size_table_y / 2,
self.size_table_z / 2)
self.body_table_shape.SetColor(chrono.ChColor(0.4, 0.4, 0.5))
self.body_table.GetAssets().push_back(self.body_table_shape)
self.body_table_texture = chrono.ChTexture()
self.body_table_texture.SetTextureFilename(chrono.GetChronoDataPath() +
'/concrete.jpg')
self.body_table.GetAssets().push_back(self.body_table_texture)
self.rev_pend_sys.Add(self.body_table)
self.link_slider = chrono.ChLinkLockPrismatic()
z2x = chrono.ChQuaternionD()
z2x.Q_from_AngAxis(-chrono.CH_C_PI / 2, chrono.ChVectorD(0, 1, 0))
self.link_slider.Initialize(
self.body_table, self.body_floor,
chrono.ChCoordsysD(chrono.ChVectorD(0, 0, 0), z2x))
self.rev_pend_sys.Add(self.link_slider)
self.act_initpos = chrono.ChVectorD(0, 0, 0)
self.actuator = chrono.ChLinkMotorLinearForce()
self.actuator.Initialize(self.body_table, self.body_floor,
chrono.ChFrameD(self.act_initpos))
self.rev_pend_sys.Add(self.actuator)
self.rod_pin = chrono.ChMarker()
self.body_rod.AddMarker(self.rod_pin)
self.rod_pin.Impose_Abs_Coord(
chrono.ChCoordsysD(chrono.ChVectorD(0, 0, 0)))
self.table_pin = chrono.ChMarker()
self.body_table.AddMarker(self.table_pin)
self.table_pin.Impose_Abs_Coord(
chrono.ChCoordsysD(chrono.ChVectorD(0, 0, 0)))
self.pin_joint = chrono.ChLinkLockRevolute()
self.pin_joint.Initialize(self.rod_pin, self.table_pin)
self.rev_pend_sys.Add(self.pin_joint)
if self.render_setup:
self.myapplication.AssetBindAll()
self.myapplication.AssetUpdateAll()
self.isdone = False
self.steps = 0
self.step(np.array([[0]]))
return self.get_ob()
bodyC.SetRot(chrono.ChQuaternionD(1, 0, 0, 0))
bodyC.SetMass(346.17080777653)
bodyC.SetInertiaXX(
chrono.ChVectorD(48583.2418823358, 526927.118351673, 490689.966726565))
bodyC.SetInertiaXY(
chrono.ChVectorD(1.70380722975012e-11, 1.40840344485366e-11,
-2.31869065456271e-12))
bodyC.SetFrame_COG_to_REF(
chrono.ChFrameD(
chrono.ChVectorD(68.9923703887577, -60.1266363930238,
70.1327223302498), chrono.ChQuaternionD(1, 0, 0, 0)))
myasset = chrono.ChObjShapeFile()
myasset.SetFilename("shapes/test.obj")
bodyC.GetAssets().push_back(myasset)
# Add a revolute joint
rev = chrono.ChLinkLockRevolute()
rev.SetName('Revolute')
rev.Initialize(
bodyA, bodyC,
chrono.ChCoordsysD(chrono.ChVectorD(1, 2, 3),
chrono.ChQuaternionD(1, 0, 0, 0)))
my_system.AddLink(rev)
# Iterate over added links (Python style)
print('Names of all links in the system:')
for alink in my_system.Get_linklist():
print(' link: ', alink.GetName())
print('Done...')
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 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(chrono.ChContactMethod_SMC)
my_hmmwv.SetInitPosition(
chrono.ChCoordsysD(chrono.ChVectorD(-5, -2, 0.6),
chrono.ChQuaternionD(1, 0, 0, 0)))
my_hmmwv.SetPowertrainType(veh.PowertrainModelType_SHAFTS)
my_hmmwv.SetDriveType(veh.DrivelineType_AWD)
my_hmmwv.SetTireType(veh.TireModelType_RIGID)
my_hmmwv.Initialize()
my_hmmwv.SetChassisVisualizationType(veh.VisualizationType_NONE)
my_hmmwv.SetSuspensionVisualizationType(veh.VisualizationType_PRIMITIVES)
my_hmmwv.SetSteeringVisualizationType(veh.VisualizationType_PRIMITIVES)
my_hmmwv.SetWheelVisualizationType(veh.VisualizationType_NONE)
my_hmmwv.SetTireVisualizationType(veh.VisualizationType_MESH)
# Create the (custom) driver
driver = MyDriver(my_hmmwv.GetVehicle(), 0.5)
driver.Initialize()
# Create the SCM deformable terrain patch
terrain = veh.SCMDeformableTerrain(my_hmmwv.GetSystem())
terrain.SetSoilParameters(
2e6, # Bekker Kphi
0, # Bekker Kc
1.1, # Bekker n exponent
0, # Mohr cohesive limit (Pa)
30, # Mohr friction limit (degrees)
0.01, # Janosi shear coefficient (m)
2e8, # Elastic stiffness (Pa/m), before plastic yield
3e4 # Damping (Pa s/m), proportional to negative vertical speed (optional)
)
# Optionally, enable moving patch feature (single patch around vehicle chassis)
terrain.AddMovingPatch(my_hmmwv.GetChassisBody(),
chrono.ChVectorD(0, 0,
0), chrono.ChVectorD(5, 3, 1))
# Set plot type for SCM (false color plotting)
terrain.SetPlotType(veh.SCMDeformableTerrain.PLOT_SINKAGE, 0, 0.1)
# Initialize the SCM terrain, specifying the initial mesh grid
terrain.Initialize(terrainLength, terrainWidth, delta)
# Create the vehicle Irrlicht interface
app = veh.ChWheeledVehicleIrrApp(my_hmmwv.GetVehicle(),
'HMMWV Deformable Soil 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, 1.75), 6.0, 0.5)
app.SetTimestep(step_size)
app.AssetBindAll()
app.AssetUpdateAll()
# Simulation loop
while (app.GetDevice().run()):
time = my_hmmwv.GetSystem().GetChTime()
# End simulation
if (time >= 4):
break
# 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)
return 0
def reset(self):
self.generate_track()
self.vehicle = veh.Sedan()
self.vehicle.SetContactMethod(chrono.ChMaterialSurface.NSC)
self.vehicle.SetChassisCollisionType(veh.ChassisCollisionType_NONE)
self.vehicle.SetChassisFixed(False)
self.vehicle.SetInitPosition(
chrono.ChCoordsysD(self.initLoc, self.initRot))
self.vehicle.SetTireType(veh.TireModelType_RIGID)
self.vehicle.SetTireStepSize(self.timestep)
self.vehicle.Initialize()
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.vehicle.SetTireVisualizationType(veh.VisualizationType_PRIMITIVES)
self.system = self.vehicle.GetSystem()
self.chassis_body = self.vehicle.GetChassisBody()
# Create contact model
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()
# Driver
self.driver = Driver(self.vehicle.GetVehicle())
# Throttle controller
#self.throttle_controller = PIDThrottleController()
#self.throttle_controller.SetGains(Kp=0.4, Ki=0, Kd=0)
#self.throttle_controller.SetTargetSpeed(speed=self.target_speed)
# Rigid terrain
self.terrain = veh.RigidTerrain(self.system)
patch = self.terrain.AddPatch(
chrono.ChCoordsysD(chrono.ChVectorD(0, 0, self.terrainHeight - 5),
chrono.QUNIT),
chrono.ChVectorD(self.terrainLength, self.terrainWidth, 10))
patch.SetContactFrictionCoefficient(0.9)
patch.SetContactRestitutionCoefficient(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 barriers
self.barriers = []
self.DrawBarriers(self.track.left.points)
self.DrawBarriers(self.track.right.points)
# 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.5
# time to go from 0 to +1 (or from 0 to -1)
throttle_time = 0.5
# time to go from 0 to +1
braking_time = 0.3
# time to go from 0 to +1
self.driver.SetSteeringDelta(self.timestep / steering_time)
self.driver.SetThrottleDelta(self.timestep / throttle_time)
self.driver.SetBrakingDelta(self.timestep / braking_time)
self.manager = sens.ChSensorManager(self.system)
self.manager.scene.AddPointLight(chrono.ChVectorF(100, 100, 100),
chrono.ChVectorF(1, 1, 1), 500.0)
self.manager.scene.AddPointLight(chrono.ChVectorF(-100, -100, 100),
chrono.ChVectorF(1, 1, 1), 500.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),
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
(self.camera_height / self.camera_width) * chrono.CH_C_PI /
3. # vertical field of view
)
self.camera.SetName("Camera Sensor")
self.camera.FilterList().append(sens.ChFilterRGBA8Access())
self.manager.AddSensor(self.camera)
self.old_dist = self.track.center.calcDistance(
self.chassis_body.GetPos())
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()
##robot.SetVisualizationTypeChassis(robosimian::VisualizationType::MESH)
##robot.SetVisualizationTypeLimb(robosimian::FL, robosimian::VisualizationType::COLLISION)
##robot.SetVisualizationTypeLimb(robosimian::FR, robosimian::VisualizationType::COLLISION)
##robot.SetVisualizationTypeLimb(robosimian::RL, robosimian::VisualizationType::COLLISION)
##robot.SetVisualizationTypeLimb(robosimian::RR, robosimian::VisualizationType::COLLISION)
##robot.SetVisualizationTypeLimbs(robosimian::VisualizationType::NONE)
##robot.SetVisualizationTypeChassis(robosimian::VisualizationType::MESH)
##robot.SetVisualizationTypeSled(robosimian::VisualizationType::MESH)
##robot.SetVisualizationTypeLimbs(robosimian::VisualizationType::MESH)
# Initialize Robosimian robot
##robot.Initialize(ChCoordsys<>(chrono.ChVectorD(0, 0, 0), QUNIT))
robot.Initialize(
chrono.ChCoordsysD(chrono.ChVectorD(0, 0, 0),
chrono.Q_from_AngX(chrono.CH_C_PI)))
# -----------------------------------
# Create a driver and attach to robot
# -----------------------------------
if mode == robosimian.LocomotionMode_WALK:
driver = robosimian.RS_Driver(
"", # start input file
chrono.GetChronoDataFile("robot/robosimian/actuation/walking_cycle.txt"
), # cycle input file
"", # stop input file
True)
elif mode == robosimian.LocomotionMode_SCULL:
driver = robosimian.RS_Driver(
chrono.GetChronoDataFile(
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
body_particles.GetCollisionModel().ClearModel()
body_particles.GetCollisionModel().AddSphere(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)
body_particles_shape = chrono.ChSphereShape()
body_particles_shape.GetSphereGeometry().rad = 0.005
body_particles.GetAssets().push_back(body_particles_shape)
my_system.Add(body_particles)
# Create the floor: a simple fixed rigid body with a collision shape
# and a visualization shape
body_floor = chrono.ChBody()
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()
def reset(self):
print("reset")
self.generator.generatePath(difficulty=50, seed=randint(1, 1000))
self.path = Path(self.generator)
self.path_tracker = PathTracker(self.path)
self.initLoc = self.path_tracker.GetInitLoc()
self.initRot = self.path_tracker.GetInitRot()
self.vehicle = veh.WheeledVehicle(self.vehicle_file,
chrono.ChMaterialSurface.NSC)
self.vehicle.Initialize(chrono.ChCoordsysD(self.initLoc, self.initRot))
self.vehicle.SetStepsize(self.timestep)
self.vehicle.SetChassisVisualizationType(
veh.VisualizationType_PRIMITIVES)
self.vehicle.SetSuspensionVisualizationType(
veh.VisualizationType_PRIMITIVES)
self.vehicle.SetSteeringVisualizationType(
veh.VisualizationType_PRIMITIVES)
self.vehicle.SetWheelVisualizationType(veh.VisualizationType_NONE)
# Create and initialize the powertrain system
self.powertrain = veh.SimplePowertrain(self.simplepowertrain_file)
self.vehicle.InitializePowertrain(self.powertrain)
# Create the ground
self.terrain = veh.RigidTerrain(self.vehicle.GetSystem(),
self.rigidterrain_file)
for axle in self.vehicle.GetAxles():
tireL = veh.RigidTire(self.rigidtire_file)
self.vehicle.InitializeTire(tireL, axle.m_wheels[0],
veh.VisualizationType_MESH)
tireR = veh.RigidTire(self.rigidtire_file)
self.vehicle.InitializeTire(tireR, axle.m_wheels[1],
veh.VisualizationType_MESH)
# -------------
# Create driver
# -------------
self.driver = Driver(self.vehicle)
# Time interval between two render frames
render_step_size = 1.0 / 60 # FPS = 60
# 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 = 1.0
# time to go from 0 to +1
braking_time = 0.3
# time to go from 0 to +1
self.driver.SetSteeringDelta(render_step_size / steering_time)
self.driver.SetThrottleDelta(render_step_size / throttle_time)
self.driver.SetBrakingDelta(render_step_size / braking_time)
vec = chrono.ChVectorD(0, 0, 0)
self.path_tracker.calcClosestPoint(self.vehicle.GetVehiclePos(), vec)
self.last_dist = vec.Length()
self.last_throttle, self.last_braking, self.last_steering = 0, 0, 0
if self.render:
road = self.vehicle.GetSystem().NewBody()
road.SetBodyFixed(True)
self.vehicle.GetSystem().AddBody(road)
num_points = self.path.getNumPoints()
path_asset = chrono.ChLineShape()
path_asset.SetLineGeometry(
chrono.ChLineBezier(self.path_tracker.path))
path_asset.SetColor(chrono.ChColor(0.0, 0.8, 0.0))
path_asset.SetNumRenderPoints(max(2 * num_points, 400))
road.AddAsset(path_asset)
if self.render:
self.app = veh.ChVehicleIrrApp(self.vehicle)
self.app.SetHUDLocation(500, 20)
self.app.SetSkyBox()
self.app.AddTypicalLogo()
self.app.AddTypicalLights(chronoirr.vector3df(-150., -150., 200.),
chronoirr.vector3df(-150., 150., 200.),
100, 100)
self.app.AddTypicalLights(chronoirr.vector3df(150., -150., 200.),
chronoirr.vector3df(150., 150., 200.),
100, 100)
self.app.EnableGrid(False)
self.app.SetChaseCamera(self.trackPoint, 6.0, 0.5)
self.app.SetTimestep(self.timestep)
# ---------------------------------------------------------------------
#
# Create an Irrlicht application to visualize the system
#
# ==IMPORTANT!== Use this function for adding a ChIrrNodeAsset to all items
# in the system. These ChIrrNodeAsset assets are 'proxies' to the Irrlicht meshes.
# If you need a finer control on which item really needs a visualization proxy
# Irrlicht, just use application.AssetBind(myitem); on a per-item basis.
self.app.AssetBindAll()
# ==IMPORTANT!== Use this function for 'converting' into Irrlicht meshes the assets
# that you added to the bodies into 3D shapes, they can be visualized by Irrlicht!
self.app.AssetUpdateAll()
self.isdone = False
self.steps = 0
self.step(np.zeros(3))
self.tracknum = self.tracknum = 0 if self.tracknum >= 3 else self.tracknum + 1
# self.tracknum = self.tracknum + 1
return self.get_ob()
cyl2 = chrono.ChCylinderShape()
cyl2.GetCylinderGeometry().p1 = chrono.ChVectorD(-0.2, 0, 0)
cyl2.GetCylinderGeometry().p2 = chrono.ChVectorD(0.2, 0, 0)
cyl2.GetCylinderGeometry().rad = 0.2
slider2.AddAsset(cyl2)
col2 = chrono.ChColorAsset()
col2.SetColor(chrono.ChColor(0, 0, 0.6))
slider2.AddAsset(col2)
# Create prismatic joints between ground a sliders
prismatic1 = chrono.ChLinkLockPrismatic()
prismatic1.Initialize(
slider1, ground,
chrono.ChCoordsysD(chrono.ChVectorD(0, 0, -1),
chrono.Q_from_AngY(chrono.CH_C_PI_2)))
system.AddLink(prismatic1)
prismatic2 = chrono.ChLinkLockPrismatic()
prismatic2.Initialize(
slider2, ground,
chrono.ChCoordsysD(chrono.ChVectorD(0, 0, 1),
chrono.Q_from_AngY(chrono.CH_C_PI_2)))
system.AddLink(prismatic2)
# Sine function parameters
freq = 1
ampl = 4
omg = 2 * chrono.CH_C_PI * freq
mod = chrono.ChFunction_Sine(0, freq, ampl)
def reset(self):
self.isdone = False
self.hexapod_sys.Clear()
self.exported_items = chrono.ImportSolidWorksSystem(self.fpath)
self.csys = []
self.frames = []
self.revs = []
self.motors = []
self.limits = []
for con, coi in zip(self.con_link, self.coi_link):
indices = []
for i in range(len(self.exported_items)):
if con == self.exported_items[i].GetName(
) or coi == self.exported_items[i].GetName():
indices.append(i)
rev = self.exported_items[indices[0]]
af0 = rev.GetAssetsFrame()
# Revolute joints and ChLinkMotorRotation are z oriented, while parallel is x oriented.
# Event though this Frame won't be used anymore is good practice to create a copy before editing its value.
af = chrono.ChFrameD(af0)
af.SetRot(af0.GetRot() % chrono.Q_ROTATE_X_TO_Z)
self.frames.append(af)
for i in reversed(indices):
del self.exported_items[i]
# ADD IMPORTED ITEMS TO THE SYSTEM
for my_item in self.exported_items:
self.hexapod_sys.Add(my_item)
"""
$$$$$$$$ FIND THE SW DEFINED CONSTRAINTS, GET THEIR MARKERS AND GET RID OF EM $$$$$$$$
"""
self.hips = [
self.hexapod_sys.SearchBody(name) for name in self.hip_names
]
self.femurs = [
self.hexapod_sys.SearchBody(name) for name in self.femur_names
]
self.tibias = [
self.hexapod_sys.SearchBody(name) for name in self.tibia_names
]
self.feet = [
self.hexapod_sys.SearchBody(name) for name in self.feet_names
]
self.centralbody = self.hexapod_sys.SearchBody('Body-1')
# Bodies are used to replace constraints and detect unwanted collision, so feet are excluded
self.bodies = [self.centralbody
] + self.hips + self.femurs + self.tibias
self.centralbody.SetBodyFixed(False)
self.y0 = self.centralbody.GetPos().y
"""
# SNIPPET FOR COLOR
orange = chrono.ChColorAsset()
orange.SetColor(chrono.ChColor(255/255,77/255,6/255))
black = chrono.ChColorAsset()
black.SetColor(chrono.ChColor(0,0,0))
for body in self.bodies[:-1]:
assets = body.GetAssets()
for ast in assets:
ass_lev = chrono.CastToChAssetLevel(ast)
ass_lev.GetAssets().push_back(orange)
assets = self.hand.GetAssets()
for ast in assets:
ass_lev = chrono.CastToChAssetLevel(ast)
ass_lev.GetAssets().push_back(black)
"""
for i in range(len(self.con_link)):
revolute = chrono.ChLinkLockRevolute()
cs = chrono.ChCoordsysD(self.frames[i].GetPos(),
self.frames[i].GetRot())
self.csys.append(cs)
if i < 6:
j = 0
else:
j = i - 5
revolute.Initialize(self.bodies[j], self.bodies[i + 1],
self.csys[i])
self.revs.append(revolute)
self.hexapod_sys.Add(self.revs[i])
lim = self.revs[i].GetLimit_Rz()
self.limits.append(lim)
self.limits[i].SetActive(True)
self.limits[i].SetMin(self.minRot[i] * (math.pi / 180))
self.limits[i].SetMax(self.maxRot[i] * (math.pi / 180))
m = chrono.ChLinkMotorRotationTorque()
m.SetSpindleConstraint(False, False, False, False, False)
m.Initialize(self.bodies[j], self.bodies[i + 1], self.frames[i])
self.motors.append(m)
self.hexapod_sys.Add(self.motors[i])
self.body_floor = chrono.ChBody()
self.body_floor.SetBodyFixed(True)
self.body_floor.SetPos(chrono.ChVectorD(0, -1 - 0.128 - 0.0045, 10))
# Floor Collision.
self.body_floor.GetCollisionModel().ClearModel()
self.body_floor.GetCollisionModel().AddBox(self.my_material, 50, 1, 50,
chrono.ChVectorD(0, 0, 0))
self.body_floor.GetCollisionModel().BuildModel()
self.body_floor.SetCollide(True)
# Visualization shape
body_floor_shape = chrono.ChBoxShape()
body_floor_shape.GetBoxGeometry().Size = chrono.ChVectorD(4, 1, 15)
body_floor_shape.SetColor(chrono.ChColor(0.4, 0.4, 0.5))
self.body_floor.GetAssets().push_back(body_floor_shape)
body_floor_texture = chrono.ChTexture()
texpath = os.path.join(chrono.GetChronoDataPath(), 'concrete.jpg')
body_floor_texture.SetTextureFilename(texpath)
self.body_floor.GetAssets().push_back(body_floor_texture)
self.hexapod_sys.Add(self.body_floor)
self.numsteps = 0
if (self.render_setup):
self.myapplication.AssetBindAll()
self.myapplication.AssetUpdateAll()
return self.get_ob()
def main():
#print("Copyright (c) 2017 projectchrono.org\nChrono version: ", CHRONO_VERSION , "\n\n")
# --------------------------
# Create the various modules
# --------------------------
# Create the vehicle system
vehicle = veh.WheeledVehicle(vehicle_file, chrono.ChContactMethod_NSC)
vehicle.Initialize(chrono.ChCoordsysD(initLoc, initRot))
#vehicle.GetChassis().SetFixed(True)
vehicle.SetChassisVisualizationType(veh.VisualizationType_MESH)
vehicle.SetSuspensionVisualizationType(veh.VisualizationType_PRIMITIVES)
vehicle.SetSteeringVisualizationType(veh.VisualizationType_PRIMITIVES)
vehicle.SetWheelVisualizationType(veh.VisualizationType_MESH)
# Create and initialize the vehicle tires
for axle in vehicle.GetAxles():
tireL = veh.TMeasyTire(vehicle_tire_file)
vehicle.InitializeTire(tireL, axle.m_wheels[0],
veh.VisualizationType_MESH)
tireR = veh.TMeasyTire(vehicle_tire_file)
vehicle.InitializeTire(tireR, axle.m_wheels[1],
veh.VisualizationType_MESH)
# Create and initialize the powertrain system
powertrain = veh.SimpleMapPowertrain(vehicle_powertrain_file)
vehicle.InitializePowertrain(powertrain)
# Create and initialize the trailer
trailer = veh.WheeledTrailer(vehicle.GetSystem(), trailer_file)
trailer.Initialize(vehicle.GetChassis())
trailer.SetChassisVisualizationType(veh.VisualizationType_PRIMITIVES)
trailer.SetSuspensionVisualizationType(veh.VisualizationType_PRIMITIVES)
trailer.SetWheelVisualizationType(veh.VisualizationType_NONE)
# Create abd initialize the trailer tires
for axle in trailer.GetAxles():
tireL = veh.TMeasyTire(trailer_tire_file)
trailer.InitializeTire(tireL, axle.m_wheels[0],
veh.VisualizationType_PRIMITIVES)
tireR = veh.TMeasyTire(trailer_tire_file)
trailer.InitializeTire(tireR, axle.m_wheels[1],
veh.VisualizationType_PRIMITIVES)
# Create the ground
terrain = veh.RigidTerrain(vehicle.GetSystem(), rigidterrain_file)
app = veh.ChVehicleIrrApp(vehicle, 'Sedan+Trailer (JSON specification)',
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()
driver = veh.ChIrrGuiDriver(app)
# Set the time response for steering and throttle keyboard 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 = 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
# ---------------
realtime_timer = chrono.ChRealtimeStepTimer()
while (app.GetDevice().run()):
# Render scene
app.BeginScene(True, True, irr.SColor(255, 140, 161, 192))
app.DrawAll()
app.EndScene()
# Collect output data from modules (for inter-module communication)
driver_inputs = driver.GetInputs()
# Update modules (process inputs from other modules)
time = vehicle.GetSystem().GetChTime()
driver.Synchronize(time)
vehicle.Synchronize(time, driver_inputs, terrain)
trailer.Synchronize(time, driver_inputs.m_braking, terrain)
terrain.Synchronize(time)
app.Synchronize(driver.GetInputModeAsString(), driver_inputs)
# Advance simulation for one timestep for all modules
driver.Advance(step_size)
vehicle.Advance(step_size)
trailer.Advance(step_size)
terrain.Advance(step_size)
app.Advance(step_size)
# Spin in place for real time to catch up
realtime_timer.Spin(step_size)
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"))
# ...the rotating bar support for the two epicycloidal wheels
mbody_train = chrono.ChBodyEasyBox(8, 1.5, 1.0, 1000, True, False, mat)
mphysicalSystem.Add(mbody_train)
mbody_train.SetPos(chrono.ChVectorD(3, 0, 0))
# ...which must rotate respect to truss along Z axis, in 0,0,0
link_revoluteTT = chrono.ChLinkLockRevolute()
link_revoluteTT.Initialize(
mbody_truss, mbody_train,
chrono.ChCoordsysD(chrono.ChVectorD(0, 0, 0), chrono.QUNIT))
mphysicalSystem.AddLink(link_revoluteTT)
# ...the first gear
mbody_gearA = chrono.ChBodyEasyCylinder(radA, 0.5, 1000, True, False, mat)
mphysicalSystem.Add(mbody_gearA)
mbody_gearA.SetPos(chrono.ChVectorD(0, 0, -1))
mbody_gearA.SetRot(chrono.Q_from_AngX(m.pi / 2))
mbody_gearA.AddAsset(cylinder_texture)
# for aesthetic reasons, also add a thin cylinder only as a visualization
mshaft_shape = chrono.ChCylinderShape()
mshaft_shape.GetCylinderGeometry().p1 = chrono.ChVectorD(0, -3, 0)
mshaft_shape.GetCylinderGeometry().p2 = chrono.ChVectorD(0, 10, 0)
mshaft_shape.GetCylinderGeometry().rad = radA * 0.4
mbody_gearA.AddAsset(mshaft_shape)
20, # number of sections (spans)
chrono.ChVectorD(0, 0, 0), # start point
chrono.ChVectorD(beam_L, 0, 0), # end point
chrono.VECT_Y, # suggested Y direction of section
1) # order (3 = cubic, etc)
node_mid = builder.GetLastBeamNodes()[m.floor(
builder.GetLastBeamNodes().size() / 2.0)]
# Create the flywheel and attach it to the center of the beam
mbodyflywheel = chrono.ChBodyEasyCylinder(0.24, 0.05, 7800) # R, h, density
mbodyflywheel.SetCoord(
chrono.ChCoordsysD(
node_mid.GetPos() + chrono.ChVectorD(
0, 0.05, 0), # flywheel initial center (plus Y offset)
chrono.Q_from_AngAxis(CH_C_PI / 2.0, chrono.VECT_Z)
) # flywheel initial alignment (rotate 90° so cylinder axis is on X)
)
my_system.Add(mbodyflywheel)
myjoint = chrono.ChLinkMateFix()
myjoint.Initialize(node_mid, mbodyflywheel)
my_system.Add(myjoint)
# Create the truss
truss = chrono.ChBody()
truss.SetBodyFixed(True)
my_system.Add(truss)
# Create the end bearing
def main():
# print("Copyright (c) 2017 projectchrono.org\nChrono version: ", CHRONO_VERSION , "\n\n")
# ---------
# Load path
# ---------
path_file = "../data/paths/loop2.txt"
path = np.genfromtxt(path_file, delimiter=",")
ds = 5 # [m] distance of each intepolated points
sp = Spline2D(path[:, 0], path[:, 1])
s = np.arange(0, sp.s[-1], ds)
px, py = [], []
for i_s in s:
ix, iy = sp.calc_position(i_s)
px.append(ix)
py.append(iy)
px.append(px[0])
py.append(py[0])
initLoc = chrono.ChVectorD(px[0], py[0], 0.5)
# --------------------------
# Create the various modules
# --------------------------
# Create the vehicle system
vehicle = veh.WheeledVehicle(vehicle_file, chrono.ChMaterialSurface.NSC)
vehicle.Initialize(chrono.ChCoordsysD(initLoc, initRot))
# vehicle.GetChassis().SetFixed(True)
vehicle.SetStepsize(step_size)
vehicle.SetChassisVisualizationType(veh.VisualizationType_PRIMITIVES)
vehicle.SetSuspensionVisualizationType(veh.VisualizationType_PRIMITIVES)
vehicle.SetSteeringVisualizationType(veh.VisualizationType_PRIMITIVES)
vehicle.SetWheelVisualizationType(veh.VisualizationType_NONE)
# Create the ground
terrain = veh.RigidTerrain(vehicle.GetSystem(), rigidterrain_file)
# Create and initialize the powertrain system
powertrain = veh.SimplePowertrain(simplepowertrain_file)
vehicle.InitializePowertrain(powertrain)
# Create and initialize the tires
for axle in vehicle.GetAxles():
tireL = veh.RigidTire(rigidtire_file)
vehicle.InitializeTire(tireL, axle.m_wheels[0],
veh.VisualizationType_MESH)
tireR = veh.RigidTire(rigidtire_file)
vehicle.InitializeTire(tireR, axle.m_wheels[1],
veh.VisualizationType_MESH)
# -------------
# Create driver
# -------------
driver = Driver(vehicle)
# 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 = 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)
# ------------------
# Draw path
# ------------------
if irrlicht:
road = vehicle.GetSystem().NewBody()
road.SetBodyFixed(True)
vehicle.GetSystem().AddBody(road)
num_points = len(px)
lines = chrono.ChLinePath()
for i in range(num_points - 1):
lines.AddSubLine(
chrono.ChLineSegment(
chrono.ChVectorD(px[i], py[i], 0.1),
chrono.ChVectorD(px[i + 1], py[i + 1], 0.1)))
path_asset = chrono.ChLineShape()
path_asset.SetLineGeometry(lines)
path_asset.SetColor(chrono.ChColor(0.0, 0.8, 0.0))
path_asset.SetNumRenderPoints(max(2 * num_points, 400))
road.AddAsset(path_asset)
# --------------------
# Create controller(s)
# --------------------
controller = MPCController(vehicle, driver, path)
# -----------------------
# Initialize irrlicht app
# -----------------------
if irrlicht:
app = veh.ChVehicleIrrApp(vehicle)
app.SetHUDLocation(500, 20)
app.SetSkyBox()
app.AddTypicalLogo()
app.AddTypicalLights(
chronoirr.vector3df(-150.0, -150.0, 200.0),
chronoirr.vector3df(-150.0, 150.0, 200.0),
100,
100,
)
app.AddTypicalLights(
chronoirr.vector3df(150.0, -150.0, 200.0),
chronoirr.vector3df(150.0, 150.0, 200.0),
100,
100,
)
app.EnableGrid(False)
app.SetChaseCamera(trackPoint, 6.0, 0.5)
app.SetTimestep(step_size)
app.AssetBindAll()
app.AssetUpdateAll()
# -----------------
# Initialize output
# -----------------
if output:
try:
os.mkdir(out_dir)
except:
print("Error creating directory ")
# Generate JSON information with available output channels
out_json = vehicle.ExportComponentList()
print(out_json)
vehicle.ExportComponentList(out_dir + "/component_list.json")
# ---------------
# Simulation loop
# ---------------
# Number of simulation steps between miscellaneous events
render_steps = int(math.ceil(render_step_size / step_size))
# Initialize simulation frame counter and simulation time
step_number = 0
time = 0
if irrlicht:
while app.GetDevice().run():
# Render scene
if step_number % render_steps == 0:
app.BeginScene(True, True,
chronoirr.SColor(255, 140, 161, 192))
app.DrawAll()
app.EndScene()
# Collect output data from modules (for inter-module communication)
driver_inputs = driver.GetInputs()
# Update modules (process inputs from other modules)
time = vehicle.GetSystem().GetChTime()
driver.Synchronize(time)
vehicle.Synchronize(time, driver_inputs, terrain)
terrain.Synchronize(time)
app.Synchronize("", driver_inputs)
# Advance simulation for one timestep for all modules
step = step_size
# Update controllers
controller.Advance(step)
driver.Advance(step)
vehicle.Advance(step)
terrain.Advance(step)
app.Advance(step)
# Increment frame number
step_number += 1
else:
while True:
# Collect output data from modules (for inter-module communication)
driver_inputs = driver.GetInputs()
# Update modules (process inputs from other modules)
time = vehicle.GetSystem().GetChTime()
driver.Synchronize(time)
vehicle.Synchronize(time, driver_inputs, terrain)
terrain.Synchronize(time)
# Advance simulation for one timestep for all modules
step = step_size
# Update controllers
steering_controller.Advance(step)
throttle_controller.Advance(step)
driver.Advance(step)
vehicle.Advance(step)
terrain.Advance(step)
vis.Advance(step)
# Increment frame number
step_number += 1
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()
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 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())
patch = terrain.AddPatch(chrono.ChCoordsysD(chrono.ChVectorD(0, 0, terrainHeight - 5), chrono.QUNIT), chrono.ChVectorD(terrainLength, terrainWidth, 10))
patch.SetContactFrictionCoefficient(0.9)
patch.SetContactRestitutionCoefficient(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))
terrain.Initialize()
# Create the vehicle Irrlicht interface
# please note that wchar_t conversion requres some workaround
app = veh.ChWheeledVehicleIrrApp(my_hmmwv.GetVehicle())
app.SetSkyBox()
app.AddTypicalLights(chronoirr.vector3df(30, -30, 100), chronoirr.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(chronoirr.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, chronoirr.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
def main():
# 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.SetVehicleStepSize(step_size)
my_hmmwv.Initialize()
#VisualizationType tire_vis_type =
# (tire_model == TireModelType::RIGID_MESH) ? VisualizationType::MESH : VisualizationType::NONE;
tire_vis_type = veh.VisualizationType_MESH # if tire_model == veh.TireModelType_RIGID_MESH else tire_vis_type = veh.VisualizationType_NONE
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())
#patch = veh.Patch()
"""
switch (terrain_model) {
case RigidTerrain::BOX:
patch = terrain.AddPatch(ChCoordsys<>(ChVector<>(0, 0, terrainHeight - 5), QUNIT),
ChVector<>(terrainLength, terrainWidth, 10));
patch->SetTexture(vehicle::GetDataFile("terrain/textures/tile4.jpg"), 200, 200);
break;
case RigidTerrain::HEIGHT_MAP:
patch = terrain.AddPatch(CSYSNORM, vehicle::GetDataFile("terrain/height_maps/test64.bmp"), "test64", 128,
128, 0, 4);
patch->SetTexture(vehicle::GetDataFile("terrain/textures/grass.jpg"), 16, 16);
break;
case RigidTerrain::MESH:
patch = terrain.AddPatch(CSYSNORM, vehicle::GetDataFile("terrain/meshes/test.obj"), "test_mesh");
patch->SetTexture(vehicle::GetDataFile("terrain/textures/grass.jpg"), 100, 100);
break;
}
"""
patch = terrain.AddPatch(
chrono.ChCoordsysD(chrono.ChVectorD(0, 0, terrainHeight - 5),
chrono.QUNIT),
chrono.ChVectorD(terrainLength, terrainWidth, 10))
patch.SetContactFrictionCoefficient(0.9)
patch.SetContactRestitutionCoefficient(0.01)
patch.SetContactMaterialProperties(2e7, 0.3)
patch.SetColor(chrono.ChColor(0.8, 0.8, 0.5))
terrain.Initialize()
# Create the vehicle Irrlicht interface
# please note that wchar_t conversion requres some workaround
app = veh.ChWheeledVehicleIrrApp(
my_hmmwv.GetVehicle(), my_hmmwv.GetPowertrain()) #, "HMMWV Demo")
app.SetSkyBox()
app.AddTypicalLights(chronoirr.vector3df(30, -30, 100),
chronoirr.vector3df(30, 50, 100), 250, 130)
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 ")
if (povray_output):
try:
os.mkdir(pov_dir)
except:
print("Error creating POV directory ")
terrain.ExportMeshPovray(out_dir)
# Initialize output file for driver inputs
#driver_file = out_dir + "/driver_inputs.txt"
# no RECORD so far
#utils::CSV_writer driver_csv(" ");
# 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 driver system
# 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)
# If in playback mode, attach the data file to the driver system and
# force it to playback the driver inputs.
if (driver_mode == "PLAYBACK"):
#driver.SetInputDataFile(driver_file)
driver.SetInputMode(veh.ChIrrGuiDriver.DATAFILE)
driver.Initialize()
# Simulation loop
"""
if (debug_output) :
GetLog() << "\n\n============ System Configuration ============\n"
my_hmmwv.LogHardpointLocations()"""
# 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
realtime_timer = chrono.ChRealtimeStepTimer()
step_number = 0
render_frame = 0
time = 0
if (contact_vis):
app.SetSymbolscale(1e-4)
#app.SetContactsDrawMode(chronoirr.eCh_ContactsDrawMode::CONTACT_FORCES);
while (app.GetDevice().run()):
time = my_hmmwv.GetSystem().GetChTime()
#End simulation
if (time >= t_end):
break
app.BeginScene(True, True, chronoirr.SColor(255, 140, 161, 192))
app.DrawAll()
app.AddTypicalLogo(chrono.GetChronoDataPath() +
'logo_pychrono_alpha.png')
# Output POV-Ray data
if (povray_output and step_number % render_steps == 0):
#char filename[100];
print('filename', "%s/data_%03d.dat", pov_dir.c_str(),
render_frame + 1)
#utils::WriteShapesPovray(my_hmmwv.GetSystem(), filename);
#render_frame++;
#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)
# Collect output data from modules (for inter-module communication)
throttle_input = driver.GetThrottle()
steering_input = driver.GetSteering()
braking_input = driver.GetBraking()
# Driver output
"""
if (driver_mode == RECORD) {
driver_csv << time << steering_input << throttle_input << braking_input << std::endl;
}"""
# Update modules (process inputs from other modules)
driver.Synchronize(time)
terrain.Synchronize(time)
my_hmmwv.Synchronize(time, steering_input, braking_input,
throttle_input, terrain)
app.Synchronize(driver.GetInputModeAsString(), steering_input,
throttle_input, braking_input)
# Advance simulation for one timestep for all modules
step = realtime_timer.SuggestSimulationStep(step_size)
driver.Advance(step)
terrain.Advance(step)
my_hmmwv.Advance(step)
app.Advance(step)
# Increment frame number
step_number += 1
app.EndScene()
"""
if (driver_mode == RECORD) {
driver_csv.write_to_file(driver_file);
}"""
return 0
