def build_external_cylinder(cyl_radius, shape_length, density, contact_method,
offset):
"""
Build a left and right cylinder next to the shape_lenght that must be centered
on 0,0,0 coord system.
:param cyl_radius: radius of the two external cylinders
:param shape_length: lenght of the shape
:param density: Density of the cylinders
:param contact_method: SMC or NSC
:param offset: distance between disk and shape
:return: two ChEasyCylinder
"""
height = 0.01 * shape_length
qCylinder = chrono.Q_from_AngX(90 * chrono.CH_C_DEG_TO_RAD)
left_cyl = chrono.ChBodyEasyCylinder(cyl_radius, height, density, True,
False, contact_method)
right_cyl = chrono.ChBodyEasyCylinder(cyl_radius, height, density, True,
False, contact_method)
left_cyl.SetRot(qCylinder)
right_cyl.SetRot(qCylinder)
left_cyl.SetPos(chrono.ChVectorD(0, 0, -(shape_length / 2. + offset)))
right_cyl.SetPos(chrono.ChVectorD(0, 0, shape_length / 2. + offset))
left_cyl.SetBodyFixed(True)
right_cyl.SetBodyFixed(True)
return left_cyl, right_cyl
## 6. Perform the simulation.
## Specify the step-size.
application.SetTimestep(0.01)
application.SetTryRealtime(True)
while (application.GetDevice().run()):
## Initialize the graphical scene.
application.BeginScene()
## Render all visualization objects.
application.DrawAll()
## Render the distance constraint.
chronoirr.ChIrrTools.drawSegment(application.GetVideoDriver(),
dist_crank_slider.GetEndPoint1Abs(),
dist_crank_slider.GetEndPoint2Abs(),
chronoirr.SColor(255, 200, 20, 0), True)
## Draw an XZ grid at the global origin to add in visualization.
chronoirr.ChIrrTools.drawGrid(
application.GetVideoDriver(), 1, 1, 20, 20,
chrono.ChCoordsysD(chrono.ChVectorD(0, 0, 0),
chrono.Q_from_AngX(chrono.CH_C_PI_2)),
chronoirr.SColor(255, 80, 100, 100), True)
## Advance simulation by one step.
application.DoStep()
## Finalize the graphical scene.
application.EndScene()
mbody_train = chrono.ChBodyEasyBox(8, 1.5, 1.0, 1000, True, False, mat)
mphysicalSystem.Add(mbody_train)
mbody_train.SetPos(chrono.ChVectorD(3, 0, 0))
# ...which must rotate respect to truss along Z axis, in 0,0,0
link_revoluteTT = chrono.ChLinkLockRevolute()
link_revoluteTT.Initialize(
mbody_truss, mbody_train,
chrono.ChCoordsysD(chrono.ChVectorD(0, 0, 0), chrono.QUNIT))
mphysicalSystem.AddLink(link_revoluteTT)
# ...the first gear
mbody_gearA = chrono.ChBodyEasyCylinder(radA, 0.5, 1000, True, False, mat)
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)
##robot.SetVisualizationTypeChassis(robosimian::VisualizationType::MESH)
##robot.SetVisualizationTypeLimb(robosimian::FL, robosimian::VisualizationType::COLLISION)
##robot.SetVisualizationTypeLimb(robosimian::FR, robosimian::VisualizationType::COLLISION)
##robot.SetVisualizationTypeLimb(robosimian::RL, robosimian::VisualizationType::COLLISION)
##robot.SetVisualizationTypeLimb(robosimian::RR, robosimian::VisualizationType::COLLISION)
##robot.SetVisualizationTypeLimbs(robosimian::VisualizationType::NONE)
##robot.SetVisualizationTypeChassis(robosimian::VisualizationType::MESH)
##robot.SetVisualizationTypeSled(robosimian::VisualizationType::MESH)
##robot.SetVisualizationTypeLimbs(robosimian::VisualizationType::MESH)
# Initialize Robosimian robot
##robot.Initialize(ChCoordsys<>(chrono.ChVectorD(0, 0, 0), QUNIT))
robot.Initialize(
chrono.ChCoordsysD(chrono.ChVectorD(0, 0, 0),
chrono.Q_from_AngX(chrono.CH_C_PI)))
# -----------------------------------
# Create a driver and attach to robot
# -----------------------------------
if mode == robosimian.LocomotionMode_WALK:
driver = robosimian.RS_Driver(
"", # start input file
chrono.GetChronoDataFile("robot/robosimian/actuation/walking_cycle.txt"
), # cycle input file
"", # stop input file
True)
elif mode == robosimian.LocomotionMode_SCULL:
driver = robosimian.RS_Driver(
chrono.GetChronoDataFile(
application.AssetBindAll() application.AssetUpdateAll() # 12. Perform the simulation. # Specify the step-size. application.SetTimestep(0.001) application.SetTryRealtime(False) while application.GetDevice().run() : # Initialize the graphical scene. application.BeginScene() # Render all visualization objects. application.DrawAll() # Draw an XZ grid at the global origin to add in visualization. chronoirr.ChIrrTools.drawGrid(application.GetVideoDriver(), 0.1, 0.1, 20, 20, chrono.ChCoordsysD(chrono.ChVectorD(0, 0, 0), chrono.Q_from_AngX(chrono.CH_C_PI_2)), chronoirr.SColor(255, 80, 100, 100), True) # Advance simulation by one step. application.DoStep() # Finalize the graphical scene. application.EndScene()
vel, # relative angular speed
link): # back-pointer to associated link
torque = -spring_coef * (angle - rest_angle) - damping_coef * vel
return torque
# =============================================================================
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