def render(self, mode='human'):
if not (self.play_mode == True):
raise Exception('Please set play_mode=True to render')
if not self.render_setup:
if False:
vis_camera = sens.ChCameraSensor(
self.chassis_body, # body camera is attached to
30, # scanning rate in Hz
chrono.ChFrameD(
chrono.ChVectorD(0, 0, 25),
chrono.Q_from_AngAxis(chrono.CH_C_PI / 2,
chrono.ChVectorD(0, 1, 0))),
# offset pose
1280, # number of horizontal samples
720, # number of vertical channels
chrono.CH_C_PI / 3, # horizontal field of view
(720 / 1280) * chrono.CH_C_PI /
3. # vertical field of view
)
vis_camera.SetName("Birds Eye Camera Sensor")
# self.camera.FilterList().append(sens.ChFilterVisualize(self.camera_width, self.camera_height, "RGB Camera"))
# vis_camera.FilterList().append(sens.ChFilterVisualize(1280, 720, "Visualization Camera"))
if False:
self.camera.FilterList().append(sens.ChFilterSave())
self.manager.AddSensor(vis_camera)
if True:
vis_camera = sens.ChCameraSensor(
self.chassis_body, # body camera is attached to
30, # scanning rate in Hz
chrono.ChFrameD(
chrono.ChVectorD(-8, 0, 3),
chrono.Q_from_AngAxis(chrono.CH_C_PI / 20,
chrono.ChVectorD(0, 1, 0))),
# offset pose
1280, # number of horizontal samples
720, # number of vertical channels
chrono.CH_C_PI / 3, # horizontal field of view
(720 / 1280) * chrono.CH_C_PI /
3. # vertical field of view
)
vis_camera.SetName("Follow Camera Sensor")
# self.camera.FilterList().append(sens.ChFilterVisualize(self.camera_width, self.camera_height, "RGB Camera"))
# vis_camera.FilterList().append(sens.ChFilterVisualize(1280, 720, "Visualization Camera"))
if True:
vis_camera.FilterList().append(sens.ChFilterSave())
# self.camera.FilterList().append(sens.ChFilterSave())
self.manager.AddSensor(vis_camera)
# -----------------------------------------------------------------
# Create a filter graph for post-processing the data from the lidar
# -----------------------------------------------------------------
# self.camera.FilterList().append(sens.ChFilterVisualize("RGB Camera"))
# vis_camera.FilterList().append(sens.ChFilterVisualize("Visualization Camera"))
self.render_setup = True
if (mode == 'rgb_array'):
return self.get_ob()
def render(self, mode='human'):
if not (self.play_mode == True):
raise Exception('Please set play_mode=True to render')
if not self.render_setup:
vis = True
save = False
birds_eye = False
third_person = True
width = 1280
height = 720
if birds_eye:
body = chrono.ChBodyAuxRef()
body.SetBodyFixed(True)
self.system.AddBody(body)
vis_camera = sens.ChCameraSensor(
body, # body camera is attached to
20, # scanning rate in Hz
chrono.ChFrameD(chrono.ChVectorD(0, 0, 200),
chrono.Q_from_AngAxis(chrono.CH_C_PI / 2, chrono.ChVectorD(0, 1, 0))),
# offset pose
width, # number of horizontal samples
height, # number of vertical channels
chrono.CH_C_PI / 3 # horizontal field of view
)
vis_camera.SetName("Birds Eye Camera Sensor")
if vis:
vis_camera.PushFilter(sens.ChFilterVisualize(width, height, "Visualization Camera"))
if save:
vis_camera.PushFilter(sens.ChFilterSave())
self.manager.AddSensor(vis_camera)
if third_person:
vis_camera = sens.ChCameraSensor(
self.chassis_body, # body camera is attached to
20, # scanning rate in Hz
chrono.ChFrameD(chrono.ChVectorD(-8, 0, 3),
chrono.Q_from_AngAxis(chrono.CH_C_PI / 20, chrono.ChVectorD(0, 1, 0))),
# offset pose
width, # number of horizontal samples
height, # number of vertical channels
chrono.CH_C_PI / 3 # horizontal field of view
)
vis_camera.SetName("Follow Camera Sensor")
if vis:
vis_camera.PushFilter(sens.ChFilterVisualize(width, height, "Visualization Camera"))
if save:
vis_camera.PushFilter(sens.ChFilterSave())
self.manager.AddSensor(vis_camera)
# -----------------------------------------------------------------
# Create a filter graph for post-processing the data from the lidar
# -----------------------------------------------------------------
# self.camera.PushFilter(sens.ChFilterVisualize("RGB Camera"))
# vis_camera.PushFilter(sens.ChFilterVisualize("Visualization Camera"))
self.render_setup = True
if (mode == 'rgb_array'):
return self.get_ob()
def Transform(self, pos, scale=1, ang=0):
self.mesh.body.SetPos(pos)
self.mesh.mesh.Transform(chrono.ChVectorD(0, 0, 0), chrono.ChMatrix33D(scale))
self.mesh.body.SetRot(chrono.Q_from_AngAxis(ang, chrono.ChVectorD(0, 0, 1)))
self.pos = pos
self.scale = scale
self.ang = ang
self.rot = chrono.Q_from_AngAxis(ang, chrono.ChVectorD(0, 0, 1))
self.mesh.Scale(scale)
def render(self, mode='human'):
if not (self.play_mode == True):
raise Exception('Please set play_mode=True to render')
if not self.render_setup:
vis_camera = sens.ChCameraSensor(
self.chassis_body, # body camera is attached to
30, # scanning rate in Hz
chrono.ChFrameD(
chrono.ChVectorD(-8, 0, 3),
chrono.Q_from_AngAxis(chrono.CH_C_PI / 20,
chrono.ChVectorD(0, 1, 0))),
# offset pose
1280, # number of horizontal samples
720, # number of vertical channels
chrono.CH_C_PI / 3, # horizontal field of view
#(720/1280) * chrono.CH_C_PI / 3. # vertical field of view
)
vis_camera.SetName("Vis Camera Sensor")
vis_camera.PushFilter(sens.ChFilterVisualize(1280, 720))
self.manager.AddSensor(vis_camera)
self.render_setup = True
if (mode == 'rgb_array'):
return self.get_ob()
"""
def AddFixedObstacles(system):
# Create contact material, of appropriate type. Use default properties
material = None
if (NSC_SMC == chrono.ChContactMethod_NSC):
matNSC = chrono.ChMaterialSurfaceNSC()
#Change NSC material properties as desired
material = matNSC
elif (NSC_SMC == chrono.ChContactMethod_SMC):
matSMC = chrono.ChMaterialSurfaceSMC()
# Change SMC material properties as desired
material = matSMC
else:
raise ("Unvalid Contact Method")
radius = 3
length = 10
obstacle = chrono.ChBodyEasyCylinder(radius, length, 2000, True, True,
material)
obstacle.SetPos(chrono.ChVectorD(-20, 0, -2.7))
obstacle.SetBodyFixed(True)
system.AddBody(obstacle)
for i in range(8):
stoneslab = chrono.ChBodyEasyBox(0.5, 2.5, 0.25, 2000, True, True,
material)
stoneslab.SetPos(chrono.ChVectorD(-1.2 * i + 22, -1.5, -0.05))
stoneslab.SetRot(
chrono.Q_from_AngAxis(15 * chrono.CH_C_DEG_TO_RAD, chrono.VECT_Y))
stoneslab.SetBodyFixed(True)
system.AddBody(stoneslab)
def GenerateFrame(self, pos, ang, scale):
# Calculate quaternion
rot = chrono.Q_from_AngAxis(ang, chrono.ChVectorD(0, 0, 1))
# Generate ChFrame which will then be scaled
frame = chrono.ChFrameD(pos, rot)
# Scale frame
mat = frame.GetA().GetMatr()
mat = [[x * scale for x in z] for z in mat]
frame.GetA().SetMatr(mat)
return frame
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()
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
bearing = chrono.ChLinkMateGeneric(False, True, True, False, True, True)
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):
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()
npmat = np.asarray(ma.GetMatr(
)) # Create a 2D npy array from the list extracted from ChMatrixDynamic
w, v = LA.eig(npmat) # get eigenvalues and eigenvectors using numpy
mb = chrono.ChMatrixDynamicD(4, 4)
prod = v * npmat # you can perform linear algebra operations with numpy and then feed results into a ChMatrixDynamicD using SetMatr
mb.SetMatr(v.tolist()) # create a ChMatrixDynamicD from the numpy eigenvectors
mr = chrono.ChMatrix33D()
mr.SetMatr([[1, 2, 3], [4, 5, 6], [7, 8, 9]])
print(mr * my_vect1)
# Test frames -
# create a frame representing a translation and a rotation
# of 20 degrees on X axis
my_frame = chrono.ChFrameD(
my_vect2,
chrono.Q_from_AngAxis(20 * chrono.CH_C_DEG_TO_RAD,
chrono.ChVectorD(1, 0, 0)))
my_vect5 = my_vect1 >> my_frame
# Print the class hierarchy of a chrono class
import inspect
inspect.getmro(chrono.ChStreamOutAsciiFile)
# Use the ChFunction classes
my_funct = chrono.ChFunction_Sine(0, 0.5, 3)
print('function f(0.2)=', my_funct.Get_y(0.2))
# Inherit from the ChFunction, from the Python side,
# (do not forget the __init__ constructor)
class MySquareFunct(chrono.ChFunction):
def __init__(self,
sys,
gravity,
material,
width,
height,
position_base,
position_tip,
damping,
elements,
torque=10,
origin=True,
stator_constraint=None):
self.mesh = fea.ChMesh()
self.mesh.SetAutomaticGravity(gravity)
self.section = fea.ChBeamSectionAdvanced()
self.section.SetAsRectangularSection(width, height)
self.section.SetYoungModulus(material.modulus)
self.section.SetGshearModulus(material.shear)
self.section.SetDensity(material.density)
self.section.SetBeamRaleyghDamping(damping)
self.position_base = chrono.ChVectorD(*position_base)
self.position_tip = chrono.ChVectorD(*position_tip)
self.builder = fea.ChBuilderBeamEuler(
) #Use the beam builder assembly method to assembly each beam
self.builder.BuildBeam(
self.mesh,
self.section,
elements,
self.position_base,
self.position_tip,
chrono.ChVectorD(0, 1, 0),
)
self.stator = chrono.ChBodyEasyCylinder(0.01, 0.01, 1000)
self.stator.SetPos(self.position_base)
self.stator.SetBodyFixed(origin)
self.frame = chrono.ChFrameD(self.stator)
sys.Add(self.stator)
if (origin == False) and (
stator_constraint is not None
): #If the beam is not located at the origin, it must need to be constrained to another beam
self.constraint = chrono.ChLinkMateGeneric()
self.constraint.Initialize(self.stator, stator_constraint, False,
chrono.ChFrameD(self.stator),
chrono.ChFrameD(stator_constraint))
self.constraint.SetConstrainedCoords(True, True, True, True, True,
True)
sys.Add(self.constraint)
self.frame = chrono.ChFrameD(self.stator)
self.rotor = chrono.ChBodyEasyCylinder(0.011, 0.011, 1000)
self.rotor.SetPos(self.position_base)
sys.Add(self.rotor)
self.frame.SetRot(
chrono.Q_from_AngAxis(chrono.CH_C_PI_2, chrono.VECT_X)
) #Rotate the direction of rotation to be planar (x-z plane)
self.motor = chrono.ChLinkMotorRotationTorque()
self.motor.Initialize(self.rotor, self.stator, self.frame)
sys.Add(self.motor)
self.motor.SetTorqueFunction(chrono.ChFunction_Const(torque))
self.arm_base = (self.builder.GetLastBeamNodes().front())
self.arm_tip = (self.builder.GetLastBeamNodes().back())
self.mate = chrono.ChLinkMateGeneric()
self.mate.Initialize(self.builder.GetLastBeamNodes().front(),
self.rotor,
chrono.ChFrameD(self.builder.GetLastBeamNodes(
).front().GetPos())) #constrain beam to rotor
self.mate.SetConstrainedCoords(
True, True, True, True, True, True
) #constraints must be in format: (True,True,True,True,True,True) to constrain x,y,z,rotx,roty,rotz coordinates
sys.Add(self.mate)
self.visual = fea.ChVisualizationFEAmesh(self.mesh)
self.visual.SetFEMdataType(
fea.ChVisualizationFEAmesh.E_PLOT_ELEM_BEAM_MZ)
self.visual.SetColorscaleMinMax(-1.4, 1.4)
self.visual.SetSmoothFaces(True)
self.visual.SetWireframe(False)
self.mesh.AddAsset(self.visual)
sys.Add(self.mesh)
def main():
print("Copyright (c) 2017 projectchrono.org" + "\n\n")
# Create systems
# Create the HMMWV vehicle, set parameters, and initialize
my_hmmwv = veh.HMMWV_Full()
my_hmmwv.SetContactMethod(contact_method)
my_hmmwv.SetChassisCollisionType(chassis_collision_type)
my_hmmwv.SetChassisFixed(False)
my_hmmwv.SetInitPosition(chrono.ChCoordsysD(initLoc, initRot))
my_hmmwv.SetPowertrainType(powertrain_model)
my_hmmwv.SetDriveType(drive_type)
my_hmmwv.SetSteeringType(steering_type)
my_hmmwv.SetTireType(tire_model)
my_hmmwv.SetTireStepSize(tire_step_size)
my_hmmwv.Initialize()
my_hmmwv.SetChassisVisualizationType(chassis_vis_type)
my_hmmwv.SetSuspensionVisualizationType(suspension_vis_type)
my_hmmwv.SetSteeringVisualizationType(steering_vis_type)
my_hmmwv.SetWheelVisualizationType(wheel_vis_type)
my_hmmwv.SetTireVisualizationType(tire_vis_type)
# Create the terrain
terrain = veh.RigidTerrain(my_hmmwv.GetSystem())
if (contact_method == chrono.ChContactMethod_NSC):
patch_mat = chrono.ChMaterialSurfaceNSC()
patch_mat.SetFriction(0.9)
patch_mat.SetRestitution(0.01)
elif (contact_method == chrono.ChContactMethod_SMC):
patch_mat = chrono.ChMaterialSurfaceSMC()
patch_mat.SetFriction(0.9)
patch_mat.SetRestitution(0.01)
patch_mat.SetYoungModulus(2e7)
patch = terrain.AddPatch(patch_mat, chrono.ChVectorD(0, 0, 0),
chrono.ChVectorD(0, 0, 1), terrainLength,
terrainWidth)
patch.SetTexture(veh.GetDataFile("terrain/textures/tile4.jpg"), 200, 200)
patch.SetColor(chrono.ChColor(0.8, 0.8, 0.5))
terrain.Initialize()
# Create the vehicle Irrlicht interface
app = veh.ChWheeledVehicleIrrApp(my_hmmwv.GetVehicle(), 'HMMWV',
irr.dimension2du(1000, 800))
app.SetSkyBox()
app.AddTypicalLights(irr.vector3df(30, -30, 100),
irr.vector3df(30, 50, 100), 250, 130)
app.AddTypicalLogo(chrono.GetChronoDataFile('logo_pychrono_alpha.png'))
app.SetChaseCamera(trackPoint, 6.0, 0.5)
app.SetTimestep(step_size)
app.AssetBindAll()
app.AssetUpdateAll()
# Initialize output
try:
os.mkdir(out_dir)
except:
print("Error creating directory ")
# Set up vehicle output
my_hmmwv.GetVehicle().SetChassisOutput(True)
my_hmmwv.GetVehicle().SetSuspensionOutput(0, True)
my_hmmwv.GetVehicle().SetSteeringOutput(0, True)
my_hmmwv.GetVehicle().SetOutput(veh.ChVehicleOutput.ASCII, out_dir,
"output", 0.1)
# Generate JSON information with available output channels
my_hmmwv.GetVehicle().ExportComponentList(out_dir + "/component_list.json")
# Create the interactive driver system
driver = veh.ChIrrGuiDriver(app)
# Set the time response for steering and throttle keyboard inputs.
steering_time = 1.0 # time to go from 0 to +1 (or from 0 to -1)
throttle_time = 1.0 # time to go from 0 to +1
braking_time = 0.3 # time to go from 0 to +1
driver.SetSteeringDelta(render_step_size / steering_time)
driver.SetThrottleDelta(render_step_size / throttle_time)
driver.SetBrakingDelta(render_step_size / braking_time)
driver.Initialize()
# Simulation loop
# Number of simulation steps between miscellaneous events
render_steps = m.ceil(render_step_size / step_size)
debug_steps = m.ceil(debug_step_size / step_size)
# Initialize simulation frame counter and simulation time
step_number = 0
render_frame = 0
if (contact_vis):
app.SetSymbolscale(1e-4)
# app.SetContactsDrawMode(irr.eCh_ContactsDrawMode::CONTACT_FORCES);
# ---------------------------------------------
# Create a sensor manager and add a point light
# ---------------------------------------------
manager = sens.ChSensorManager(my_hmmwv.GetSystem())
manager.scene.AddPointLight(chrono.ChVectorF(0, 0, 100),
chrono.ChVectorF(2, 2, 2), 5000)
manager.SetKeyframeSizeFromTimeStep(.001, 1 / 5)
# ------------------------------------------------
# Create a camera and add it to the sensor manager
# ------------------------------------------------
fov = 1.408
lag = 0
update_rate = 5
exposure_time = 1 / update_rate
offset_pose = chrono.ChFrameD(chrono.ChVectorD(-5, 0, 2))
cam = sens.ChCameraSensor(
my_hmmwv.GetChassisBody(), # body camera is attached to
update_rate, # update rate in Hz
offset_pose, # offset pose
image_width, # image width
image_height, # image height
fov # camera's horizontal field of view
)
cam.SetName("Camera Sensor")
# cam.SetLag(0);
# cam.SetCollectionWindow(0);
# Visualizes the image
if vis:
cam.PushFilter(
sens.ChFilterVisualize(image_width, image_height, "HMMWV Camera"))
# Save the current image to a png file at the specified path
if save:
cam.PushFilter(sens.ChFilterSave(out_dir + "cam/"))
# Add a camera to a sensor manager
manager.AddSensor(cam)
# ----------------------------------------------
# Create an IMU sensor and add it to the manager
# ----------------------------------------------
offset_pose = chrono.ChFrameD(
chrono.ChVectorD(-8, 0, 1),
chrono.Q_from_AngAxis(0, chrono.ChVectorD(0, 1, 0)))
imu = sens.ChIMUSensor(
my_hmmwv.GetChassisBody(), # body imu is attached to
imu_update_rate, # update rate in Hz
offset_pose, # offset pose
imu_noise_none # noise model
)
imu.SetName("IMU Sensor")
imu.SetLag(imu_lag)
imu.SetCollectionWindow(imu_collection_time)
# Provides the host access to the imu data
imu.PushFilter(sens.ChFilterIMUAccess())
# Add the imu to the sensor manager
manager.AddSensor(imu)
# ----------------------------------------------
# Create an GPS sensor and add it to the manager
# ----------------------------------------------
offset_pose = chrono.ChFrameD(
chrono.ChVectorD(-8, 0, 1),
chrono.Q_from_AngAxis(0, chrono.ChVectorD(0, 1, 0)))
gps = sens.ChGPSSensor(
my_hmmwv.GetChassisBody(), # body imu is attached to
gps_update_rate, # update rate in Hz
offset_pose, # offset pose
gps_reference,
gps_noise_none # noise model
)
gps.SetName("GPS Sensor")
gps.SetLag(gps_lag)
gps.SetCollectionWindow(gps_collection_time)
# Provides the host access to the gps data
gps.PushFilter(sens.ChFilterGPSAccess())
# Add the gps to the sensor manager
manager.AddSensor(gps)
realtime_timer = chrono.ChRealtimeStepTimer()
while (app.GetDevice().run()):
time = my_hmmwv.GetSystem().GetChTime()
#End simulation
if (time >= t_end):
break
if (step_number % render_steps == 0):
app.BeginScene(True, True, irr.SColor(255, 140, 161, 192))
app.DrawAll()
app.EndScene()
#Debug logging
if (debug_output and step_number % debug_steps == 0):
print("\n\n============ System Information ============\n")
print("Time = " << time << "\n\n")
#my_hmmwv.DebugLog(OUT_SPRINGS | OUT_SHOCKS | OUT_CONSTRAINTS)
marker_driver = my_hmmwv.GetChassis().GetMarkers()[0].GetAbsCoord(
).pos
marker_com = my_hmmwv.GetChassis().GetMarkers()[1].GetAbsCoord(
).pos
print("Markers\n")
print(" Driver loc: ", marker_driver.x, " ", marker_driver.y,
" ", marker_driver.z)
print(" Chassis COM loc: ", marker_com.x, " ", marker_com.y, " ",
marker_com.z)
# Get driver inputs
driver_inputs = driver.GetInputs()
# Update modules (process inputs from other modules)
driver.Synchronize(time)
terrain.Synchronize(time)
my_hmmwv.Synchronize(time, driver_inputs, terrain)
app.Synchronize(driver.GetInputModeAsString(), driver_inputs)
# Advance simulation for one timestep for all modules
driver.Advance(step_size)
terrain.Advance(step_size)
my_hmmwv.Advance(step_size)
app.Advance(step_size)
# Update sensor manager
# Will render/save/filter automatically
manager.Update()
# Increment frame number
step_number += 1
# Spin in place for real time to catch up
realtime_timer.Spin(step_size)
return 0
def reset(self):
self.generate_track(starting_index=350, z=.40)
self.vehicle = veh.RCCar()
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.chassis_body = self.vehicle.GetChassisBody()
self.system = self.vehicle.GetVehicle().GetSystem()
# Driver
self.driver = Driver(self.vehicle.GetVehicle())
# 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 = .65
# 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.driver.SetSteeringDelta(self.timestep / steering_time)
self.driver.SetThrottleDelta(self.timestep / throttle_time)
self.driver.SetBrakingDelta(self.timestep / braking_time)
# Longitudinal controller (throttle and braking)
self.long_controller = PIDLongitudinalController(
self.vehicle, self.driver)
self.long_controller.SetGains(Kp=0.4, Ki=0, Kd=0)
self.long_controller.SetTargetSpeed(speed=self.target_speed)
# Mesh 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.system)
coord_sys = chrono.ChCoordsysD(offset,
chrono.ChQuaternionD(1, 0, 0, 0))
patch = self.terrain.AddPatch(
coord_sys,
chrono.GetChronoDataFile("sensor/textures/hallway_contact.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()
f = 0
start_light = 0
end_light = 8
self.manager = sens.ChSensorManager(self.system)
for i in range(8):
f += 3
if i < start_light or i > end_light:
continue
self.manager.scene.AddPointLight(
chrono.ChVectorF(f, 1.25, 2.3) + offsetF,
chrono.ChVectorF(1, 1, 1), 5)
self.manager.scene.AddPointLight(
chrono.ChVectorF(f, 3.75, 2.3) + offsetF,
chrono.ChVectorF(1, 1, 1), 5)
# -----------------------------------------------------
# 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(-.075, 0, .15),
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)
# create obstacles
self.cones = []
self.DrawCones(self.track.left.points, 'green', z=.31)
self.DrawCones(self.track.right.points, 'red', z=.31)
self.old_dist = self.track.center.calcDistance(
self.chassis_body.GetPos())
self.step_number = 0
self.isdone = False
self.render_setup = False
if self.play_mode:
self.render()
return self.get_ob()
# Test matrices ma = chrono.ChMatrixDynamicD(4,4) ma.FillDiag(-2) mb = chrono.ChMatrixDynamicD(4,4) mb.FillElem(10) mc = (ma-mb)*0.1; # operator overloading of +,-,* is supported print (mc); mr = chrono.ChMatrix33D() mr.FillDiag(20) print (mr*my_vect1); # Test frames - # create a frame representing a translation and a rotation # of 20 degrees on X axis my_frame = chrono.ChFrameD(my_vect2, chrono.Q_from_AngAxis(20*chrono.CH_C_DEG_TO_RAD, chrono.ChVectorD(1,0,0))) my_vect5 = my_vect1 >> my_frame # Print the class hierarchy of a chrono class import inspect inspect.getmro(chrono.ChStreamOutAsciiFile) # Use matrices my_matr = chrono.ChMatrixDynamicD(6,4) my_matr[1,2]=4 my_matrb = chrono.ChMatrixDynamicD(4,6) my_matrb.FillRandom(-2,2) print ( my_matrb * 4 ) # Use the ChFunction classes
# 1) a FEA tetrahedron(s):
# Create a material, that must be assigned to each solid element in the mesh,
# and set its parameters
mmaterial = fea.ChContinuumElastic()
mmaterial.Set_E(0.01e9) # rubber 0.01e9, steel 200e9
mmaterial.Set_v(0.3)
mmaterial.Set_RayleighDampingK(0.003)
mmaterial.Set_density(1000)
for i in range(4):
try:
cdown = chrono.ChCoordsysD(chrono.ChVectorD(0, -0.4, 0))
crot = chrono.ChCoordsysD(
chrono.VNULL,
chrono.Q_from_AngAxis(chrono.CH_C_2PI * chrono.ChRandom(),
chrono.VECT_Y) *
chrono.Q_from_AngAxis(chrono.CH_C_PI_2, chrono.VECT_X))
cydisp = chrono.ChCoordsysD(chrono.ChVectorD(-0.3, 0.1 + i * 0.1,
-0.3))
ctot = cydisp.TransformLocalToParent(
crot.TransformLocalToParent(cdown))
mrot = chrono.ChMatrix33D(ctot.rot)
fea.ChMeshFileLoader.FromTetGenFile(
my_mesh, chrono.GetChronoDataFile("fea/beam.node"),
chrono.GetChronoDataFile("fea/beam.ele"), mmaterial, ctot.pos,
mrot)
except:
print('Error Loading meshes')
break
# Create the contact surface(s).
