def render(self):
if not self.animate:
print(
'It seems that for efficiency reasons visualization has been turned OFF. To render the simulation set self.animate = True in ChronoHexapod.py'
)
sys.exit(1)
if not self.render_setup:
self.myapplication = chronoirr.ChIrrApp(
self.hexapod_sys, '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 # angle of FOV
self.myapplication.AssetBindAll()
self.myapplication.AssetUpdateAll()
self.render_setup = True
#self.myapplication.GetSceneManager().getActiveCamera().setPosition(chronoirr.vector3df(self.centralbody.GetPos().x - 0.75 , 0.4, self.centralbody.GetPos().z + 0.25))
#self.myapplication.GetSceneManager().getActiveCamera().setTarget(chronoirr.vector3df(self.centralbody.GetPos().x , 0.25, self.centralbody.GetPos().z))
self.myapplication.GetDevice().run()
self.myapplication.BeginScene()
self.myapplication.DrawAll()
self.myapplication.EndScene()
def load_shape(filepath, contact_method, texture):
"""
Import the mesh stored in filepath as a shape
:param filepath: Path to the mesh
:param contact_method: SMC or NSC
:param texture: Path to the texture .jpg
:return: A ChBody shape
"""
shape = chrono.ChBody(contact_method)
shape.SetBodyFixed(True)
shape_mesh = chrono.ChObjShapeFile()
shape_mesh.SetFilename(filepath)
shape.AddAsset(shape_mesh)
tmc = chrono.ChTriangleMeshConnected()
tmc.LoadWavefrontMesh(filepath)
# Add a collision mesh to the shape
shape.GetCollisionModel().ClearModel()
shape.GetCollisionModel().AddTriangleMesh(tmc, True, True)
shape.GetCollisionModel().BuildModel()
shape.SetShowCollisionMesh(True)
shape.SetCollide(False)
# Add a skin texture
skin_texture = chrono.ChTexture()
skin_texture.SetTextureFilename(chrono.GetChronoDataPath() + texture)
shape.GetAssets().push_back(skin_texture)
return shape
def render(self):
if not self.render_setup :
self.myapplication = chronoirr.ChIrrApp(self.ant_sys)
self.myapplication.AddShadowAll()
self.myapplication.SetTimestep(self.timestep)
self.myapplication.AddTypicalSky(chrono.GetChronoDataPath() + '/skybox/')
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
self.myapplication.AssetBindAll()
self.myapplication.AssetUpdateAll()
self.render_setup = True
self.myapplication.GetDevice().run()
self.myapplication.BeginScene()
self.myapplication.DrawAll()
#self.myapplication.DoStep()
self.myapplication.EndScene()
def render(self):
if not self.render_setup:
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
self.myapplication.AssetBindAll()
self.myapplication.AssetUpdateAll()
self.render_setup = True
self.myapplication.GetDevice().run()
self.myapplication.BeginScene()
self.myapplication.DrawAll()
self.myapplication.EndScene()
body_brick.GetAssets().push_back(body_brick_shape)
my_system.Add(body_brick)
# ---------------------------------------------------------------------
#
# Render a short animation by generating scripts
# to be used with POV-Ray
#
pov_exporter = postprocess.ChPovRay(my_system)
# Sets some file names for in-out processes.
pov_exporter.SetTemplateFile (chrono.GetChronoDataPath() + "_template_POV.pov")
pov_exporter.SetOutputScriptFile ("rendering_frames.pov")
if not os.path.exists("output"):
os.mkdir("output")
if not os.path.exists("anim"):
os.mkdir("anim")
pov_exporter.SetOutputDataFilebase("output/my_state")
pov_exporter.SetPictureFilebase("anim/picture")
pov_exporter.SetCamera(chrono.ChVectorD(0.2,0.3,0.5), chrono.ChVectorD(0,0,0), 35)
pov_exporter.SetLight(chrono.ChVectorD(-2,2,-1), chrono.ChColor(1.1,1.2,1.2), True)
pov_exporter.SetPictureSize(640,480)
pov_exporter.SetAmbientLight(chrono.ChColor(2,2,2))
# Add additional POV objects/lights/materials in the following way
pov_exporter.SetCustomPOVcommandsScript(
def reset(self):
self.isdone = False
self.hexapod_sys.Clear()
self.exported_items = chrono.ImportSolidWorksSystem(self.fpath)
self.csys = []
self.frames = []
self.revs = []
self.motors = []
self.limits = []
for con, coi in zip(self.con_link, self.coi_link):
indices = []
for i in range(len(self.exported_items)):
if con == self.exported_items[i].GetName(
) or coi == self.exported_items[i].GetName():
indices.append(i)
rev = self.exported_items[indices[0]]
af0 = rev.GetAssetsFrame()
# Revolute joints and ChLinkMotorRotation are z oriented, while parallel is x oriented.
# Event though this Frame won't be used anymore is good practice to create a copy before editing its value.
af = chrono.ChFrameD(af0)
af.SetRot(af0.GetRot() % chrono.Q_ROTATE_X_TO_Z)
self.frames.append(af)
for i in reversed(indices):
del self.exported_items[i]
# ADD IMPORTED ITEMS TO THE SYSTEM
for my_item in self.exported_items:
self.hexapod_sys.Add(my_item)
"""
$$$$$$$$ FIND THE SW DEFINED CONSTRAINTS, GET THEIR MARKERS AND GET RID OF EM $$$$$$$$
"""
self.hips = [
self.hexapod_sys.SearchBody(name) for name in self.hip_names
]
self.femurs = [
self.hexapod_sys.SearchBody(name) for name in self.femur_names
]
self.tibias = [
self.hexapod_sys.SearchBody(name) for name in self.tibia_names
]
self.feet = [
self.hexapod_sys.SearchBody(name) for name in self.feet_names
]
self.centralbody = self.hexapod_sys.SearchBody('Body-1')
# Bodies are used to replace constraints and detect unwanted collision, so feet are excluded
self.bodies = [self.centralbody
] + self.hips + self.femurs + self.tibias
self.centralbody.SetBodyFixed(False)
self.y0 = self.centralbody.GetPos().y
"""
# SNIPPET FOR COLOR
orange = chrono.ChColorAsset()
orange.SetColor(chrono.ChColor(255/255,77/255,6/255))
black = chrono.ChColorAsset()
black.SetColor(chrono.ChColor(0,0,0))
for body in self.bodies[:-1]:
assets = body.GetAssets()
for ast in assets:
ass_lev = chrono.CastToChAssetLevel(ast)
ass_lev.GetAssets().push_back(orange)
assets = self.hand.GetAssets()
for ast in assets:
ass_lev = chrono.CastToChAssetLevel(ast)
ass_lev.GetAssets().push_back(black)
"""
for i in range(len(self.con_link)):
revolute = chrono.ChLinkLockRevolute()
cs = chrono.ChCoordsysD(self.frames[i].GetPos(),
self.frames[i].GetRot())
self.csys.append(cs)
if i < 6:
j = 0
else:
j = i - 5
revolute.Initialize(self.bodies[j], self.bodies[i + 1],
self.csys[i])
self.revs.append(revolute)
self.hexapod_sys.Add(self.revs[i])
lim = self.revs[i].GetLimit_Rz()
self.limits.append(lim)
self.limits[i].SetActive(True)
self.limits[i].SetMin(self.minRot[i] * (math.pi / 180))
self.limits[i].SetMax(self.maxRot[i] * (math.pi / 180))
m = chrono.ChLinkMotorRotationTorque()
m.SetSpindleConstraint(False, False, False, False, False)
m.Initialize(self.bodies[j], self.bodies[i + 1], self.frames[i])
self.motors.append(m)
self.hexapod_sys.Add(self.motors[i])
self.body_floor = chrono.ChBody()
self.body_floor.SetBodyFixed(True)
self.body_floor.SetPos(chrono.ChVectorD(0, -1 - 0.128 - 0.0045, 10))
# Floor Collision.
self.body_floor.GetCollisionModel().ClearModel()
self.body_floor.GetCollisionModel().AddBox(self.my_material, 50, 1, 50,
chrono.ChVectorD(0, 0, 0))
self.body_floor.GetCollisionModel().BuildModel()
self.body_floor.SetCollide(True)
# Visualization shape
body_floor_shape = chrono.ChBoxShape()
body_floor_shape.GetBoxGeometry().Size = chrono.ChVectorD(4, 1, 15)
body_floor_shape.SetColor(chrono.ChColor(0.4, 0.4, 0.5))
self.body_floor.GetAssets().push_back(body_floor_shape)
body_floor_texture = chrono.ChTexture()
texpath = os.path.join(chrono.GetChronoDataPath(), 'concrete.jpg')
body_floor_texture.SetTextureFilename(texpath)
self.body_floor.GetAssets().push_back(body_floor_texture)
self.hexapod_sys.Add(self.body_floor)
self.numsteps = 0
if (self.render_setup):
self.myapplication.AssetBindAll()
self.myapplication.AssetUpdateAll()
return self.get_ob()
# -------------------------------
# Cast rays into collision models
# -------------------------------
caster = RayCaster(
my_sys,
chrono.ChFrameD(chrono.ChVectorD(0, -2, -1),
chrono.Q_from_AngX(-chrono.CH_C_PI_2)), [2.5, 2.5], 0.02)
# -------------------------------
# Create the visualization window
# -------------------------------
application = chronoirr.ChIrrApp(my_sys, "RoboSimian - Rigid terrain",
chronoirr.dimension2du(800, 600))
application.AddTypicalLogo(chrono.GetChronoDataPath() +
'logo_pychrono_alpha.png')
application.AddTypicalSky()
application.AddTypicalCamera(chronoirr.vector3df(1, -2.75, 0.2),
chronoirr.vector3df(1, 0, 0))
application.AddTypicalLights(chronoirr.vector3df(100, 100, 100),
chronoirr.vector3df(100, -100, 80))
application.AddLightWithShadow(chronoirr.vector3df(10, -6, 3),
chronoirr.vector3df(0, 0, 0), 3, -10, 10, 40,
512)
application.AssetBindAll()
application.AssetUpdateAll()
# -----------------------------
# Initialize output directories
my_system = chrono.ChSystemNSC()
# Set the collision margins. This is expecially important for very large or
# very small objects (as in this example)! Do this before creating shapes.
chrono.ChCollisionModel.SetDefaultSuggestedEnvelope(0.001)
chrono.ChCollisionModel.SetDefaultSuggestedMargin(0.001)
# ---------------------------------------------------------------------
#
# load the file generated by the SolidWorks CAD plugin
# and add it to the system
#
print("Loading C::E scene...")
exported_items = chrono.ImportSolidWorksSystem(chrono.GetChronoDataPath() +
"solid_works/swiss_escapement")
print("...done!")
# Print exported items
for my_item in exported_items:
print(my_item.GetName())
# Add items to the physical system
for my_item in exported_items:
my_system.Add(my_item)
# ---------------------------------------------------------------------
#
# Create an Irrlicht application to visualize the system
print ("\n\nOk, Simulation done!");
time.sleep(2)
if m_visualization == "irrlicht":
# ---------------------------------------------------------------------
#
# Create an Irrlicht application to visualize the system
#
myapplication = chronoirr.ChIrrApp(my_system, 'Test', chronoirr.dimension2du(1280,720))
myapplication.AddTypicalSky(chrono.GetChronoDataPath() + 'skybox/')
myapplication.AddTypicalLogo(chrono.GetChronoDataPath() + 'logo_pychrono_alpha.png')
myapplication.AddTypicalCamera(chronoirr.vector3df(1,1,1),chronoirr.vector3df(0.0,0.0,0.0))
myapplication.AddTypicalLights()
#myapplication.AddLightWithShadow(chronoirr.vector3df(10,20,10),chronoirr.vector3df(0,2.6,0), 10 ,10,40, 60, 512);
# ==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!
def main():
#print("Copyright (c) 2017 projectchrono.org\nChrono version: ", CHRONO_VERSION , "\n\n")
# Create systems
# Create the HMMWV vehicle, set parameters, and initialize
my_hmmwv = veh.HMMWV_Full()
my_hmmwv.SetContactMethod(contact_method)
my_hmmwv.SetChassisCollisionType(chassis_collision_type)
my_hmmwv.SetChassisFixed(False)
my_hmmwv.SetInitPosition(chrono.ChCoordsysD(initLoc, initRot))
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())
patch = terrain.AddPatch(
chrono.ChCoordsysD(chrono.ChVectorD(0, 0, terrainHeight - 5),
chrono.QUNIT),
chrono.ChVectorD(terrainLength, terrainWidth, 10))
patch.SetContactFrictionCoefficient(0.9)
patch.SetContactRestitutionCoefficient(0.01)
patch.SetContactMaterialProperties(2e7, 0.3)
patch.SetTexture(veh.GetDataFile("terrain/textures/tile4.jpg"), 200, 200)
patch.SetColor(chrono.ChColor(0.8, 0.8, 0.5))
terrain.Initialize()
# Create the vehicle Irrlicht interface
# please note that wchar_t conversion requres some workaround
app = veh.ChWheeledVehicleIrrApp(my_hmmwv.GetVehicle())
app.SetSkyBox()
app.AddTypicalLights(chronoirr.vector3df(30, -30, 100),
chronoirr.vector3df(30, 50, 100), 250, 130)
app.AddTypicalLogo(chrono.GetChronoDataPath() + 'logo_pychrono_alpha.png')
app.SetChaseCamera(trackPoint, 6.0, 0.5)
app.SetTimestep(step_size)
app.AssetBindAll()
app.AssetUpdateAll()
# Initialize output
try:
os.mkdir(out_dir)
except:
print("Error creating directory ")
# Set up vehicle output
my_hmmwv.GetVehicle().SetChassisOutput(True)
my_hmmwv.GetVehicle().SetSuspensionOutput(0, True)
my_hmmwv.GetVehicle().SetSteeringOutput(0, True)
my_hmmwv.GetVehicle().SetOutput(veh.ChVehicleOutput.ASCII, out_dir,
"output", 0.1)
# Generate JSON information with available output channels
my_hmmwv.GetVehicle().ExportComponentList(out_dir + "/component_list.json")
# Create the interactive driver system
driver = veh.ChIrrGuiDriver(app)
# Set the time response for steering and throttle keyboard inputs.
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
driver.SetSteeringDelta(render_step_size / steering_time)
driver.SetThrottleDelta(render_step_size / throttle_time)
driver.SetBrakingDelta(render_step_size / braking_time)
driver.Initialize()
# Simulation loop
# Number of simulation steps between miscellaneous events
render_steps = m.ceil(render_step_size / step_size)
debug_steps = m.ceil(debug_step_size / step_size)
# Initialize simulation frame counter and simulation time
step_number = 0
render_frame = 0
if (contact_vis):
app.SetSymbolscale(1e-4)
#app.SetContactsDrawMode(chronoirr.eCh_ContactsDrawMode::CONTACT_FORCES);
realtime_timer = chrono.ChRealtimeStepTimer()
while (app.GetDevice().run()):
time = my_hmmwv.GetSystem().GetChTime()
#End simulation
if (time >= t_end):
break
app.BeginScene(True, True, chronoirr.SColor(255, 140, 161, 192))
app.DrawAll()
app.EndScene()
#Debug logging
if (debug_output and step_number % debug_steps == 0):
print("\n\n============ System Information ============\n")
print("Time = " << time << "\n\n")
#my_hmmwv.DebugLog(OUT_SPRINGS | OUT_SHOCKS | OUT_CONSTRAINTS)
marker_driver = my_hmmwv.GetChassis().GetMarkers()[0].GetAbsCoord(
).pos
marker_com = my_hmmwv.GetChassis().GetMarkers()[1].GetAbsCoord(
).pos
print("Markers\n")
print(" Driver loc: ", marker_driver.x, " ", marker_driver.y,
" ", marker_driver.z)
print(" Chassis COM loc: ", marker_com.x, " ", marker_com.y, " ",
marker_com.z)
# 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.GetInputModeAsString(), 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)
# Increment frame number
step_number += 1
# Spin in place for real time to catch up
realtime_timer.Spin(step_size)
return 0
# Global -- Dont touch
UNIT_FACTOR = 0.01
metrics = ['mm', 'cm', 'dm', 'm']
metric = metrics[int(math.fabs(round(math.log(UNIT_FACTOR, 10))))]
filename = SHAPE_PATH.split('/')[-1].split('.')[0]
# ---------------------------------------------------------------------------
# Chrono setup
chrono.SetChronoDataPath("../data/")
mysystem = chrono.ChSystemSMC()
mysystem.Set_G_acc(chrono.ChVectorD(0., 0., 0.)) # Remove gravity
contact_method = chrono.ChMaterialSurface.SMC
# Shape
filepath = tool.obj_from_millimeter(chrono.GetChronoDataPath() + SHAPE_PATH,
UNIT_FACTOR, f"_{metric}")
shape = tool.load_shape(filepath, contact_method, 'textures/skin.jpg')
# Get shape bounding box dimensions
bbmin, bbmax = chrono.ChVectorD(), chrono.ChVectorD()
shape.GetTotalAABB(bbmin, bbmax)
bbmin, bbmax = eval(str(bbmin)), eval(str(bbmax))
print(bbmin, bbmax)
print(shape.GetPos())
bb_dx = bbmax[0] - bbmin[0]
bb_dy = bbmax[1] - bbmin[1]
bb_dz = bbmax[2] - bbmin[2]
# Align shape to the center of axis system
shape.SetPos(
chrono.ChVectorD(-bb_dx / 2. - bbmin[0], -bb_dy / 2. - bbmin[1],
if not(fixed_L):
mbodyG = chrono.ChBodyEasyBox(1,0.5 , thick*2.2, 1000,True,True)
mbodyG.SetPos( chrono.ChVectorD(-1, L2.y-0.5/2, 0 ) )
mbodyG.SetBodyFixed(True)
mbodyG.SetMaterialSurface(brick_material)
mysystem.Add(mbodyG)
# ---------------------------------------------------------------------
#
# Create an Irrlicht application to visualize the system
#
myapplication = chronoirr.ChIrrApp(mysystem, 'Test', chronoirr.dimension2du(1024,768))
myapplication.AddTypicalLogo(chrono.GetChronoDataPath() + "logo_pychrono_alpha.png")
myapplication.AddTypicalSky(chrono.GetChronoDataPath() + "skybox/")
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();
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)
nstep = nstep + 1
print("\n\nOk, Simulation done!")
time.sleep(2)
if m_visualization == "irrlicht":
# ---------------------------------------------------------------------
#
# Create an Irrlicht application to visualize the system
#
myapplication = chronoirr.ChIrrApp(my_system, 'Test',
chronoirr.dimension2du(1280, 720))
myapplication.AddTypicalSky(chrono.GetChronoDataPath() + 'skybox/')
myapplication.AddTypicalLogo(chrono.GetChronoDataPath() +
'logo_pychrono_alpha.png')
myapplication.AddTypicalCamera(chronoirr.vector3df(0.5, 0.5, 0.5),
chronoirr.vector3df(0.0, 0.0, 0.0))
myapplication.AddTypicalLights()
#myapplication.AddLightWithShadow(chronoirr.vector3df(10,20,10),chronoirr.vector3df(0,2.6,0), 10 ,10,40, 60, 512);
# ==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
def reset(self):
self.isdone = False
self.robosystem.Clear()
#action = (np.random.rand(6,)-0.5)*2
#torques = np.multiply(action, self.maxT)
self.exported_items = chrono.ImportSolidWorksSystem(self.fpath)
self.csys = []
self.frames = []
self.revs = []
self.motors = []
self.limits = []
for con, coi in zip(self.con_link, self.coi_link):
indices = []
for i in range(len(self.exported_items)):
if con == self.exported_items[i].GetName(
) or coi == self.exported_items[i].GetName():
indices.append(i)
rev = self.exported_items[indices[0]]
af0 = rev.GetAssetsFrame()
# Revolute joints and ChLinkMotorRotation are z oriented, while parallel is x oriented.
# Event though this Frame won't be used anymore is good practice to create a copy before editing its value.
af = chrono.ChFrameD(af0)
af.SetRot(af0.GetRot() % chrono.Q_ROTATE_X_TO_Z)
self.frames.append(af)
for i in indices:
del self.exported_items[i]
# ADD IMPORTED ITEMS TO THE SYSTEM
for my_item in self.exported_items:
self.robosystem.Add(my_item)
"""
$$$$$$$$ FIND THE SW DEFINED CONSTRAINTS, GET THEIR MARKERS AND GET RID OF EM $$$$$$$$
"""
self.bodies = [
self.robosystem.SearchBody(name) for name in self.bodiesNames
]
self.hand = self.robosystem.SearchBody('Hand_base_and_p07-2')
self.base = self.robosystem.SearchBody('Racer3_p01-3')
self.biceps = self.robosystem.SearchBody('Racer3_p03-1')
self.forearm = self.robosystem.SearchBody('Racer3_p05-1')
self.finger1 = self.robosystem.SearchBody('HAND_e_finger-1')
self.finger2 = self.robosystem.SearchBody('HAND_e_finger-2')
for i in range(len(self.con_link)):
revolute = chrono.ChLinkLockRevolute()
cs = chrono.ChCoordsysD(self.frames[i].GetPos(),
self.frames[i].GetRot())
self.csys.append(cs)
revolute.Initialize(self.bodies[i], self.bodies[i + 1],
self.csys[i])
self.revs.append(revolute)
self.robosystem.Add(self.revs[i])
lim = self.revs[i].GetLimit_Rz()
self.limits.append(lim)
self.limits[i].SetActive(True)
self.limits[i].SetMin(self.minRot[i] * (math.pi / 180))
self.limits[i].SetMax(self.maxRot[i] * (math.pi / 180))
m = chrono.ChLinkMotorRotationTorque()
m.Initialize(self.bodies[i], self.bodies[i + 1], self.frames[i])
#self.robosystem.Add(m2)
#m2.SetTorqueFunction(chrono.ChFunction_Const(5))
self.motors.append(m)
#self.motors[i].SetTorqueFunction(chrono.ChFunction_Const(float(torques[i])))
self.robosystem.Add(self.motors[i])
self.body_floor = chrono.ChBody()
self.body_floor.SetBodyFixed(True)
self.body_floor.SetPos(chrono.ChVectorD(0, -1, 0))
# Floor Collision.
self.body_floor.GetCollisionModel().ClearModel()
self.body_floor.GetCollisionModel().AddBox(self.my_material, 5, 1, 5,
chrono.ChVectorD(0, 0, 0))
self.body_floor.GetCollisionModel().BuildModel()
self.body_floor.SetCollide(True)
# Visualization shape
body_floor_shape = chrono.ChBoxShape()
body_floor_shape.GetBoxGeometry().Size = chrono.ChVectorD(5, 1, 5)
body_floor_shape.SetColor(chrono.ChColor(0.4, 0.4, 0.5))
self.body_floor.GetAssets().push_back(body_floor_shape)
body_floor_texture = chrono.ChTexture()
texpath = os.path.join(chrono.GetChronoDataPath(), 'concrete.jpg')
body_floor_texture.SetTextureFilename(texpath)
self.body_floor.GetAssets().push_back(body_floor_texture)
self.robosystem.Add(self.body_floor)
r = np.random.rand(2, ) - np.asarray([0.5, 0.5])
self.targ_init_pos = [
-0.52 + 2 * (r[0] * 0.05), 0.015, 2 * r[1] * 0.05
]
self.targ_box = chrono.ChBody()
# UNset to grasp
self.targ_box.SetBodyFixed(True)
self.targ_box.SetPos(
chrono.ChVectorD(self.targ_init_pos[0], self.targ_init_pos[1],
self.targ_init_pos[2]))
# Floor Collision.
self.targ_box.GetCollisionModel().ClearModel()
self.targ_box.GetCollisionModel().AddBox(self.my_material, 0.015,
0.015, 0.015,
chrono.ChVectorD(0, 0, 0))
self.targ_box.GetCollisionModel().BuildModel()
self.targ_box.SetCollide(True)
# Visualization shape
targ_box_shape = chrono.ChBoxShape()
targ_box_shape.GetBoxGeometry().Size = chrono.ChVectorD(
0.015, 0.015, 0.015)
col = chrono.ChColorAsset()
col.SetColor(chrono.ChColor(1.0, 0, 0))
self.targ_box.GetAssets().push_back(targ_box_shape)
self.targ_box.GetAssets().push_back(col)
self.robosystem.Add(self.targ_box)
self.numsteps = 0
if (self.render_setup):
self.myapplication.AssetBindAll()
self.myapplication.AssetUpdateAll()
return self.get_ob()
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 main():
# Create systems
# Create the HMMWV vehicle, set parameters, and initialize
my_hmmwv = veh.HMMWV_Full()
my_hmmwv.SetContactMethod(contact_method)
my_hmmwv.SetChassisCollisionType(chassis_collision_type)
my_hmmwv.SetChassisFixed(False)
my_hmmwv.SetInitPosition(chrono.ChCoordsysD(initLoc, initRot))
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.SetVehicleStepSize(step_size)
my_hmmwv.Initialize()
#VisualizationType tire_vis_type =
# (tire_model == TireModelType::RIGID_MESH) ? VisualizationType::MESH : VisualizationType::NONE;
tire_vis_type = veh.VisualizationType_MESH # if tire_model == veh.TireModelType_RIGID_MESH else tire_vis_type = veh.VisualizationType_NONE
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())
#patch = veh.Patch()
"""
switch (terrain_model) {
case RigidTerrain::BOX:
patch = terrain.AddPatch(ChCoordsys<>(ChVector<>(0, 0, terrainHeight - 5), QUNIT),
ChVector<>(terrainLength, terrainWidth, 10));
patch->SetTexture(vehicle::GetDataFile("terrain/textures/tile4.jpg"), 200, 200);
break;
case RigidTerrain::HEIGHT_MAP:
patch = terrain.AddPatch(CSYSNORM, vehicle::GetDataFile("terrain/height_maps/test64.bmp"), "test64", 128,
128, 0, 4);
patch->SetTexture(vehicle::GetDataFile("terrain/textures/grass.jpg"), 16, 16);
break;
case RigidTerrain::MESH:
patch = terrain.AddPatch(CSYSNORM, vehicle::GetDataFile("terrain/meshes/test.obj"), "test_mesh");
patch->SetTexture(vehicle::GetDataFile("terrain/textures/grass.jpg"), 100, 100);
break;
}
"""
patch = terrain.AddPatch(
chrono.ChCoordsysD(chrono.ChVectorD(0, 0, terrainHeight - 5),
chrono.QUNIT),
chrono.ChVectorD(terrainLength, terrainWidth, 10))
patch.SetContactFrictionCoefficient(0.9)
patch.SetContactRestitutionCoefficient(0.01)
patch.SetContactMaterialProperties(2e7, 0.3)
patch.SetColor(chrono.ChColor(0.8, 0.8, 0.5))
terrain.Initialize()
# Create the vehicle Irrlicht interface
# please note that wchar_t conversion requres some workaround
app = veh.ChWheeledVehicleIrrApp(
my_hmmwv.GetVehicle(), my_hmmwv.GetPowertrain()) #, "HMMWV Demo")
app.SetSkyBox()
app.AddTypicalLights(chronoirr.vector3df(30, -30, 100),
chronoirr.vector3df(30, 50, 100), 250, 130)
app.SetChaseCamera(trackPoint, 6.0, 0.5)
app.SetTimestep(step_size)
app.AssetBindAll()
app.AssetUpdateAll()
# Initialize output
try:
os.mkdir(out_dir)
except:
print("Error creating directory ")
if (povray_output):
try:
os.mkdir(pov_dir)
except:
print("Error creating POV directory ")
terrain.ExportMeshPovray(out_dir)
# Initialize output file for driver inputs
#driver_file = out_dir + "/driver_inputs.txt"
# no RECORD so far
#utils::CSV_writer driver_csv(" ");
# Set up vehicle output
my_hmmwv.GetVehicle().SetChassisOutput(True)
my_hmmwv.GetVehicle().SetSuspensionOutput(0, True)
my_hmmwv.GetVehicle().SetSteeringOutput(0, True)
my_hmmwv.GetVehicle().SetOutput(veh.ChVehicleOutput.ASCII, out_dir,
"output", 0.1)
# Generate JSON information with available output channels
my_hmmwv.GetVehicle().ExportComponentList(out_dir + "/component_list.json")
# Create the driver system
# Create the interactive driver system
driver = veh.ChIrrGuiDriver(app)
# Set the time response for steering and throttle keyboard inputs.
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
driver.SetSteeringDelta(render_step_size / steering_time)
driver.SetThrottleDelta(render_step_size / throttle_time)
driver.SetBrakingDelta(render_step_size / braking_time)
# If in playback mode, attach the data file to the driver system and
# force it to playback the driver inputs.
if (driver_mode == "PLAYBACK"):
#driver.SetInputDataFile(driver_file)
driver.SetInputMode(veh.ChIrrGuiDriver.DATAFILE)
driver.Initialize()
# Simulation loop
"""
if (debug_output) :
GetLog() << "\n\n============ System Configuration ============\n"
my_hmmwv.LogHardpointLocations()"""
# Number of simulation steps between miscellaneous events
render_steps = m.ceil(render_step_size / step_size)
debug_steps = m.ceil(debug_step_size / step_size)
# Initialize simulation frame counter and simulation time
realtime_timer = chrono.ChRealtimeStepTimer()
step_number = 0
render_frame = 0
time = 0
if (contact_vis):
app.SetSymbolscale(1e-4)
#app.SetContactsDrawMode(chronoirr.eCh_ContactsDrawMode::CONTACT_FORCES);
while (app.GetDevice().run()):
time = my_hmmwv.GetSystem().GetChTime()
#End simulation
if (time >= t_end):
break
app.BeginScene(True, True, chronoirr.SColor(255, 140, 161, 192))
app.DrawAll()
app.AddTypicalLogo(chrono.GetChronoDataPath() +
'logo_pychrono_alpha.png')
# Output POV-Ray data
if (povray_output and step_number % render_steps == 0):
#char filename[100];
print('filename', "%s/data_%03d.dat", pov_dir.c_str(),
render_frame + 1)
#utils::WriteShapesPovray(my_hmmwv.GetSystem(), filename);
#render_frame++;
#Debug logging
if (debug_output and step_number % debug_steps == 0):
print("\n\n============ System Information ============\n")
print("Time = " << time << "\n\n")
#my_hmmwv.DebugLog(OUT_SPRINGS | OUT_SHOCKS | OUT_CONSTRAINTS)
marker_driver = my_hmmwv.GetChassis().GetMarkers()[0].GetAbsCoord(
).pos
marker_com = my_hmmwv.GetChassis().GetMarkers()[1].GetAbsCoord(
).pos
print("Markers\n")
print(" Driver loc: ", marker_driver.x, " ", marker_driver.y,
" ", marker_driver.z)
print(" Chassis COM loc: ", marker_com.x, " ", marker_com.y, " ",
marker_com.z)
# Collect output data from modules (for inter-module communication)
throttle_input = driver.GetThrottle()
steering_input = driver.GetSteering()
braking_input = driver.GetBraking()
# Driver output
"""
if (driver_mode == RECORD) {
driver_csv << time << steering_input << throttle_input << braking_input << std::endl;
}"""
# Update modules (process inputs from other modules)
driver.Synchronize(time)
terrain.Synchronize(time)
my_hmmwv.Synchronize(time, steering_input, braking_input,
throttle_input, terrain)
app.Synchronize(driver.GetInputModeAsString(), steering_input,
throttle_input, braking_input)
# Advance simulation for one timestep for all modules
step = realtime_timer.SuggestSimulationStep(step_size)
driver.Advance(step)
terrain.Advance(step)
my_hmmwv.Advance(step)
app.Advance(step)
# Increment frame number
step_number += 1
app.EndScene()
"""
if (driver_mode == RECORD) {
driver_csv.write_to_file(driver_file);
}"""
return 0
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
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.SetCollide(True)
self.body_abdomen.GetCollisionModel().ClearModel()
self.body_abdomen.GetCollisionModel().AddEllipsoid(self.ant_material, 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].SetCollide(True)
self.ankle_body[i].GetCollisionModel().ClearModel()
self.ankle_body[i].GetCollisionModel().AddSphere(self.ant_material, 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 ))
# Floor Collision.
self.body_floor.GetCollisionModel().ClearModel()
self.body_floor.GetCollisionModel().AddBox(self.ant_material, 50, 1, 50, chrono.ChVectorD(0, 0, 0 ))
self.body_floor.GetCollisionModel().BuildModel()
self.body_floor.SetCollide(True)
# Visualization shape
body_floor_shape = chrono.ChBoxShape()
body_floor_shape.GetBoxGeometry().Size = chrono.ChVectorD(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.GetChronoDataPath() + '/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.render_setup):
self.myapplication.AssetBindAll()
self.myapplication.AssetUpdateAll()
self.numsteps= 0
self.step(np.zeros(8))
return self.get_ob()
# -*- coding: utf-8 -*-
"""
Created on Thu Jan 10 11:01:21 2019
@author: SB
"""
import pychrono as chrono
import chtrain_ant
import chtrain_pendulum
# Properly set path to Chrono data directory, taking into account the location of the chrono-tensorflow
# demos relative to the other PyChrono demos.
# If running from a different directory, you must change this path accordingly.
chrono.SetChronoDataPath('../../' + chrono.GetChronoDataPath())
def Init(env_name, render):
if env_name == 'ChronoAnt':
return chtrain_ant.Model(render)
elif env_name == 'ChronoPendulum':
return chtrain_pendulum.Model(render)
else:
print('Unvalid environment name')
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])
chrono.ChCollisionModel.SetDefaultSuggestedMargin(0.001);
# ---------------------------------------------------------------------
#
# load the file generated by the SolidWorks CAD plugin
# and add it to the system
#
print ("Loading Chrono scene...");
# Note that the ImportSolidWorksSystem() function requires the name of the
# .py file generated by the Chrono::SolidWorks plugin, but without the ".py"
# suffix. Here we use an example mechanism that we stored in the data/ directory,
# but we could also use an absolute path as ..("C:/my_path/swiss_escapement")
exported_items = chrono.ImportSolidWorksSystem(chrono.GetChronoDataPath() + "solid_works/swiss_escapement")
print ("...done!");
# Print exported items
for my_item in exported_items:
print (my_item.GetName())
# Add items to the physical system
for my_item in exported_items:
my_system.Add(my_item)
# ---------------------------------------------------------------------
#
# Render a short animation by generating scripts
def main() :
#print("Copyright (c) 2017 projectchrono.org\nChrono version: ", CHRONO_VERSION , "\n\n")
# --------------------------
# Create the various modules
# --------------------------
# Create the vehicle system
vehicle = veh.WheeledVehicle(vehicle_file ,chrono.ChMaterialSurface.NSC)
vehicle.Initialize(chrono.ChCoordsysD(initLoc, initRot))
#vehicle.GetChassis().SetFixed(True)
vehicle.SetChassisVisualizationType(veh.VisualizationType_PRIMITIVES)
vehicle.SetSuspensionVisualizationType(veh.VisualizationType_PRIMITIVES)
vehicle.SetSteeringVisualizationType(veh.VisualizationType_PRIMITIVES)
vehicle.SetWheelVisualizationType(veh.VisualizationType_NONE)
# Create the ground
terrain = veh.RigidTerrain(vehicle.GetSystem(), rigidterrain_file)
# Create and initialize the powertrain system
powertrain = veh.SimplePowertrain(simplepowertrain_file)
vehicle.InitializePowertrain(powertrain)
# Create and initialize the tires
for axle in vehicle.GetAxles() :
tireL = veh.RigidTire(rigidtire_file)
vehicle.InitializeTire(tireL, axle.m_wheels[0], veh.VisualizationType_MESH)
tireR = veh.RigidTire(rigidtire_file)
vehicle.InitializeTire(tireR, axle.m_wheels[1], veh.VisualizationType_MESH)
app = veh.ChVehicleIrrApp(vehicle)
app.SetSkyBox()
app.AddTypicalLights(chronoirr.vector3df(30, -30, 100), chronoirr.vector3df(30, 50, 100), 250, 130)
app.AddTypicalLogo(chrono.GetChronoDataPath() + 'logo_pychrono_alpha.png')
app.SetChaseCamera(trackPoint, 6.0, 0.5)
app.SetTimestep(step_size)
app.AssetBindAll()
app.AssetUpdateAll()
driver = veh.ChIrrGuiDriver(app)
# Set the time response for steering and throttle keyboard 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
driver.SetSteeringDelta(render_step_size / steering_time)
driver.SetThrottleDelta(render_step_size / throttle_time)
driver.SetBrakingDelta(render_step_size / braking_time)
driver.Initialize()
# -----------------
# Initialize output
# -----------------
try:
os.mkdir(out_dir)
except:
print("Error creating directory " )
# Generate JSON information with available output channels
out_json = vehicle.ExportComponentList()
print(out_json)
vehicle.ExportComponentList(out_dir + "/component_list.json")
# ---------------
# Simulation loop
# ---------------
realtime_timer = chrono.ChRealtimeStepTimer()
while (app.GetDevice().run()) :
# Render scene
app.BeginScene(True, True, chronoirr.SColor(255, 140, 161, 192))
app.DrawAll()
app.EndScene()
# Collect output data from modules (for inter-module communication)
driver_inputs = driver.GetInputs()
# Update modules (process inputs from other modules)
time = vehicle.GetSystem().GetChTime()
driver.Synchronize(time)
vehicle.Synchronize(time, driver_inputs, terrain)
terrain.Synchronize(time)
app.Synchronize(driver.GetInputModeAsString(), driver_inputs)
# Advance simulation for one timestep for all modules
driver.Advance(step_size)
vehicle.Advance(step_size)
terrain.Advance(step_size)
app.Advance(step_size)
# Spin in place for real time to catch up
realtime_timer.Spin(step_size)
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()
# Increment frame number
step_number += 1
# 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
chrono.ChCollisionModel.SetDefaultSuggestedEnvelope(0.001)
chrono.ChCollisionModel.SetDefaultSuggestedMargin(0.001)
# ---------------------------------------------------------------------
#
# load the file generated by the SolidWorks CAD plugin
# and add it to the system
#
print("Loading Chrono scene...")
# Note that the ImportSolidWorksSystem() function requires the name of the
# .py file generated by the Chrono::SolidWorks plugin, but without the ".py"
# suffix. Here we use an example mechanism that we stored in the data/ directory,
# but we could also use an absolute path as ..("C:/my_path/swiss_escapement")
exported_items = chrono.ImportSolidWorksSystem(chrono.GetChronoDataPath() +
"solid_works/swiss_escapement")
print("...done!")
# Print exported items
for my_item in exported_items:
print(my_item.GetName())
# Add items to the physical system
for my_item in exported_items:
my_system.Add(my_item)
# ---------------------------------------------------------------------
#
# Render a short animation by generating scripts