def __init__(self, filename, bounding_box=None):
self.filename = filename
# If bounding box is not passed in, calculate it
if bounding_box == None:
self.bounding_box = CalcBoundingBox()
else:
self.bounding_box = bounding_box
self.mesh = chrono.ChTriangleMeshConnected()
self.mesh.LoadWavefrontMesh(chrono.GetChronoDataFile(filename), False,
True)
self.shape = chrono.ChTriangleMeshShape()
self.shape.SetMesh(self.mesh)
self.shape.SetStatic(True)
self.body = chrono.ChBody()
self.body.AddAsset(self.shape)
self.body.SetCollide(False)
self.body.SetBodyFixed(True)
self.scaled = False
self.pos = chrono.ChVectorD()
self.scale = 1
self.ang = 0
def __init__(self, filename, scale_range):
self.filename = filename
self.scale_range = scale_range
self.ready = False
self.mesh = chrono.ChTriangleMeshConnected()
self.mesh.LoadWavefrontMesh(chrono.GetChronoDataFile(self.filename), True, True)
self.shape = chrono.ChTriangleMeshShape()
self.shape.SetMesh(self.mesh)
self.shape.SetStatic(True)
self.body = chrono.ChBody()
self.body.AddAsset(self.shape)
self.body.SetCollide(True)
self.body.SetBodyFixed(True)
surface_mat = chrono.ChMaterialSurfaceNSC()
surface_mat.SetFriction(0.9)
surface_mat.SetRestitution(0.01)
self.body.GetCollisionModel().ClearModel()
self.body.GetCollisionModel().AddTriangleMesh(surface_mat, self.mesh, False, False)
self.body.GetCollisionModel().BuildModel()
self.scale = 1
def DrawCones(self, points, color, z=.3, n=10):
for p in points[::n]:
p.z += z
cmesh = chrono.ChTriangleMeshConnected()
if color == 'red':
cmesh.LoadWavefrontMesh(
chrono.GetChronoDataFile("sensor/cones/red_cone.obj"),
False, True)
elif color == 'green':
cmesh.LoadWavefrontMesh(
chrono.GetChronoDataFile("sensor/cones/green_cone.obj"),
False, True)
cshape = chrono.ChTriangleMeshShape()
cshape.SetMesh(cmesh)
cshape.SetName("Cone")
cshape.SetStatic(True)
cbody = chrono.ChBody()
cbody.SetPos(p)
cbody.AddAsset(cshape)
cbody.SetBodyFixed(True)
cbody.SetCollide(False)
if color == 'red':
cbody.AddAsset(chrono.ChColorAsset(1, 0, 0))
elif color == 'green':
cbody.AddAsset(chrono.ChColorAsset(0, 1, 0))
self.system.Add(cbody)
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 __init__(self,
system: 'WAChronoSystem',
filename: str,
vehicle: 'WAChronoVehicle' = None,
body: 'WABody' = None):
if missing_chrono_sensor:
sens_import_error('WAChronoSensor.__init__')
super().__init__(vehicle, body)
j = _load_json(filename)
# Validate the json file
_check_field(j, 'Type', value='Sensor')
_check_field(j,
'Template',
allowed_values=['Camera', 'Lidar', 'IMU', 'GPS']) # noqa
_check_field(j, 'Offset Pose', field_type=dict)
offset_pose = ChFrame_from_json(j['Offset Pose'])
if vehicle is not None:
body = vehicle._vehicle.GetChassisBody()
else:
asset = body
body = chrono.ChBody()
body.SetPos(WAVector_to_ChVector(asset.position))
body.SetBodyFixed(True)
system._system.AddBody(body)
# Create the chrono sensor through the Sensor chrono class
self._sensor = sens.Sensor.CreateFromJSON(filename, body,
offset_pose) # noqa
def addLeaders(self, system, path):
self.leaders = []
self.path = path
for i in range(self.numlead):
leader = chrono.ChBody()
leader.AddAsset(self.trimesh_shape)
system.Add(leader)
self.leaders.append(leader)
self.Update()
def placeObstacle(self, numob):
for i in range(numob):
side = randint(1, 2)
path = randint(0, len(self.obst_paths) - 1)
obst = chrono.ChBody()
#vis_mesh = self.vis_meshes[path]
obst.AddAsset(self.trimeshes[path])
x, y, z = self.obst_bound[path]
obst.GetCollisionModel().ClearModel()
obst.GetCollisionModel().AddBox(self.surf_material, x / 2, y / 2,
z / 2) # must set half sizes
obst.GetCollisionModel().BuildModel()
obst.SetCollide(True)
p0, q = self.path.getPosRot((i + 1) / (numob + 1))
dist = np.max([x, y]) + self.leader_box[1]
pos = p0 + q.Rotate(chrono.VECT_Y) * (
dist * pow(-1, side)) - chrono.ChVectorD(0, 0, p0.z)
obst.SetPos(pos)
obst.SetBodyFixed(True)
self.obstacles.append(obst)
self.system.Add(obst)
def __init__(self, system, mesh):
# Create a section, i.e. thickness and material properties
# for beams. This will be shared among some beams.
msection_cable2 = fea.ChBeamSectionCable()
msection_cable2.SetDiameter(0.015)
msection_cable2.SetYoungModulus(0.01e9)
msection_cable2.SetBeamRaleyghDamping(0.000)
# This ChBuilderCableANCF helper object is very useful because it will
# subdivide 'beams' into sequences of finite elements of beam type, ex.
# one 'beam' could be made of 5 FEM elements of ChElementBeamANCF class.
# If new nodes are needed, it will create them for you.
builder = fea.ChBuilderCableANCF()
# Now, simply use BuildBeam to create a beam from a poto another:
builder.BuildBeam(
mesh, # the mesh where to put the created nodes and elements
msection_cable2, # the ChBeamSectionCable to use for the ChElementBeamANCF elements
10, # the number of ChElementBeamANCF to create
chrono.ChVectorD(0, 0,
-0.1), # the 'A' poin space (beginning of beam)
chrono.ChVectorD(0.5, 0, -0.1)) # the 'B' poin space (end of beam)
# After having used BuildBeam(), you can retrieve the nodes used for the beam,
# For example say you want to fix both pos and dir of A end and apply a force to the B end:
# builder.GetLastBeamNodes().back().SetFixed(True)
builder.GetLastBeamNodes().front().SetForce(
chrono.ChVectorD(0, -0.2, 0))
# For instance, now retrieve the A end and add a constrato
# block the position only of that node:
mtruss = chrono.ChBody()
mtruss.SetBodyFixed(True)
constraint_hinge = fea.ChLinkPointFrame()
constraint_hinge.Initialize(builder.GetLastBeamNodes().back(), mtruss)
system.Add(constraint_hinge)
def make_box(size,
pos,
fixed=False,
collide=True,
euler_angles=(0, 0, 0),
color=(1., 0, 1.)):
body = chrono.ChBody()
body.SetBodyFixed(fixed)
body.SetPos(chrono.ChVectorD(*(swap_yz(pos))))
body.SetRot(chrono.Q_from_Euler123(chrono.ChVectorD(*euler_angles)))
body.GetCollisionModel().ClearModel()
# swap_yz(size)
body.GetCollisionModel().AddBox(size[0], size[2],
size[1]) # must set half sizes
body.GetCollisionModel().BuildModel()
body.SetCollide(collide)
mboxasset = chrono.ChBoxShape()
mboxasset.GetBoxGeometry().Size = chrono.ChVectorD(*(swap_yz(size)))
mboxasset.SetColor(chrono.ChColor(color[0], color[1], color[2]))
body.AddAsset(mboxasset)
# system.Add(body)
return body
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()
## 1. Create the physical system that will handle all bodies and constraints.
## Specify the gravitational acceleration vector, consistent with the
## global reference frame having Z up.
system = chrono.ChSystemNSC()
system.Set_G_acc(chrono.ChVectorD(0, 0, -9.81))
## 2. Create the rigid bodies of the slider-crank mechanical system.
## For each body, specify:
## - a unique identifier
## - mass and moments of inertia
## - position and orientation of the (centroidal) body frame
## - visualization assets (defined with respect to the body frame)
## Ground
ground = chrono.ChBody()
system.AddBody(ground)
ground.SetIdentifier(-1)
ground.SetName("ground")
ground.SetBodyFixed(True)
cyl_g = chrono.ChCylinderShape()
cyl_g.GetCylinderGeometry().p1 = chrono.ChVectorD(0, 0.2, 0)
cyl_g.GetCylinderGeometry().p2 = chrono.ChVectorD(0, -0.2, 0)
cyl_g.GetCylinderGeometry().rad = 0.03
ground.AddAsset(cyl_g)
col_g = chrono.ChColorAsset()
col_g.SetColor(chrono.ChColor(0.6, 0.6, 0.2))
ground.AddAsset(col_g)
body_particles.AddParticle(chrono.ChCoordsysD(chrono.ChVectorD(ix/100,0.1+iy/100, iz/100))) # Visualization shape (shared by all particle clones) body_particles_shape = chrono.ChSphereShape() body_particles_shape.GetSphereGeometry().rad = 0.005 body_particles.GetAssets().push_back(body_particles_shape) my_system.Add(body_particles) # Create the floor: a simple fixed rigid body with a collision shape # and a visualization shape body_floor = chrono.ChBody() body_floor.SetBodyFixed(True) # Collision shape body_floor.GetCollisionModel().ClearModel() body_floor.GetCollisionModel().AddBox(0.1, 0.02, 0.1) # hemi sizes body_floor.GetCollisionModel().BuildModel() body_floor.SetCollide(True) # Visualization shape body_floor_shape = chrono.ChBoxShape() body_floor_shape.GetBoxGeometry().Size = chrono.ChVectorD(0.1, 0.02, 0.1) body_floor_shape.SetColor(chrono.ChColor(0.5,0.5,0.5)) body_floor.GetAssets().push_back(body_floor_shape) my_system.Add(body_floor)
def reset(self):
#print("reset")
self.isdone = False
self.rev_pend_sys.Clear()
# create it
self.body_rod = chrono.ChBody()
# set initial position
self.body_rod.SetPos(chrono.ChVectorD(0, self.size_rod_y / 2, 0))
# set mass properties
self.body_rod.SetMass(self.mass_rod)
self.body_rod.SetInertiaXX(
chrono.ChVectorD(self.inertia_rod_x, self.inertia_rod_y,
self.inertia_rod_x))
# set collision surface properties
self.body_rod.SetMaterialSurface(self.rod_material)
# Visualization shape, for rendering animation
self.cyl_base1 = chrono.ChVectorD(0, -self.size_rod_y / 2, 0)
self.cyl_base2 = chrono.ChVectorD(0, self.size_rod_y / 2, 0)
self.body_rod_shape = chrono.ChCylinderShape()
self.body_rod_shape.GetCylinderGeometry().p1 = self.cyl_base1
self.body_rod_shape.GetCylinderGeometry().p2 = self.cyl_base2
self.body_rod_shape.GetCylinderGeometry().rad = self.radius_rod
self.body_rod.AddAsset(self.body_rod_shape)
self.rev_pend_sys.Add(self.body_rod)
self.body_floor = chrono.ChBody()
self.body_floor.SetBodyFixed(True)
self.body_floor.SetPos(chrono.ChVectorD(0, -5, 0))
self.body_floor.SetMaterialSurface(self.rod_material)
if self.render:
self.body_floor_shape = chrono.ChBoxShape()
self.body_floor_shape.GetBoxGeometry().Size = chrono.ChVectorD(
3, 1, 3)
self.body_floor.GetAssets().push_back(self.body_floor_shape)
self.body_floor_texture = chrono.ChTexture()
self.body_floor_texture.SetTextureFilename(
'../../../data/concrete.jpg')
self.body_floor.GetAssets().push_back(self.body_floor_texture)
self.rev_pend_sys.Add(self.body_floor)
self.body_table = chrono.ChBody()
self.body_table.SetPos(chrono.ChVectorD(0, -self.size_table_y / 2, 0))
self.body_table.SetMaterialSurface(self.rod_material)
if self.render:
self.body_table_shape = chrono.ChBoxShape()
self.body_table_shape.GetBoxGeometry().Size = chrono.ChVectorD(
self.size_table_x / 2, self.size_table_y / 2,
self.size_table_z / 2)
self.body_table_shape.SetColor(chrono.ChColor(0.4, 0.4, 0.5))
self.body_table.GetAssets().push_back(self.body_table_shape)
self.body_table_texture = chrono.ChTexture()
self.body_table_texture.SetTextureFilename(
'../../../data/concrete.jpg')
self.body_table.GetAssets().push_back(self.body_table_texture)
self.body_table.SetMass(0.1)
self.rev_pend_sys.Add(self.body_table)
self.link_slider = chrono.ChLinkLockPrismatic()
z2x = chrono.ChQuaternionD()
z2x.Q_from_AngAxis(-chrono.CH_C_PI / 2, chrono.ChVectorD(0, 1, 0))
self.link_slider.Initialize(
self.body_table, self.body_floor,
chrono.ChCoordsysD(chrono.ChVectorD(0, 0, 0), z2x))
self.rev_pend_sys.Add(self.link_slider)
self.act_initpos = chrono.ChVectorD(0, 0, 0)
self.actuator = chrono.ChLinkMotorLinearForce()
self.actuator.Initialize(self.body_table, self.body_floor,
chrono.ChFrameD(self.act_initpos))
self.rev_pend_sys.Add(self.actuator)
self.rod_pin = chrono.ChMarker()
self.body_rod.AddMarker(self.rod_pin)
self.rod_pin.Impose_Abs_Coord(
chrono.ChCoordsysD(chrono.ChVectorD(0, 0, 0)))
self.table_pin = chrono.ChMarker()
self.body_table.AddMarker(self.table_pin)
self.table_pin.Impose_Abs_Coord(
chrono.ChCoordsysD(chrono.ChVectorD(0, 0, 0)))
self.pin_joint = chrono.ChLinkLockRevolute()
self.pin_joint.Initialize(self.rod_pin, self.table_pin)
self.rev_pend_sys.Add(self.pin_joint)
if self.render:
# ---------------------------------------------------------------------
#
# Create an Irrlicht application to visualize the system
#
# ==IMPORTANT!== Use this function for adding a ChIrrNodeAsset to all items
# in the system. These ChIrrNodeAsset assets are 'proxies' to the Irrlicht meshes.
# If you need a finer control on which item really needs a visualization proxy
# Irrlicht, just use application.AssetBind(myitem); on a per-item basis.
self.myapplication.AssetBindAll()
# ==IMPORTANT!== Use this function for 'converting' into Irrlicht meshes the assets
# that you added to the bodies into 3D shapes, they can be visualized by Irrlicht!
self.myapplication.AssetUpdateAll()
self.isdone = False
self.steps = 0
self.step(np.array([[0]]))
return self.get_ob()
def reset(self):
#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()
#
# Recall that Irrlicht uses a left-hand frame, so everything is rendered with
# left and right flipped.
#
# =============================================================================
import pychrono as chrono
import pychrono.irrlicht as irr
import math as m
print("Copyright (c) 2017 projectchrono.org")
system = chrono.ChSystemNSC()
# Create ground body
ground = chrono.ChBody()
system.AddBody(ground)
ground.SetIdentifier(-1)
ground.SetBodyFixed(True)
ground.SetCollide(False)
# Visualization for revolute joint
cyl_rev = chrono.ChCylinderShape()
cyl_rev.GetCylinderGeometry().p1 = chrono.ChVectorD(0, 0, 0.2)
cyl_rev.GetCylinderGeometry().p2 = chrono.ChVectorD(0, 0, -0.2)
cyl_rev.GetCylinderGeometry().rad = 0.04
ground.AddAsset(cyl_rev)
# Create a pendulum body
pend = chrono.ChBody()
system.AddBody(pend)
def __init__(self, system, mesh, n_chains=6):
self.bodies = [] #[chrono.ChBodyEasyBox for i in range(n_chains)]
msection_cable2 = fea.ChBeamSectionCable()
msection_cable2.SetDiameter(0.015)
msection_cable2.SetYoungModulus(0.01e9)
msection_cable2.SetBeamRaleyghDamping(0.000)
mtruss = chrono.ChBody()
mtruss.SetBodyFixed(True)
for j in range(n_chains):
builder = fea.ChBuilderCableANCF()
# Now, simply use BuildBeam to create a beam from a poto another:
builder.BuildBeam(
mesh, # the mesh where to put the created nodes and elements
msection_cable2, # ChBeamSectionCable to use for the ChElementBeamANCF elements
1 + j, # number of ChElementBeamANCF to create
chrono.ChVectorD(0, 0, -0.1 * j), # poA (beginning of beam)
chrono.ChVectorD(0.1 + 0.1 * j, 0,
-0.1 * j) # poB (end of beam)
)
builder.GetLastBeamNodes().back().SetForce(
chrono.ChVectorD(0, -0.2, 0))
constraint_hinge = fea.ChLinkPointFrame()
constraint_hinge.Initialize(builder.GetLastBeamNodes().front(),
mtruss)
system.Add(constraint_hinge)
msphere = chrono.ChSphereShape()
msphere.GetSphereGeometry().rad = 0.02
constraint_hinge.AddAsset(msphere)
# make a box and connect it
mbox = chrono.ChBodyEasyBox(0.2, 0.04, 0.04, 1000)
mbox.SetPos(builder.GetLastBeamNodes().back().GetPos() +
chrono.ChVectorD(0.1, 0, 0))
system.Add(mbox)
constraint_pos = fea.ChLinkPointFrame()
constraint_pos.Initialize(builder.GetLastBeamNodes().back(), mbox)
system.Add(constraint_pos)
constraint_dir = fea.ChLinkDirFrame()
constraint_dir.Initialize(builder.GetLastBeamNodes().back(), mbox)
constraint_dir.SetDirectionInAbsoluteCoords(
chrono.ChVectorD(1, 0, 0))
system.Add(constraint_dir)
# make another beam
builder.BuildBeam(
mesh, # mesh where to put the created nodes and elements
msection_cable2, # ChBeamSectionCable to use for the ChElementBeamANCF elements
1 + (n_chains - j), # number of ChElementBeamANCF to create
chrono.ChVectorD(mbox.GetPos().x + 0.1, 0,
-0.1 * j), # poA (beginning of beam)
chrono.ChVectorD(mbox.GetPos().x + 0.1 + 0.1 * (n_chains - j),
0, -0.1 * j) # poB (end of beam)
)
constraint_pos2 = fea.ChLinkPointFrame()
constraint_pos2.Initialize(builder.GetLastBeamNodes().front(),
mbox)
system.Add(constraint_pos2)
constraint_dir2 = fea.ChLinkDirFrame()
constraint_dir2.Initialize(builder.GetLastBeamNodes().front(),
mbox)
constraint_dir2.SetDirectionInAbsoluteCoords(
chrono.ChVectorD(1, 0, 0))
system.Add(constraint_dir2)
# make a box and connect it
self.bodies.append(chrono.ChBodyEasyBox(0.2, 0.04, 0.04, 1000))
self.bodies[j].SetPos(builder.GetLastBeamNodes().back().GetPos() +
chrono.ChVectorD(0.1, 0, 0))
system.Add(self.bodies[j])
constraint_pos3 = fea.ChLinkPointFrame()
constraint_pos3.Initialize(builder.GetLastBeamNodes().back(),
self.bodies[j])
system.Add(constraint_pos3)
constraint_dir3 = fea.ChLinkDirFrame()
constraint_dir3.Initialize(builder.GetLastBeamNodes().back(),
self.bodies[j])
constraint_dir3.SetDirectionInAbsoluteCoords(
chrono.ChVectorD(1, 0, 0))
system.Add(constraint_dir3)
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()
# Created: 1/01/2019
# Copyright: (c) ProjectChrono 2019
#------------------------------------------------------------------------------
print ("Second tutorial: create and populate a physical system");
# Load the Chrono::Engine core module!
import pychrono as chrono
# Create a physical system,
my_system = chrono.ChSystemNSC()
# Add two bodies
my_shbodyA = chrono.ChBody()
my_shbodyA.SetMass(20)
my_shbodyA.SetName('BodyA')
my_shbodyA.SetInertiaXX( chrono.ChVectorD(10,10,10) )
print (my_shbodyA.GetInertia() )
my_shbodyA.SetPos(chrono.ChVectorD(1,-1,0))
my_shbodyA.GetCollisionModel().AddBox(10,1,10)
my_shbodyA.SetBodyFixed(True)
my_shbodyA.SetCollide(True)
my_shbodyB = chrono.ChBody()
my_shbodyB.SetName('BodyB')
my_shbodyB.SetPos(chrono.ChVectorD(0,2,0))
my_shbodyB.GetCollisionModel().AddBox(1,1,1)
my_shbodyB.SetCollide(True)
# =============================================================================
print("Copyright (c) 2017 projectchrono.org")
system = chrono.ChSystemNSC()
system.Set_G_acc(chrono.ChVectorD(0, 0, 0))
# Revolute joint frame
rev_rot = chrono.Q_from_AngX(m.pi / 6.0)
rev_dir = rev_rot.GetZaxis()
rev_pos = chrono.ChVectorD(+1, 0, 0)
# Create ground body
ground = chrono.ChBody()
system.AddBody(ground)
ground.SetIdentifier(-1)
ground.SetBodyFixed(True)
ground.SetCollide(False)
# Visualization for revolute joint
cyl_rev = chrono.ChCylinderShape()
cyl_rev.GetCylinderGeometry().p1 = rev_pos + rev_dir * 0.2
cyl_rev.GetCylinderGeometry().p2 = rev_pos - rev_dir * 0.2
cyl_rev.GetCylinderGeometry().rad = 0.1
ground.AddAsset(cyl_rev)
# Offset from joint to body COM
offset = chrono.ChVectorD(1.5, 0, 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
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)
tilt_factor = 1.1 # no tilt; higher number tilts body toward positive x
leg_ang = np.array([
tilt_factor * math.pi / 2, tilt_factor * math.pi / 2
]) #(1/4)*math.pi+(1/2)*math.pi*np.array([0,1])
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, 0, 1))
# postion of leg CM from abdomen
self.leg_pos[i] = chrono.ChVectorD(
(0.5 * self.leg_length * math.cos(leg_ang[i])),
(self.abdomen_y0 - self.leg_length / 2 * math.sin(leg_ang[i]) -
self.abdomen_y * math.cos(self.CMtohip_angle)),
(-1)**i * self.abdomen_z * math.sin(self.CMtohip_angle))
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(0, self.abdomen_y0,
0)) # connection to abdomen
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)
# Limits need to turn back to true
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 / 3)
self.Ankle_rev[i].GetLimit_Rz().SetMax(math.pi / 3)
# 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, -self.abdomen_y0 -
self.abdomen_y * math.cos(self.CMtohip_angle) -
self.ankle_radius, 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('../data/grass.jpg')
self.body_floor.GetAssets().push_back(body_floor_texture)
self.ant_sys.Add(self.body_floor)
# this was commented out to not fix the segments
# this is helpful to check geometry of humanoid
#self.body_abdomen.SetBodyFixed(True)
#self.leg_body[0].SetBodyFixed(True)
#self.leg_body[1].SetBodyFixed(True)
#self.ankle_body[0].SetBodyFixed(True)
#self.ankle_body[1].SetBodyFixed(True)
if (self.animate):
self.myapplication.AssetBindAll()
self.myapplication.AssetUpdateAll()
self.numsteps = 0
self.step(np.zeros(4))
return self.get_ob()
body_belt.SetCoord(my_marker.GetAbsCoord()) # note: the CAD reference was on surface, so we must move down COG that is at middle of surface: #body_belt.ConcatenatePreTransformation(chrono.ChFrameMovingD(chrono.ChVectorD(0, -0.005-conv_thick/2., 0))) my_system.Add(body_belt) # Create the room floor: a simple fixed rigid body with a collision shape # and a visualization shape body_floor = chrono.ChBody() body_floor.SetBodyFixed(True) body_floor.SetPos(chrono.ChVectorD(0, -1, 0 )) body_floor.SetMaterialSurface(pebble_material) # Collision shape (shared by all particle clones) body_floor.GetCollisionModel().ClearModel() body_floor.GetCollisionModel().AddBox(3, 1, 3) # hemi sizes body_floor.GetCollisionModel().BuildModel() body_floor.SetCollide(True) # Visualization shape (shared by all particle clones) body_floor_shape = chrono.ChBoxShape() body_floor_shape.GetBoxGeometry().Size = chrono.ChVectorD(3, 1, 3) body_floor.GetAssets().push_back(body_floor_shape) body_floor_texture = chrono.ChTexture()
force = -spring_coef * (length - rest_length) - damping_coef * vel
return force
# =============================================================================
print("Copyright (c) 2017 projectchrono.org")
system = chrono.ChSystemNSC()
system.Set_G_acc(chrono.ChVectorD(0, 0, 0))
# Create the ground body with two visualization spheres
# -----------------------------------------------------
ground = chrono.ChBody()
system.AddBody(ground)
ground.SetIdentifier(-1)
ground.SetBodyFixed(True)
ground.SetCollide(False)
sph_1 = chrono.ChSphereShape()
sph_1.GetSphereGeometry().rad = 0.1
sph_1.Pos = chrono.ChVectorD(-1, 0, 0)
ground.AddAsset(sph_1)
sph_2 = chrono.ChSphereShape()
sph_2.GetSphereGeometry().rad = 0.1
sph_2.Pos = chrono.ChVectorD(1, 0, 0)
ground.AddAsset(sph_2)
# Create a body suspended through a ChLinkTSDA (default linear)
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()
# Set solver settings system.SetSolverMaxIterations(100) system.SetSolverForceTolerance(0) # -------------------------------------------------- # Create a contact material, shared among all bodies # -------------------------------------------------- material = chrono.ChMaterialSurfaceSMC() material.SetFriction(friction) # ---------- # Add bodies # ---------- container = chrono.ChBody() system.Add(container) container.SetPos(chrono.ChVectorD(0, 0, 0)) container.SetBodyFixed(True) container.SetIdentifier(-1) container.SetCollide(True) container.GetCollisionModel().ClearModel() chrono.AddBoxGeometry(container, material, chrono.ChVectorD(4, 0.5, 4), chrono.ChVectorD(0, -0.5, 0)) container.GetCollisionModel().BuildModel() container.AddAsset(chrono.ChColorAsset(chrono.ChColor(0.4, 0.4, 0.4))) box1 = chrono.ChBody() box1.SetMass(10) box1.SetInertiaXX(chrono.ChVectorD(1, 1, 1))
import pychrono.irrlicht as irr
import math as m
try:
import numpy as np
from numpy import linalg as LA
except ImportError:
print("You need NumPy to run this demo!")
print("Copyright (c) 2017 projectchrono.org")
system = chrono.ChSystemNSC()
system.Set_G_acc(chrono.ChVectorD(0, 0, 0))
# Create the ground body
ground = chrono.ChBody()
system.AddBody(ground)
ground.SetIdentifier(-1)
ground.SetBodyFixed(True)
ground.SetCollide(False)
rail1 = chrono.ChBoxShape()
rail1.GetBoxGeometry().SetLengths(chrono.ChVectorD(8, 0.1, 0.1))
rail1.GetBoxGeometry().Pos = chrono.ChVectorD(0, 0, -1)
ground.AddAsset(rail1)
rail2 = chrono.ChBoxShape()
rail2.GetBoxGeometry().SetLengths(chrono.ChVectorD(8, 0.1, 0.1))
rail2.GetBoxGeometry().Pos = chrono.ChVectorD(0, 0, 1)
ground.AddAsset(rail2)
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()
mbodyflywheel = chrono.ChBodyEasyCylinder(0.24, 0.05, 7800) # R, h, density
mbodyflywheel.SetCoord(
chrono.ChCoordsysD(
node_mid.GetPos() + chrono.ChVectorD(
0, 0.05, 0), # flywheel initial center (plus Y offset)
chrono.Q_from_AngAxis(CH_C_PI / 2.0, chrono.VECT_Z)
) # flywheel initial alignment (rotate 90° so cylinder axis is on X)
)
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)
print("Second tutorial: create and populate a physical system")
# Load the Chrono::Engine core module!
import pychrono as chrono
# Create a physical system,
my_system = chrono.ChSystemNSC()
# Create a contact material, shared by all collision shapes
material = chrono.ChMaterialSurfaceNSC()
material.SetFriction(0.3)
material.SetCompliance(0)
# Add two bodies
bodyA = chrono.ChBody()
bodyA.SetMass(20)
bodyA.SetName('BodyA')
bodyA.SetInertiaXX(chrono.ChVectorD(10, 10, 10))
print(bodyA.GetInertia())
bodyA.SetPos(chrono.ChVectorD(1, -1, 0))
bodyA.GetCollisionModel().AddBox(material, 10, 1, 10)
bodyA.SetBodyFixed(True)
bodyA.SetCollide(True)
bodyB = chrono.ChBody()
bodyB.SetName('BodyB')
bodyB.SetPos(chrono.ChVectorD(0, 2, 0))
bodyB.GetCollisionModel().AddBox(material, 1, 1, 1)
bodyB.SetCollide(True)
## 1. Create the physical system that will handle all bodies and constraints.
## Specify the gravitational acceleration vector, consistent with the
## global reference frame having Z up.
system = chrono.ChSystemNSC()
system.Set_G_acc(chrono.ChVectorD(0, 0, -9.81))
## 2. Create the rigid bodies of the slider-crank mechanical system.
## For each body, specify:
## - a unique identifier
## - mass and moments of inertia
## - position and orientation of the (centroidal) body frame
## - visualization assets (defined with respect to the body frame)
## Ground
ground = chrono.ChBody()
system.AddBody(ground)
ground.SetIdentifier(-1)
ground.SetName("ground")
ground.SetBodyFixed(True)
cyl_g = chrono.ChCylinderShape()
cyl_g.GetCylinderGeometry().p1 = chrono.ChVectorD(0, 0.2, 0)
cyl_g.GetCylinderGeometry().p2 = chrono.ChVectorD(0, -0.2, 0)
cyl_g.GetCylinderGeometry().rad = 0.03
ground.AddAsset(cyl_g)
col_g = chrono.ChColorAsset()
col_g.SetColor(chrono.ChColor(0.6, 0.6, 0.2))
ground.AddAsset(col_g)
application.AddTypicalLogo() application.AddTypicalSky() application.AddTypicalLights() application.AddTypicalCamera(chronoirr.vector3df(0.0, 0.6, -1.0)) L = 1 H = 0.25 K = 0.05 vA = chrono.ChVectorD(0, 0, 0) vC = chrono.ChVectorD(L, 0, 0) vB = chrono.ChVectorD(L, -H, 0) vG = chrono.ChVectorD(L - K, -H, 0) vd = chrono.ChVectorD(0, 0, 0.0001) # Create a truss: body_truss = chrono.ChBody() body_truss.SetBodyFixed(True) my_system.AddBody(body_truss) # Attach a 'box' shape asset for visualization. mboxtruss = chrono.ChBoxShape() mboxtruss.GetBoxGeometry().Pos = chrono.ChVectorD(-0.01, 0, 0) mboxtruss.GetBoxGeometry().SetLengths(chrono.ChVectorD(0.02, 0.2, 0.1)) body_truss.AddAsset(mboxtruss) # Create body for crank body_crank = chrono.ChBody() body_crank.SetPos((vB + vG) * 0.5) my_system.AddBody(body_crank)