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