# 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()
mjointC.Initialize(
mpiston, mfloor,
chrono.ChCoordsysD(
chrono.ChQuaternionD(1, 0, 0, 0)))
motor.SetAngleFunction(chrono.ChFunction_Ramp(0, 1))
mysystem.AddLink(motor)
# Connect the second shaft to ground through a cylindrical joint
# --------------------------------------------------------------
# Use a cylindrical joint so that we do not have redundant constraints
# (note that, technically Chrono could deal with a revolute joint here).
# the joint is located at the origin of the second shaft.
cyljoint = chrono.ChLinkLockCylindrical()
mysystem.AddLink(cyljoint)
cyljoint.Initialize(
ground, shaft_2,
chrono.ChCoordsysD(chrono.ChVectorD(0, -hl * sina, hl * cosa), rot))
# Connect the two shafts through a universal joint
# ------------------------------------------------
# The joint is located at the global origin. Its kinematic constraints will
# enforce orthogonality of the associated cross.
ujoint = chrono.ChLinkUniversal()
mysystem.AddLink(ujoint)
ujoint.Initialize(shaft_1, shaft_2,
chrono.ChFrameD(chrono.ChVectorD(0, 0, 0), rot))
# ---------------------------------------------------------------------
#
# Create an Irrlicht application to visualize the system
render_step_size = 1.0 / 50 # FPS = 50
# =============================================================================
#print ( "Copyright (c) 2017 projectchrono.org\nChrono version: ", chrono.CHRONO_VERSION , "\n\n")
# --------------
# Create systems
# --------------
# Create the MAN 10t vehicle, set parameters, and initialize
my_truck = veh.MAN_10t()
my_truck.SetContactMethod(contact_method)
my_truck.SetChassisCollisionType(chassis_collision_type)
my_truck.SetChassisFixed(False)
my_truck.SetInitPosition(chrono.ChCoordsysD(initLoc, initRot))
my_truck.SetTireType(tire_model)
my_truck.SetTireStepSize(tire_step_size)
my_truck.SetShaftBasedDrivetrain(True)
my_truck.Initialize()
my_truck.SetChassisVisualizationType(chassis_vis_type)
my_truck.SetSuspensionVisualizationType(suspension_vis_type)
my_truck.SetSteeringVisualizationType(steering_vis_type)
my_truck.SetWheelVisualizationType(wheel_vis_type)
my_truck.SetTireVisualizationType(tire_vis_type)
# Create the terrain
terrain = veh.RigidTerrain(my_truck.GetSystem())
patch = terrain.AddPatch(
chrono.ChCoordsysD(chrono.ChVectorD(0, 0, terrainHeight - 5),
render_step_size = 1.0 / 50 # FPS = 50 # ============================================================================= #print ( "Copyright (c) 2017 projectchrono.org\nChrono version: ", chrono.CHRONO_VERSION , "\n\n") # -------------- # Create systems # -------------- # Create the City Bus vehicle, set parameters, and initialize my_bus = veh.CityBus() my_bus.SetContactMethod(contact_method) my_bus.SetChassisCollisionType(chassis_collision_type) my_bus.SetChassisFixed(False) my_bus.SetInitPosition(chrono.ChCoordsysD(initLoc, initRot)) my_bus.SetTireType(tire_model) my_bus.SetTireStepSize(tire_step_size) my_bus.Initialize() tire_vis_type = veh.VisualizationType_MESH # : VisualizationType::PRIMITIVES my_bus.SetChassisVisualizationType(chassis_vis_type) my_bus.SetSuspensionVisualizationType(suspension_vis_type) my_bus.SetSteeringVisualizationType(steering_vis_type) my_bus.SetWheelVisualizationType(wheel_vis_type) my_bus.SetTireVisualizationType(tire_vis_type) # Create the terrain terrain = veh.RigidTerrain(my_bus.GetSystem()) if (contact_method == chrono.ChContactMethod_NSC):
# 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)
# ---------------------------------------------------------------------
#
# Create an Irrlicht application to visualize the system
motor = chrono.ChLinkMotorRotationAngle()
motor.SetSpindleConstraint(chrono.ChLinkMotorRotation.SpindleConstraint_OLDHAM)
motor.SetAngleFunction(chrono.ChFunction_Ramp(0, math.pi / 4))
motor.Initialize(body, ground,
chrono.ChFrameD(tire_center, chrono.Q_from_AngY(math.pi / 2)))
mysystem.Add(motor)
# ------------------------
# Create SCM terrain patch
# ------------------------
# Note that SCMDeformableTerrain uses a default ISO reference frame (Z up). Since the mechanism is modeled here in
# a Y-up global frame, we rotate the terrain plane by -90 degrees about the X axis.
terrain = veh.SCMDeformableTerrain(mysystem)
terrain.SetPlane(
chrono.ChCoordsysD(chrono.ChVectorD(0, 0.2, 0),
chrono.Q_from_AngX(-math.pi / 2)))
terrain.Initialize(2.0, 6.0, 0.04)
my_params = MySoilParams()
if var_params:
# Location-dependent soil properties
terrain.RegisterSoilParametersCallback(my_params)
else:
# Constant soil properties
terrain.SetSoilParameters(
0.2e6, # Bekker Kphi
0, # Bekker Kc
1.1, # Bekker n exponent
0, # Mohr cohesive limit (Pa)
30, # Mohr friction limit (degrees)
0.01, # Janosi shear coefficient (m)
render_step_size = 1.0 / 50 # FPS = 50 # ============================================================================= #print ( "Copyright (c) 2017 projectchrono.org\nChrono version: ", chrono.CHRONO_VERSION , "\n\n") # -------------- # Create systems # -------------- # Create the City Bus vehicle, set parameters, and initialize my_bus = veh.CityBus() my_bus.SetContactMethod(contact_method) my_bus.SetChassisCollisionType(chassis_collision_type) my_bus.SetChassisFixed(False) my_bus.SetInitPosition(chrono.ChCoordsysD(initLoc, initRot)) my_bus.SetTireType(tire_model) my_bus.SetTireStepSize(tire_step_size) my_bus.Initialize() tire_vis_type = veh.VisualizationType_MESH # : VisualizationType::PRIMITIVES my_bus.SetChassisVisualizationType(chassis_vis_type) my_bus.SetSuspensionVisualizationType(suspension_vis_type) my_bus.SetSteeringVisualizationType(steering_vis_type) my_bus.SetWheelVisualizationType(wheel_vis_type) my_bus.SetTireVisualizationType(tire_vis_type) # Create the terrain terrain = veh.RigidTerrain(my_bus.GetSystem()) patch = terrain.AddPatch(chrono.ChCoordsysD(chrono.ChVectorD(0, 0, terrainHeight - 5), chrono.QUNIT),
mfloor.SetBodyFixed(True)
mysystem.Add(mfloor)
mcolor = chrono.ChColorAsset(0.3, 0.3, 0.8)
mfloor.AddAsset(mcolor)
# We want to move the hand of the robot using a trajectory.
# Since in this demo all joints are 'off' (i.e just revolute joints),
# it follows that if we move the hand all the robot will automatically
# move as in inverse kinematics.
# Create a ChLinePath geometry, for the hand path, and insert arc/lines sub-paths:
mpath = chrono.ChLinePath()
ma1 = chrono.ChLineArc(
chrono.ChCoordsysD(mrigidBody_hand.GetPos(), # arc center position
chrono.Q_ROTATE_X_TO_Z), # arc plane alignment (default: xy plane)
0.3, # radius
-chrono.CH_C_PI_2, # start arc ngle (counterclockwise, from local x)
-chrono.CH_C_PI_2+chrono.CH_C_2PI, # end arc angle
True)
mpath.AddSubLine(ma1)
mpath.SetPathDuration(2)
mpath.Set_closed(True)
# Create a ChLineShape, a visualization asset for lines.
mpathasset = chrono.ChLineShape()
mpathasset.SetLineGeometry(mpath)
mfloor.AddAsset(mpathasset)
# This is the constraint that uses the trajectory
mtrajectory = chrono.ChLinkTrajectory()
# Output directories
out_dir = "./UAZBUS"
# =============================================================================
print("Copyright (c) 2017 projectchrono.org\n")
# --------------
# Create systems
# --------------
# Create the vehicle, set parameters, and initialize
uaz = veh.UAZBUS()
uaz.SetChassisFixed(False)
uaz.SetInitPosition(chrono.ChCoordsysD(initLoc, initRot))
uaz.SetTireType(tire_model)
uaz.SetTireStepSize(tire_step_size)
uaz.SetInitFwdVel(0.0)
uaz.Initialize()
uaz.SetChassisVisualizationType(chassis_vis_type)
uaz.SetSuspensionVisualizationType(suspension_vis_type)
uaz.SetSteeringVisualizationType(steering_vis_type)
uaz.SetWheelVisualizationType(wheel_vis_type)
uaz.SetTireVisualizationType(tire_vis_type)
suspF = veh.CastToChToeBarLeafspringAxle(uaz.GetVehicle().GetSuspension(0))
leftAngle = suspF.GetKingpinAngleLeft()
rightAngle = suspF.GetKingpinAngleRight()
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. cyl_2 = chrono.ChCylinderShape()
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.
cyl_2 = chrono.ChCylinderShape()
mysystem = chrono.ChSystemNSC()
mfloor = chrono.ChBodyEasyBox(3, 0.2, 3, 1000)
mfloor.SetBodyFixed(True)
mysystem.Add(mfloor)
#
# EXAMPLE1
#
# Create a ChLinePath geometry, and insert sub-paths:
mpath = chrono.ChLinePath()
mseg1 = chrono.ChLineSegment(chrono.ChVectorD(1, 2, 0),
chrono.ChVectorD(2, 2, 0))
mpath.AddSubLine(mseg1)
marc1 = chrono.ChLineArc(chrono.ChCoordsysD(chrono.ChVectorD(2, 2.5, 0)), 0.5,
-chrono.CH_C_PI_2, chrono.CH_C_PI_2, True)
mpath.AddSubLine(marc1)
mseg2 = chrono.ChLineSegment(chrono.ChVectorD(2, 3, 0),
chrono.ChVectorD(1, 3, 0))
mpath.AddSubLine(mseg2)
marc2 = chrono.ChLineArc(chrono.ChCoordsysD(chrono.ChVectorD(1, 2.5, 0)), 0.5,
chrono.CH_C_PI_2, -chrono.CH_C_PI_2, True)
mpath.AddSubLine(marc2)
mpath.Set_closed(True)
# Create a ChLineShape, a visualization asset for lines.
# The ChLinePath is a special type of ChLine and it can be visualized.
mpathasset = chrono.ChLineShape()
mpathasset.SetLineGeometry(mpath)
mfloor.AddAsset(mpathasset)
body.SetBodyFixed(False)
mysystem.Add(body)
body.SetPos(chrono.ChVectorD(-1.25, -0.75, 0.1))
body.SetWvel_loc(chrono.ChVectorD(0.1, 0.1, 0.1))
body_col = chrono.ChColorAsset()
body_col.SetColor(chrono.ChColor(0.9, 0.4, 0.1))
body.AddAsset(body_col)
# Create the plane-plane constraint
# Constrain the sliding body to move and rotate in the x-y plane
# (i.e. the plane whose normal is the z-axis of the specified coord sys)
plane_plane = chrono.ChLinkLockPlanePlane()
plane_plane.Initialize(ground,
body,
chrono.ChCoordsysD(chrono.ChVectorD(-1.25, -0.75, 0), chrono.ChQuaternionD(1, 0, 0, 0)))
mysystem.AddLink(plane_plane)
# Create a linear spring (with default sprint & damping coefficients)
spring = chrono.ChLinkTSDA()
spring.SetSpringCoefficient(100)
spring.SetDampingCoefficient(5)
spring.Initialize(ground,
body,
True,
chrono.ChVectorD(0, 0, 2),
chrono.ChVectorD(0, 0, 0),
False,
1.9)
mysystem.AddLink(spring)
