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 render(self, mode='human'): if not (self.play_mode == True): raise Exception('Please set play_mode=True to render') if not self.render_setup: if False: vis_camera = sens.ChCameraSensor( self.chassis_body, # body camera is attached to 30, # scanning rate in Hz chrono.ChFrameD( chrono.ChVectorD(0, 0, 25), chrono.Q_from_AngAxis(chrono.CH_C_PI / 2, chrono.ChVectorD(0, 1, 0))), # offset pose 1280, # number of horizontal samples 720, # number of vertical channels chrono.CH_C_PI / 3, # horizontal field of view (720 / 1280) * chrono.CH_C_PI / 3. # vertical field of view ) vis_camera.SetName("Birds Eye Camera Sensor") # self.camera.FilterList().append(sens.ChFilterVisualize(self.camera_width, self.camera_height, "RGB Camera")) # vis_camera.FilterList().append(sens.ChFilterVisualize(1280, 720, "Visualization Camera")) if False: self.camera.FilterList().append(sens.ChFilterSave()) self.manager.AddSensor(vis_camera) if True: vis_camera = sens.ChCameraSensor( self.chassis_body, # body camera is attached to 30, # scanning rate in Hz chrono.ChFrameD( chrono.ChVectorD(-8, 0, 3), chrono.Q_from_AngAxis(chrono.CH_C_PI / 20, chrono.ChVectorD(0, 1, 0))), # offset pose 1280, # number of horizontal samples 720, # number of vertical channels chrono.CH_C_PI / 3, # horizontal field of view (720 / 1280) * chrono.CH_C_PI / 3. # vertical field of view ) vis_camera.SetName("Follow Camera Sensor") # self.camera.FilterList().append(sens.ChFilterVisualize(self.camera_width, self.camera_height, "RGB Camera")) # vis_camera.FilterList().append(sens.ChFilterVisualize(1280, 720, "Visualization Camera")) if True: vis_camera.FilterList().append(sens.ChFilterSave()) # self.camera.FilterList().append(sens.ChFilterSave()) self.manager.AddSensor(vis_camera) # ----------------------------------------------------------------- # Create a filter graph for post-processing the data from the lidar # ----------------------------------------------------------------- # self.camera.FilterList().append(sens.ChFilterVisualize("RGB Camera")) # vis_camera.FilterList().append(sens.ChFilterVisualize("Visualization Camera")) self.render_setup = True if (mode == 'rgb_array'): return self.get_ob()
def render(self, mode='human'): if not (self.play_mode == True): raise Exception('Please set play_mode=True to render') if not self.render_setup: vis_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 GenerateFrame(self, pos, ang, scale): # Calculate quaternion rot = chrono.Q_from_AngAxis(ang, chrono.ChVectorD(0, 0, 1)) # Generate ChFrame which will then be scaled frame = chrono.ChFrameD(pos, rot) # Scale frame mat = frame.GetA().GetMatr() mat = [[x * scale for x in z] for z in mat] frame.GetA().SetMatr(mat) return frame
def __init__(self, system: 'WAChronoSystem', vehicle: 'WAChronoVehicle', vehicle_inputs: 'WAVehicleInputs', environment: 'WAEnvironment' = None, opponents: list = [], record: bool = False, record_folder: str = "OUTPUT/"): self._render_steps = int( ceil(system.render_step_size / system.step_size)) self._system = system self._vehicle_inputs = vehicle_inputs self._record = record self._record_folder = record_folder self._manager = WAChronoSensorManager(system) self._manager._manager.scene.AddPointLight( chrono.ChVectorF(0, 0, 100), chrono.ChVectorF(2, 2, 2), 5000) update_rate = 30. offset_pose = chrono.ChFrameD(chrono.ChVectorD(-8, 0, 2)) image_width = 1280 image_height = 720 fov = 1.408 cam = sens.ChCameraSensor( vehicle._vehicle.GetChassisBody( ), # body camera is attached to update_rate, # update rate in Hz offset_pose, # offset pose image_width, # image width image_height, # image height fov # camera's horizontal field of view ) cam.PushFilter(sens.ChFilterVisualize(image_width, image_height)) if self._record: cam.PushFilter(sens.ChFilterSave(self._record_folder)) self._manager._manager.AddSensor(cam) if environment is not None: self.visualize(environment.get_assets())
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]))
import pychrono as chrono import pychrono.fea as fea import copy print("Copyright (c) 2017 projectchrono.org ") # Create the physical system my_system = chrono.ChSystemSMC() # Create a mesh: my_mesh = fea.ChMesh() my_system.Add(my_mesh) # Create some nodes. mnodeA = fea.ChNodeFEAxyzrot(chrono.ChFrameD(chrono.ChVectorD(0, 0, 0))) mnodeB = fea.ChNodeFEAxyzrot(chrono.ChFrameD(chrono.ChVectorD(2, 0, 0))) # Default mass for FEM nodes is zero mnodeA.SetMass(0.0) mnodeB.SetMass(0.0) my_mesh.AddNode(mnodeA) my_mesh.AddNode(mnodeB) # Create beam section & material msection = fea.ChBeamSectionEulerAdvanced() beam_wy = 0.1 beam_wz = 0.2 msection.SetAsRectangularSection(beam_wy, beam_wz) msection.SetYoungModulus(0.01e9)
mphysicalSystem.Add(mbody_gearA) mbody_gearA.SetPos(chrono.ChVectorD(0, 0, -1)) mbody_gearA.SetRot(chrono.Q_from_AngX(m.pi / 2)) mbody_gearA.AddAsset(cylinder_texture) # for aesthetic reasons, also add a thin cylinder only as a visualization mshaft_shape = chrono.ChCylinderShape() mshaft_shape.GetCylinderGeometry().p1 = chrono.ChVectorD(0, -3, 0) mshaft_shape.GetCylinderGeometry().p2 = chrono.ChVectorD(0, 10, 0) mshaft_shape.GetCylinderGeometry().rad = radA * 0.4 mbody_gearA.AddAsset(mshaft_shape) # ...impose rotation speed between the first gear and the fixed truss link_motor = chrono.ChLinkMotorRotationSpeed() link_motor.Initialize(mbody_gearA, mbody_truss, chrono.ChFrameD(chrono.ChVectorD(0, 0, 0), chrono.QUNIT)) link_motor.SetSpeedFunction(chrono.ChFunction_Const(6)) mphysicalSystem.AddLink(link_motor) # ...the second gear interaxis12 = radA + radB mbody_gearB = chrono.ChBodyEasyCylinder(radB, 0.4, 1000, True, False, mat) mphysicalSystem.Add(mbody_gearB) mbody_gearB.SetPos(chrono.ChVectorD(interaxis12, 0, -1)) mbody_gearB.SetRot(chrono.Q_from_AngX(m.pi / 2)) mbody_gearB.AddAsset(cylinder_texture) # ...the second gear is fixed to the rotating bar link_revolute = chrono.ChLinkLockRevolute() link_revolute.Initialize( mbody_gearB, mbody_train,
def reset(self): self.isdone = False self.hexapod_sys.Clear() self.exported_items = chrono.ImportSolidWorksSystem(self.fpath) self.csys = [] self.frames = [] self.revs = [] self.motors = [] self.limits = [] for con, coi in zip(self.con_link, self.coi_link): indices = [] for i in range(len(self.exported_items)): if con == self.exported_items[i].GetName( ) or coi == self.exported_items[i].GetName(): indices.append(i) rev = self.exported_items[indices[0]] af0 = rev.GetAssetsFrame() # Revolute joints and ChLinkMotorRotation are z oriented, while parallel is x oriented. # Event though this Frame won't be used anymore is good practice to create a copy before editing its value. af = chrono.ChFrameD(af0) af.SetRot(af0.GetRot() % chrono.Q_ROTATE_X_TO_Z) self.frames.append(af) for i in reversed(indices): del self.exported_items[i] # ADD IMPORTED ITEMS TO THE SYSTEM for my_item in self.exported_items: self.hexapod_sys.Add(my_item) """ $$$$$$$$ FIND THE SW DEFINED CONSTRAINTS, GET THEIR MARKERS AND GET RID OF EM $$$$$$$$ """ self.hips = [ self.hexapod_sys.SearchBody(name) for name in self.hip_names ] self.femurs = [ self.hexapod_sys.SearchBody(name) for name in self.femur_names ] self.tibias = [ self.hexapod_sys.SearchBody(name) for name in self.tibia_names ] self.feet = [ self.hexapod_sys.SearchBody(name) for name in self.feet_names ] self.centralbody = self.hexapod_sys.SearchBody('Body-1') # Bodies are used to replace constraints and detect unwanted collision, so feet are excluded self.bodies = [self.centralbody ] + self.hips + self.femurs + self.tibias self.centralbody.SetBodyFixed(False) self.y0 = self.centralbody.GetPos().y """ # SNIPPET FOR COLOR orange = chrono.ChColorAsset() orange.SetColor(chrono.ChColor(255/255,77/255,6/255)) black = chrono.ChColorAsset() black.SetColor(chrono.ChColor(0,0,0)) for body in self.bodies[:-1]: assets = body.GetAssets() for ast in assets: ass_lev = chrono.CastToChAssetLevel(ast) ass_lev.GetAssets().push_back(orange) assets = self.hand.GetAssets() for ast in assets: ass_lev = chrono.CastToChAssetLevel(ast) ass_lev.GetAssets().push_back(black) """ for i in range(len(self.con_link)): revolute = chrono.ChLinkLockRevolute() cs = chrono.ChCoordsysD(self.frames[i].GetPos(), self.frames[i].GetRot()) self.csys.append(cs) if i < 6: j = 0 else: j = i - 5 revolute.Initialize(self.bodies[j], self.bodies[i + 1], self.csys[i]) self.revs.append(revolute) self.hexapod_sys.Add(self.revs[i]) lim = self.revs[i].GetLimit_Rz() self.limits.append(lim) self.limits[i].SetActive(True) self.limits[i].SetMin(self.minRot[i] * (math.pi / 180)) self.limits[i].SetMax(self.maxRot[i] * (math.pi / 180)) m = chrono.ChLinkMotorRotationTorque() m.SetSpindleConstraint(False, False, False, False, False) m.Initialize(self.bodies[j], self.bodies[i + 1], self.frames[i]) self.motors.append(m) self.hexapod_sys.Add(self.motors[i]) self.body_floor = chrono.ChBody() self.body_floor.SetBodyFixed(True) self.body_floor.SetPos(chrono.ChVectorD(0, -1 - 0.128 - 0.0045, 10)) # Floor Collision. self.body_floor.GetCollisionModel().ClearModel() self.body_floor.GetCollisionModel().AddBox(self.my_material, 50, 1, 50, chrono.ChVectorD(0, 0, 0)) self.body_floor.GetCollisionModel().BuildModel() self.body_floor.SetCollide(True) # Visualization shape body_floor_shape = chrono.ChBoxShape() body_floor_shape.GetBoxGeometry().Size = chrono.ChVectorD(4, 1, 15) body_floor_shape.SetColor(chrono.ChColor(0.4, 0.4, 0.5)) self.body_floor.GetAssets().push_back(body_floor_shape) body_floor_texture = chrono.ChTexture() texpath = os.path.join(chrono.GetChronoDataPath(), 'concrete.jpg') body_floor_texture.SetTextureFilename(texpath) self.body_floor.GetAssets().push_back(body_floor_texture) self.hexapod_sys.Add(self.body_floor) self.numsteps = 0 if (self.render_setup): self.myapplication.AssetBindAll() self.myapplication.AssetUpdateAll() return self.get_ob()
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 mb.SetMatr(v.tolist()) # create a ChMatrixDynamicD from the numpy eigenvectors mr = chrono.ChMatrix33D() mr.SetMatr([[1, 2, 3], [4, 5, 6], [7, 8, 9]]) print(mr * my_vect1) # Test frames - # create a frame representing a translation and a rotation # of 20 degrees on X axis my_frame = chrono.ChFrameD( my_vect2, chrono.Q_from_AngAxis(20 * chrono.CH_C_DEG_TO_RAD, chrono.ChVectorD(1, 0, 0))) my_vect5 = my_vect1 >> my_frame # Print the class hierarchy of a chrono class import inspect inspect.getmro(chrono.ChStreamOutAsciiFile) # Use the ChFunction classes my_funct = chrono.ChFunction_Sine(0, 0.5, 3) print('function f(0.2)=', my_funct.Get_y(0.2)) # Inherit from the ChFunction, from the Python side, # (do not forget the __init__ constructor)
#### Replace the revolute joint between ground and crank with a #### ChLinkMotorRotationSpeed element and enforce constant angular speed of #### 90 degrees/s. #### ------------------------------------------------------------------------- # Create a ChFunction object that always returns the constant value PI/2. fun = chrono.ChFunction_Const() fun.Set_yconst(chrono.CH_C_PI) # Motor between ground and crank. # Note that this also acts as a revolute joint (i.e. it enforces the same # kinematic constraints as a revolute joint). As before, we apply the 'z2y' # rotation to align the rotation axis with the Y axis of the global frame. engine_ground_crank = chrono.ChLinkMotorRotationSpeed() engine_ground_crank.SetName("engine_ground_crank") engine_ground_crank.Initialize(ground, crank, chrono.ChFrameD(chrono.ChVectorD(0, 0, 0), z2y)) engine_ground_crank.SetSpeedFunction(fun) system.AddLink(engine_ground_crank) ## Prismatic joint between ground and slider. ## The translational axis of a prismatic joint is along the Z axis of the ## specified joint coordinate system. Here, we apply the 'z2x' rotation to ## align it with the X axis of the global reference frame. prismatic_ground_slider = chrono.ChLinkLockPrismatic() prismatic_ground_slider.SetName("prismatic_ground_slider") prismatic_ground_slider.Initialize(ground, slider, chrono.ChCoordsysD(chrono.ChVectorD(2, 0, 0), z2x)) system.AddLink(prismatic_ground_slider)
# 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() body_1_1_shape.SetFilename(shapes_dir + 'body_1_1.obj') body_1_1_level = chrono.ChAssetLevel() body_1_1_level.GetFrame().SetPos(chrono.ChVectorD(0, 0, 0)) body_1_1_level.GetFrame().SetRot(chrono.ChQuaternionD(1, 0, 0, 0)) body_1_1_level.GetAssets().push_back(body_1_1_shape) body_1.GetAssets().push_back(body_1_1_level) # Collision shape(s) body_1.GetCollisionModel().ClearModel() mr = chrono.ChMatrix33D() mr[0, 0] = 0
def __init__(self, sys, gravity, material, width, height, position_base, position_tip, damping, elements, torque=10, origin=True, stator_constraint=None): self.mesh = fea.ChMesh() self.mesh.SetAutomaticGravity(gravity) self.section = fea.ChBeamSectionAdvanced() self.section.SetAsRectangularSection(width, height) self.section.SetYoungModulus(material.modulus) self.section.SetGshearModulus(material.shear) self.section.SetDensity(material.density) self.section.SetBeamRaleyghDamping(damping) self.position_base = chrono.ChVectorD(*position_base) self.position_tip = chrono.ChVectorD(*position_tip) self.builder = fea.ChBuilderBeamEuler( ) #Use the beam builder assembly method to assembly each beam self.builder.BuildBeam( self.mesh, self.section, elements, self.position_base, self.position_tip, chrono.ChVectorD(0, 1, 0), ) self.stator = chrono.ChBodyEasyCylinder(0.01, 0.01, 1000) self.stator.SetPos(self.position_base) self.stator.SetBodyFixed(origin) self.frame = chrono.ChFrameD(self.stator) sys.Add(self.stator) if (origin == False) and ( stator_constraint is not None ): #If the beam is not located at the origin, it must need to be constrained to another beam self.constraint = chrono.ChLinkMateGeneric() self.constraint.Initialize(self.stator, stator_constraint, False, chrono.ChFrameD(self.stator), chrono.ChFrameD(stator_constraint)) self.constraint.SetConstrainedCoords(True, True, True, True, True, True) sys.Add(self.constraint) self.frame = chrono.ChFrameD(self.stator) self.rotor = chrono.ChBodyEasyCylinder(0.011, 0.011, 1000) self.rotor.SetPos(self.position_base) sys.Add(self.rotor) self.frame.SetRot( chrono.Q_from_AngAxis(chrono.CH_C_PI_2, chrono.VECT_X) ) #Rotate the direction of rotation to be planar (x-z plane) self.motor = chrono.ChLinkMotorRotationTorque() self.motor.Initialize(self.rotor, self.stator, self.frame) sys.Add(self.motor) self.motor.SetTorqueFunction(chrono.ChFunction_Const(torque)) self.arm_base = (self.builder.GetLastBeamNodes().front()) self.arm_tip = (self.builder.GetLastBeamNodes().back()) self.mate = chrono.ChLinkMateGeneric() self.mate.Initialize(self.builder.GetLastBeamNodes().front(), self.rotor, chrono.ChFrameD(self.builder.GetLastBeamNodes( ).front().GetPos())) #constrain beam to rotor self.mate.SetConstrainedCoords( True, True, True, True, True, True ) #constraints must be in format: (True,True,True,True,True,True) to constrain x,y,z,rotx,roty,rotz coordinates sys.Add(self.mate) self.visual = fea.ChVisualizationFEAmesh(self.mesh) self.visual.SetFEMdataType( fea.ChVisualizationFEAmesh.E_PLOT_ELEM_BEAM_MZ) self.visual.SetColorscaleMinMax(-1.4, 1.4) self.visual.SetSmoothFaces(True) self.visual.SetWireframe(False) self.mesh.AddAsset(self.visual) sys.Add(self.mesh)
def reset(self): self.isdone = False self.robosystem.Clear() #action = (np.random.rand(6,)-0.5)*2 #torques = np.multiply(action, self.maxT) self.exported_items = chrono.ImportSolidWorksSystem(self.fpath) self.csys = [] self.frames = [] self.revs = [] self.motors = [] self.limits = [] for con, coi in zip(self.con_link, self.coi_link): indices = [] for i in range(len(self.exported_items)): if con == self.exported_items[i].GetName( ) or coi == self.exported_items[i].GetName(): indices.append(i) rev = self.exported_items[indices[0]] af0 = rev.GetAssetsFrame() # Revolute joints and ChLinkMotorRotation are z oriented, while parallel is x oriented. # Event though this Frame won't be used anymore is good practice to create a copy before editing its value. af = chrono.ChFrameD(af0) af.SetRot(af0.GetRot() % chrono.Q_ROTATE_X_TO_Z) self.frames.append(af) for i in indices: del self.exported_items[i] # ADD IMPORTED ITEMS TO THE SYSTEM for my_item in self.exported_items: self.robosystem.Add(my_item) """ $$$$$$$$ FIND THE SW DEFINED CONSTRAINTS, GET THEIR MARKERS AND GET RID OF EM $$$$$$$$ """ self.bodies = [ self.robosystem.SearchBody(name) for name in self.bodiesNames ] self.hand = self.robosystem.SearchBody('Hand_base_and_p07-2') self.base = self.robosystem.SearchBody('Racer3_p01-3') self.biceps = self.robosystem.SearchBody('Racer3_p03-1') self.forearm = self.robosystem.SearchBody('Racer3_p05-1') self.finger1 = self.robosystem.SearchBody('HAND_e_finger-1') self.finger2 = self.robosystem.SearchBody('HAND_e_finger-2') for i in range(len(self.con_link)): revolute = chrono.ChLinkLockRevolute() cs = chrono.ChCoordsysD(self.frames[i].GetPos(), self.frames[i].GetRot()) self.csys.append(cs) revolute.Initialize(self.bodies[i], self.bodies[i + 1], self.csys[i]) self.revs.append(revolute) self.robosystem.Add(self.revs[i]) lim = self.revs[i].GetLimit_Rz() self.limits.append(lim) self.limits[i].SetActive(True) self.limits[i].SetMin(self.minRot[i] * (math.pi / 180)) self.limits[i].SetMax(self.maxRot[i] * (math.pi / 180)) m = chrono.ChLinkMotorRotationTorque() m.Initialize(self.bodies[i], self.bodies[i + 1], self.frames[i]) #self.robosystem.Add(m2) #m2.SetTorqueFunction(chrono.ChFunction_Const(5)) self.motors.append(m) #self.motors[i].SetTorqueFunction(chrono.ChFunction_Const(float(torques[i]))) self.robosystem.Add(self.motors[i]) self.body_floor = chrono.ChBody() self.body_floor.SetBodyFixed(True) self.body_floor.SetPos(chrono.ChVectorD(0, -1, 0)) # Floor Collision. self.body_floor.GetCollisionModel().ClearModel() self.body_floor.GetCollisionModel().AddBox(self.my_material, 5, 1, 5, chrono.ChVectorD(0, 0, 0)) self.body_floor.GetCollisionModel().BuildModel() self.body_floor.SetCollide(True) # Visualization shape body_floor_shape = chrono.ChBoxShape() body_floor_shape.GetBoxGeometry().Size = chrono.ChVectorD(5, 1, 5) body_floor_shape.SetColor(chrono.ChColor(0.4, 0.4, 0.5)) self.body_floor.GetAssets().push_back(body_floor_shape) body_floor_texture = chrono.ChTexture() texpath = os.path.join(chrono.GetChronoDataPath(), 'concrete.jpg') body_floor_texture.SetTextureFilename(texpath) self.body_floor.GetAssets().push_back(body_floor_texture) self.robosystem.Add(self.body_floor) r = np.random.rand(2, ) - np.asarray([0.5, 0.5]) self.targ_init_pos = [ -0.52 + 2 * (r[0] * 0.05), 0.015, 2 * r[1] * 0.05 ] self.targ_box = chrono.ChBody() # UNset to grasp self.targ_box.SetBodyFixed(True) self.targ_box.SetPos( chrono.ChVectorD(self.targ_init_pos[0], self.targ_init_pos[1], self.targ_init_pos[2])) # Floor Collision. self.targ_box.GetCollisionModel().ClearModel() self.targ_box.GetCollisionModel().AddBox(self.my_material, 0.015, 0.015, 0.015, chrono.ChVectorD(0, 0, 0)) self.targ_box.GetCollisionModel().BuildModel() self.targ_box.SetCollide(True) # Visualization shape targ_box_shape = chrono.ChBoxShape() targ_box_shape.GetBoxGeometry().Size = chrono.ChVectorD( 0.015, 0.015, 0.015) col = chrono.ChColorAsset() col.SetColor(chrono.ChColor(1.0, 0, 0)) self.targ_box.GetAssets().push_back(targ_box_shape) self.targ_box.GetAssets().push_back(col) self.robosystem.Add(self.targ_box) self.numsteps = 0 if (self.render_setup): self.myapplication.AssetBindAll() self.myapplication.AssetUpdateAll() return self.get_ob()
beam_wy = 0.012 beam_wz = 0.025 msection.SetAsRectangularSection(beam_wy, beam_wz) msection.SetYoungModulus(0.01e9) msection.SetGshearModulus(0.01e9 * 0.3) msection.SetBeamRaleyghDamping(0.000) #msection.SetCentroid(0,0.02) #msection.SetShearCenter(0,0.1) #msection.SetSectionRotation(45*chrono.CH_C_RAD_TO_DEG) # Add some EULER-BERNOULLI BEAMS: beam_L = 0.1 hnode1 = fea.ChNodeFEAxyzrot(chrono.ChFrameD(chrono.ChVectorD(0, 0, 0))) hnode2 = fea.ChNodeFEAxyzrot(chrono.ChFrameD(chrono.ChVectorD(beam_L, 0, 0))) hnode3 = fea.ChNodeFEAxyzrot(chrono.ChFrameD(chrono.ChVectorD(beam_L * 2, 0, 0))) my_mesh.AddNode(hnode1) my_mesh.AddNode(hnode2) my_mesh.AddNode(hnode3) belement1 = fea.ChElementBeamEuler() belement1.SetNodes(hnode1, hnode2) belement1.SetSection(msection) my_mesh.AddElement(belement1) belement2 = fea.ChElementBeamEuler()
#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) melement.SetNodes(nodearray[(il - 1) * (nels_W + 1) + (iw - 1)],
body_1.SetName('HAND_e_finger-1') body_1.SetPos( chrono.ChVectorD(-0.52964000188, 0.684998394063328, 1.11022302462516e-16)) body_1.SetRot( chrono.ChQuaternionD(0.707106781186546, 1.43973102426035e-15, -1.03316459816659e-15, 0.707106781186549)) body_1.SetMass(0.0938874355859725) body_1.SetInertiaXX( chrono.ChVectorD(1.89819866463215e-05, 4.06468272765221e-05, 3.48488688224795e-05)) body_1.SetInertiaXY( chrono.ChVectorD(9.26966996175154e-06, -9.45479551365122e-06, 4.9223535683921e-06)) body_1.SetFrame_COG_to_REF( chrono.ChFrameD( chrono.ChVectorD(-0.00724869989644222, 0.0206425177285978, 0.0224205323500859), chrono.ChQuaternionD(1, 0, 0, 0))) # Visualization shape body_1_1_shape = chrono.ChObjShapeFile() body_1_1_shape.SetFilename(shapes_dir + 'body_1_1.obj') body_1_1_level = chrono.ChAssetLevel() body_1_1_level.GetFrame().SetPos(chrono.ChVectorD(0, 0, 0)) body_1_1_level.GetFrame().SetRot(chrono.ChQuaternionD(1, 0, 0, 0)) body_1_1_level.GetAssets().push_back(body_1_1_shape) body_1.GetAssets().push_back(body_1_1_level) # Collision shapes body_1.GetCollisionModel().ClearModel() mr = chrono.ChMatrix33D() mr[0, 0] = 1
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) # una volta che le hao calcolate inserisci inerzie diverse 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 # vettori visual cilindro self.cyl_base1 = chrono.ChVectorD(0, -self.size_rod_y / 2, 0) self.cyl_base2 = chrono.ChVectorD(0, self.size_rod_y / 2, 0) #body_rod_shape = chrono.ChCylinder(cyl_base1, cyl_base2, radius_rod) 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 #body_rod.GetAssets().push_back(body_rod_shape) 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) # Visualization shape 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) # Visualization shape 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) # Create a constraint that blocks free 3 x y z translations and 3 rx ry rz rotations # of the table respect to the floor, and impose that the relative imposed position # depends on a specified motion law. 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.link_slider.SetMotion_axis(chrono.ChVectorD(1, 0, 0)) #attuatore lineare 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) # ..create the function for imposed y vertical motion, etc. # tolta: solo traslazione asse z #mfunY = chrono.ChFunction_Sine(0,1.5,0.001) # phase, frequency, amplitude #link_slider.SetMotion_Y(mfunY) # ..create the function for imposed z horizontal motion, etc. #self.mfunX = chrono.ChFunction_Sine(0,1.5,0.4) # phase, frequency, amplitude #self.link_slider.SetMotion_X(self.action) # Note that you could use other types of ChFunction_ objects, or create # your custom function by class inheritance (see demo_python.py), or also # set a function for table rotation , etc. # REVLOLUTE JOINT: # create frames for the joint 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() # If you want to show shadows because you used "AddLightWithShadow()' # you must remember this: self.isdone = False self.steps = 0 self.step(np.array([[0]])) return self.get_ob()
def reset(self): self.generate_track() self.vehicle = veh.Sedan() self.vehicle.SetContactMethod(chrono.ChMaterialSurface.NSC) self.vehicle.SetChassisCollisionType(veh.ChassisCollisionType_NONE) self.vehicle.SetChassisFixed(False) self.vehicle.SetInitPosition( chrono.ChCoordsysD(self.initLoc, self.initRot)) self.vehicle.SetTireType(veh.TireModelType_RIGID) self.vehicle.SetTireStepSize(self.timestep) self.vehicle.Initialize() self.vehicle.SetChassisVisualizationType( veh.VisualizationType_PRIMITIVES) self.vehicle.SetWheelVisualizationType( veh.VisualizationType_PRIMITIVES) self.vehicle.SetSuspensionVisualizationType( veh.VisualizationType_PRIMITIVES) self.vehicle.SetSteeringVisualizationType( veh.VisualizationType_PRIMITIVES) self.vehicle.SetTireVisualizationType(veh.VisualizationType_PRIMITIVES) self.system = self.vehicle.GetSystem() self.chassis_body = self.vehicle.GetChassisBody() # Create contact model self.chassis_body.GetCollisionModel().ClearModel() size = chrono.ChVectorD(3, 2, 0.2) self.chassis_body.GetCollisionModel().AddBox(0.5 * size.x, 0.5 * size.y, 0.5 * size.z) self.chassis_body.GetCollisionModel().BuildModel() # Driver self.driver = Driver(self.vehicle.GetVehicle()) # Throttle controller #self.throttle_controller = PIDThrottleController() #self.throttle_controller.SetGains(Kp=0.4, Ki=0, Kd=0) #self.throttle_controller.SetTargetSpeed(speed=self.target_speed) # Rigid terrain self.terrain = veh.RigidTerrain(self.system) patch = self.terrain.AddPatch( chrono.ChCoordsysD(chrono.ChVectorD(0, 0, self.terrainHeight - 5), chrono.QUNIT), chrono.ChVectorD(self.terrainLength, self.terrainWidth, 10)) patch.SetContactFrictionCoefficient(0.9) patch.SetContactRestitutionCoefficient(0.01) patch.SetContactMaterialProperties(2e7, 0.3) patch.SetTexture(veh.GetDataFile("terrain/textures/tile4.jpg"), 200, 200) patch.SetColor(chrono.ChColor(0.8, 0.8, 0.5)) self.terrain.Initialize() ground_body = patch.GetGroundBody() ground_asset = ground_body.GetAssets()[0] visual_asset = chrono.CastToChVisualization(ground_asset) vis_mat = chrono.ChVisualMaterial() vis_mat.SetKdTexture(chrono.GetChronoDataFile("concrete.jpg")) visual_asset.material_list.append(vis_mat) # create barriers self.barriers = [] self.DrawBarriers(self.track.left.points) self.DrawBarriers(self.track.right.points) # Set the time response for steering and throttle inputs. # NOTE: this is not exact, since we do not render quite at the specified FPS. steering_time = 0.5 # time to go from 0 to +1 (or from 0 to -1) throttle_time = 0.5 # time to go from 0 to +1 braking_time = 0.3 # time to go from 0 to +1 self.driver.SetSteeringDelta(self.timestep / steering_time) self.driver.SetThrottleDelta(self.timestep / throttle_time) self.driver.SetBrakingDelta(self.timestep / braking_time) self.manager = sens.ChSensorManager(self.system) self.manager.scene.AddPointLight(chrono.ChVectorF(100, 100, 100), chrono.ChVectorF(1, 1, 1), 500.0) self.manager.scene.AddPointLight(chrono.ChVectorF(-100, -100, 100), chrono.ChVectorF(1, 1, 1), 500.0) # ----------------------------------------------------- # Create a self.camera and add it to the sensor manager # ----------------------------------------------------- self.camera = sens.ChCameraSensor( self.chassis_body, # body camera is attached to 50, # scanning rate in Hz chrono.ChFrameD( chrono.ChVectorD(1.5, 0, .875), chrono.Q_from_AngAxis(0, chrono.ChVectorD(0, 1, 0))), # offset pose self.camera_width, # number of horizontal samples self.camera_height, # number of vertical channels chrono.CH_C_PI / 2, # horizontal field of view (self.camera_height / self.camera_width) * chrono.CH_C_PI / 3. # vertical field of view ) self.camera.SetName("Camera Sensor") self.camera.FilterList().append(sens.ChFilterRGBA8Access()) self.manager.AddSensor(self.camera) self.old_dist = self.track.center.calcDistance( self.chassis_body.GetPos()) self.step_number = 0 self.c_f = 0 self.isdone = False self.render_setup = False if self.play_mode: self.render() return self.get_ob()
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()
z2x = chrono.ChQuaternionD() z2y.Q_from_AngAxis(-chrono.CH_C_PI / 2, chrono.ChVectorD(1, 0, 0)) z2x.Q_from_AngAxis(chrono.CH_C_PI / 2, chrono.ChVectorD(0, 1, 0)) ## Create a ChFunction object that always returns the constant value PI/2. fun = chrono.ChFunction_Const() fun.Set_yconst(chrono.CH_C_PI) ## Motor between ground and crank. ## Note that this also acts as a revolute joint (i.e. it enforces the same ## kinematic constraints as a revolute joint). As before, we apply the 'z2y' ## rotation to align the rotation axis with the Y axis of the global frame. engine_ground_crank = chrono.ChLinkMotorRotationSpeed() engine_ground_crank.SetName("engine_ground_crank") engine_ground_crank.Initialize(ground, crank, chrono.ChFrameD(chrono.ChVectorD(0, 0, 0), z2y)) engine_ground_crank.SetSpeedFunction(fun) system.AddLink(engine_ground_crank) ## Prismatic joint between ground and slider. ## The translational axis of a prismatic joint is along the Z axis of the ## specified joint coordinate system. Here, we apply the 'z2x' rotation to ## align it with the X axis of the global reference frame. prismatic_ground_slider = chrono.ChLinkLockPrismatic() prismatic_ground_slider.SetName("prismatic_ground_slider") prismatic_ground_slider.Initialize( ground, slider, chrono.ChCoordsysD(chrono.ChVectorD(2, 0, 0), z2x)) system.AddLink(prismatic_ground_slider) ## Spherical joint between crank and rod spherical_crank_rod = chrono.ChLinkLockSpherical()
def reset(self): del self.manager material = chrono.ChMaterialSurfaceNSC() self.vehicle = veh.HMMWV_Reduced() self.vehicle.SetContactMethod(chrono.ChContactMethod_NSC) 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() #self.vehicle.SetStepsize(self.timestep) if self.play_mode == True: self.vehicle.SetChassisVisualizationType( veh.VisualizationType_MESH) self.vehicle.SetWheelVisualizationType(veh.VisualizationType_MESH) self.vehicle.SetTireVisualizationType(veh.VisualizationType_MESH) else: self.vehicle.SetChassisVisualizationType( veh.VisualizationType_PRIMITIVES) self.vehicle.SetWheelVisualizationType( veh.VisualizationType_PRIMITIVES) self.vehicle.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(material, 0.5 * size.x, 0.5 * size.y, 0.5 * size.z) self.chassis_body.GetCollisionModel().BuildModel() # Driver self.driver = veh.ChDriver(self.vehicle.GetVehicle()) # Rigid terrain self.system = self.vehicle.GetSystem() self.terrain = veh.RigidTerrain(self.system) patch = self.terrain.AddPatch( material, chrono.ChVectorD(0, 0, self.terrainHeight - .5), chrono.VECT_Z, self.terrainLength, self.terrainWidth, 10) material.SetFriction(0.9) material.SetRestitution(0.01) #patch.SetContactMaterialProperties(2e7, 0.3) patch.SetTexture(veh.GetDataFile("terrain/textures/tile4.jpg"), 200, 200) patch.SetColor(chrono.ChColor(0.8, 0.8, 0.5)) self.terrain.Initialize() ground_body = patch.GetGroundBody() ground_asset = ground_body.GetAssets()[0] visual_asset = chrono.CastToChVisualization(ground_asset) vis_mat = chrono.ChVisualMaterial() vis_mat.SetKdTexture(chrono.GetChronoDataFile("concrete.jpg")) visual_asset.material_list.append(vis_mat) # create obstacles self.boxes = [] for i in range(3): box = chrono.ChBodyEasyBox(2, 2, 10, 1000, True, True) box.SetPos( chrono.ChVectorD(25 + 25 * i, (np.random.rand(1)[0] - 0.5) * 10, .05)) 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)) vis_mat.SetDiffuseColor(chrono.ChVectorF(.2, .2, .9)) vis_mat.SetSpecularColor(chrono.ChVectorF(.9, .9, .9)) visual_asset.material_list.append(vis_mat) visual_asset.SetStatic(True) self.boxes.append(box) self.system.Add(box) # 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 = .5 # time to go from 0 to +1 braking_time = 0.3 # time to go from 0 to +1 self.SteeringDelta = (self.timestep / steering_time) self.ThrottleDelta = (self.timestep / throttle_time) self.BrakingDelta = (self.timestep / braking_time) self.manager = sens.ChSensorManager(self.system) self.manager.scene.AddPointLight(chrono.ChVectorF(100, 100, 100), chrono.ChVectorF(1, 1, 1), 4000.0) self.manager.scene.AddPointLight(chrono.ChVectorF(-100, -100, 100), chrono.ChVectorF(1, 1, 1), 4000.0) # ------------------------------------------------ # Create a self.camera and add it to the sensor manager # ------------------------------------------------ self.camera = sens.ChCameraSensor( self.chassis_body, # body camera is attached to 50, # scanning rate in Hz chrono.ChFrameD(chrono.ChVectorD(1.5, 0, .875)), # offset pose self.camera_width, # number of horizontal samples self.camera_height, # number of vertical channels chrono.CH_C_PI / 3, # horizontal field of view #(self.camera_height / self.camera_width) * chrono.CH_C_PI / 3. # vertical field of view ) self.camera.SetName("Camera Sensor") self.camera.PushFilter(sens.ChFilterRGBA8Access()) self.manager.AddSensor(self.camera) # ----------------------------------------------------------------- # Create a filter graph for post-processing the data from the lidar # ----------------------------------------------------------------- self.d_old = np.linalg.norm(self.Xtarg + self.Ytarg) self.step_number = 0 self.c_f = 0 self.isdone = False self.render_setup = False if self.play_mode: self.render() return self.get_ob()
# 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 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()
def reset(self): x_half_length = 90 y_half_length = 40 self.path = BezierPath(x_half_length, y_half_length, 0.5, self.interval) pos, rot = self.path.getPosRot(self.path.current_t - self.interval) self.initLoc = chrono.ChVectorD(pos) self.initRot = chrono.ChQuaternionD(rot) self.vehicle = veh.HMMWV_Reduced() self.vehicle.SetContactMethod(chrono.ChContactMethod_NSC) self.surf_material = chrono.ChMaterialSurfaceNSC() self.vehicle.SetChassisCollisionType( veh.ChassisCollisionType_PRIMITIVES) self.vehicle.SetChassisFixed(False) self.vehicle.SetInitPosition( chrono.ChCoordsysD(self.initLoc, self.initRot)) self.vehicle.SetPowertrainType(veh.PowertrainModelType_SHAFTS) self.vehicle.SetDriveType(veh.DrivelineType_AWD) # self.vehicle.SetSteeringType(veh.SteeringType_PITMAN_ARM) self.vehicle.SetTireType(veh.TireModelType_TMEASY) self.vehicle.SetTireStepSize(self.timestep) self.vehicle.Initialize() if self.play_mode == True: # self.vehicle.SetChassisVisualizationType(veh.VisualizationType_MESH) self.vehicle.SetChassisVisualizationType( veh.VisualizationType_PRIMITIVES) self.vehicle.SetWheelVisualizationType(veh.VisualizationType_MESH) self.vehicle.SetTireVisualizationType(veh.VisualizationType_MESH) else: self.vehicle.SetChassisVisualizationType( veh.VisualizationType_PRIMITIVES) self.vehicle.SetWheelVisualizationType( veh.VisualizationType_PRIMITIVES) self.vehicle.SetSuspensionVisualizationType( veh.VisualizationType_PRIMITIVES) self.vehicle.SetSteeringVisualizationType( veh.VisualizationType_PRIMITIVES) self.chassis_body = self.vehicle.GetChassisBody() self.chassis_body.GetCollisionModel().ClearModel() size = chrono.ChVectorD(3, 2, 0.2) self.chassis_body.GetCollisionModel().AddBox(self.surf_material, 0.5 * size.x, 0.5 * size.y, 0.5 * size.z) self.chassis_body.GetCollisionModel().BuildModel() self.m_inputs = veh.Inputs() self.system = self.vehicle.GetVehicle().GetSystem() self.manager = sens.ChSensorManager(self.system) self.manager.scene.AddPointLight(chrono.ChVectorF(100, 100, 100), chrono.ChVectorF(1, 1, 1), 5000.0) self.manager.scene.AddPointLight(chrono.ChVectorF(-100, -100, 100), chrono.ChVectorF(1, 1, 1), 5000.0) # Driver #self.driver = veh.ChDriver(self.vehicle.GetVehicle()) self.terrain = veh.RigidTerrain(self.system) patch_mat = chrono.ChMaterialSurfaceNSC() patch_mat.SetFriction(0.9) patch_mat.SetRestitution(0.01) patch = self.terrain.AddPatch(patch_mat, chrono.ChVectorD(0, 0, 0), chrono.ChVectorD(0, 0, 1), self.terrainLength * 1.5, self.terrainWidth * 1.5) patch.SetTexture(veh.GetDataFile("terrain/textures/grass.jpg"), 200, 200) patch.SetColor(chrono.ChColor(0.8, 0.8, 0.5)) self.terrain.Initialize() self.groundBody = patch.GetGroundBody() ground_asset = self.groundBody.GetAssets()[0] visual_asset = chrono.CastToChVisualization(ground_asset) vis_mat = chrono.ChVisualMaterial() vis_mat.SetKdTexture(veh.GetDataFile("terrain/textures/grass.jpg")) visual_asset.material_list.append(vis_mat) self.leaders.addLeaders(self.system, self.path) self.leader_box = self.leaders.getBBox() self.lead_pos = (self.chassis_body.GetPos() - self.leaders[0].GetPos()).Length() # Add obstacles: self.obstacles = [] self.placeObstacle(8) # for leader in self.leaders: # ------------------------------------------------ # Create a self.camera and add it to the sensor manager # ------------------------------------------------ self.camera = sens.ChCameraSensor( self.chassis_body, # body camera is attached to 10, # scanning rate in Hz chrono.ChFrameD(chrono.ChVectorD(1.5, 0, .875)), # offset pose self.camera_width, # number of horizontal samples self.camera_height, # number of vertical channels chrono.CH_C_PI / 2, # horizontal field of view 6) self.camera.SetName("Camera Sensor") self.camera.PushFilter(sens.ChFilterRGBA8Access()) self.manager.AddSensor(self.camera) # ----------------------------------------------------- # Create a self.gps and add it to the sensor manager # ----------------------------------------------------- gps_noise_none = sens.ChGPSNoiseNone() self.AgentGPS = sens.ChGPSSensor( self.chassis_body, 10, chrono.ChFrameD( chrono.ChVectorD(0, 0, 0), chrono.Q_from_AngAxis(0, chrono.ChVectorD(0, 1, 0))), self.origin, gps_noise_none) self.AgentGPS.SetName("AgentGPS Sensor") self.AgentGPS.PushFilter(sens.ChFilterGPSAccess()) self.manager.AddSensor(self.AgentGPS) ### Target GPS self.TargetGPS = sens.ChGPSSensor( self.leaders[0], 10, chrono.ChFrameD( chrono.ChVectorD(0, 0, 0), chrono.Q_from_AngAxis(0, chrono.ChVectorD(0, 1, 0))), self.origin, gps_noise_none) self.TargetGPS.SetName("TargetGPS Sensor") self.TargetGPS.PushFilter(sens.ChFilterGPSAccess()) self.manager.AddSensor(self.TargetGPS) self.step_number = 0 self.num_frames = 0 self.num_updates = 0 self.c_f = 0 self.old_ac = [0, 0] self.isdone = False self.render_setup = False if self.play_mode: self.render() return self.get_ob()
def reset(self): #print("reset") self.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()
) my_system.Add(mbodyflywheel) myjoint = chrono.ChLinkMateFix() myjoint.Initialize(node_mid, mbodyflywheel) my_system.Add(myjoint) # Create the truss truss = chrono.ChBody() truss.SetBodyFixed(True) my_system.Add(truss) # Create the end bearing bearing = chrono.ChLinkMateGeneric(False, True, True, False, True, True) bearing.Initialize(builder.GetLastBeamNodes().back(), truss, chrono.ChFrameD(builder.GetLastBeamNodes().back().GetPos())) my_system.Add(bearing) # Create the motor that rotates the beam rotmotor1 = chrono.ChLinkMotorRotationSpeed() # Connect the rotor and the stator and add the motor to the system: rotmotor1.Initialize( builder.GetLastBeamNodes().front(), # body A (slave) truss, # body B (master) chrono.ChFrameD( builder.GetLastBeamNodes().front().GetPos(), chrono.Q_from_AngAxis(CH_C_PI / 2.0, chrono.VECT_Y)) # motor frame, in abs. coords ) my_system.Add(rotmotor1)
# i.e. each node has coordinates of type: position, rotation, # where X axis of rotated system is the direction of the beam, # Y and Z are the section plane. beam_nodes = [] length = 1.2 # beam length, in meters N_nodes = 16 for i_n in range(N_nodes): # i-th node position position = chrono.ChVectorD(length * (i_n / (N_nodes - 1)), # node position, x 0.5, # node position, y 0) # node position, z # create the node node = fea.ChNodeFEAxyzrot( chrono.ChFrameD(position) ) # add it to mesh mesh.AddNode(node) # add it to the auxiliary beam_nodes beam_nodes.append(node) # 5. Create the elements for ie in range(N_nodes - 1): # create the element element = fea.ChElementBeamEuler()
def reset(self): n = 2 * np.random.randint(0, 4) b1 = 0 b2 = 0 r1 = n r2 = n r3 = n r4 = n r5 = n t1 = 0 t2 = 0 t3 = 0 c = 0 self.assets = AssetList(b1, b2, r1, r2, r3, r4, r5, t1, t2, t3, c) # Create systems self.system = chrono.ChSystemNSC() self.system.Set_G_acc(chrono.ChVectorD(0, 0, -9.81)) self.system.SetSolverType(chrono.ChSolver.Type_BARZILAIBORWEIN) self.system.SetSolverMaxIterations(150) self.system.SetMaxPenetrationRecoverySpeed(4.0) # Create the terrain rigid_terrain = True self.terrain = veh.RigidTerrain(self.system) if rigid_terrain: patch_mat = chrono.ChMaterialSurfaceNSC() patch_mat.SetFriction(0.9) patch_mat.SetRestitution(0.01) patch = self.terrain.AddPatch(patch_mat, chrono.ChVectorD(0, 0, 0), chrono.ChVectorD(0, 0, 1), self.terrain_length * 1.5, self.terrain_width * 1.5) else: self.bitmap_file = os.path.dirname( os.path.realpath(__file__)) + "/../utils/height_map.bmp" self.bitmap_file_backup = os.path.dirname( os.path.realpath(__file__)) + "/../utils/height_map_backup.bmp" shape = (252, 252) generate_random_bitmap(shape=shape, resolutions=[(2, 2)], mappings=[(-1.5, 1.5)], file_name=self.bitmap_file) try: patch = self.terrain.AddPatch( chrono.CSYSNORM, # position self.bitmap_file, # heightmap file (.bmp) "test", # mesh name self.terrain_length * 1.5, # sizeX self.terrain_width * 1.5, # sizeY self.min_terrain_height, # hMin self.max_terrain_height) # hMax except Exception: print('Corrupt Bitmap File') patch = self.terrain.AddPatch( chrono.CSYSNORM, # position self.bitmap_file_backup, # heightmap file (.bmp) "test", # mesh name self.terrain_length * 1.5, # sizeX self.terrain_width * 1.5, # sizeY self.min_terrain_height, # hMin self.max_terrain_height) # hMax patch.SetTexture(veh.GetDataFile("terrain/textures/grass.jpg"), 200, 200) patch.SetColor(chrono.ChColor(0.8, 0.8, 0.5)) self.terrain.Initialize() ground_body = patch.GetGroundBody() ground_asset = ground_body.GetAssets()[0] visual_asset = chrono.CastToChVisualization(ground_asset) visual_asset.SetStatic(True) vis_mat = chrono.ChVisualMaterial() vis_mat.SetKdTexture(veh.GetDataFile("terrain/textures/grass.jpg")) visual_asset.material_list.append(vis_mat) theta = random.random() * 2 * np.pi x, y = self.terrain_length * 0.5 * np.cos( theta), self.terrain_width * 0.5 * np.sin(theta) z = self.terrain.GetHeight(chrono.ChVectorD(x, y, 0)) + 0.25 ang = np.pi + theta self.initLoc = chrono.ChVectorD(x, y, z) self.initRot = chrono.Q_from_AngZ(ang) self.vehicle = veh.Gator(self.system) self.vehicle.SetContactMethod(chrono.ChContactMethod_NSC) self.vehicle.SetChassisCollisionType(veh.ChassisCollisionType_NONE) self.vehicle.SetChassisFixed(False) self.m_inputs = veh.Inputs() self.vehicle.SetInitPosition( chrono.ChCoordsysD(self.initLoc, self.initRot)) self.vehicle.SetTireType(veh.TireModelType_TMEASY) self.vehicle.SetTireStepSize(self.timestep) self.vehicle.Initialize() if self.play_mode: self.vehicle.SetChassisVisualizationType( veh.VisualizationType_MESH) self.vehicle.SetWheelVisualizationType(veh.VisualizationType_MESH) self.vehicle.SetTireVisualizationType(veh.VisualizationType_MESH) else: self.vehicle.SetChassisVisualizationType( veh.VisualizationType_PRIMITIVES) self.vehicle.SetWheelVisualizationType( veh.VisualizationType_PRIMITIVES) self.vehicle.SetTireVisualizationType( veh.VisualizationType_PRIMITIVES) self.vehicle.SetSuspensionVisualizationType( veh.VisualizationType_PRIMITIVES) self.vehicle.SetSteeringVisualizationType( veh.VisualizationType_PRIMITIVES) self.chassis_body = self.vehicle.GetChassisBody() # self.chassis_body.GetCollisionModel().ClearModel() # size = chrono.ChVectorD(3,2,0.2) # self.chassis_body.GetCollisionModel().AddBox(0.5 * size.x, 0.5 * size.y, 0.5 * size.z) # self.chassis_body.GetCollisionModel().BuildModel() self.chassis_collision_box = chrono.ChVectorD(3, 2, 0.2) # Driver self.driver = veh.ChDriver(self.vehicle.GetVehicle()) # create goal # pi/4 ang displ delta_theta = (random.random() - 0.5) * 1.0 * np.pi gx, gy = self.terrain_length * 0.5 * np.cos( theta + np.pi + delta_theta), self.terrain_width * 0.5 * np.sin(theta + np.pi + delta_theta) self.goal = chrono.ChVectorD( gx, gy, self.terrain.GetHeight(chrono.ChVectorD(gx, gy, 0)) + 1.0) i = 0 while (self.goal - self.initLoc).Length() < 15: gx = random.random() * self.terrain_length - self.terrain_length / 2 gy = random.random() * self.terrain_width - self.terrain_width / 2 self.goal = chrono.ChVectorD(gx, gy, self.max_terrain_height + 1) if i > 100: print('Break') break i += 1 # self.goal = chrono.ChVectorD(75, 0, 0) # Origin in Madison WI self.origin = chrono.ChVectorD(43.073268, -89.400636, 260.0) self.goal_coord = chrono.ChVectorD(self.goal) sens.Cartesian2GPS(self.goal_coord, self.origin) self.goal_sphere = chrono.ChBodyEasySphere(.55, 1000, True, False) self.goal_sphere.SetBodyFixed(True) sphere_asset = self.goal_sphere.GetAssets()[0] visual_asset = chrono.CastToChVisualization(sphere_asset) vis_mat = chrono.ChVisualMaterial() vis_mat.SetAmbientColor(chrono.ChVectorF(0, 0, 0)) vis_mat.SetDiffuseColor(chrono.ChVectorF(.2, .2, .9)) vis_mat.SetSpecularColor(chrono.ChVectorF(.9, .9, .9)) visual_asset.material_list.append(vis_mat) visual_asset.SetStatic(True) self.goal_sphere.SetPos(self.goal) if self.play_mode: self.system.Add(self.goal_sphere) # create obstacles # start = t.time() self.assets.Clear() self.assets.RandomlyPositionAssets(self.system, self.initLoc, self.goal, self.terrain, self.terrain_length * 1.5, self.terrain_width * 1.5, should_scale=False) # Set the time response for steering and throttle inputs. # NOTE: this is not exact, since we do not render quite at the specified FPS. steering_time = 0.75 # time to go from 0 to +1 (or from 0 to -1) throttle_time = .5 # time to go from 0 to +1 braking_time = 0.3 # time to go from 0 to +1 self.SteeringDelta = (self.timestep / steering_time) self.ThrottleDelta = (self.timestep / throttle_time) self.BrakingDelta = (self.timestep / braking_time) self.manager = sens.ChSensorManager(self.system) self.manager.scene.AddPointLight(chrono.ChVectorF(100, 100, 100), chrono.ChVectorF(1, 1, 1), 5000.0) self.manager.scene.AddPointLight(chrono.ChVectorF(-100, -100, 100), chrono.ChVectorF(1, 1, 1), 5000.0) # Let's not, for the moment, give a different scenario during test """ if self.play_mode: self.manager.scene.GetBackground().has_texture = True; self.manager.scene.GetBackground().env_tex = "sensor/textures/qwantani_8k.hdr" self.manager.scene.GetBackground().has_changed = True; """ # ----------------------------------------------------- # Create a self.camera and add it to the sensor manager # ----------------------------------------------------- #chrono.ChFrameD(chrono.ChVectorD(1.5, 0, .875), chrono.Q_from_AngAxis(0, chrono.ChVectorD(0, 1, 0))), self.camera = sens.ChCameraSensor( self.chassis_body, # body camera is attached to 20, # scanning rate in Hz chrono.ChFrameD( chrono.ChVectorD(.65, 0, .75), chrono.Q_from_AngAxis(0, chrono.ChVectorD(0, 1, 0))), # offset pose self.camera_width, # number of horizontal samples self.camera_height, # number of vertical channels chrono.CH_C_PI / 2, # horizontal field of view 6 # supersampling factor ) self.camera.SetName("Camera Sensor") self.camera.PushFilter(sens.ChFilterRGBA8Access()) if self.play_mode: self.camera.PushFilter( sens.ChFilterVisualize(self.camera_width, self.camera_height, "RGB Camera")) self.manager.AddSensor(self.camera) # ----------------------------------------------------- # Create a self.gps and add it to the sensor manager # ----------------------------------------------------- gps_noise_none = sens.ChGPSNoiseNone() self.gps = sens.ChGPSSensor( self.chassis_body, 15, chrono.ChFrameD( chrono.ChVectorD(0, 0, 0), chrono.Q_from_AngAxis(0, chrono.ChVectorD(0, 1, 0))), self.origin, gps_noise_none) self.gps.SetName("GPS Sensor") self.gps.PushFilter(sens.ChFilterGPSAccess()) self.manager.AddSensor(self.gps) # have to reconstruct scene because sensor loads in meshes separately (ask Asher) # start = t.time() if self.assets.GetNum() > 0: # self.assets.TransformAgain() # start = t.time() for asset in self.assets.assets: if len(asset.frames) > 0: self.manager.AddInstancedStaticSceneMeshes( asset.frames, asset.mesh.shape) # self.manager.ReconstructScenes() # self.manager.AddInstancedStaticSceneMeshes(self.assets.frames, self.assets.shapes) # self.manager.Update() # print('Reconstruction :: ', t.time() - start) self.old_dist = (self.goal - self.initLoc).Length() self.step_number = 0 self.c_f = 0 self.isdone = False self.render_setup = False self.dist0 = (self.goal - self.chassis_body.GetPos()).Length() if self.play_mode: self.render() # print(self.get_ob()[1]) return self.get_ob()
else: raise ('Unvalid contact method') cbk = robosimian.RS_DriverCallback(robot) driver.RegisterPhaseChangeCallback(cbk) driver.SetTimeOffsets(duration_pose, duration_settle_robot) robot.SetDriver(driver) # ------------------------------- # Cast rays into collision models # ------------------------------- caster = RayCaster( my_sys, chrono.ChFrameD(chrono.ChVectorD(0, -2, -1), chrono.Q_from_AngX(-chrono.CH_C_PI_2)), [2.5, 2.5], 0.02) # ------------------------------- # Create the visualization window # ------------------------------- application = chronoirr.ChIrrApp(my_sys, "RoboSimian - Rigid terrain", chronoirr.dimension2du(800, 600)) application.AddTypicalLogo(chrono.GetChronoDataPath() + 'logo_pychrono_alpha.png') application.AddTypicalSky() application.AddTypicalCamera(chronoirr.vector3df(1, -2.75, 0.2), chronoirr.vector3df(1, 0, 0)) application.AddTypicalLights(chronoirr.vector3df(100, 100, 100), chronoirr.vector3df(100, -100, 80)) application.AddLightWithShadow(chronoirr.vector3df(10, -6, 3),