def createChronoSystem(): """ Default method for creating a lone chrono system""" 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) return sys
def __init__(self): ChronoBaseEnv.__init__(self) #self._set_observation_space(np.ndarray([4,])) #self.action_space = np.zeros([4,]) low = np.full(4, -1000) high = np.full(4, 1000) self.observation_space = spaces.Box(low, high, dtype=np.float32) self.action_space = spaces.Box(low=-1.0, high=1.0, shape=(1, ), dtype=np.float32) self.info = {"timeout": 100000} self.timestep = 0.01 # --------------------------------------------------------------------- # # Create the simulation system and add items # self.rev_pend_sys = chrono.ChSystemNSC() chrono.ChCollisionModel.SetDefaultSuggestedEnvelope(0.001) chrono.ChCollisionModel.SetDefaultSuggestedMargin(0.001) #rev_pend_sys.SetSolverType(chrono.ChSolver.Type_BARZILAIBORWEIN) # precise, more slow self.rev_pend_sys.SetMaxItersSolverSpeed(70) # Create a contact material (surface property)to share between all objects. self.rod_material = chrono.ChMaterialSurfaceNSC() self.rod_material.SetFriction(0.5) self.rod_material.SetDampingF(0.2) self.rod_material.SetCompliance(0.0000001) self.rod_material.SetComplianceT(0.0000001) # Create the set of rods in a vertical stack, along Y axis self.size_rod_y = 2.0 self.radius_rod = 0.05 self.density_rod = 50 # kg/m^3 self.mass_rod = self.density_rod * self.size_rod_y * chrono.CH_C_PI * ( self.radius_rod**2) self.inertia_rod_y = (self.radius_rod**2) * self.mass_rod / 2 self.inertia_rod_x = (self.mass_rod / 12) * ((self.size_rod_y**2) + 3 * (self.radius_rod**2)) self.size_table_x = 0.3 self.size_table_y = 0.3 self.size_table_z = 0.3 self.reset()
def __init__(self, step_size: float = 3e-3, render_step_size: float = 2e-2, end_time: float = 120, contact_method: str = "NSC", args: 'argparse.Namespace' = None): super().__init__(step_size, render_step_size, end_time, args) if not isinstance(contact_method, str): raise TypeError("Contact method must be of type str") if contact_method == "NSC": system = chrono.ChSystemNSC() elif contact_method == "SMC": system = chrono.ChSystemSMC() system.Set_G_acc(chrono.ChVectorD(0, 0, -9.81)) system.SetSolverType(chrono.ChSolver.Type_BARZILAIBORWEIN) system.SetSolverMaxIterations(150) system.SetMaxPenetrationRecoverySpeed(4.0) self._system = system
def main(): mysystem = chrono.ChSystemNSC() model.make_model(mysystem) # --------------------------------------------------------------------- # # Create an Irrlicht application to visualize the system # myapplication = chronoirr.ChIrrApp(mysystem, 'Oswald', chronoirr.dimension2du(1024, 768)) # myapplication.AddTypicalSky() # myapplication.AddShadowAll() myapplication.AddTypicalLogo() myapplication.AddTypicalCamera(chronoirr.vector3df(0.6, 0.6, 0.8)) myapplication.AddTypicalLights() # ==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 in # Irrlicht, just use application.AssetBind(myitem); on a per-item basis. 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! myapplication.AssetUpdateAll() # --------------------------------------------------------------------- # # Run the simulation # myapplication.SetTimestep(0.005) # myapplication.SetTryRealtime(True) while myapplication.GetDevice().run(): myapplication.BeginScene() myapplication.DrawAll() myapplication.DoStep() myapplication.EndScene()
def __init__(self, render): self.animate = render self.observation_space = np.empty([30,]) self.action_space = np.zeros([8,]) self.info = {} # --------------------------------------------------------------------- # # Create the simulation system and add items # self.Xtarg = 1000.0 self.Ytarg = 0.0 self.d_old = np.linalg.norm(self.Xtarg + self.Ytarg) self.ant_sys = chrono.ChSystemNSC() # Set the default outward/inward shape margins for collision detection, # this is epecially important for very large or very small objects. chrono.ChCollisionModel.SetDefaultSuggestedEnvelope(0.001) chrono.ChCollisionModel.SetDefaultSuggestedMargin(0.001) #ant_sys.SetSolverType(chrono.ChSolver.Type_BARZILAIBORWEIN) # precise, more slow self.ant_sys.SetSolverMaxIterations(70) self.ant_material = chrono.ChMaterialSurfaceNSC() self.ant_material.SetFriction(0.5) self.ant_material.SetDampingF(0.2) self.ant_material.SetCompliance (0.0005) self.ant_material.SetComplianceT(0.0005) self.timestep = 0.01 self.abdomen_x = 0.25 self.abdomen_y = 0.25 self.abdomen_z = 0.25 self.leg_density = 250 # kg/m^3 self.abdomen_density = 100 self.abdomen_y0 = 0.4 self.leg_length = 0.3 self.leg_radius = 0.04 self.ankle_angle = 60*(math.pi/180) self.ankle_length = 0.4 self.ankle_radius = 0.04 self.gain = 30 self.abdomen_mass = self.abdomen_density * ((4/3)*chrono.CH_C_PI*self.abdomen_x*self.abdomen_y*self.abdomen_z) self.abdomen_inertia = chrono.ChVectorD((1/5)*self.abdomen_mass*(pow(self.abdomen_y,2)+pow(self.abdomen_z,2)),(1/5)*self.abdomen_mass*(pow(self.abdomen_x,2)+pow(self.abdomen_z,2)),(1/5)*self.abdomen_mass*(pow(self.abdomen_y,2)+pow(self.abdomen_x,2))) self.leg_mass = self.leg_density * self.leg_length * math.pi* pow (self.leg_radius,2) self.leg_inertia = chrono.ChVectorD(0.5*self.leg_mass*pow(self.leg_radius,2), (self.leg_mass/12)*(3*pow(self.leg_radius,2)+pow(self.leg_length,2)),(self.leg_mass/12)*(3*pow(self.leg_radius,2)+pow(self.leg_length,2))) self.ankle_mass = self.leg_density * self.ankle_length * math.pi* pow (self.ankle_radius,2) self.ankle_inertia = chrono.ChVectorD(0.5*self.ankle_mass*pow(self.ankle_radius,2), (self.ankle_mass/12)*(3*pow(self.ankle_radius,2)+pow(self.ankle_length,2)),(self.ankle_mass/12)*(3*pow(self.ankle_radius,2)+pow(self.ankle_length,2))) self.leg_limit = chrono.ChLinkLimit() self.ankle_limit = chrono.ChLinkLimit() self.leg_limit.SetRmax(math.pi/9) self.leg_limit.SetRmin(-math.pi/9) self.ankle_limit.SetRmax(math.pi/9) self.ankle_limit.SetRmin(-math.pi/9) if (self.animate) : self.myapplication = chronoirr.ChIrrApp(self.ant_sys) self.myapplication.AddShadowAll() self.myapplication.SetStepManage(True) self.myapplication.SetTimestep(self.timestep) self. myapplication.SetTryRealtime(True) self.myapplication.AddTypicalSky() self.myapplication.AddTypicalLogo(chrono.GetChronoDataFile('logo_pychrono_alpha.png')) self.myapplication.AddTypicalCamera(chronoirr.vector3df(0,1.5,0)) self.myapplication.AddLightWithShadow(chronoirr.vector3df(4,4,0), # point chronoirr.vector3df(0,0,0), # aimpoint 20, # radius (power) 1,9, # near, far 90) # angle of FOV
# # Demonstration of the gear constraint (ChLinkGear) as a method to impose a # transmission ratio between two shafts as they were connected by gears, # without the need of performing collision detection between gear teeth # geometries (which would be inefficient) # # ============================================================================= import pychrono as chrono import pychrono.irrlicht as chronoirr import math as m print("Copyright (c) 2017 projectchrono.org") # Create a Chrono::Engine physical system mphysicalSystem = chrono.ChSystemNSC() # Create the Irrlicht visualization (open the Irrlicht device, # bind a simple user interface, etc, etc.) application = chronoirr.ChIrrApp(mphysicalSystem, "Gears annd pulleys", chronoirr.dimension2du(800, 600), False) # Easy shortcuts to add camera, lights, logo, and sky in Irrlicht scene: application.AddTypicalLogo() application.AddTypicalSky() application.AddTypicalLights() application.AddTypicalCamera(chronoirr.vector3df(12, 15, -20)) # Contact material shared among all bodies mat = chrono.ChMaterialSurfaceNSC()
def __init__(self): ChronoBaseEnv.__init__(self) # Set to False to avoid vis shapes loading. Once you do this, you can no longer render until this is re set to True self.animate = True low = np.full(53, -1000) high = np.full(53, 1000) self.observation_space = spaces.Box(low, high, dtype=np.float32) self.action_space = spaces.Box(low=-1.0, high=1.0, shape=(18, ), dtype=np.float32) self.Xtarg = 0.0 self.Ztarg = 1000.0 self.d_old = np.linalg.norm(self.Xtarg + self.Ztarg) #self.d_old = np.linalg.norm(self.Xtarg + self.Ytarg) self.hexapod_sys = chrono.ChSystemNSC() chrono.ChCollisionModel.SetDefaultSuggestedEnvelope(0.0001) chrono.ChCollisionModel.SetDefaultSuggestedMargin(0.0001) #hexapod_sys.SetSolverType(chrono.ChSolver.Type_BARZILAIBORWEIN) # precise, more slow self.info = {"timeout": 3200.0} if self.animate: m_filename = "hexapod" else: m_filename = "hexapod_novis" self.timestep = 0.005 m_length = 1.0 self.my_material = chrono.ChMaterialSurfaceNSC() self.my_material.SetFriction(0.5) self.my_material.SetDampingF(0.2) self.my_material.SetCompliance(0.0000001) self.my_material.SetComplianceT(0.0000001) #self.my_material.SetCompliance (0.0005) #self.my_material.SetComplianceT(0.0005) #m_visualization = "irrlicht" print(" file to load is ", m_filename) print(" timestep is ", self.timestep) print(" length is ", m_length) print(" data path for fonts etc.: ", chrono.GetChronoDataPath()) # --------------------------------------------------------------------- # # load the file generated by the SolidWorks CAD plugin # and add it to the ChSystem. # Remove the trailing .py and add / in case of file without ./ #m_absfilename = os.path.abspath(m_filename) #m_modulename = os.path.splitext(m_absfilename)[0] print("Loading C::E scene...") dir_path = os.path.dirname(os.path.realpath(__file__)) self.fpath = os.path.join(dir_path, m_filename) #exported_items = chrono.ImportSolidWorksSystem(self.fpath) #self.exported_items = chrono.ImportSolidWorksSystem(m_modulename) print("...loading done!") # Print exported items #for my_item in exported_items: #print (my_item.GetName()) # Optionally set some solver parameters. #self.hexapod_sys.SetMaxPenetrationRecoverySpeed(1.00) solver = chrono.ChSolverBB() self.hexapod_sys.SetSolver(solver) solver.SetMaxIterations(600) solver.EnableWarmStart(True) self.hexapod_sys.Set_G_acc(chrono.ChVectorD(0, -9.8, 0)) """ $$$$$$$$ FIND THE SW DEFINED CONSTRAINTS, GET THEIR self.frames AND GET RID OF EM $$$$$$$$ """ self.con_link = [] self.coi_link = [] self.hip_names = [] self.femur_names = [] self.tibia_names = [] self.feet_names = [] self.maxSpeed = [] self.maxRot = [] self.minRot = [] Tmax = [] for i in range(1, 19): self.con_link.append("Concentric" + str(i)) self.coi_link.append("Coincident" + str(i)) self.maxSpeed.append(354) # 59 RPM to deg/s self.maxRot.append(90) #deg self.minRot.append(-90) #deg self.minRot.append(-90) #deg Tmax.append(1.5) #deg for i in range(1, 7): self.hip_names.append("Hip-" + str(i)) self.femur_names.append("Femur-" + str(i)) self.tibia_names.append("Tibia-" + str(i)) self.feet_names.append("Foot-" + str(i)) self.maxT = np.asarray(Tmax)
# ============================================================================= # ------------ # Timed events # ------------ time_create_terrain = duration_pose # create terrain after robot assumes initial pose time_start = time_create_terrain + duration_settle_robot # start actual simulation after robot settling time_end = time_start + duration_sim # end simulation after specified duration # ------------- # Create system # ------------- if contact_method == chrono.ChContactMethod_NSC: my_sys = chrono.ChSystemNSC() chrono.ChCollisionModel.SetDefaultSuggestedEnvelope(0.001) chrono.ChCollisionModel.SetDefaultSuggestedMargin(0.001) if contact_method == chrono.ChContactMethod_SMC: my_sys = chrono.ChSystemSMC() my_sys.SetSolverMaxIterations(200) my_sys.SetSolverType(chrono.ChSolver.Type_BARZILAIBORWEIN) my_sys.Set_G_acc(chrono.ChVectorD(0, 0, -9.8)) # ----------------------- # Create RoboSimian robot # -----------------------
def __init__(self, render): self.render = render self.observation_space = np.empty([4, 1]) self.action_space = np.empty([1, 1]) self.info = {} self.timestep = 0.01 # --------------------------------------------------------------------- # # Create the simulation system and add items # self.rev_pend_sys = chrono.ChSystemNSC() chrono.ChCollisionModel.SetDefaultSuggestedEnvelope(0.001) chrono.ChCollisionModel.SetDefaultSuggestedMargin(0.001) #rev_pend_sys.SetSolverType(chrono.ChSolver.Type_BARZILAIBORWEIN) # precise, more slow self.rev_pend_sys.SetMaxItersSolverSpeed(70) # Create a contact material (surface property)to share between all objects. self.rod_material = chrono.ChMaterialSurfaceNSC() self.rod_material.SetFriction(0.5) self.rod_material.SetDampingF(0.2) self.rod_material.SetCompliance(0.0000001) self.rod_material.SetComplianceT(0.0000001) # Create the set of rods in a vertical stack, along Y axis self.size_rod_y = 2.0 self.radius_rod = 0.05 self.density_rod = 50 # kg/m^3 self.mass_rod = self.density_rod * self.size_rod_y * chrono.CH_C_PI * ( self.radius_rod**2) self.inertia_rod_y = (self.radius_rod**2) * self.mass_rod / 2 self.inertia_rod_x = (self.mass_rod / 12) * ((self.size_rod_y**2) + 3 * (self.radius_rod**2)) self.size_table_x = 0.3 self.size_table_y = 0.3 if self.render: self.size_table_z = 0.3 self.myapplication = chronoirr.ChIrrApp(self.rev_pend_sys) self.myapplication.AddShadowAll() self.myapplication.SetStepManage(True) self.myapplication.SetTimestep(0.01) self.myapplication.SetTryRealtime(True) self.myapplication.AddTypicalSky('../data/skybox/') self.myapplication.AddTypicalCamera( chronoirr.vector3df(0.5, 0.5, 1.0)) self.myapplication.AddLightWithShadow( chronoirr.vector3df(2, 4, 2), # point chronoirr.vector3df(0, 0, 0), # aimpoint 9, # radius (power) 1, 9, # near, far 30) # angle of FOV
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): ChronoBaseEnv.__init__(self) low = np.full(18, -1000) high = np.full(18, 1000) self.observation_space = spaces.Box(low, high, dtype=np.float32) self.action_space = spaces.Box(low=-1.0, high=1.0, shape=(6, ), dtype=np.float32) self.fingerdist = chrono.ChVectorD(0, 0.1117, 0) self.robosystem = chrono.ChSystemNSC() chrono.ChCollisionModel.SetDefaultSuggestedEnvelope(0.001) chrono.ChCollisionModel.SetDefaultSuggestedMargin(0.001) self.timestep = 0.01 self.info = {"timeout": 2000.0} m_filename = "ComauR3" self.timestep = 0.001 m_length = 1.0 self.my_material = chrono.ChMaterialSurfaceNSC() self.my_material.SetFriction(0.5) self.my_material.SetDampingF(0.2) self.my_material.SetCompliance(0.0000001) self.my_material.SetComplianceT(0.0000001) #m_visualization = "irrlicht" print(" file to load is ", m_filename) print(" timestep is ", self.timestep) print(" length is ", m_length) print(" data path for fonts etc.: ", chrono.GetChronoDataPath()) # --------------------------------------------------------------------- # # load the file generated by the SolidWorks CAD plugin # and add it to the ChSystem. # Remove the trailing .py and add / in case of file without ./ #m_absfilename = os.path.abspath(m_filename) #m_modulename = os.path.splitext(m_absfilename)[0] print("Loading C::E scene...") dir_path = os.path.dirname(os.path.realpath(__file__)) self.fpath = os.path.join(dir_path, m_filename) #exported_items = chrono.ImportSolidWorksSystem(self.fpath) #self.exported_items = chrono.ImportSolidWorksSystem(m_modulename) print("...loading done!") # Print exported items #for my_item in exported_items: #print (my_item.GetName()) # Optionally set some solver parameters. #self.robosystem.SetMaxPenetrationRecoverySpeed(1.00) solver = chrono.ChSolverBB() self.robosystem.SetSolver(solver) solver.SetMaxIterations(600) solver.EnableWarmStart(True) self.robosystem.Set_G_acc(chrono.ChVectorD(0, -9.8, 0)) """ $$$$$$$$ FIND THE SW DEFINED CONSTRAINTS, GET THEIR self.frames AND GET RID OF EM $$$$$$$$ """ self.con_link = [ "Concentric1", "Concentric2", "Concentric3", "Concentric4", "Concentric5", "Concentric6" ] self.coi_link = [ "Coincident1", "Coincident2", "Coincident3", "Coincident4", "Coincident5", "Coincident6" ] self.bodiesNames = [ "Racer3_p01-3", "Racer3_p02-1", "Racer3_p03-1", "Racer3_p04-1", "Racer3_p05-1", "Racer3_p06-1", "Hand_base_and_p07-2" ] self.maxSpeed = [430, 450, 500, 600, 600, 900] #°/s self.maxRot = [170, 135, 90, 179, 100, 179] # ° self.minRot = [-170, -95, -155, -179, -100, -179] # ° self.maxT = np.asarray([100, 100, 50, 7.36, 7.36, 4.41])
def __init__(self, render): self.animate = render self.observation_space = np.empty([ 18, ]) self.action_space = np.zeros([ 6, ]) #TODO: check if targ is reachable self.fingerdist = chrono.ChVectorD(0, 0.1117, 0) #self.d_old = np.linalg.norm(self.Xtarg + self.Ytarg) self.robosystem = chrono.ChSystemNSC() chrono.ChCollisionModel.SetDefaultSuggestedEnvelope(0.001) chrono.ChCollisionModel.SetDefaultSuggestedMargin(0.001) #robosystem.SetSolverType(chrono.ChSolver.Type_BARZILAIBORWEIN) # precise, more slow self.timestep = 0.01 self.info = {} m_filename = "ABB_IRB120" self.timestep = 0.001 m_length = 1.0 self.my_material = chrono.ChMaterialSurfaceNSC() self.my_material.SetFriction(0.5) self.my_material.SetDampingF(0.2) self.my_material.SetCompliance(0.0000001) self.my_material.SetComplianceT(0.0000001) #m_visualization = "irrlicht" self.m_datapath = "../../../../../../codes/Chrono/Chrono_Source/data" chrono.SetChronoDataPath(self.m_datapath) print(" file to load is ", m_filename) print(" timestep is ", self.timestep) print(" length is ", m_length) print(" data path for fonts etc.: ", self.m_datapath) # --------------------------------------------------------------------- # # load the file generated by the SolidWorks CAD plugin # and add it to the ChSystem. # Remove the trailing .py and add / in case of file without ./ #m_absfilename = os.path.abspath(m_filename) #m_modulename = os.path.splitext(m_absfilename)[0] print("Loading C::E scene...") dir_path = os.path.dirname(os.path.realpath(__file__)) self.fpath = os.path.join(dir_path, m_filename) #exported_items = chrono.ImportSolidWorksSystem(self.fpath) #self.exported_items = chrono.ImportSolidWorksSystem(m_modulename) print("...loading done!") # Print exported items #for my_item in exported_items: #print (my_item.GetName()) # Optionally set some solver parameters. #self.robosystem.SetMaxPenetrationRecoverySpeed(1.00) self.robosystem.SetSolverType(chrono.ChSolver.Type_BARZILAIBORWEIN) self.robosystem.SetMaxItersSolverSpeed(600) self.robosystem.SetSolverWarmStarting(True) self.robosystem.Set_G_acc(chrono.ChVectorD(0, -9.8, 0)) """ $$$$$$$$ FIND THE SW DEFINED CONSTRAINTS, GET THEIR self.frames AND GET RID OF EM $$$$$$$$ """ self.con_link = [ "Concentric1", "Concentric2", "Concentric3", "Concentric4", "Concentric5", "Concentric6" ] self.coi_link = [ "Coincident1", "Coincident2", "Coincident3", "Coincident4", "Coincident5", "Coincident6" ] self.bodiesNames = [ "Racer3_p01-1", "Racer3_p02-1", "Racer3_p03-1", "Racer3_p04-1", "Racer3_p05-1", "Racer3_p06-2", "Hand_base_and_p07-3" ] # self.maxSpeed = [430, 450, 500, 600, 600, 900] #°/s -->COMAU self.maxSpeed = [250, 250, 250, 320, 320, 420] #°/s --> ABB # TODO: convert to rads (converted in every operation) #self.limits: # TODO: check signs # TODO: peridodicity ignored # REAL self.limits #self.maxRot = [170, 135, 90, 200, 125, 2700] # ° #self.minRot = [-170, -95, -155, -200, -125, -2700] # ° # MODIFIED self.limits # self.maxRot = [170, 135, 90, 179, 50, 79] # ° --> COMAU # self.minRot = [-170, -95, -155, -179, -50, -79] # ° --> COMAU self.maxRot = [165, 110, 70, 160, 120, 179] # ° --> ABB self.minRot = [-165, -110, -110, -160, -100, -179] # ° --> ABB #self.maxT = np.asarray([28, 28, 28, 7.36, 7.36, 4.41]) self.maxT = np.asarray([100, 100, 50, 7.36, 7.36, 4.41]) # --> LASCIATI UGUALI A COMAU if (self.animate): self.myapplication = chronoirr.ChIrrApp( self.robosystem, 'Test', chronoirr.dimension2du(1280, 720)) self.myapplication.AddShadowAll() self.myapplication.SetStepManage(True) self.myapplication.SetTimestep(self.timestep) self.myapplication.AddTypicalSky(chrono.GetChronoDataPath() + 'skybox/') self.myapplication.AddTypicalLogo(chrono.GetChronoDataPath() + 'logo_pychrono_alpha.png') self.myapplication.AddTypicalCamera( chronoirr.vector3df(1, 1, 1), chronoirr.vector3df(0.0, 0.0, 0.0)) self.myapplication.AddTypicalLights() # angle of FOV
def __init__(self): ChronoBaseEnv.__init__(self) low = np.full(30, -1000) high = np.full(30, 1000) self.observation_space = spaces.Box(low, high, dtype=np.float32) self.action_space = spaces.Box(low=-1.0, high=1.0, shape=(8,), dtype=np.float32) self.info = {"timeout": 10000.0} # --------------------------------------------------------------------- # # Create the simulation system and add items # self.Xtarg = 1000.0 self.Ytarg = 0.0 self.d_old = np.linalg.norm(self.Xtarg + self.Ytarg) self.ant_sys = chrono.ChSystemNSC() # Set the default outward/inward shape margins for collision detection, # this is epecially important for very large or very small objects. chrono.ChCollisionModel.SetDefaultSuggestedEnvelope(0.001) chrono.ChCollisionModel.SetDefaultSuggestedMargin(0.001) #ant_sys.SetSolverType(chrono.ChSolver.Type_BARZILAIBORWEIN) # precise, more slow self.ant_sys.SetSolverMaxIterations(70) self.ant_material = chrono.ChMaterialSurfaceNSC() self.ant_material.SetFriction(0.5) self.ant_material.SetDampingF(0.2) self.ant_material.SetCompliance (0.0005) self.ant_material.SetComplianceT(0.0005) self.timestep = 0.01 self.abdomen_x = 0.25 self.abdomen_y = 0.25 self.abdomen_z = 0.25 self.leg_density = 250 # kg/m^3 self.abdomen_density = 100 self.abdomen_y0 = 0.4 self.leg_length = 0.3 self.leg_radius = 0.04 self.ankle_angle = 60*(math.pi/180) self.ankle_length = 0.4 self.ankle_radius = 0.04 self.gain = 30 self.abdomen_mass = self.abdomen_density * ((4/3)*chrono.CH_C_PI*self.abdomen_x*self.abdomen_y*self.abdomen_z) self.abdomen_inertia = chrono.ChVectorD((1/5)*self.abdomen_mass*(pow(self.abdomen_y,2)+pow(self.abdomen_z,2)),(1/5)*self.abdomen_mass*(pow(self.abdomen_x,2)+pow(self.abdomen_z,2)),(1/5)*self.abdomen_mass*(pow(self.abdomen_y,2)+pow(self.abdomen_x,2))) self.leg_mass = self.leg_density * self.leg_length * math.pi* pow (self.leg_radius,2) self.leg_inertia = chrono.ChVectorD(0.5*self.leg_mass*pow(self.leg_radius,2), (self.leg_mass/12)*(3*pow(self.leg_radius,2)+pow(self.leg_length,2)),(self.leg_mass/12)*(3*pow(self.leg_radius,2)+pow(self.leg_length,2))) self.ankle_mass = self.leg_density * self.ankle_length * math.pi* pow (self.ankle_radius,2) self.ankle_inertia = chrono.ChVectorD(0.5*self.ankle_mass*pow(self.ankle_radius,2), (self.ankle_mass/12)*(3*pow(self.ankle_radius,2)+pow(self.ankle_length,2)),(self.ankle_mass/12)*(3*pow(self.ankle_radius,2)+pow(self.ankle_length,2))) self.leg_limit = chrono.ChLinkLimit() self.ankle_limit = chrono.ChLinkLimit() self.leg_limit.SetRmax(math.pi/9) self.leg_limit.SetRmin(-math.pi/9) self.ankle_limit.SetRmax(math.pi/9) self.ankle_limit.SetRmin(-math.pi/9)
def __init__(self, render): self.render = render #self.size_rod_y = l self.observation_space = np.empty([4, 1]) self.action_space = np.empty([1, 1]) self.info = {} # --------------------------------------------------------------------- # # Create the simulation system and add items # self.rev_pend_sys = chrono.ChSystemNSC() # Set the default outward/inward shape margins for collision detection, # this is epecially important for very large or very small objects. chrono.ChCollisionModel.SetDefaultSuggestedEnvelope(0.001) chrono.ChCollisionModel.SetDefaultSuggestedMargin(0.001) #rev_pend_sys.SetSolverType(chrono.ChSolver.Type_BARZILAIBORWEIN) # precise, more slow self.rev_pend_sys.SetMaxItersSolverSpeed(70) # Create a contact material (surface property)to share between all objects. # The rolling and spinning parameters are optional - if enabled they double # the computational time. # non ho cantatti, da vedere se è necessario tenere tutto il baraccone self.rod_material = chrono.ChMaterialSurfaceNSC() self.rod_material.SetFriction(0.5) self.rod_material.SetDampingF(0.2) self.rod_material.SetCompliance(0.0000001) self.rod_material.SetComplianceT(0.0000001) # rod_material.SetRollingFriction(rollfrict_param) # rod_material.SetSpinningFriction(0) # rod_material.SetComplianceRolling(0.0000001) # rod_material.SetComplianceSpinning(0.0000001) # Create the set of rods in a vertical stack, along Y axis self.size_rod_y = 2.0 self.radius_rod = 0.05 self.density_rod = 50 # kg/m^3 self.mass_rod = self.density_rod * self.size_rod_y * chrono.CH_C_PI * ( self.radius_rod**2) self.inertia_rod_y = (self.radius_rod**2) * self.mass_rod / 2 self.inertia_rod_x = (self.mass_rod / 12) * ((self.size_rod_y**2) + 3 * (self.radius_rod**2)) self.size_table_x = 0.3 self.size_table_y = 0.3 if self.render: self.size_table_z = 0.3 self.myapplication = chronoirr.ChIrrApp(self.rev_pend_sys) self.myapplication.AddShadowAll() self.myapplication.SetStepManage(True) self.myapplication.SetTimestep(0.01) self.myapplication.SetTryRealtime(True) self.myapplication.AddTypicalSky('../data/skybox/') self.myapplication.AddTypicalCamera( chronoirr.vector3df(0.5, 0.5, 1.0)) self.myapplication.AddLightWithShadow( chronoirr.vector3df(2, 4, 2), # point chronoirr.vector3df(0, 0, 0), # aimpoint 9, # radius (power) 1, 9, # near, far 30) # angle of FOV
def __init__(self, name, timestep): self.system = chrono.ChSystemNSC() self.name = name self.system.SetSolver(mkl.ChSolverMKL()) self.timestep = timestep
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()
## ## The simulation is animated with Irrlicht. ## ## ============================================================================= import pychrono as chrono from pychrono import irrlicht as chronoirr #CHRONO_DATA_DIR = chrono.SetChronoDataPath('C:/codes/Chrono/Chrono_Source/data/') ## 1. Create the physical system that will handle all bodies and constraints. ## Specify the gravitational acceleration vector, consistent with the ## global reference frame having Z up. system = chrono.ChSystemNSC() system.Set_G_acc(chrono.ChVectorD(0, 0, -9.81)) ## 2. Create the rigid bodies of the slider-crank mechanical system. ## For each body, specify: ## - a unique identifier ## - mass and moments of inertia ## - position and orientation of the (centroidal) body frame ## - visualization assets (defined with respect to the body frame) ## Ground ground = chrono.ChBody() system.AddBody(ground) ground.SetIdentifier(-1) ground.SetName("ground") ground.SetBodyFixed(True)
def __init__(self, name): self.system = chrono.ChSystemNSC() self.name = name solver = chrono.ChSolverSparseLU() self.system.SetSolver(solver)