crank.SetMass(1.0)
crank.SetInertiaXX(chrono.ChVectorD(0.005, 0.1, 0.1))
crank.SetPos(chrono.ChVectorD(-1, 0, 0))
crank.SetRot(chrono.ChQuaternionD(1, 0, 0, 0))
box_c = chrono.ChBoxShape()
box_c.GetBoxGeometry().Size = chrono.ChVectorD(0.95, 0.05, 0.05)
crank.AddAsset(box_c)
cyl_c = chrono.ChCylinderShape()
cyl_c.GetCylinderGeometry().p1 = chrono.ChVectorD(1, 0.1, 0)
cyl_c.GetCylinderGeometry().p2 = chrono.ChVectorD(1, -0.1, 0)
cyl_c.GetCylinderGeometry().rad = 0.05
crank.AddAsset(cyl_c)
sph_c = chrono.ChSphereShape()
sph_c.GetSphereGeometry().center = chrono.ChVectorD(-1, 0, 0)
sph_c.GetSphereGeometry().rad = 0.05
crank.AddAsset(sph_c)
col_c = chrono.ChColorAsset()
col_c.SetColor(chrono.ChColor(0.6, 0.2, 0.2))
crank.AddAsset(col_c)
## Slider
slider = chrono.ChBody()
system.AddBody(slider)
slider.SetIdentifier(2)
slider.SetName("slider")
slider.SetMass(1.0)
slider.SetInertiaXX(chrono.ChVectorD(0.05, 0.05, 0.05))
body_particles.SetInertiaXX(chrono.ChVectorD(inertia,inertia,inertia));
# Collision shape (shared by all particle clones) Must be defined BEFORE adding particles
body_particles.GetCollisionModel().ClearModel()
body_particles.GetCollisionModel().AddSphere(0.005)
body_particles.GetCollisionModel().BuildModel()
body_particles.SetCollide(True)
# add particles
for ix in range(0,5):
for iy in range(0,5):
for iz in range(0,3):
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
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()
lin_vel = ang_vel % offset # Create pendulum body body = chrono.ChBody() system.AddBody(body) body.SetPos(rev_pos + offset) body.SetPos_dt(lin_vel) body.SetWvel_par(ang_vel) body.SetIdentifier(1) body.SetBodyFixed(False) body.SetCollide(False) body.SetMass(1) body.SetInertiaXX(chrono.ChVectorD(1, 1, 1)) # Attach visualization assets sph = chrono.ChSphereShape() sph.GetSphereGeometry().rad = 0.3 body.AddAsset(sph) cyl = chrono.ChCylinderShape() cyl.GetCylinderGeometry().p1 = chrono.ChVectorD(-1.5, 0, 0) cyl.GetCylinderGeometry().p2 = chrono.ChVectorD(0, 0, 0) cyl.GetCylinderGeometry().rad = 0.1 body.AddAsset(cyl) col = chrono.ChColorAsset() col.SetColor(chrono.ChColor(0.7, 0.8, 0.8)) body.AddAsset(col) # Create revolute joint between body and ground rev = chrono.ChLinkLockRevolute() rev.Initialize(body, ground, chrono.ChCoordsysD(rev_pos, rev_rot)) system.AddLink(rev)
# We will create two directories for saving some files, we need this: import os # Create a physical system, my_system = chrono.ChSystemNSC() my_systemB = my_system my_system.SetTol(2) print(my_systemB.GetTol()) # Create a body body_1 = chrono.ChBodyAuxRef() my_system.Add(body_1) # Attach a visualization asset to the body (ex.: a sphere) myasset = chrono.ChSphereShape() myasset.GetSphereGeometry().rad = 0.2 body_1.GetAssets().push_back(myasset) # Assets can be shared, ex. to save memory... body_2 = chrono.ChBodyAuxRef() body_2.SetPos(chrono.ChVectorD(0.5, 0, 0)) my_system.Add(body_2) body_2.GetAssets().push_back(myasset) # # Create an exporter to POVray !!! # pov_exporter = postprocess.ChPovRay(my_system)
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()
inertia_pebble = (2./5.)*mass_pebble*(pow((diameter_pebble*0.5),2)) pebbles = chrono.ChParticlesClones() pebbles.SetMass(mass_pebble) pebbles.SetInertiaXX(chrono.ChVectorD(inertia_pebble,inertia_pebble,inertia_pebble)) pebbles.SetMaterialSurface(pebble_material) pebbles.GetCollisionModel().ClearModel() pebbles.GetCollisionModel().AddSphere(diameter_pebble/2.) pebbles.GetCollisionModel().BuildModel() pebbles.SetCollide(True) pebbles_shape = chrono.ChSphereShape() pebbles_shape.GetSphereGeometry().rad = diameter_pebble/2. pebbles.GetAssets().push_back(pebbles_shape) my_system.Add(pebbles) max_pebbles = 1000 created_pebbles = 0 n_outlet_x = 5 n_outlet_z = 5 size_outlet_x = 0.28 size_outlet_z = 0.28 #pos_outlet = chrono.ChVectorD(0,1.0,0) # to drop stuff directly on vertical pos_outlet = chrono.ChVectorD(-2,1.6,0) # to drop stuff into the hopper
# =============================================================================
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)
# -------------------------------------------------------------
body_1 = chrono.ChBody()
system.AddBody(body_1)
body_1.SetPos(chrono.ChVectorD(-1, -3, 0))
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)
# Create a constrabetween the vertical and horizontal beams:
constr_bb = chrono.ChLinkMateGeneric()
constr_bb.Initialize(node_top, node_tip, False, node_top.Frame(),
node_top.Frame())
my_system.Add(constr_bb)
constr_bb.SetConstrainedCoords(
True,
True,
True, # x, y, z
False,
False,
False) # Rx, Ry, Rz
# For example, attach small shape to show the constraint
msphereconstr2 = chrono.ChSphereShape()
msphereconstr2.GetSphereGeometry().rad = 0.01
constr_bb.AddAsset(msphereconstr2)
# Create a beam as a crank
msection3 = fea.ChBeamSectionEulerAdvanced()
crankbeam_d = 0.048
msection3.SetDensity(2700)
msection3.SetYoungModulus(73.0e9)
msection3.SetGwithPoissonRatio(0.3)
msection3.SetBeamRaleyghDamping(0.000)
msection3.SetAsCircularSection(crankbeam_d)
builderB = fea.ChBuilderBeamEuler()
builderB.BuildBeam(
my_mesh, # the mesh where to put the created nodes and elements
