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)
