mysystem.Add(mpiston)
# Now create constraints and motors between the bodies.
# Create crank-truss joint: a motor that spins the crank flywheel
my_motor = chrono.ChLinkMotorRotationSpeed()
my_motor.Initialize(
mcrank, # the first connected body
mfloor, # the second connected body
chrono.ChFrameD(crank_center)) # where to create the motor in abs.space
my_angularspeed = chrono.ChFunction_Const(chrono.CH_C_PI) # ang.speed: 180°/s
my_motor.SetMotorFunction(my_angularspeed)
mysystem.Add(my_motor)
# Create crank-rod joint
mjointA = chrono.ChLinkLockRevolute()
mjointA.Initialize(
mrod, mcrank,
chrono.ChCoordsysD(crank_center + chrono.ChVectorD(crank_rad, 0, 0)))
mysystem.Add(mjointA)
# Create rod-piston joint
mjointB = chrono.ChLinkLockRevolute()
mjointB.Initialize(
mpiston, mrod,
chrono.ChCoordsysD(crank_center +
chrono.ChVectorD(crank_rad + rod_length, 0, 0)))
mysystem.Add(mjointB)
# Create piston-truss joint
mjointC = chrono.ChLinkLockPrismatic()
frame_marker_turret_bicep = make_frame_from_name("Assem10/Assem4/marker#2", root_frame)
frame_marker_bicep_elbow = make_frame_from_name("Assem10/Assem1/marker#2", root_frame)
frame_marker_elbow_forearm = make_frame_from_name("Assem10/Assem5/marker#2", root_frame)
frame_marker_forearm_wrist = make_frame_from_name("Assem10/Assem7/marker#2", root_frame)
frame_marker_wrist_hand = make_frame_from_name("Assem10/Assem6/marker#2", root_frame)
frame_marker_turret_cylinder= make_frame_from_name("Assem10/Assem4/marker#3", root_frame)
frame_marker_cylinder_rod = make_frame_from_name("Assem10/Assem3/marker#2", root_frame)
frame_marker_rod_bicep = make_frame_from_name("Assem10/Assem2/marker#2", root_frame)
# Create joints between the parts.
# This can be done by creating link objects between couples of the bodies
# created in the section above, where the joint position is one of the
# frame_marker_xxxx_zzzz frames created above.
my_link1 = chrono.ChLinkLockRevolute()
my_link1.Initialize(mrigidBody_base, mrigidBody_turret, frame_marker_base_turret.GetCoord())
mysystem.Add(my_link1)
my_link2 = chrono.ChLinkLockRevolute()
my_link2.Initialize(mrigidBody_turret, mrigidBody_bicep, frame_marker_turret_bicep.GetCoord())
mysystem.Add(my_link2)
my_link3 = chrono.ChLinkLockRevolute()
my_link3.Initialize(mrigidBody_bicep, mrigidBody_elbow, frame_marker_bicep_elbow.GetCoord())
mysystem.Add(my_link3)
my_link4 = chrono.ChLinkLockRevolute()
my_link4.Initialize(mrigidBody_elbow, mrigidBody_forearm, frame_marker_elbow_forearm.GetCoord())
mysystem.Add(my_link4)
print("Warning. Desired STEP file could not be opened/parsed \n")
# Create a revolute joint between the two parts
# as in a pendulum. We assume we already know in advance
# the aboslute position of the joint (ex. we used measuring tools in the 3D CAD)
measured_joint_pos_mm = chrono.ChVectorD(0, 48, 120)
scale = 1. / 1000. # because we use meters instead of mm
joint_pos = chrono.ChVectorD(
root_frame.TransformPointLocalToParent(measured_joint_pos_mm * scale))
# transform because we rotated everything
if (mrigidBody1 and mrigidBody2):
my_link = chrono.ChLinkLockRevolute()
my_link.Initialize(mrigidBody1, mrigidBody2, chrono.ChCoordsysD(joint_pos))
mysystem.Add(my_link)
# Create a large cube as a floor.
mfloor = chrono.ChBodyEasyBox(1, 0.2, 1, 1000)
mfloor.SetPos(chrono.ChVectorD(0, -0.3, 0))
mfloor.SetBodyFixed(True)
mysystem.Add(mfloor)
mcolor = chrono.ChColorAsset(0.3, 0.3, 0.8)
mfloor.AddAsset(mcolor)
# ---------------------------------------------------------------------
#
cyl_1.GetCylinderGeometry().p1 = chrono.ChVectorD(-1, 0, 0) cyl_1.GetCylinderGeometry().p2 = chrono.ChVectorD(1, 0, 0) cyl_1.GetCylinderGeometry().rad = 0.2 pend_1.AddAsset(cyl_1) col_1 = chrono.ChColorAsset(0.6, 0, 0) pend_1.AddAsset(col_1) # Specify the intial position of the pendulum (horizontal, pointing towards # positive X). In this case, we set the absolute position of its center of # mass pend_1.SetPos(chrono.ChVectorD(1, 0, 1)) # Create a revolute joint to connect pendulum to ground. We specify the link # coordinate frame in the absolute frame. rev_1 = chrono.ChLinkLockRevolute() rev_1.Initialize(ground, pend_1, chrono.ChCoordsysD(chrono.ChVectorD(0, 0, 1), chrono.ChQuaternionD(1, 0, 0, 0))) system.AddLink(rev_1) # Create a pendulum modeled using ChBodyAuxRef pend_2 = chrono.ChBodyAuxRef() system.Add(pend_2) pend_2.SetIdentifier(2) pend_2.SetBodyFixed(False) pend_2.SetCollide(False) pend_2.SetMass(1) pend_2.SetInertiaXX(chrono.ChVectorD(0.2, 1, 1)) # NOTE: the inertia tensor must still be expressed in the centroidal frame! # Attach a visualizationn asset. Note that now the cylinder is defined with # respect to the body reference frame.