def AddMovingObstacles(system):
# Create contact material, of appropriate type. Use default properties
material = None
if (NSC_SMC == chrono.ChContactMethod_NSC):
matNSC = chrono.ChMaterialSurfaceNSC()
#Change NSC material properties as desired
material = matNSC
elif (NSC_SMC == chrono.ChContactMethod_SMC):
matSMC = chrono.ChMaterialSurfaceSMC()
# Change SMC material properties as desired
material = matSMC
else:
raise ("Unvalid Contact Method")
sizeX = 300
sizeY = 300
height = 0
numObstacles = 10
for i in range(numObstacles):
o_sizeX = 1.0 + 3.0 * chrono.ChRandom()
o_sizeY = 0.3 + 0.2 * chrono.ChRandom()
o_sizeZ = 0.05 + 0.1 * chrono.ChRandom()
obstacle = chrono.ChBodyEasyBox(o_sizeX, o_sizeY, o_sizeZ, 2000.0,
True, True, material)
o_posX = (chrono.ChRandom() - 0.5) * 0.4 * sizeX
o_posY = (chrono.ChRandom() - 0.5) * 0.4 * sizeY
o_posZ = height + 4
rot = chrono.ChQuaternionD(chrono.ChRandom(), chrono.ChRandom(),
chrono.ChRandom(), chrono.ChRandom())
rot.Normalize()
obstacle.SetPos(chrono.ChVectorD(o_posX, o_posY, o_posZ))
obstacle.SetRot(rot)
system.AddBody(obstacle)
def DrawBarriers(self, points, n=5, height=1, width=1):
points = points[::n]
if points[-1] != points[0]:
points.append(points[-1])
for i in range(len(points) - 1):
p1 = points[i]
p2 = points[i + 1]
box = chrono.ChBodyEasyBox((p2 - p1).Length(), height, width, 1000,
True, True)
box.SetPos(p1)
q = chrono.ChQuaternionD()
v1 = p2 - p1
v2 = chrono.ChVectorD(1, 0, 0)
ang = math.atan2((v1 % v2).Length(), v1 ^ v2)
if chrono.ChVectorD(0, 0, 1) ^ (v1 % v2) > 0.0:
ang *= -1
q.Q_from_AngZ(ang)
box.SetRot(q)
box.SetBodyFixed(True)
color = chrono.ChColorAsset()
if i % 2 == 0:
color.SetColor(chrono.ChColor(1, 0, 0))
else:
color.SetColor(chrono.ChColor(1, 1, 1))
box.AddAsset(color)
self.system.Add(box)
def GetInitRot(self):
y_axis = chrono.ChVectorD(1, 0, 0)
vec = self.ch_path[self.starting_index +
1] - self.ch_path[self.starting_index]
theta = math.acos((y_axis ^ vec) / (vec.Length() * y_axis.Length()))
q = chrono.ChQuaternionD()
q.Q_from_AngZ(-theta)
return q
def WAQuaternion_to_ChQuaternion(quaternion: WAQuaternion):
"""Converts a WAQuaternion to a ChQuaternion
Args:
quaternion (WAQuaternion): The quaternion to convert
"""
return chrono.ChQuaternionD(quaternion.x, quaternion.y, quaternion.z,
quaternion.w)
def SetCamera(self, pos, targ):
delta = targ - pos
q0 = chrono.ChQuaternionD()
q0.Q_from_AngAxis(-math.pi / 2, chrono.VECT_X)
alpha = 0
if (delta.z <= 0 and delta.x != 0):
alpha = -math.asin(delta.x / math.hypot(delta.x, delta.z))
if (delta.z > 0):
alpha = math.asin(delta.x / math.hypot(delta.x, delta.z)) + math.pi
q1 = chrono.ChQuaternionD()
q1.Q_from_AngAxis(alpha, q0.GetZaxis())
q2 = chrono.ChQuaternionD()
q2 = q1 * q0
beta = math.asin(delta.y / delta.Length())
q3 = chrono.ChQuaternionD()
q3.Q_from_AngAxis(beta, q2.GetXaxis())
qc0 = chrono.ChQuaternionD()
qc0 = q3 * q2
qcy = chrono.ChQuaternionD()
qcy.Q_from_AngAxis(math.pi, qc0.GetYaxis())
qc = chrono.ChQuaternionD()
qc = qcy * qc0
q = Quat(qc.e0, qc.e1, qc.e2, qc.e3)
self.mycamera.setPos(pos.x, pos.y, pos.z)
self.mycamera.setQuat(q)
def __init__(self, render):
self.render = render
self.observation_space = np.empty([9, 1])
self.action_space = np.empty([
3,
])
self.info = {}
self.timestep = 0.01
# ---------------------------------------------------------------------
#
# Create the simulation system and add items
#
self.timeend = 30
# Create the vehicle system
chrono.SetChronoDataPath("/home/aaron/chrono/data/")
veh.SetDataPath("/home/aaron/chrono/data/vehicle/")
# JSON file for vehicle model
self.vehicle_file = veh.GetDataPath(
) + "hmmwv/vehicle/HMMWV_Vehicle.json"
# JSON files for terrain
self.rigidterrain_file = veh.GetDataPath() + "terrain/RigidPlane.json"
# JSON file for powertrain (simple)
self.simplepowertrain_file = veh.GetDataPath(
) + "generic/powertrain/SimplePowertrain.json"
# JSON files tire models (rigid)
self.rigidtire_file = veh.GetDataPath(
) + "hmmwv/tire/HMMWV_RigidTire.json"
# Initial vehicle position
self.initLoc = chrono.ChVectorD(-125, -130, 0.5)
# Initial vehicle orientation
self.initRot = chrono.ChQuaternionD(1, 0, 0, 0)
# Rigid terrain dimensions
self.terrainHeight = 0
self.terrainLength = 300.0 # size in X direction
self.terrainWidth = 300.0 # size in Y direction
# Point on chassis tracked by the camera (Irrlicht only)
self.trackPoint = chrono.ChVectorD(0.0, 0.0, 1.75)
self.dist = 5.0
self.generator = RandomPathGenerator(width=100,
height=100,
maxDisplacement=2,
steps=1)
self.tracknum = 0
def Advance(self, step):
if self.irrlicht:
if not self.app.GetDevice().run():
return -1
if self.step_number % self.render_steps == 0:
self.app.BeginScene(True, True,
chronoirr.SColor(255, 140, 161, 192))
self.app.DrawAll()
self.app.EndScene()
# Update modules (process inputs from other modules)
time = self.system.GetChTime()
self.vehicle.Synchronize(time)
self.terrain.Synchronize(time)
if self.irrlicht:
self.app.Synchronize("", self.vehicle.driver.GetInputs())
if self.obstacles != None:
for n in range(len(self.obstacles)):
i = list(self.obstacles)[n]
obstacle = self.obstacles[i]
if obstacle.Update(step):
self.obstacles = RandomObstacleGenerator.moveObstacle(
self.track.center, self.obstacles, obstacle, i)
self.boxes[n].SetPos(obstacle.p1)
q = chrono.ChQuaternionD()
v1 = obstacle.p2 - obstacle.p1
v2 = chrono.ChVectorD(1, 0, 0)
ang = math.atan2((v1 % v2).Length(), v1 ^ v2)
if chrono.ChVectorD(0, 0, 1) ^ (v1 % v2) > 0.0:
ang *= -1
q.Q_from_AngZ(ang)
self.boxes[n].SetRot(q)
if self.opponents != None:
for opponent in self.opponents:
opponent.Update(time, step)
# Advance simulation for one timestep for all modules
self.vehicle.Advance(step)
self.terrain.Advance(step)
if self.irrlicht:
self.app.Advance(step)
self.step_number += 1
if self.pov:
self.pov_exporter.ExportData()
self.system.DoStepDynamics(step)
def DrawBarriers(self, points, n=5, height=1, width=1):
points = points[::n]
if points[-1] != points[0]:
points.append(points[-1])
for i in range(len(points) - 1):
p1 = points[i]
p2 = points[i + 1]
box = chrono.ChBodyEasyBox((p2 - p1).Length(), height, width, 1000,
True, True)
box.SetPos(p1)
q = chrono.ChQuaternionD()
v1 = p2 - p1
v2 = chrono.ChVectorD(1, 0, 0)
ang = math.atan2((v1 % v2).Length(), v1 ^ v2)
if chrono.ChVectorD(0, 0, 1) ^ (v1 % v2) > 0.0:
ang *= -1
q.Q_from_AngZ(ang)
box.SetRot(q)
box.SetBodyFixed(True)
box_asset = box.GetAssets()[0]
visual_asset = chrono.CastToChVisualization(box_asset)
vis_mat = chrono.ChVisualMaterial()
vis_mat.SetAmbientColor(chrono.ChVectorF(0, 0, 0))
if i % 2 == 0:
vis_mat.SetDiffuseColor(chrono.ChVectorF(1.0, 0, 0))
else:
vis_mat.SetDiffuseColor(chrono.ChVectorF(1.0, 1.0, 1.0))
vis_mat.SetSpecularColor(chrono.ChVectorF(0.9, 0.9, 0.9))
vis_mat.SetFresnelMin(0)
vis_mat.SetFresnelMax(0.1)
visual_asset.material_list.append(vis_mat)
color = chrono.ChColorAsset()
if i % 2 == 0:
color.SetColor(chrono.ChColor(1, 0, 0))
else:
color.SetColor(chrono.ChColor(1, 1, 1))
box.AddAsset(color)
self.system.Add(box)
self.barriers.append(box)
def calcPose(p1, p2, z=0.0):
""" Calculates pose (position and orientation) from two points """
if isinstance(p1, list):
p1 = chrono.ChVectorD(p1[0], p1[1], z)
p2 = chrono.ChVectorD(p2[0], p2[1], z)
loc = p1
rot = chrono.ChQuaternionD()
v1 = p2 - p1
v2 = chrono.ChVectorD(1, 0, 0)
ang = math.atan2((v1 % v2).Length(), v1 ^ v2)
if chrono.ChVectorD(0, 0, 1) ^ (v1 % v2) > 0.0:
ang *= -1
rot.Q_from_AngZ(ang)
return loc, rot
def ChFrame_from_json(j: dict):
"""Creates a ChFrame from a json object.
Args:
j (dict): The json object that will be converted to a ChFrame
Returns:
ChFrameD: The frame created from the json object
"""
# Validate the json file
_check_field(j, 'Position', field_type=list)
_check_field(j, 'Rotation', field_type=list)
# Do the conversion
pos = j['Position']
rot = j['Rotation']
return chrono.ChFrameD(
chrono.ChVectorD(pos[0], pos[1], pos[2]),
chrono.ChQuaternionD(rot[0], rot[1], rot[2], rot[3]))
def DrawObstacles(self, obstacles, z=0.0):
self.boxes = []
for i, o in obstacles.items():
p1 = o.p1
p2 = o.p2
box = chrono.ChBodyEasyBox(o.length, o.width, 1, 1000, True, True)
box.SetPos(p1)
q = chrono.ChQuaternionD()
v1 = p2 - p1
v2 = chrono.ChVectorD(1, 0, 0)
ang = math.atan2((v1 % v2).Length(), v1 ^ v2)
if chrono.ChVectorD(0, 0, 1) ^ (v1 % v2) > 0.0:
ang *= -1
q.Q_from_AngZ(ang)
box.SetRot(q)
box.SetBodyFixed(True)
box_asset = box.GetAssets()[0]
visual_asset = chrono.CastToChVisualization(box_asset)
self.system.Add(box)
self.boxes.append(box)
def CalcInitialPose(p1: chrono.ChVectorD,
p2: chrono.ChVectorD,
z=0.1,
reversed=0):
if not isinstance(p1, chrono.ChVectorD):
raise TypeError
elif not isinstance(p2, chrono.ChVectorD):
raise TypeError
p1.z = p2.z = z
initLoc = p1
vec = p2 - p1
theta = math.atan2((vec % chrono.ChVectorD(1, 0, 0)).Length(),
vec ^ chrono.ChVectorD(1, 0, 0))
if reversed:
theta *= -1
initRot = chrono.ChQuaternionD()
initRot.Q_from_AngZ(theta)
return initLoc, initRot
def __init__(self):
ChronoBaseEnv.__init__(self)
SetChronoDataDirectories()
# Define action and observation space
# They must be gym.spaces objects
# Example when using discrete actions:
self.camera_width = 80
self.camera_height = 45
self.action_space = spaces.Box(low=-1.0,
high=1.0,
shape=(2, ),
dtype=np.float32)
self.observation_space = spaces.Box(low=0,
high=255,
shape=(self.camera_height,
self.camera_width, 3),
dtype=np.uint8)
self.info = {"timeout": 10000.0}
self.timestep = 3e-3
# ---------------------------------------------------------------------
#
# Create the simulation system and add items
#
self.Xtarg = 100.0
self.Ytarg = 0.0
self.timeend = 20
self.control_frequency = 10
self.initLoc = chrono.ChVectorD(0, 0, 1.0)
self.initRot = chrono.ChQuaternionD(1, 0, 0, 0)
self.terrain_model = veh.RigidTerrain.PatchType_BOX
self.terrainHeight = 0 # terrain height (FLAT terrain only)
self.terrainLength = 250.0 # size in X direction
self.terrainWidth = 15.0 # size in Y direction
self.render_setup = False
self.play_mode = False
self.manager = 0
if hasattr(builtins, 'exported_system_relpath'):
shapes_dir = builtins.exported_system_relpath + shapes_dir
exported_items = []
body_0 = chrono.ChBodyAuxRef()
body_0.SetName('ground')
body_0.SetBodyFixed(True)
exported_items.append(body_0)
# Rigid body part
body_1 = chrono.ChBodyAuxRef()
body_1.SetName('escape_wheel^escapement-1')
body_1.SetPos(chrono.ChVectorD(0, 0.000381819559584939, 0))
body_1.SetRot(chrono.ChQuaternionD(0, 0, 0.707106781186548, 0.707106781186547))
body_1.SetMass(0.385093622816182)
body_1.SetInertiaXX(
chrono.ChVectorD(0.000614655341550614, 0.00114774663635329,
0.000614655341550614))
body_1.SetInertiaXY(
chrono.ChVectorD(1.04945260437012e-19, -5.29910899706164e-19,
5.85921324575995e-19))
body_1.SetFrame_COG_to_REF(
chrono.ChFrameD(
chrono.ChVectorD(-1.29340068058665e-17,
4.10138104133823e-17, 0.00633921901294084),
chrono.ChQuaternionD(1, 0, 0, 0)))
# Visualization shape
body_1_1_shape = chrono.ChObjShapeFile()
# Iterate over added bodies (Python style)
print ('Positions of all bodies in the system:')
for abody in my_system.Get_bodylist():
print (' body pos=', abody.GetPos() )
# Move a body, using a ChFrame
my_displacement = chrono.ChFrameMovingD(chrono.ChVectorD(5,1,0));
my_shbodyA %= my_displacement
# ..also as:
# my_shbody.ConcatenatePreTransformation(my_displacement)
print ('Moved body pos=', my_shbodyA.GetPos() )
# Use a body with an auxiliary reference (REF) that does not correspond
# to the center of gravity (COG)
body_1= chrono.ChBodyAuxRef()
body_1.SetName('Parte1-1')
body_1.SetPos(chrono.ChVectorD(-0.0445347481124079,0.0676266363930238,-0.0230808979433518))
body_1.SetRot(chrono.ChQuaternionD(1,0,0,0))
body_1.SetMass(346.17080777653)
body_1.SetInertiaXX(chrono.ChVectorD(48583.2418823358,526927.118351673,490689.966726565))
body_1.SetInertiaXY(chrono.ChVectorD(1.70380722975012e-11,1.40840344485366e-11,-2.31869065456271e-12))
body_1.SetFrame_COG_to_REF(chrono.ChFrameD(chrono.ChVectorD(68.9923703887577,-60.1266363930238,70.1327223302498),chrono.ChQuaternionD(1,0,0,0)))
myasset = chrono.ChObjShapeFile()
myasset.SetFilename("shapes/test.obj")
body_1.GetAssets().push_back(myasset)
print ('Done...')
def main():
#print("Copyright (c) 2017 projectchrono.org\nChrono version: ", CHRONO_VERSION , "\n\n")
# Create the HMMWV vehicle, set parameters, and initialize
my_hmmwv = veh.HMMWV_Full()
my_hmmwv.SetContactMethod(chrono.ChContactMethod_SMC)
my_hmmwv.SetInitPosition(
chrono.ChCoordsysD(chrono.ChVectorD(-5, -2, 0.6),
chrono.ChQuaternionD(1, 0, 0, 0)))
my_hmmwv.SetPowertrainType(veh.PowertrainModelType_SHAFTS)
my_hmmwv.SetDriveType(veh.DrivelineType_AWD)
my_hmmwv.SetTireType(veh.TireModelType_RIGID)
my_hmmwv.Initialize()
my_hmmwv.SetChassisVisualizationType(veh.VisualizationType_NONE)
my_hmmwv.SetSuspensionVisualizationType(veh.VisualizationType_PRIMITIVES)
my_hmmwv.SetSteeringVisualizationType(veh.VisualizationType_PRIMITIVES)
my_hmmwv.SetWheelVisualizationType(veh.VisualizationType_NONE)
my_hmmwv.SetTireVisualizationType(veh.VisualizationType_MESH)
# Create the (custom) driver
driver = MyDriver(my_hmmwv.GetVehicle(), 0.5)
driver.Initialize()
# Create the SCM deformable terrain patch
terrain = veh.SCMDeformableTerrain(my_hmmwv.GetSystem())
terrain.SetSoilParameters(
2e6, # Bekker Kphi
0, # Bekker Kc
1.1, # Bekker n exponent
0, # Mohr cohesive limit (Pa)
30, # Mohr friction limit (degrees)
0.01, # Janosi shear coefficient (m)
2e8, # Elastic stiffness (Pa/m), before plastic yield
3e4 # Damping (Pa s/m), proportional to negative vertical speed (optional)
)
# Optionally, enable moving patch feature (single patch around vehicle chassis)
terrain.AddMovingPatch(my_hmmwv.GetChassisBody(),
chrono.ChVectorD(0, 0,
0), chrono.ChVectorD(5, 3, 1))
# Set plot type for SCM (false color plotting)
terrain.SetPlotType(veh.SCMDeformableTerrain.PLOT_SINKAGE, 0, 0.1)
# Initialize the SCM terrain, specifying the initial mesh grid
terrain.Initialize(terrainLength, terrainWidth, delta)
# Create the vehicle Irrlicht interface
app = veh.ChWheeledVehicleIrrApp(my_hmmwv.GetVehicle(),
'HMMWV Deformable Soil Demo',
irr.dimension2du(1000, 800))
app.SetSkyBox()
app.AddTypicalLights(irr.vector3df(30, -30, 100),
irr.vector3df(30, 50, 100), 250, 130)
app.AddTypicalLogo(chrono.GetChronoDataFile('logo_pychrono_alpha.png'))
app.SetChaseCamera(chrono.ChVectorD(0.0, 0.0, 1.75), 6.0, 0.5)
app.SetTimestep(step_size)
app.AssetBindAll()
app.AssetUpdateAll()
# Simulation loop
while (app.GetDevice().run()):
time = my_hmmwv.GetSystem().GetChTime()
# End simulation
if (time >= 4):
break
# Draw scene
app.BeginScene(True, True, irr.SColor(255, 140, 161, 192))
app.DrawAll()
app.EndScene()
# Get driver inputs
driver_inputs = driver.GetInputs()
# Update modules (process inputs from other modules)
driver.Synchronize(time)
terrain.Synchronize(time)
my_hmmwv.Synchronize(time, driver_inputs, terrain)
app.Synchronize("", driver_inputs)
# Advance simulation for one timestep for all modules
driver.Advance(step_size)
terrain.Advance(step_size)
my_hmmwv.Advance(step_size)
app.Advance(step_size)
return 0
def reset(self):
#print("reset")
self.isdone = False
self.rev_pend_sys.Clear()
# create it
self.body_rod = chrono.ChBody()
# set initial position
self.body_rod.SetPos(chrono.ChVectorD(0, self.size_rod_y / 2, 0))
# set mass properties
self.body_rod.SetMass(self.mass_rod)
self.body_rod.SetInertiaXX(
chrono.ChVectorD(self.inertia_rod_x, self.inertia_rod_y,
self.inertia_rod_x))
# set collision surface properties
self.body_rod.SetMaterialSurface(self.rod_material)
self.cyl_base1 = chrono.ChVectorD(0, -self.size_rod_y / 2, 0)
self.cyl_base2 = chrono.ChVectorD(0, self.size_rod_y / 2, 0)
self.body_rod_shape = chrono.ChCylinderShape()
self.body_rod_shape.GetCylinderGeometry().p1 = self.cyl_base1
self.body_rod_shape.GetCylinderGeometry().p2 = self.cyl_base2
self.body_rod_shape.GetCylinderGeometry().rad = self.radius_rod
self.body_rod.AddAsset(self.body_rod_shape)
self.rev_pend_sys.Add(self.body_rod)
self.body_floor = chrono.ChBody()
self.body_floor.SetBodyFixed(True)
self.body_floor.SetPos(chrono.ChVectorD(0, -5, 0))
self.body_floor.SetMaterialSurface(self.rod_material)
self.body_floor_shape = chrono.ChBoxShape()
self.body_floor_shape.GetBoxGeometry().Size = chrono.ChVectorD(3, 1, 3)
self.body_floor.GetAssets().push_back(self.body_floor_shape)
self.body_floor_texture = chrono.ChTexture()
self.body_floor_texture.SetTextureFilename(chrono.GetChronoDataPath() +
'/concrete.jpg')
self.body_floor.GetAssets().push_back(self.body_floor_texture)
self.rev_pend_sys.Add(self.body_floor)
self.body_table = chrono.ChBody()
self.body_table.SetPos(chrono.ChVectorD(0, -self.size_table_y / 2, 0))
self.body_table.SetMaterialSurface(self.rod_material)
self.body_table.SetMass(0.1)
self.body_table_shape = chrono.ChBoxShape()
self.body_table_shape.GetBoxGeometry().Size = chrono.ChVectorD(
self.size_table_x / 2, self.size_table_y / 2,
self.size_table_z / 2)
self.body_table_shape.SetColor(chrono.ChColor(0.4, 0.4, 0.5))
self.body_table.GetAssets().push_back(self.body_table_shape)
self.body_table_texture = chrono.ChTexture()
self.body_table_texture.SetTextureFilename(chrono.GetChronoDataPath() +
'/concrete.jpg')
self.body_table.GetAssets().push_back(self.body_table_texture)
self.rev_pend_sys.Add(self.body_table)
self.link_slider = chrono.ChLinkLockPrismatic()
z2x = chrono.ChQuaternionD()
z2x.Q_from_AngAxis(-chrono.CH_C_PI / 2, chrono.ChVectorD(0, 1, 0))
self.link_slider.Initialize(
self.body_table, self.body_floor,
chrono.ChCoordsysD(chrono.ChVectorD(0, 0, 0), z2x))
self.rev_pend_sys.Add(self.link_slider)
self.act_initpos = chrono.ChVectorD(0, 0, 0)
self.actuator = chrono.ChLinkMotorLinearForce()
self.actuator.Initialize(self.body_table, self.body_floor,
chrono.ChFrameD(self.act_initpos))
self.rev_pend_sys.Add(self.actuator)
self.rod_pin = chrono.ChMarker()
self.body_rod.AddMarker(self.rod_pin)
self.rod_pin.Impose_Abs_Coord(
chrono.ChCoordsysD(chrono.ChVectorD(0, 0, 0)))
self.table_pin = chrono.ChMarker()
self.body_table.AddMarker(self.table_pin)
self.table_pin.Impose_Abs_Coord(
chrono.ChCoordsysD(chrono.ChVectorD(0, 0, 0)))
self.pin_joint = chrono.ChLinkLockRevolute()
self.pin_joint.Initialize(self.rod_pin, self.table_pin)
self.rev_pend_sys.Add(self.pin_joint)
if self.render_setup:
self.myapplication.AssetBindAll()
self.myapplication.AssetUpdateAll()
self.isdone = False
self.steps = 0
self.step(np.array([[0]]))
return self.get_ob()
cyl_g.GetCylinderGeometry().rad = 0.03
ground.AddAsset(cyl_g)
col_g = chrono.ChColorAsset()
col_g.SetColor(chrono.ChColor(0.6, 0.6, 0.2))
ground.AddAsset(col_g)
## Crank
crank = chrono.ChBody()
system.AddBody(crank)
crank.SetIdentifier(1)
crank.SetName("crank")
crank.SetMass(1.0)
crank.SetInertiaXX(chrono.ChVectorD(0.005, 0.1, 0.1))
crank.SetPos(chrono.ChVectorD(-1, 0, 0))
crank.SetRot(chrono.ChQuaternionD(1, 0, 0, 0))
box_c = chrono.ChBoxShape()
box_c.GetBoxGeometry().Size = chrono.ChVectorD(0.95, 0.05, 0.05)
crank.AddAsset(box_c)
cyl_c = chrono.ChCylinderShape()
cyl_c.GetCylinderGeometry().p1 = chrono.ChVectorD(1, 0.1, 0)
cyl_c.GetCylinderGeometry().p2 = chrono.ChVectorD(1, -0.1, 0)
cyl_c.GetCylinderGeometry().rad = 0.05
crank.AddAsset(cyl_c)
sph_c = chrono.ChSphereShape()
sph_c.GetSphereGeometry().center = chrono.ChVectorD(-1, 0, 0)
sph_c.GetSphereGeometry().rad = 0.05
crank.AddAsset(sph_c)
def main():
#print("Copyright (c) 2017 projectchrono.org\nChrono version: ", CHRONO_VERSION , "\n\n")
# Create the HMMWV vehicle, set parameters, and initialize
my_hmmwv = veh.HMMWV_Full()
my_hmmwv.SetContactMethod(contact_method)
my_hmmwv.SetChassisFixed(False)
my_hmmwv.SetInitPosition(
chrono.ChCoordsysD(initLoc, chrono.ChQuaternionD(1, 0, 0, 0)))
my_hmmwv.SetPowertrainType(powertrain_model)
my_hmmwv.SetDriveType(drive_type)
my_hmmwv.SetSteeringType(steering_type)
my_hmmwv.SetTireType(tire_model)
my_hmmwv.SetTireStepSize(tire_step_size)
my_hmmwv.Initialize()
my_hmmwv.SetChassisVisualizationType(chassis_vis_type)
my_hmmwv.SetSuspensionVisualizationType(suspension_vis_type)
my_hmmwv.SetSteeringVisualizationType(steering_vis_type)
my_hmmwv.SetWheelVisualizationType(wheel_vis_type)
my_hmmwv.SetTireVisualizationType(tire_vis_type)
# Create the terrain
terrain = veh.RigidTerrain(my_hmmwv.GetSystem())
if (contact_method == chrono.ChContactMethod_NSC):
patch_mat = chrono.ChMaterialSurfaceNSC()
patch_mat.SetFriction(0.9)
patch_mat.SetRestitution(0.01)
elif (contact_method == chrono.ChContactMethod_SMC):
patch_mat = chrono.ChMaterialSurfaceSMC()
patch_mat.SetFriction(0.9)
patch_mat.SetRestitution(0.01)
patch_mat.SetYoungModulus(2e7)
patch = terrain.AddPatch(patch_mat, chrono.ChVectorD(0, 0, 0),
chrono.ChVectorD(0, 0, 1), 300, 50)
patch.SetTexture(veh.GetDataFile("terrain/textures/tile4.jpg"), 200, 200)
patch.SetColor(chrono.ChColor(0.8, 0.8, 0.5))
terrain.Initialize()
# Create the path-follower, cruise-control driver
# Use a parameterized ISO double lane change (to left)
path = veh.DoubleLaneChangePath(initLoc, 13.5, 4.0, 11.0, 50.0, True)
driver = veh.ChPathFollowerDriver(my_hmmwv.GetVehicle(), path, "my_path",
target_speed)
driver.GetSteeringController().SetLookAheadDistance(5)
driver.GetSteeringController().SetGains(0.8, 0, 0)
driver.GetSpeedController().SetGains(0.4, 0, 0)
driver.Initialize()
# Create the vehicle Irrlicht interface
app = veh.ChWheeledVehicleIrrApp(my_hmmwv.GetVehicle(), 'HMMWV',
irr.dimension2du(1000, 800))
app.SetSkyBox()
app.AddTypicalLights(irr.vector3df(-60, -30, 100),
irr.vector3df(60, 30, 100), 250, 130)
app.AddTypicalLogo(chrono.GetChronoDataFile('logo_pychrono_alpha.png'))
app.SetChaseCamera(chrono.ChVectorD(0.0, 0.0, 1.75), 6.0, 0.5)
app.SetTimestep(step_size)
app.AssetBindAll()
app.AssetUpdateAll()
# Visualization of controller points (sentinel & target)
ballS = app.GetSceneManager().addSphereSceneNode(0.1)
ballT = app.GetSceneManager().addSphereSceneNode(0.1)
ballS.getMaterial(0).EmissiveColor = irr.SColor(0, 255, 0, 0)
ballT.getMaterial(0).EmissiveColor = irr.SColor(0, 0, 255, 0)
# Simulation loop
realtime_timer = chrono.ChRealtimeStepTimer()
while (app.GetDevice().run()):
time = my_hmmwv.GetSystem().GetChTime()
# End simulation
if (time >= t_end):
break
# Update sentinel and target location markers for the path-follower controller.
pS = driver.GetSteeringController().GetSentinelLocation()
pT = driver.GetSteeringController().GetTargetLocation()
ballS.setPosition(irr.vector3df(pS.x, pS.y, pS.z))
ballT.setPosition(irr.vector3df(pT.x, pT.y, pT.z))
# Draw scene
app.BeginScene(True, True, irr.SColor(255, 140, 161, 192))
app.DrawAll()
app.EndScene()
# Get driver inputs
driver_inputs = driver.GetInputs()
# Update modules (process inputs from other modules)
driver.Synchronize(time)
terrain.Synchronize(time)
my_hmmwv.Synchronize(time, driver_inputs, terrain)
app.Synchronize("", driver_inputs)
# Advance simulation for one timestep for all modules
driver.Advance(step_size)
terrain.Advance(step_size)
my_hmmwv.Advance(step_size)
app.Advance(step_size)
# Spin in place for real time to catch up
realtime_timer.Spin(step_size)
return 0
chrono.GetLog().Bar()
# Test vectors
my_vect1 = chrono.ChVectorD()
my_vect1.x = 5
my_vect1.y = 2
my_vect1.z = 3
my_vect2 = chrono.ChVectorD(3, 4, 5)
my_vect4 = my_vect1 * 10 + my_vect2
my_len = my_vect4.Length()
print('vect sum =', my_vect1 + my_vect2)
print('vect cross =', my_vect1 % my_vect2)
print('vect dot =', my_vect1 ^ my_vect2)
# Test quaternions
my_quat = chrono.ChQuaternionD(1, 2, 3, 4)
my_qconjugate = ~my_quat
print('quat. conjugate =', my_qconjugate)
print('quat. dot product=', my_qconjugate ^ my_quat)
print('quat. product=', my_qconjugate % my_quat)
# Test matrices and NumPy interoperability
mlist = [[1, 2, 3, 4], [5, 6, 7, 8], [9, 10, 11, 12], [13, 14, 15, 16]]
ma = chrono.ChMatrixDynamicD()
ma.SetMatr(
mlist) # Create a Matrix from a list. Size is adjusted automatically.
npmat = np.asarray(ma.GetMatr(
)) # Create a 2D npy array from the list extracted from ChMatrixDynamic
w, v = LA.eig(npmat) # get eigenvalues and eigenvectors using numpy
mb = chrono.ChMatrixDynamicD(4, 4)
prod = v * npmat # you can perform linear algebra operations with numpy and then feed results into a ChMatrixDynamicD using SetMatr
def __init__(self, step_size, sys, irrlicht=False, terrain_type='json', height=-0.5, width=300, length=300):
# Chrono parameters
self.step_size = step_size
self.irrlicht = irrlicht
self.step_number = 0
# Rigid terrain dimensions
self.height = height
self.length = length # size in X direction
self.width = width # size in Y direction
self.sys = sys
if terrain_type == 'json':
import os
# JSON files for terrain
self.rigidterrain_file = veh.GetDataPath() + os.path.join('terrain', 'RigidPlane.json')
checkFile(self.rigidterrain_file)
# Create the ground
self.terrain = veh.RigidTerrain(self.sys, self.rigidterrain_file)
elif terrain_type == 'concrete':
# Create the terrain
self.terrain = veh.RigidTerrain(self.sys)
patch = self.terrain.AddPatch(chrono.ChCoordsysD(chrono.ChVectorD(0, 0, self.height - 5), chrono.QUNIT),
chrono.ChVectorD(self.width, self.length, 10))
patch.SetContactFrictionCoefficient(0.9)
patch.SetContactRestitutionCoefficient(0.01)
patch.SetContactMaterialProperties(2e7, 0.3)
patch.SetTexture(chrono.GetChronoDataFile("concrete.jpg"), self.length, self.width)
patch.SetColor(chrono.ChColor(0.8, 0.8, 0.5))
self.terrain.Initialize()
try:
ground_body = patch.GetGroundBody()
ground_asset = ground_body.GetAssets()[0]
visual_asset = chrono.CastToChVisualization(ground_asset)
vis_mat = chrono.ChVisualMaterial()
vis_mat.SetKdTexture(chrono.GetChronoDataFile("concrete.jpg"))
vis_mat.SetFresnelMax(0);
visual_asset.material_list.append(vis_mat)
except:
print("Not Visual Material")
elif terrain_type == 'hallway':
y_max = 5.65
x_max = 23
offset = chrono.ChVectorD(-x_max/2, -y_max/2, .21)
offsetF = chrono.ChVectorF(offset.x, offset.y, offset.z)
self.terrain = veh.RigidTerrain(self.sys)
coord_sys = chrono.ChCoordsysD(offset, chrono.ChQuaternionD(1,0,0,0))
patch = self.terrain.AddPatch(coord_sys, chrono.GetChronoDataFile("sensor/textures/hallway.obj"), "mesh", 0.01, False)
vis_mesh = chrono.ChTriangleMeshConnected()
vis_mesh.LoadWavefrontMesh(chrono.GetChronoDataFile("sensor/textures/hallway.obj"), True, True)
trimesh_shape = chrono.ChTriangleMeshShape()
trimesh_shape.SetMesh(vis_mesh)
trimesh_shape.SetName("mesh_name")
trimesh_shape.SetStatic(True)
patch.GetGroundBody().AddAsset(trimesh_shape)
patch.SetContactFrictionCoefficient(0.9)
patch.SetContactRestitutionCoefficient(0.01)
patch.SetContactMaterialProperties(2e7, 0.3)
self.terrain.Initialize()
def reset(self):
#print("reset")
self.isdone = False
self.rev_pend_sys.Clear()
# create it
self.body_rod = chrono.ChBody()
# set initial position
self.body_rod.SetPos(chrono.ChVectorD(0, self.size_rod_y / 2, 0))
# set mass properties
self.body_rod.SetMass(self.mass_rod)
self.body_rod.SetInertiaXX(
chrono.ChVectorD(self.inertia_rod_x, self.inertia_rod_y,
self.inertia_rod_x))
# set collision surface properties
self.body_rod.SetMaterialSurface(self.rod_material)
# Visualization shape, for rendering animation
self.cyl_base1 = chrono.ChVectorD(0, -self.size_rod_y / 2, 0)
self.cyl_base2 = chrono.ChVectorD(0, self.size_rod_y / 2, 0)
self.body_rod_shape = chrono.ChCylinderShape()
self.body_rod_shape.GetCylinderGeometry().p1 = self.cyl_base1
self.body_rod_shape.GetCylinderGeometry().p2 = self.cyl_base2
self.body_rod_shape.GetCylinderGeometry().rad = self.radius_rod
self.body_rod.AddAsset(self.body_rod_shape)
self.rev_pend_sys.Add(self.body_rod)
self.body_floor = chrono.ChBody()
self.body_floor.SetBodyFixed(True)
self.body_floor.SetPos(chrono.ChVectorD(0, -5, 0))
self.body_floor.SetMaterialSurface(self.rod_material)
if self.render:
self.body_floor_shape = chrono.ChBoxShape()
self.body_floor_shape.GetBoxGeometry().Size = chrono.ChVectorD(
3, 1, 3)
self.body_floor.GetAssets().push_back(self.body_floor_shape)
self.body_floor_texture = chrono.ChTexture()
self.body_floor_texture.SetTextureFilename(
'../../../data/concrete.jpg')
self.body_floor.GetAssets().push_back(self.body_floor_texture)
self.rev_pend_sys.Add(self.body_floor)
self.body_table = chrono.ChBody()
self.body_table.SetPos(chrono.ChVectorD(0, -self.size_table_y / 2, 0))
self.body_table.SetMaterialSurface(self.rod_material)
if self.render:
self.body_table_shape = chrono.ChBoxShape()
self.body_table_shape.GetBoxGeometry().Size = chrono.ChVectorD(
self.size_table_x / 2, self.size_table_y / 2,
self.size_table_z / 2)
self.body_table_shape.SetColor(chrono.ChColor(0.4, 0.4, 0.5))
self.body_table.GetAssets().push_back(self.body_table_shape)
self.body_table_texture = chrono.ChTexture()
self.body_table_texture.SetTextureFilename(
'../../../data/concrete.jpg')
self.body_table.GetAssets().push_back(self.body_table_texture)
self.body_table.SetMass(0.1)
self.rev_pend_sys.Add(self.body_table)
self.link_slider = chrono.ChLinkLockPrismatic()
z2x = chrono.ChQuaternionD()
z2x.Q_from_AngAxis(-chrono.CH_C_PI / 2, chrono.ChVectorD(0, 1, 0))
self.link_slider.Initialize(
self.body_table, self.body_floor,
chrono.ChCoordsysD(chrono.ChVectorD(0, 0, 0), z2x))
self.rev_pend_sys.Add(self.link_slider)
self.act_initpos = chrono.ChVectorD(0, 0, 0)
self.actuator = chrono.ChLinkMotorLinearForce()
self.actuator.Initialize(self.body_table, self.body_floor,
chrono.ChFrameD(self.act_initpos))
self.rev_pend_sys.Add(self.actuator)
self.rod_pin = chrono.ChMarker()
self.body_rod.AddMarker(self.rod_pin)
self.rod_pin.Impose_Abs_Coord(
chrono.ChCoordsysD(chrono.ChVectorD(0, 0, 0)))
self.table_pin = chrono.ChMarker()
self.body_table.AddMarker(self.table_pin)
self.table_pin.Impose_Abs_Coord(
chrono.ChCoordsysD(chrono.ChVectorD(0, 0, 0)))
self.pin_joint = chrono.ChLinkLockRevolute()
self.pin_joint.Initialize(self.rod_pin, self.table_pin)
self.rev_pend_sys.Add(self.pin_joint)
if self.render:
# ---------------------------------------------------------------------
#
# Create an Irrlicht application to visualize the system
#
# ==IMPORTANT!== Use this function for adding a ChIrrNodeAsset to all items
# in the system. These ChIrrNodeAsset assets are 'proxies' to the Irrlicht meshes.
# If you need a finer control on which item really needs a visualization proxy
# Irrlicht, just use application.AssetBind(myitem); on a per-item basis.
self.myapplication.AssetBindAll()
# ==IMPORTANT!== Use this function for 'converting' into Irrlicht meshes the assets
# that you added to the bodies into 3D shapes, they can be visualized by Irrlicht!
self.myapplication.AssetUpdateAll()
self.isdone = False
self.steps = 0
self.step(np.array([[0]]))
return self.get_ob()
def reset(self):
x_half_length = 90
y_half_length = 40
self.path = BezierPath(x_half_length, y_half_length, 0.5,
self.interval)
pos, rot = self.path.getPosRot(self.path.current_t - self.interval)
self.initLoc = chrono.ChVectorD(pos)
self.initRot = chrono.ChQuaternionD(rot)
self.vehicle = veh.HMMWV_Reduced()
self.vehicle.SetContactMethod(chrono.ChContactMethod_NSC)
self.surf_material = chrono.ChMaterialSurfaceNSC()
self.vehicle.SetChassisCollisionType(
veh.ChassisCollisionType_PRIMITIVES)
self.vehicle.SetChassisFixed(False)
self.vehicle.SetInitPosition(
chrono.ChCoordsysD(self.initLoc, self.initRot))
self.vehicle.SetPowertrainType(veh.PowertrainModelType_SHAFTS)
self.vehicle.SetDriveType(veh.DrivelineType_AWD)
# self.vehicle.SetSteeringType(veh.SteeringType_PITMAN_ARM)
self.vehicle.SetTireType(veh.TireModelType_TMEASY)
self.vehicle.SetTireStepSize(self.timestep)
self.vehicle.Initialize()
if self.play_mode == True:
# self.vehicle.SetChassisVisualizationType(veh.VisualizationType_MESH)
self.vehicle.SetChassisVisualizationType(
veh.VisualizationType_PRIMITIVES)
self.vehicle.SetWheelVisualizationType(veh.VisualizationType_MESH)
self.vehicle.SetTireVisualizationType(veh.VisualizationType_MESH)
else:
self.vehicle.SetChassisVisualizationType(
veh.VisualizationType_PRIMITIVES)
self.vehicle.SetWheelVisualizationType(
veh.VisualizationType_PRIMITIVES)
self.vehicle.SetSuspensionVisualizationType(
veh.VisualizationType_PRIMITIVES)
self.vehicle.SetSteeringVisualizationType(
veh.VisualizationType_PRIMITIVES)
self.chassis_body = self.vehicle.GetChassisBody()
self.chassis_body.GetCollisionModel().ClearModel()
size = chrono.ChVectorD(3, 2, 0.2)
self.chassis_body.GetCollisionModel().AddBox(self.surf_material,
0.5 * size.x,
0.5 * size.y,
0.5 * size.z)
self.chassis_body.GetCollisionModel().BuildModel()
self.m_inputs = veh.Inputs()
self.system = self.vehicle.GetVehicle().GetSystem()
self.manager = sens.ChSensorManager(self.system)
self.manager.scene.AddPointLight(chrono.ChVectorF(100, 100, 100),
chrono.ChVectorF(1, 1, 1), 5000.0)
self.manager.scene.AddPointLight(chrono.ChVectorF(-100, -100, 100),
chrono.ChVectorF(1, 1, 1), 5000.0)
# Driver
#self.driver = veh.ChDriver(self.vehicle.GetVehicle())
self.terrain = veh.RigidTerrain(self.system)
patch_mat = chrono.ChMaterialSurfaceNSC()
patch_mat.SetFriction(0.9)
patch_mat.SetRestitution(0.01)
patch = self.terrain.AddPatch(patch_mat, chrono.ChVectorD(0, 0, 0),
chrono.ChVectorD(0, 0, 1),
self.terrainLength * 1.5,
self.terrainWidth * 1.5)
patch.SetTexture(veh.GetDataFile("terrain/textures/grass.jpg"), 200,
200)
patch.SetColor(chrono.ChColor(0.8, 0.8, 0.5))
self.terrain.Initialize()
self.groundBody = patch.GetGroundBody()
ground_asset = self.groundBody.GetAssets()[0]
visual_asset = chrono.CastToChVisualization(ground_asset)
vis_mat = chrono.ChVisualMaterial()
vis_mat.SetKdTexture(veh.GetDataFile("terrain/textures/grass.jpg"))
visual_asset.material_list.append(vis_mat)
self.leaders.addLeaders(self.system, self.path)
self.leader_box = self.leaders.getBBox()
self.lead_pos = (self.chassis_body.GetPos() -
self.leaders[0].GetPos()).Length()
# Add obstacles:
self.obstacles = []
self.placeObstacle(8)
# for leader in self.leaders:
# ------------------------------------------------
# Create a self.camera and add it to the sensor manager
# ------------------------------------------------
self.camera = sens.ChCameraSensor(
self.chassis_body, # body camera is attached to
10, # scanning rate in Hz
chrono.ChFrameD(chrono.ChVectorD(1.5, 0, .875)),
# offset pose
self.camera_width, # number of horizontal samples
self.camera_height, # number of vertical channels
chrono.CH_C_PI / 2, # horizontal field of view
6)
self.camera.SetName("Camera Sensor")
self.camera.PushFilter(sens.ChFilterRGBA8Access())
self.manager.AddSensor(self.camera)
# -----------------------------------------------------
# Create a self.gps and add it to the sensor manager
# -----------------------------------------------------
gps_noise_none = sens.ChGPSNoiseNone()
self.AgentGPS = sens.ChGPSSensor(
self.chassis_body, 10,
chrono.ChFrameD(
chrono.ChVectorD(0, 0, 0),
chrono.Q_from_AngAxis(0, chrono.ChVectorD(0, 1, 0))),
self.origin, gps_noise_none)
self.AgentGPS.SetName("AgentGPS Sensor")
self.AgentGPS.PushFilter(sens.ChFilterGPSAccess())
self.manager.AddSensor(self.AgentGPS)
### Target GPS
self.TargetGPS = sens.ChGPSSensor(
self.leaders[0], 10,
chrono.ChFrameD(
chrono.ChVectorD(0, 0, 0),
chrono.Q_from_AngAxis(0, chrono.ChVectorD(0, 1, 0))),
self.origin, gps_noise_none)
self.TargetGPS.SetName("TargetGPS Sensor")
self.TargetGPS.PushFilter(sens.ChFilterGPSAccess())
self.manager.AddSensor(self.TargetGPS)
self.step_number = 0
self.num_frames = 0
self.num_updates = 0
self.c_f = 0
self.old_ac = [0, 0]
self.isdone = False
self.render_setup = False
if self.play_mode:
self.render()
return self.get_ob()
# In case you need also damping it would add...
#mdamping = chrono_types::make_shared<ChDampingReissnerRayleigh>(melasticity,0.01)
#mat = chrono_types::make_shared<ChMaterialShellReissner>(melasticity, nullptr, mdamping)
mat.SetDensity(rho)
# Create the nodes
nels_L = 12
nels_W = 1
elarray = [fea.ChElementShellReissner4]*(nels_L * nels_W)
nodearray = [fea.ChNodeFEAxyzrot]*((nels_L + 1) * (nels_W + 1))
nodes_start = [fea.ChNodeFEAxyzrot]*(nels_W + 1)
nodes_end = [fea.ChNodeFEAxyzrot]*(nels_W + 1)
for il in range(nels_L+1) :
for iw in range(nels_W +1):
# Make nodes
nodepos = chrono.ChVectorD(rect_L * (il / nels_L), 0, rect_W * (iw / nels_W))
noderot = chrono.ChQuaternionD(chrono.QUNIT)
nodeframe = chrono.ChFrameD(nodepos, noderot)
mnode = fea.ChNodeFEAxyzrot(nodeframe)
my_mesh.AddNode(mnode)
for i in range(3):
mnode.GetInertia()[i,i] = 0 # approx]
mnode.SetMass(0)
nodearray[il * (nels_W + 1) + iw] = mnode
if (il == 0):
nodes_start[iw] = mnode
if (il == nels_L):
nodes_end[iw] = mnode
# Make elements
if (il > 0 and iw > 0) :
melement = fea.ChElementShellReissner4()
my_mesh.AddElement(melement)
def reset(self):
self.isdone = False
self.ant_sys.Clear()
self.body_abdomen = chrono.ChBody()
self.body_abdomen.SetPos(chrono.ChVectorD(0, self.abdomen_y0, 0 ))
self.body_abdomen.SetMass(self.abdomen_mass)
self.body_abdomen.SetInertiaXX(self.abdomen_inertia)
# set collision surface properties
self.body_abdomen.SetMaterialSurface(self.ant_material)
abdomen_ellipsoid = chrono.ChEllipsoid(chrono.ChVectorD(0, 0, 0 ), chrono.ChVectorD(self.abdomen_x, self.abdomen_y, self.abdomen_z ))
self.abdomen_shape = chrono.ChEllipsoidShape(abdomen_ellipsoid)
self.body_abdomen.AddAsset(self.abdomen_shape)
self.body_abdomen.SetMaterialSurface(self.ant_material)
self.body_abdomen.SetCollide(True)
self.body_abdomen.GetCollisionModel().ClearModel()
self.body_abdomen.GetCollisionModel().AddEllipsoid(self.abdomen_x, self.abdomen_y, self.abdomen_z, chrono.ChVectorD(0, 0, 0 ) )
self.body_abdomen.GetCollisionModel().BuildModel()
self.ant_sys.Add(self.body_abdomen)
leg_ang = (1/4)*math.pi+(1/2)*math.pi*np.array([0,1,2,3])
Leg_quat = [chrono.ChQuaternionD() for i in range(len(leg_ang))]
self.leg_body = [chrono.ChBody() for i in range(len(leg_ang))]
self.leg_pos= [chrono.ChVectorD() for i in range(len(leg_ang))]
leg_cyl = chrono.ChCylinder(-chrono.ChVectorD( self.leg_length/2, 0 ,0),chrono.ChVectorD( self.leg_length/2, 0 ,0), self.leg_radius)
self.leg_shape = chrono.ChCylinderShape(leg_cyl)
ankle_cyl = chrono.ChCylinder(-chrono.ChVectorD( self.ankle_length/2, 0 ,0),chrono.ChVectorD( self.ankle_length/2, 0 ,0), self.ankle_radius)
self.ankle_shape = chrono.ChCylinderShape(ankle_cyl)
foot_sphere = chrono.ChSphere(chrono.ChVectorD(self.ankle_length/2, 0, 0 ), self.ankle_radius )
self.foot_shape = chrono.ChSphereShape(foot_sphere)
Leg_qa = [ chrono.ChQuaternionD() for i in range(len(leg_ang))]
Leg_q = [ chrono.ChQuaternionD() for i in range(len(leg_ang))]
z2x_leg = [ chrono.ChQuaternionD() for i in range(len(leg_ang))]
Leg_rev_pos=[]
Leg_chordsys = []
self.legjoint_frame = []
x_rel = []
z_rel = []
self.Leg_rev = [chrono.ChLinkLockRevolute() for i in range(len(leg_ang))]
self.leg_motor = [chrono.ChLinkMotorRotationTorque() for i in range(len(leg_ang)) ]
#ankle lists
anklejoint_chordsys = []
self.anklejoint_frame = []
self.ankleCOG_frame = []
q_ankle_zrot = [ chrono.ChQuaternionD() for i in range(len(leg_ang))]
self.ankle_body = [chrono.ChBody() for i in range(len(leg_ang))]
self.Ankle_rev = [chrono.ChLinkLockRevolute() for i in range(len(leg_ang))]
self.ankle_motor = [chrono.ChLinkMotorRotationTorque() for i in range(len(leg_ang)) ]
for i in range(len(leg_ang)):
# Legs
Leg_quat[i].Q_from_AngAxis(-leg_ang[i] , chrono.ChVectorD(0, 1, 0))
self.leg_pos[i] = chrono.ChVectorD( (0.5*self.leg_length+self.abdomen_x)*math.cos(leg_ang[i]) ,self.abdomen_y0, (0.5*self.leg_length+self.abdomen_z)*math.sin(leg_ang[i]))
self.leg_body[i].SetPos(self.leg_pos[i])
self.leg_body[i].SetRot(Leg_quat[i])
self.leg_body[i].AddAsset(self.leg_shape)
self.leg_body[i].SetMass(self.leg_mass)
self.leg_body[i].SetInertiaXX(self.leg_inertia)
self.ant_sys.Add(self.leg_body[i])
x_rel.append( Leg_quat[i].Rotate(chrono.ChVectorD(1, 0, 0)))
z_rel.append( Leg_quat[i].Rotate(chrono.ChVectorD(0, 0, 1)))
Leg_qa[i].Q_from_AngAxis(-leg_ang[i] , chrono.ChVectorD(0, 1, 0))
z2x_leg[i].Q_from_AngAxis(chrono.CH_C_PI / 2 , x_rel[i])
Leg_q[i] = z2x_leg[i] * Leg_qa[i]
Leg_rev_pos.append(chrono.ChVectorD(self.leg_pos[i]-chrono.ChVectorD(math.cos(leg_ang[i])*self.leg_length/2,0,math.sin(leg_ang[i])*self.leg_length/2)))
Leg_chordsys.append(chrono.ChCoordsysD(Leg_rev_pos[i], Leg_q[i]))
self.legjoint_frame.append(chrono.ChFrameD(Leg_chordsys[i]))
self.Leg_rev[i].Initialize(self.body_abdomen, self.leg_body[i],Leg_chordsys[i])
self.ant_sys.Add(self.Leg_rev[i])
self.leg_motor[i].Initialize(self.body_abdomen, self.leg_body[i],self.legjoint_frame[i])
self.ant_sys.Add(self.leg_motor[i])
# Ankles
q_ankle_zrot[i].Q_from_AngAxis(-self.ankle_angle , z_rel[i])
anklejoint_chordsys.append(chrono.ChCoordsysD(self.leg_body[i].GetPos()+ self.leg_body[i].GetRot().Rotate(chrono.ChVectorD(self.leg_length/2, 0, 0)) , q_ankle_zrot[i] * self.leg_body[i].GetRot() ))
self.anklejoint_frame.append(chrono.ChFrameD(anklejoint_chordsys[i]))
self.ankle_body[i].SetPos(self.anklejoint_frame[i].GetPos() + self.anklejoint_frame[i].GetRot().Rotate(chrono.ChVectorD(self.ankle_length/2, 0, 0)))
self.ankle_body[i].SetRot( self.anklejoint_frame[i].GetRot() )
self.ankle_body[i].AddAsset(self.ankle_shape)
self.ankle_body[i].SetMass(self.ankle_mass)
self.ankle_body[i].SetInertiaXX(self.ankle_inertia)
self.ant_sys.Add(self.ankle_body[i])
self.Ankle_rev[i].Initialize(self.leg_body[i], self.ankle_body[i], anklejoint_chordsys[i])
self.ant_sys.Add(self.Ankle_rev[i])
self.ankle_motor[i].Initialize(self.leg_body[i], self.ankle_body[i],self.anklejoint_frame[i])
self.ant_sys.Add(self.ankle_motor[i])
# Feet collisions
self.ankle_body[i].SetMaterialSurface(self.ant_material)
self.ankle_body[i].SetCollide(True)
self.ankle_body[i].GetCollisionModel().ClearModel()
self.ankle_body[i].GetCollisionModel().AddSphere(self.ankle_radius, chrono.ChVectorD(self.ankle_length/2, 0, 0 ) )
self.ankle_body[i].GetCollisionModel().BuildModel()
self.ankle_body[i].AddAsset(self.ankle_shape)
self.ankle_body[i].AddAsset(self.foot_shape)
self.Leg_rev[i].GetLimit_Rz().SetActive(True)
self.Leg_rev[i].GetLimit_Rz().SetMin(-math.pi/3)
self.Leg_rev[i].GetLimit_Rz().SetMax(math.pi/3)
self.Ankle_rev[i].GetLimit_Rz().SetActive(True)
self.Ankle_rev[i].GetLimit_Rz().SetMin(-math.pi/2)
self.Ankle_rev[i].GetLimit_Rz().SetMax(math.pi/4)
# Create the room floor: a simple fixed rigid body with a collision shape
# and a visualization shape
self.body_floor = chrono.ChBody()
self.body_floor.SetBodyFixed(True)
self.body_floor.SetPos(chrono.ChVectorD(0, -1, 0 ))
self.body_floor.SetMaterialSurface(self.ant_material)
# Floor Collision.
self.body_floor.SetMaterialSurface(self.ant_material)
self.body_floor.GetCollisionModel().ClearModel()
self.body_floor.GetCollisionModel().AddBox(50, 1, 50, chrono.ChVectorD(0, 0, 0 ))
self.body_floor.GetCollisionModel().BuildModel()
self.body_floor.SetCollide(True)
# Visualization shape
body_floor_shape = chrono.ChBoxShape()
body_floor_shape.GetBoxGeometry().Size = chrono.ChVectorD(5, 1, 5)
body_floor_shape.SetColor(chrono.ChColor(0.4,0.4,0.5))
self.body_floor.GetAssets().push_back(body_floor_shape)
body_floor_texture = chrono.ChTexture()
body_floor_texture.SetTextureFilename(chrono.GetChronoDataFile('vehicle/terrain/textures/grass.jpg'))
self.body_floor.GetAssets().push_back(body_floor_texture)
self.ant_sys.Add(self.body_floor)
#self.body_abdomen.SetBodyFixed(True)
if (self.animate):
self.myapplication.AssetBindAll()
self.myapplication.AssetUpdateAll()
self.numsteps= 0
self.step(np.zeros(8))
return self.get_ob()
def main():
#print("Copyright (c) 2017 projectchrono.org\nChrono version: ", CHRONO_VERSION , "\n\n")
step_size = 0.005
sys = chrono.ChSystemNSC()
sys.Set_G_acc(chrono.ChVectorD(0, 0, -9.81))
sys.SetSolverType(chrono.ChSolver.Type_BARZILAIBORWEIN)
sys.SetSolverMaxIterations(150)
sys.SetMaxPenetrationRecoverySpeed(4.0)
# Create the terrain
terrain = veh.RigidTerrain(sys)
patch_mat = chrono.ChMaterialSurfaceNSC()
patch_mat.SetFriction(0.9)
patch_mat.SetRestitution(0.01)
patch = terrain.AddPatch(patch_mat, chrono.ChVectorD(0, 0, 0),
chrono.ChVectorD(0, 0, 1), 200, 100)
patch.SetColor(chrono.ChColor(0.8, 0.8, 0.5))
patch.SetTexture(veh.GetDataFile("terrain/textures/tile4.jpg"), 200, 200)
terrain.Initialize()
# Create and initialize the first vehicle
hmmwv_1 = veh.HMMWV_Reduced(sys)
hmmwv_1.SetInitPosition(
chrono.ChCoordsysD(chrono.ChVectorD(0, -1.5, 1.0),
chrono.ChQuaternionD(1, 0, 0, 0)))
hmmwv_1.SetPowertrainType(veh.PowertrainModelType_SIMPLE)
hmmwv_1.SetDriveType(veh.DrivelineType_RWD)
hmmwv_1.SetTireType(veh.TireModelType_RIGID)
hmmwv_1.Initialize()
hmmwv_1.SetChassisVisualizationType(veh.VisualizationType_PRIMITIVES)
hmmwv_1.SetSuspensionVisualizationType(veh.VisualizationType_PRIMITIVES)
hmmwv_1.SetSteeringVisualizationType(veh.VisualizationType_PRIMITIVES)
hmmwv_1.SetWheelVisualizationType(veh.VisualizationType_NONE)
hmmwv_1.SetTireVisualizationType(veh.VisualizationType_PRIMITIVES)
driver_data_1 = veh.vector_Entry([
veh.DataDriverEntry(0.0, 0.0, 0.0, 0.0),
veh.DataDriverEntry(0.5, 0.0, 0.0, 0.0),
veh.DataDriverEntry(0.7, 0.3, 0.7, 0.0),
veh.DataDriverEntry(1.0, 0.3, 0.7, 0.0),
veh.DataDriverEntry(3.0, 0.5, 0.1, 0.0)
])
driver_1 = veh.ChDataDriver(hmmwv_1.GetVehicle(), driver_data_1)
driver_1.Initialize()
# Create and initialize the second vehicle
hmmwv_2 = veh.HMMWV_Reduced(sys)
hmmwv_2.SetInitPosition(
chrono.ChCoordsysD(chrono.ChVectorD(7, 1.5, 1.0),
chrono.ChQuaternionD(1, 0, 0, 0)))
hmmwv_2.SetPowertrainType(veh.PowertrainModelType_SIMPLE)
hmmwv_2.SetDriveType(veh.DrivelineType_RWD)
hmmwv_2.SetTireType(veh.TireModelType_RIGID)
hmmwv_2.Initialize()
hmmwv_2.SetChassisVisualizationType(veh.VisualizationType_PRIMITIVES)
hmmwv_2.SetSuspensionVisualizationType(veh.VisualizationType_PRIMITIVES)
hmmwv_2.SetSteeringVisualizationType(veh.VisualizationType_PRIMITIVES)
hmmwv_2.SetWheelVisualizationType(veh.VisualizationType_NONE)
hmmwv_2.SetTireVisualizationType(veh.VisualizationType_PRIMITIVES)
driver_data_2 = veh.vector_Entry([
veh.DataDriverEntry(0.0, 0.0, 0.0, 0.0),
veh.DataDriverEntry(0.5, 0.0, 0.0, 0.0),
veh.DataDriverEntry(0.7, -0.3, 0.7, 0.0),
veh.DataDriverEntry(1.0, -0.3, 0.7, 0.0),
veh.DataDriverEntry(3.0, -0.5, 0.1, 0.0)
])
driver_2 = veh.ChDataDriver(hmmwv_2.GetVehicle(), driver_data_2)
driver_2.Initialize()
# Create the vehicle Irrlicht interface
app = veh.ChWheeledVehicleIrrApp(hmmwv_1.GetVehicle(), 'Two Car Demo',
irr.dimension2du(1000, 800))
app.SetSkyBox()
app.AddTypicalLights(irr.vector3df(30, -30, 100),
irr.vector3df(30, 50, 100), 250, 130)
app.AddTypicalLogo(chrono.GetChronoDataFile('logo_pychrono_alpha.png'))
app.SetChaseCamera(chrono.ChVectorD(0.0, 0.0, 0.75), 6.0, 0.5)
app.SetChaseCameraState(veh.ChChaseCamera.Track)
app.SetChaseCameraPosition(chrono.ChVectorD(-15, 0, 2.0))
app.SetTimestep(step_size)
app.AssetBindAll()
app.AssetUpdateAll()
# Simulation loop
realtime_timer = chrono.ChRealtimeStepTimer()
while (app.GetDevice().run()):
time = hmmwv_1.GetSystem().GetChTime()
app.BeginScene(True, True, irr.SColor(255, 140, 161, 192))
app.DrawAll()
app.EndScene()
# Get driver inputs
driver_inputs_1 = driver_1.GetInputs()
driver_inputs_2 = driver_2.GetInputs()
# Update modules (process inputs from other modules)
driver_1.Synchronize(time)
driver_2.Synchronize(time)
hmmwv_1.Synchronize(time, driver_inputs_1, terrain)
hmmwv_2.Synchronize(time, driver_inputs_2, terrain)
terrain.Synchronize(time)
app.Synchronize("", driver_inputs_1)
# Advance simulation for one timestep for all modules
driver_1.Advance(step_size)
driver_2.Advance(step_size)
hmmwv_1.Advance(step_size)
hmmwv_2.Advance(step_size)
terrain.Advance(step_size)
app.Advance(step_size)
# Advance state of entire system (containing both vehicles)
sys.DoStepDynamics(step_size)
# Spin in place for real time to catch up
realtime_timer.Spin(step_size)
return 0
def __init__(self, step_size, sys, controller, irrlicht=False, vehicle_type='json', initLoc=chrono.ChVectorD(0,0,0), initRot=chrono.ChQuaternionD(1,0,0,0), vis_balls=False, render_step_size=1.0/60):
# Chrono parameters
self.step_size = step_size
self.irrlicht = irrlicht
self.step_number = 0
# Vehicle controller
self.controller = controller
# Initial vehicle position
self.initLoc = initLoc
# Initial vehicle orientation
self.initRot = initRot
# Point on chassis tracked by the camera (Irrlicht only)
self.trackPoint = chrono.ChVectorD(0.0, 0.0, 1.75)
if vehicle_type == 'json':
# JSON file for vehicle model
self.vehicle_file = veh.GetDataPath() + os.path.join('hmmwv', 'vehicle', 'HMMWV_Vehicle.json')
checkFile(self.vehicle_file)
# JSON file for powertrain (simple)
self.simplepowertrain_file = veh.GetDataPath() + os.path.join('generic', 'powertrain', 'SimplePowertrain.json')
checkFile(self.simplepowertrain_file)
# JSON files tire models (rigid)
self.rigidtire_file = veh.GetDataPath() + os.path.join('hmmwv', 'tire', 'HMMWV_RigidTire.json')
checkFile(self.rigidtire_file)
# --------------------------
# Create the various modules
# --------------------------
if sys == None:
self.wheeled_vehicle = veh.WheeledVehicle(self.vehicle_file)
else:
self.wheeled_vehicle = veh.WheeledVehicle(sys, self.vehicle_file)
self.wheeled_vehicle.Initialize(chrono.ChCoordsysD(self.initLoc, self.initRot))
self.wheeled_vehicle.SetChassisVisualizationType(veh.VisualizationType_PRIMITIVES)
self.wheeled_vehicle.SetSuspensionVisualizationType(veh.VisualizationType_PRIMITIVES)
self.wheeled_vehicle.SetSteeringVisualizationType(veh.VisualizationType_PRIMITIVES)
self.wheeled_vehicle.SetWheelVisualizationType(veh.VisualizationType_NONE)
# Create and initialize the powertrain system
self.powertrain = veh.SimplePowertrain(self.simplepowertrain_file)
self.wheeled_vehicle.InitializePowertrain(self.powertrain)
# Create and initialize the tires
for axle in self.wheeled_vehicle.GetAxles():
tireL = veh.RigidTire(self.rigidtire_file)
self.wheeled_vehicle.InitializeTire(tireL, axle.m_wheels[0], veh.VisualizationType_MESH)
tireR = veh.RigidTire(self.rigidtire_file)
self.wheeled_vehicle.InitializeTire(tireR, axle.m_wheels[1], veh.VisualizationType_MESH)
self.vehicle = self.wheeled_vehicle
self.sys = self.wheeled_vehicle.GetSystem()
elif vehicle_type == 'rccar':
if sys == None:
self.rc_vehicle = veh.RCCar()
self.rc_vehicle.SetContactMethod(chrono.ChMaterialSurface.SMC)
self.rc_vehicle.SetChassisCollisionType(veh.ChassisCollisionType_NONE)
else:
self.rc_vehicle = veh.RCCar(sys)
self.rc_vehicle.SetChassisFixed(False)
self.rc_vehicle.SetInitPosition(chrono.ChCoordsysD(initLoc, initRot))
self.rc_vehicle.SetTireType(veh.TireModelType_RIGID)
self.rc_vehicle.SetTireStepSize(step_size)
self.rc_vehicle.Initialize()
self.rc_vehicle.SetChassisVisualizationType(veh.VisualizationType_PRIMITIVES)
self.rc_vehicle.SetSuspensionVisualizationType(veh.VisualizationType_PRIMITIVES)
self.rc_vehicle.SetSteeringVisualizationType(veh.VisualizationType_PRIMITIVES)
self.rc_vehicle.SetWheelVisualizationType(veh.VisualizationType_PRIMITIVES)
self.rc_vehicle.SetTireVisualizationType(veh.VisualizationType_PRIMITIVES)
self.vehicle = self.rc_vehicle.GetVehicle()
self.sys = self.vehicle.GetSystem()
self.trackPoint = chrono.ChVectorD(4, 0.0, .15)
elif vehicle_type == 'sedan':
if sys == None:
self.sedan = veh.Sedan()
self.sedan.SetContactMethod(chrono.ChMaterialSurface.NSC)
self.sedan.SetChassisCollisionType(veh.ChassisCollisionType_NONE)
else:
self.sedan = veh.Sedan(sys)
self.sedan.SetChassisFixed(False)
self.sedan.SetInitPosition(chrono.ChCoordsysD(initLoc, initRot))
self.sedan.SetTireType(veh.TireModelType_RIGID)
self.sedan.SetTireStepSize(step_size)
self.sedan.Initialize()
self.sedan.SetChassisVisualizationType(veh.VisualizationType_PRIMITIVES)
self.sedan.SetSuspensionVisualizationType(veh.VisualizationType_PRIMITIVES)
self.sedan.SetSteeringVisualizationType(veh.VisualizationType_PRIMITIVES)
self.sedan.SetWheelVisualizationType(veh.VisualizationType_PRIMITIVES)
self.sedan.SetTireVisualizationType(veh.VisualizationType_PRIMITIVES)
self.vehicle = self.sedan.GetVehicle()
self.sys = self.vehicle.GetVehicle().GetSystem()
# -------------
# Create driver
# -------------
self.driver = Driver(self.vehicle)
self.driver.SetStepSize(step_size)
# Set the time response for steering and throttle inputs.
# NOTE: this is not exact, since we do not render quite at the specified FPS.
steering_time = 1.0 # time to go from 0 to +1 (or from 0 to -1)
throttle_time = 1.0 # time to go from 0 to +1
braking_time = 0.3 # time to go from 0 to +1
self.driver.SetSteeringDelta(render_step_size / steering_time)
self.driver.SetThrottleDelta(render_step_size / throttle_time)
self.driver.SetBrakingDelta(render_step_size / braking_time)
self.vis_balls = vis_balls
if self.vis_balls:
self.sentinel_sphere = chrono.ChBodyEasySphere(.25, 1000, False, True)
self.sentinel_sphere.SetBodyFixed(True)
self.sentinel_sphere.AddAsset(chrono.ChColorAsset(1,0,0))
self.sys.Add(self.sentinel_sphere)
self.sentinel_target = chrono.ChBodyEasySphere(.25, 1000, False, True)
self.sentinel_target.SetBodyFixed(True)
self.sentinel_target.AddAsset(chrono.ChColorAsset(0,1,0));
self.sys.Add(self.sentinel_target)
# Vehicle parameters for matplotlib
self.length = self.vehicle.GetWheelbase() + 2.0 # [m]
self.width = self.vehicle.GetWheeltrack(0) # [m]
self.backtowheel = 1.0 # [m]
self.wheel_len = self.vehicle.GetWheel(0, 1).GetWidth() * 2 # [m]
self.wheel_width = self.vehicle.GetWheel(0, 1).GetWidth() # [m]
self.tread = self.vehicle.GetWheeltrack(0) / 2 # [m]
self.wb = self.vehicle.GetWheelbase() # [m]
self.offset = [-4.0,0] # [m]
# | |___| | | | | | (_) | | | | (_) |
# \____|_| |_|_| \___/|_| |_|\___/
# Chrono
g = np.array(opts.gravity)
# System
system = fsi.ProtChSystem()
system.ChSystem.Set_G_acc(pychrono.ChVectorD(g[0], g[1], g[2]))
system.setTimeStep(1e-5)
# Body
body = fsi.ProtChBody(system=system)
body.setBoundaryFlags([0]) # index of particle
#
chbod = body.ChBody
pos = pychrono.ChVectorD(0.5*tank_x, 0.7*tank_y, 0.)
rot = pychrono.ChQuaternionD(1., 0., 0., 0.)
mass = 2000.0#3.14*rho_0*1.5
inertia = pychrono.ChVectorD(1., 1., 1.)
chbod.SetPos(pos)
chbod.SetRot(rot)
chbod.SetMass(mass)
chbod.SetInertiaXX(inertia)
# chbod.SetBodyFixed(True)
body.setConstraints(free_x=np.array([0., 1., 0.]), free_r=np.array([0., 0., 0.]))
body.setRecordValues(all_values=True)
def sdf(t, x):
dist = np.sqrt((x[0]**2+x[1]**2+x[2]**2))
if dist < radius:
return -(radius-dist), (0., -1., 0.)
else:
# Spin in place for real time to catch up
realtime_timer.Spin(step_size)
return 0
# The path to the Chrono data directory containing various assets (meshes, textures, data files)
# is automatically set, relative to the default location of this demo.
# If running from a different directory, you must change the path to the data directory with:
#chrono.SetChronoDataPath('path/to/data')
veh.SetDataPath(chrono.GetChronoDataPath() + 'vehicle/')
# Initial vehicle location and orientation
initLoc = chrono.ChVectorD(0, 0, 1.6)
initRot = chrono.ChQuaternionD(1, 0, 0, 0)
# Visualization type for vehicle parts (PRIMITIVES, MESH, or NONE)
chassis_vis_type = veh.VisualizationType_MESH
suspension_vis_type = veh.VisualizationType_PRIMITIVES
steering_vis_type = veh.VisualizationType_PRIMITIVES
wheel_vis_type = veh.VisualizationType_MESH
tire_vis_type = veh.VisualizationType_MESH
# Collision type for chassis (PRIMITIVES, MESH, or NONE)
chassis_collision_type = veh.ChassisCollisionType_NONE
# Type of powertrain model (SHAFTS, SIMPLE)
powertrain_model = veh.PowertrainModelType_SHAFTS
# Drive type (FWD, RWD, or AWD)
print('----------')
# Iterate over added bodies (Python style)
print('Positions of all bodies in the system:')
for abody in my_system.Get_bodylist():
print(' ', abody.GetName(), ' pos =', abody.GetPos())
# Use a body with an auxiliary reference (REF) that does not correspond to the center of gravity (COG)
bodyC = chrono.ChBodyAuxRef()
my_system.AddBody(bodyC)
bodyC.SetName('Parte1-1')
bodyC.SetPos(
chrono.ChVectorD(-0.0445347481124079, 0.0676266363930238,
-0.0230808979433518))
bodyC.SetRot(chrono.ChQuaternionD(1, 0, 0, 0))
bodyC.SetMass(346.17080777653)
bodyC.SetInertiaXX(
chrono.ChVectorD(48583.2418823358, 526927.118351673, 490689.966726565))
bodyC.SetInertiaXY(
chrono.ChVectorD(1.70380722975012e-11, 1.40840344485366e-11,
-2.31869065456271e-12))
bodyC.SetFrame_COG_to_REF(
chrono.ChFrameD(
chrono.ChVectorD(68.9923703887577, -60.1266363930238,
70.1327223302498), chrono.ChQuaternionD(1, 0, 0, 0)))
myasset = chrono.ChObjShapeFile()
myasset.SetFilename("shapes/test.obj")
bodyC.GetAssets().push_back(myasset)
# Add a revolute joint
