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 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 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 WAQuaternion_to_ChQuaternion(quaternion: WAQuaternion): """Converts a WAQuaternion to a ChQuaternion Args: quaternion (WAQuaternion): The quaternion to convert """ return chrono.ChQuaternionD(quaternion.x, quaternion.y, quaternion.z, quaternion.w)
def SetCamera(self, pos, targ): delta = targ - pos q0 = chrono.ChQuaternionD() q0.Q_from_AngAxis(-math.pi / 2, chrono.VECT_X) alpha = 0 if (delta.z <= 0 and delta.x != 0): alpha = -math.asin(delta.x / math.hypot(delta.x, delta.z)) if (delta.z > 0): alpha = math.asin(delta.x / math.hypot(delta.x, delta.z)) + math.pi q1 = chrono.ChQuaternionD() q1.Q_from_AngAxis(alpha, q0.GetZaxis()) q2 = chrono.ChQuaternionD() q2 = q1 * q0 beta = math.asin(delta.y / delta.Length()) q3 = chrono.ChQuaternionD() q3.Q_from_AngAxis(beta, q2.GetXaxis()) qc0 = chrono.ChQuaternionD() qc0 = q3 * q2 qcy = chrono.ChQuaternionD() qcy.Q_from_AngAxis(math.pi, qc0.GetYaxis()) qc = chrono.ChQuaternionD() qc = qcy * qc0 q = Quat(qc.e0, qc.e1, qc.e2, qc.e3) self.mycamera.setPos(pos.x, pos.y, pos.z) self.mycamera.setQuat(q)
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): 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 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) box_asset = box.GetAssets()[0] visual_asset = chrono.CastToChVisualization(box_asset) vis_mat = chrono.ChVisualMaterial() vis_mat.SetAmbientColor(chrono.ChVectorF(0, 0, 0)) if i % 2 == 0: vis_mat.SetDiffuseColor(chrono.ChVectorF(1.0, 0, 0)) else: vis_mat.SetDiffuseColor(chrono.ChVectorF(1.0, 1.0, 1.0)) vis_mat.SetSpecularColor(chrono.ChVectorF(0.9, 0.9, 0.9)) vis_mat.SetFresnelMin(0) vis_mat.SetFresnelMax(0.1) visual_asset.material_list.append(vis_mat) 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) self.barriers.append(box)
def calcPose(p1, p2, z=0.0): """ Calculates pose (position and orientation) from two points """ if isinstance(p1, list): p1 = chrono.ChVectorD(p1[0], p1[1], z) p2 = chrono.ChVectorD(p2[0], p2[1], z) loc = p1 rot = 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 rot.Q_from_AngZ(ang) return loc, rot
def ChFrame_from_json(j: dict): """Creates a ChFrame from a json object. Args: j (dict): The json object that will be converted to a ChFrame Returns: ChFrameD: The frame created from the json object """ # Validate the json file _check_field(j, 'Position', field_type=list) _check_field(j, 'Rotation', field_type=list) # Do the conversion pos = j['Position'] rot = j['Rotation'] return chrono.ChFrameD( chrono.ChVectorD(pos[0], pos[1], pos[2]), chrono.ChQuaternionD(rot[0], rot[1], rot[2], rot[3]))
def DrawObstacles(self, obstacles, z=0.0): self.boxes = [] for i, o in obstacles.items(): p1 = o.p1 p2 = o.p2 box = chrono.ChBodyEasyBox(o.length, o.width, 1, 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) box_asset = box.GetAssets()[0] visual_asset = chrono.CastToChVisualization(box_asset) self.system.Add(box) self.boxes.append(box)
def CalcInitialPose(p1: chrono.ChVectorD, p2: chrono.ChVectorD, z=0.1, reversed=0): if not isinstance(p1, chrono.ChVectorD): raise TypeError elif not isinstance(p2, chrono.ChVectorD): raise TypeError p1.z = p2.z = z initLoc = p1 vec = p2 - p1 theta = math.atan2((vec % chrono.ChVectorD(1, 0, 0)).Length(), vec ^ chrono.ChVectorD(1, 0, 0)) if reversed: theta *= -1 initRot = chrono.ChQuaternionD() initRot.Q_from_AngZ(theta) return initLoc, initRot
def __init__(self): ChronoBaseEnv.__init__(self) SetChronoDataDirectories() # Define action and observation space # They must be gym.spaces objects # Example when using discrete actions: self.camera_width = 80 self.camera_height = 45 self.action_space = spaces.Box(low=-1.0, high=1.0, shape=(2, ), dtype=np.float32) self.observation_space = spaces.Box(low=0, high=255, shape=(self.camera_height, self.camera_width, 3), dtype=np.uint8) self.info = {"timeout": 10000.0} self.timestep = 3e-3 # --------------------------------------------------------------------- # # Create the simulation system and add items # self.Xtarg = 100.0 self.Ytarg = 0.0 self.timeend = 20 self.control_frequency = 10 self.initLoc = chrono.ChVectorD(0, 0, 1.0) self.initRot = chrono.ChQuaternionD(1, 0, 0, 0) self.terrain_model = veh.RigidTerrain.PatchType_BOX self.terrainHeight = 0 # terrain height (FLAT terrain only) self.terrainLength = 250.0 # size in X direction self.terrainWidth = 15.0 # size in Y direction self.render_setup = False self.play_mode = False self.manager = 0
if hasattr(builtins, 'exported_system_relpath'): shapes_dir = builtins.exported_system_relpath + shapes_dir exported_items = [] body_0 = chrono.ChBodyAuxRef() body_0.SetName('ground') body_0.SetBodyFixed(True) exported_items.append(body_0) # Rigid body part body_1 = chrono.ChBodyAuxRef() body_1.SetName('escape_wheel^escapement-1') body_1.SetPos(chrono.ChVectorD(0, 0.000381819559584939, 0)) body_1.SetRot(chrono.ChQuaternionD(0, 0, 0.707106781186548, 0.707106781186547)) body_1.SetMass(0.385093622816182) body_1.SetInertiaXX( chrono.ChVectorD(0.000614655341550614, 0.00114774663635329, 0.000614655341550614)) body_1.SetInertiaXY( chrono.ChVectorD(1.04945260437012e-19, -5.29910899706164e-19, 5.85921324575995e-19)) body_1.SetFrame_COG_to_REF( chrono.ChFrameD( chrono.ChVectorD(-1.29340068058665e-17, 4.10138104133823e-17, 0.00633921901294084), chrono.ChQuaternionD(1, 0, 0, 0))) # Visualization shape body_1_1_shape = chrono.ChObjShapeFile()
# Iterate over added bodies (Python style) print ('Positions of all bodies in the system:') for abody in my_system.Get_bodylist(): print (' body pos=', abody.GetPos() ) # Move a body, using a ChFrame my_displacement = chrono.ChFrameMovingD(chrono.ChVectorD(5,1,0)); my_shbodyA %= my_displacement # ..also as: # my_shbody.ConcatenatePreTransformation(my_displacement) print ('Moved body pos=', my_shbodyA.GetPos() ) # Use a body with an auxiliary reference (REF) that does not correspond # to the center of gravity (COG) body_1= chrono.ChBodyAuxRef() body_1.SetName('Parte1-1') body_1.SetPos(chrono.ChVectorD(-0.0445347481124079,0.0676266363930238,-0.0230808979433518)) body_1.SetRot(chrono.ChQuaternionD(1,0,0,0)) body_1.SetMass(346.17080777653) body_1.SetInertiaXX(chrono.ChVectorD(48583.2418823358,526927.118351673,490689.966726565)) body_1.SetInertiaXY(chrono.ChVectorD(1.70380722975012e-11,1.40840344485366e-11,-2.31869065456271e-12)) body_1.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") body_1.GetAssets().push_back(myasset) print ('Done...')
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): #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()
cyl_g.GetCylinderGeometry().rad = 0.03 ground.AddAsset(cyl_g) col_g = chrono.ChColorAsset() col_g.SetColor(chrono.ChColor(0.6, 0.6, 0.2)) ground.AddAsset(col_g) ## Crank crank = chrono.ChBody() system.AddBody(crank) crank.SetIdentifier(1) crank.SetName("crank") crank.SetMass(1.0) crank.SetInertiaXX(chrono.ChVectorD(0.005, 0.1, 0.1)) crank.SetPos(chrono.ChVectorD(-1, 0, 0)) crank.SetRot(chrono.ChQuaternionD(1, 0, 0, 0)) box_c = chrono.ChBoxShape() box_c.GetBoxGeometry().Size = chrono.ChVectorD(0.95, 0.05, 0.05) crank.AddAsset(box_c) cyl_c = chrono.ChCylinderShape() cyl_c.GetCylinderGeometry().p1 = chrono.ChVectorD(1, 0.1, 0) cyl_c.GetCylinderGeometry().p2 = chrono.ChVectorD(1, -0.1, 0) cyl_c.GetCylinderGeometry().rad = 0.05 crank.AddAsset(cyl_c) sph_c = chrono.ChSphereShape() sph_c.GetSphereGeometry().center = chrono.ChVectorD(-1, 0, 0) sph_c.GetSphereGeometry().rad = 0.05 crank.AddAsset(sph_c)
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
chrono.GetLog().Bar() # Test vectors my_vect1 = chrono.ChVectorD() my_vect1.x = 5 my_vect1.y = 2 my_vect1.z = 3 my_vect2 = chrono.ChVectorD(3, 4, 5) my_vect4 = my_vect1 * 10 + my_vect2 my_len = my_vect4.Length() print('vect sum =', my_vect1 + my_vect2) print('vect cross =', my_vect1 % my_vect2) print('vect dot =', my_vect1 ^ my_vect2) # Test quaternions my_quat = chrono.ChQuaternionD(1, 2, 3, 4) my_qconjugate = ~my_quat print('quat. conjugate =', my_qconjugate) print('quat. dot product=', my_qconjugate ^ my_quat) print('quat. product=', my_qconjugate % my_quat) # Test matrices and NumPy interoperability mlist = [[1, 2, 3, 4], [5, 6, 7, 8], [9, 10, 11, 12], [13, 14, 15, 16]] ma = chrono.ChMatrixDynamicD() ma.SetMatr( mlist) # Create a Matrix from a list. Size is adjusted automatically. 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
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): 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()
# In case you need also damping it would add... #mdamping = chrono_types::make_shared<ChDampingReissnerRayleigh>(melasticity,0.01) #mat = chrono_types::make_shared<ChMaterialShellReissner>(melasticity, nullptr, mdamping) mat.SetDensity(rho) # Create the nodes nels_L = 12 nels_W = 1 elarray = [fea.ChElementShellReissner4]*(nels_L * nels_W) nodearray = [fea.ChNodeFEAxyzrot]*((nels_L + 1) * (nels_W + 1)) nodes_start = [fea.ChNodeFEAxyzrot]*(nels_W + 1) nodes_end = [fea.ChNodeFEAxyzrot]*(nels_W + 1) for il in range(nels_L+1) : for iw in range(nels_W +1): # Make nodes nodepos = chrono.ChVectorD(rect_L * (il / nels_L), 0, rect_W * (iw / nels_W)) noderot = chrono.ChQuaternionD(chrono.QUNIT) nodeframe = chrono.ChFrameD(nodepos, noderot) mnode = fea.ChNodeFEAxyzrot(nodeframe) my_mesh.AddNode(mnode) for i in range(3): mnode.GetInertia()[i,i] = 0 # approx] mnode.SetMass(0) nodearray[il * (nels_W + 1) + iw] = mnode if (il == 0): nodes_start[iw] = mnode if (il == nels_L): nodes_end[iw] = mnode # Make elements if (il > 0 and iw > 0) : melement = fea.ChElementShellReissner4() my_mesh.AddElement(melement)
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") 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 __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]
# | |___| | | | | | (_) | | | | (_) | # \____|_| |_|_| \___/|_| |_|\___/ # Chrono g = np.array(opts.gravity) # System system = fsi.ProtChSystem() system.ChSystem.Set_G_acc(pychrono.ChVectorD(g[0], g[1], g[2])) system.setTimeStep(1e-5) # Body body = fsi.ProtChBody(system=system) body.setBoundaryFlags([0]) # index of particle # chbod = body.ChBody pos = pychrono.ChVectorD(0.5*tank_x, 0.7*tank_y, 0.) rot = pychrono.ChQuaternionD(1., 0., 0., 0.) mass = 2000.0#3.14*rho_0*1.5 inertia = pychrono.ChVectorD(1., 1., 1.) chbod.SetPos(pos) chbod.SetRot(rot) chbod.SetMass(mass) chbod.SetInertiaXX(inertia) # chbod.SetBodyFixed(True) body.setConstraints(free_x=np.array([0., 1., 0.]), free_r=np.array([0., 0., 0.])) body.setRecordValues(all_values=True) def sdf(t, x): dist = np.sqrt((x[0]**2+x[1]**2+x[2]**2)) if dist < radius: return -(radius-dist), (0., -1., 0.) else:
# Spin in place for real time to catch up realtime_timer.Spin(step_size) return 0 # The path to the Chrono data directory containing various assets (meshes, textures, data files) # is automatically set, relative to the default location of this demo. # If running from a different directory, you must change the path to the data directory with: #chrono.SetChronoDataPath('path/to/data') veh.SetDataPath(chrono.GetChronoDataPath() + 'vehicle/') # Initial vehicle location and orientation initLoc = chrono.ChVectorD(0, 0, 1.6) initRot = chrono.ChQuaternionD(1, 0, 0, 0) # Visualization type for vehicle parts (PRIMITIVES, MESH, or NONE) chassis_vis_type = veh.VisualizationType_MESH suspension_vis_type = veh.VisualizationType_PRIMITIVES steering_vis_type = veh.VisualizationType_PRIMITIVES wheel_vis_type = veh.VisualizationType_MESH tire_vis_type = veh.VisualizationType_MESH # Collision type for chassis (PRIMITIVES, MESH, or NONE) chassis_collision_type = veh.ChassisCollisionType_NONE # Type of powertrain model (SHAFTS, SIMPLE) powertrain_model = veh.PowertrainModelType_SHAFTS # Drive type (FWD, RWD, or AWD)
print('----------') # Iterate over added bodies (Python style) print('Positions of all bodies in the system:') for abody in my_system.Get_bodylist(): print(' ', abody.GetName(), ' pos =', abody.GetPos()) # Use a body with an auxiliary reference (REF) that does not correspond to the center of gravity (COG) bodyC = chrono.ChBodyAuxRef() my_system.AddBody(bodyC) bodyC.SetName('Parte1-1') bodyC.SetPos( chrono.ChVectorD(-0.0445347481124079, 0.0676266363930238, -0.0230808979433518)) 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