def get_ob(self):
camera_buffer_RGBA8 = self.camera.GetMostRecentRGBA8Buffer()
if camera_buffer_RGBA8.HasData():
rgb = camera_buffer_RGBA8.GetRGBA8Data()[:, :, 0:3]
else:
rgb = np.zeros((self.camera_height, self.camera_width, 3))
# rgb = np.zeros((self.camera_height,self.camera_width,3))
gps_buffer = self.gps.GetMostRecentGPSBuffer()
if gps_buffer.HasData():
cur_gps_data = gps_buffer.GetGPSData()
cur_gps_data = chrono.ChVectorD(cur_gps_data[1], cur_gps_data[0], cur_gps_data[2])
else:
cur_gps_data = chrono.ChVectorD(self.origin)
pos = self.chassis_body.GetPos()
vel = self.chassis_body.GetPos_dt()
head = self.vehicle.GetVehicle().GetVehicleRot().Q_to_Euler123().z
gps_data = [(self.goal - self.chassis_body.GetPos()).x, (self.goal - self.chassis_body.GetPos()).y]
dist = self.goal - self.chassis_body.GetPos()
dist_local = self.chassis_body.GetRot().RotateBack(dist)
targ_head = np.arctan2(dist_local.y, dist_local.x)
goalCart = chrono.ChVectorD(self.goal_coord)
sens.GPS2Cartesian(goalCart, self.origin)
sens.GPS2Cartesian(cur_gps_data, self.origin)
gps_dist = goalCart - cur_gps_data
loc_dist_gps = [gps_dist.x * np.cos(head) + gps_dist.y * np.sin(head),
-gps_dist.x * np.sin(head) + gps_dist.y * np.cos(head)]
array_data = np.array([loc_dist_gps[0], loc_dist_gps[1], head, targ_head, vel.Length()])
return (rgb, array_data)
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 DrawBarriers(self, points, n=5, height=1, width=1):
points = points[::n]
if points[-1] != points[0]:
points.append(points[-1])
for i in range(len(points) - 1):
p1 = points[i]
p2 = points[i + 1]
box = chrono.ChBodyEasyBox((p2 - p1).Length(), height, width, 1000,
True, True)
box.SetPos(p1)
q = chrono.ChQuaternionD()
v1 = p2 - p1
v2 = chrono.ChVectorD(1, 0, 0)
ang = math.atan2((v1 % v2).Length(), v1 ^ v2)
if chrono.ChVectorD(0, 0, 1) ^ (v1 % v2) > 0.0:
ang *= -1
q.Q_from_AngZ(ang)
box.SetRot(q)
box.SetBodyFixed(True)
color = chrono.ChColorAsset()
if i % 2 == 0:
color.SetColor(chrono.ChColor(1, 0, 0))
else:
color.SetColor(chrono.ChColor(1, 1, 1))
box.AddAsset(color)
self.system.Add(box)
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 __init__(self, x_half, y_half, z, t0):
# making 4 turns to get to the end point
q = chrono.Q_from_AngZ(randint(0, 3) * (-np.pi / 2))
flip = pow(-1, randint(0, 1))
route = randint(0, 1)
points = chrono.vector_ChVectorD()
if route == 0:
beginPos = [-x_half, -y_half * flip]
endPos = [x_half, y_half * flip]
deltaX = (endPos[0] - beginPos[0]) / 3
deltaY = (endPos[1] - beginPos[1]) / 2
for i in range(6):
point = chrono.ChVectorD(
beginPos[0] + deltaX * m.floor((i + 1) / 2),
beginPos[1] + deltaY * m.floor(i / 2), z)
point = q.Rotate(point)
points.append(point)
if route == 1:
xs = np.asarray([-1, 0, 1 / 3, 1 / 3, 0, -1])
ys = np.asarray([1, 4 / 5, 1 / 2, -1 / 2, -4 / 5, -1]) * flip
for x, y in zip(xs, ys):
point = chrono.ChVectorD(x * x_half, y * y_half, z)
point = q.Rotate(point)
points.append(point)
super(BezierPath, self).__init__(points)
self.current_t = np.random.rand(1)[0] * 0.5
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 build_external_cylinder(cyl_radius, shape_length, density, contact_method,
offset):
"""
Build a left and right cylinder next to the shape_lenght that must be centered
on 0,0,0 coord system.
:param cyl_radius: radius of the two external cylinders
:param shape_length: lenght of the shape
:param density: Density of the cylinders
:param contact_method: SMC or NSC
:param offset: distance between disk and shape
:return: two ChEasyCylinder
"""
height = 0.01 * shape_length
qCylinder = chrono.Q_from_AngX(90 * chrono.CH_C_DEG_TO_RAD)
left_cyl = chrono.ChBodyEasyCylinder(cyl_radius, height, density, True,
False, contact_method)
right_cyl = chrono.ChBodyEasyCylinder(cyl_radius, height, density, True,
False, contact_method)
left_cyl.SetRot(qCylinder)
right_cyl.SetRot(qCylinder)
left_cyl.SetPos(chrono.ChVectorD(0, 0, -(shape_length / 2. + offset)))
right_cyl.SetPos(chrono.ChVectorD(0, 0, shape_length / 2. + offset))
left_cyl.SetBodyFixed(True)
right_cyl.SetBodyFixed(True)
return left_cyl, right_cyl
def _make_nodes(self) -> None:
"""
Generate the list of nodes
"""
pos = chrono.ChVectorD(*self._pos)
mass = (self._mass / (self._subdiv[0] + 1) * (self._subdiv[1] + 1) *
(self._subdiv[2] + 1))
# for each dimension generate the list of positions where to generate the nodes
# the index of the node identify the posiion in the grid
grid = [
np.linspace(0, dim, sub + 1)
for dim, sub in zip(self._size, self._subdiv)
]
# 3d array containing the nodes
self._nodes = np.ndarray([i + 1 for i in self._subdiv],
dtype=np.dtype(fea.ChNodeFEAxyz))
# iterator for self._nodes
with np.nditer(self._nodes,
flags=["multi_index", "refs_ok"],
op_flags=["readwrite"]) as it:
for i in it:
ind = it.multi_index # index of the current node
node_pos = chrono.ChVectorD(
grid[0][ind[0]], grid[1][ind[1]],
grid[2][ind[2]]) # local position of the node
tmp = fea.ChNodeFEAxyz(pos + node_pos)
tmp.SetMass(mass)
i[...] = tmp # add the node to self._nodes
self._mesh.AddNode(tmp) # add the node to the mesh
def __init__(self, x=0.0, y=0.0, yaw=0.0, v=0.0, pos_dt=chrono.ChVectorD(0,0,0)):
self.x = x
self.y = y
self.yaw = yaw
self.v = v
self.pos_dt = pos_dt
self.pos = chrono.ChVectorD(self.x, self.y, 0)
def __init__(self, render):
self.render = render
self.observation_space = np.empty([9, 1])
self.action_space = np.empty([
3,
])
self.info = {}
self.timestep = 0.01
# ---------------------------------------------------------------------
#
# Create the simulation system and add items
#
self.timeend = 30
# Create the vehicle system
chrono.SetChronoDataPath("/home/aaron/chrono/data/")
veh.SetDataPath("/home/aaron/chrono/data/vehicle/")
# JSON file for vehicle model
self.vehicle_file = veh.GetDataPath(
) + "hmmwv/vehicle/HMMWV_Vehicle.json"
# JSON files for terrain
self.rigidterrain_file = veh.GetDataPath() + "terrain/RigidPlane.json"
# JSON file for powertrain (simple)
self.simplepowertrain_file = veh.GetDataPath(
) + "generic/powertrain/SimplePowertrain.json"
# JSON files tire models (rigid)
self.rigidtire_file = veh.GetDataPath(
) + "hmmwv/tire/HMMWV_RigidTire.json"
# Initial vehicle position
self.initLoc = chrono.ChVectorD(-125, -130, 0.5)
# Initial vehicle orientation
self.initRot = chrono.ChQuaternionD(1, 0, 0, 0)
# Rigid terrain dimensions
self.terrainHeight = 0
self.terrainLength = 300.0 # size in X direction
self.terrainWidth = 300.0 # size in Y direction
# Point on chassis tracked by the camera (Irrlicht only)
self.trackPoint = chrono.ChVectorD(0.0, 0.0, 1.75)
self.dist = 5.0
self.generator = RandomPathGenerator(width=100,
height=100,
maxDisplacement=2,
steps=1)
self.tracknum = 0
def Advance(self, step, vehicle):
state = vehicle.GetState()
current_pos = self.path.calcClosestPoint(state)
current_index = self.path.calcIndex(current_pos)
next_pos = chrono.ChVectorD(
self.path.ps[current_index] * 2 * math.cos(state.yaw) + state.x,
self.path.ps[current_index] * 2 * math.sin(state.yaw) + state.y,
0,
)
next_index = self.path.calcIndex(next_pos)
direction = (next_index > current_index) - (next_index < current_index)
sentinel_index = self.path.next_segmentation_index(
current_index, direction, 0.1, 10)
target = chrono.ChVectorD(self.path.x[sentinel_index],
self.path.y[sentinel_index], 0)
sentinel_dist = 10 - np.clip(
(self.target - current_pos).Length(), 4.0, 6)
self.sentinel = chrono.ChVectorD(
sentinel_dist * math.cos(state.yaw) + state.x,
sentinel_dist * math.sin(state.yaw) + state.y,
0,
)
point = self.path.calcClosestPoint(self.sentinel)
self.target = point #+(point-self.sentinel)
# The "error" vector is the projection onto the horizontal plane (z=0) of
# vector between sentinel and target
err_vec = self.target - self.sentinel
err_vec.z = 0
# Calculate the sign of the angle between the projections of the sentinel
# vector and the target vector (with origin at vehicle location).
sign = self.calcSign(state)
# Calculate current error (magnitude)
err = sign * err_vec.Length()
# Estimate error derivative (backward FD approximation).
self.errd = (err - self.err) / step
# Calculate current error integral (trapezoidal rule).
self.erri += (err + self.err) * step / 2
# Cache new error
self.err = err
# Return PID output (steering value)
#self.Kp * self.err + self.Ki * self.erri + self.Kd * self.errd
#self.steering = np.clip(self.err/2.5+0.01*self.erri, -1.0, 1.0)
self.steering = np.clip(self.err / 2.5 + 0.01 * self.erri, -1.0, 1.0)
vehicle.driver.SetTargetSteering(self.steering)
return self.steering
def CalcRandomPose(self, terrain, length, width, offset=0):
"""
Calculates random position within the terrain boundaries
TODO: generate some rotation (quaternion) to have mesh lay flush with the terrain
"""
x = random.randint(int(-length / 2), int(length / 2))
y = random.randint(int(-width / 2), int(width / 2))
z = terrain.GetHeight(chrono.ChVectorD(x, y, 0)) + offset
return chrono.ChVectorD(x, y, z)
def CalcBoundingBox(self):
""" Calculate approximate minimum boundary box of a mesh """
vertices = self.mesh.m_vertices
minimum = chrono.ChVectorD(
min(vertices, key=lambda x: x.x).x,
min(vertices, key=lambda x: x.y).y, 0)
maximum = chrono.ChVectorD(
max(vertices, key=lambda x: x.x).x,
max(vertices, key=lambda x: x.y).y, 0)
self.bounding_box = chrono.ChVectorD(maximum - minimum)
def RandomlyPositionAssets(self, system, initLoc, finalLoc, terrain, terrain_length, terrain_width, should_scale=False):
diag_obs = 5
for i in range(diag_obs):
x = np.linspace(initLoc.x, finalLoc.x, diag_obs + 2)[1:-1]
y = np.linspace(initLoc.y, finalLoc.y, diag_obs + 2)[1:-1]
pos = chrono.ChVectorD(x[i], y[i], 0)
pos.z = terrain.GetHeight(pos)
rot = chrono.Q_from_AngZ(np.random.uniform(0, np.pi))
offset = chrono.ChVectorD(pos.y, -pos.x, 0)
offset = offset.GetNormalized() * (np.random.random() - 0.5) * 20
rand_asset = np.random.choice(self.assets).Copy()
rand_asset.pos = pos + offset
rand_asset.rot = rot
if should_scale:
rand_asset.scale = np.random.uniform(rand_asset.scale_range[0], rand_asset.scale_range[1])
self.assets.append(rand_asset)
for i, asset in enumerate(self.assets[:-diag_obs]):
success = True
for i in range(101):
if i == 100:
success = False
break
pos = self.GenerateRandomPosition(terrain, terrain_length, terrain_width)
scale = 1
if should_scale:
scale = np.random.uniform(asset.scale_range[0], asset.scale_range[1])
if (pos - initLoc).Length() < 15 or (pos - finalLoc).Length() < 15:
continue
closest_asset = min(self.assets, key=lambda x: (x.pos - pos).Length())
overlap = 0
if (closest_asset.pos - pos).Length() < scale + closest_asset.scale + overlap:
continue
break
if not success:
continue
rot = chrono.Q_from_AngZ(np.random.uniform(0, np.pi))
asset.pos = pos
asset.rot = rot
asset.scale = scale
for asset in self.assets:
system.Add(asset.body)
asset.CreateCollisionModel()
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 calcSign(self, state):
"""
Calculate the sign of the angle between the projections of the sentinel vector
and the target vector (with origin at vehicle location).
"""
pos = chrono.ChVectorD(state.x, state.y, 0)
sentinel_vec = self.sentinel - pos
target_vec = self.target - pos
temp = (sentinel_vec % target_vec) ^ chrono.ChVectorD(0, 0, 1)
return (temp > 0) - (temp < 0)
def calcAngle(p1, p2, z=0.0):
""" Calculates angle from two points """
if isinstance(p1, list):
p1 = chrono.ChVectorD(p1[0], p1[1], z)
p2 = chrono.ChVectorD(p2[0], p2[1], z)
v1 = p2 - p1
v2 = chrono.ChVectorD(1, 0, 0)
ang = math.atan2((v1 % v2).Length(), v1 ^ v2)
if chrono.ChVectorD(0, 0, 1) ^ (v1 % v2) > 0.0:
ang *= -1
return ang
def Advance(self, step):
if self.irrlicht:
if not self.app.GetDevice().run():
return -1
if self.step_number % self.render_steps == 0:
self.app.BeginScene(True, True,
chronoirr.SColor(255, 140, 161, 192))
self.app.DrawAll()
self.app.EndScene()
# Update modules (process inputs from other modules)
time = self.system.GetChTime()
self.vehicle.Synchronize(time)
self.terrain.Synchronize(time)
if self.irrlicht:
self.app.Synchronize("", self.vehicle.driver.GetInputs())
if self.obstacles != None:
for n in range(len(self.obstacles)):
i = list(self.obstacles)[n]
obstacle = self.obstacles[i]
if obstacle.Update(step):
self.obstacles = RandomObstacleGenerator.moveObstacle(
self.track.center, self.obstacles, obstacle, i)
self.boxes[n].SetPos(obstacle.p1)
q = chrono.ChQuaternionD()
v1 = obstacle.p2 - obstacle.p1
v2 = chrono.ChVectorD(1, 0, 0)
ang = math.atan2((v1 % v2).Length(), v1 ^ v2)
if chrono.ChVectorD(0, 0, 1) ^ (v1 % v2) > 0.0:
ang *= -1
q.Q_from_AngZ(ang)
self.boxes[n].SetRot(q)
if self.opponents != None:
for opponent in self.opponents:
opponent.Update(time, step)
# Advance simulation for one timestep for all modules
self.vehicle.Advance(step)
self.terrain.Advance(step)
if self.irrlicht:
self.app.Advance(step)
self.step_number += 1
if self.pov:
self.pov_exporter.ExportData()
self.system.DoStepDynamics(step)
def Advance(self, step):
self.sentinel = self.vehicle.GetChassisBody().GetFrame_REF_to_abs(
).TransformPointLocalToParent(chrono.ChVectorD(self.dist, 0, 0))
self.tracker.calcClosestPoint(self.sentinel, self.target)
# The "error" vector is the projection onto the horizontal plane (z=0) of
# vector between sentinel and target
err_vec = self.target - self.sentinel
err_vec.z = 0
# Calculate the sign of the angle between the projections of the sentinel
# vector and the target vector (with origin at vehicle location).
sign = self.calcSign()
# Calculate current error (magnitude)
err = sign * err_vec.Length()
# Estimate error derivative (backward FD approximation).
self.errd = (err - self.err) / step
# Calculate current error integral (trapezoidal rule).
self.erri += (err + self.err) * step / 2
# Cache new error
self.err = err
# Return PID output (steering value)
steering = np.clip(
self.Kp * self.err + self.Ki * self.erri + self.Kd * self.errd,
-1.0, 1.0)
self.driver.SetTargetSteering(steering)
def draw_path_in_irrlicht(system: 'WAChronoSystem', path: 'WAPath'):
"""Draw a WAPath representation as a ChBezierCurve in irrlicht
Basically just copies over the path's points into something viewable for Chrono
Args:
system (WAChronoSystem): system that manages the simulation
path (WAPath): WA path object to visualize in irrlicht
"""
points = path.get_points()
ch_points = chrono.vector_ChVectorD()
for p in points:
ch_points.push_back(chrono.ChVectorD(p[0], p[1], p[2]))
curve = chrono.ChBezierCurve(ch_points)
road = system._system.NewBody()
road.SetBodyFixed(True)
system._system.AddBody(road)
num_points = len(points)
path_asset = chrono.ChLineShape()
path_asset.SetLineGeometry(chrono.ChLineBezier(curve))
path_asset.SetColor(chrono.ChColor(0, 0.8, 0))
path_asset.SetNumRenderPoints(max(2 * num_points, 400))
road.AddAsset(path_asset)
def __init__(self, filename, bounding_box=None):
self.filename = filename
# If bounding box is not passed in, calculate it
if bounding_box == None:
self.bounding_box = CalcBoundingBox()
else:
self.bounding_box = bounding_box
self.mesh = chrono.ChTriangleMeshConnected()
self.mesh.LoadWavefrontMesh(chrono.GetChronoDataFile(filename), False,
True)
self.shape = chrono.ChTriangleMeshShape()
self.shape.SetMesh(self.mesh)
self.shape.SetStatic(True)
self.body = chrono.ChBody()
self.body.AddAsset(self.shape)
self.body.SetCollide(False)
self.body.SetBodyFixed(True)
self.scaled = False
self.pos = chrono.ChVectorD()
self.scale = 1
self.ang = 0
def AddMovingObstacles(system):
# Create contact material, of appropriate type. Use default properties
material = None
if (NSC_SMC == chrono.ChContactMethod_NSC):
matNSC = chrono.ChMaterialSurfaceNSC()
#Change NSC material properties as desired
material = matNSC
elif (NSC_SMC == chrono.ChContactMethod_SMC):
matSMC = chrono.ChMaterialSurfaceSMC()
# Change SMC material properties as desired
material = matSMC
else:
raise ("Unvalid Contact Method")
sizeX = 300
sizeY = 300
height = 0
numObstacles = 10
for i in range(numObstacles):
o_sizeX = 1.0 + 3.0 * chrono.ChRandom()
o_sizeY = 0.3 + 0.2 * chrono.ChRandom()
o_sizeZ = 0.05 + 0.1 * chrono.ChRandom()
obstacle = chrono.ChBodyEasyBox(o_sizeX, o_sizeY, o_sizeZ, 2000.0,
True, True, material)
o_posX = (chrono.ChRandom() - 0.5) * 0.4 * sizeX
o_posY = (chrono.ChRandom() - 0.5) * 0.4 * sizeY
o_posZ = height + 4
rot = chrono.ChQuaternionD(chrono.ChRandom(), chrono.ChRandom(),
chrono.ChRandom(), chrono.ChRandom())
rot.Normalize()
obstacle.SetPos(chrono.ChVectorD(o_posX, o_posY, o_posZ))
obstacle.SetRot(rot)
system.AddBody(obstacle)
def WAVector_to_ChVector(vector: WAVector):
"""Converts a WAVector to a ChVector
Args:
vector (WAVector): The vector to convert
"""
return chrono.ChVectorD(vector.x, vector.y, vector.z)
def __init__(self, path):
self.Kp = 0
self.Ki = 0
self.Kd = 0
self.dist = 0
self.target = chrono.ChVectorD(0, 0, 0)
self.sentinel = chrono.ChVectorD(0, 0, 0)
self.steering = 0
self.err = 0
self.errd = 0
self.erri = 0
self.path = path
def Advance(self, step, vehicle):
"""
calculate horizonatal line across the path
find the center of the line
track
params:
- state
- vehicle model
returns:
- path
"""
state = vehicle.GetState()
self.sentinel = chrono.ChVectorD(
self.dist * math.cos(state.yaw) + state.x,
self.dist * math.sin(state.yaw) + state.y,
0,
)
#self.target = self.path.calcClosestPoint(self.sentinel)
leftPath = self.track.left
rightPath = self.track.right
# ---find closest points on left and right path
# ---calculate line from these points
# points on the boundaries
leftPoint = leftPath.calcClosestPoint(self.sentinel)
rightPoint = rightPath.calcClosestPoint(self.sentinel)
# plot line
plt.plot([leftPoint.x, leftPoint.y], [rightPoint.x, rightPoint.y])
def release(self):
"""
Release the forces of the nodes by setting the force to 0
:return:
"""
for fn in self.fea_nodes:
fn.SetForce(chrono.ChVectorD(0., 0., 0.))
def shift_mesh(mesh, shift_x=0, shift_y=0, shift_z=0):
for n in mesh.GetNodes():
p = n.GetPos()
n.SetPos(
chrono.ChVectorD(
eval(p[0]) + shift_x,
eval(p[1]) + shift_y,
eval(p[2]) + shift_z))
def GetInitRot(self):
y_axis = chrono.ChVectorD(1, 0, 0)
vec = self.ch_path[self.starting_index +
1] - self.ch_path[self.starting_index]
theta = math.acos((y_axis ^ vec) / (vec.Length() * y_axis.Length()))
q = chrono.ChQuaternionD()
q.Q_from_AngZ(-theta)
return q
def is_done(self, pos):
vec = chrono.ChVectorD(0, 0, 0)
self.path_tracker.calcClosestPoint(pos, vec)
err = vec - pos
if self.vehicle.GetSystem().GetChTime() > self.timeend:
self.isdone = True
elif err.Length() > 6:
self.isdone = True
