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