body.GetCollisionModel().ClearModel()
body.GetCollisionModel().AddTriangleMesh(
material, # contact material
mesh, # the mesh
False, # is it static?
False, # is it convex?
chrono.ChVectorD(0, 0, 0), # position on body
chrono.ChMatrix33D(1), # orientation on body
0.01) # "thickness" for increased robustness
body.GetCollisionModel().BuildModel()
body.SetCollide(True)
# Create motor
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)
col.SetColor(chrono.ChColor(0.2, 0.4, 0.8))
shaft_2.AddAsset(col)
# Connect the first shaft to ground
# ---------------------------------
# Use a rotational motor to impose both the revolute joint constraints, as well
# as constant angular velocity. Here, we drive the motor angle with a ramp function.
# Alternatively, we could use a ChLinkMotorAngularSpeed with constant speed.
# The joint is located at the origin of the first shaft.
motor = chrono.ChLinkMotorRotationAngle()
motor.Initialize(
ground, shaft_1,
chrono.ChFrameD(chrono.ChVectorD(0, 0, -hl),
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))
mpathasset.SetLineGeometry(mpath)
mfloor.AddAsset(mpathasset)
# This is the constraint that uses the trajectory
mtrajectory = chrono.ChLinkTrajectory()
# Define which parts are connected (the trajectory is considered in the 2nd body).
mtrajectory.Initialize(mrigidBody_hand, # body1 that follows the trajectory
mfloor, # body2 that 'owns' the trajectory
chrono.VNULL, # point on body1 that will follow the trajectory, in body1 coords
mpath # the trajectory (reuse the one already added to body2 as asset)
)
mysystem.Add(mtrajectory)
# Optionally, set a function that gets the curvilinear
# abscyssa s of the line, as a function of time s(t).
# By default it was simply s=t.
mspacefx = chrono.ChFunction_Ramp(0, 0.5)
mtrajectory.Set_space_fx(mspacefx)
# Just to constraint the hand rotation:
mparallelism = chrono.ChLinkLockParallel()
mparallelism.Initialize(mrigidBody_hand, mfloor, frame_marker_wrist_hand.GetCoord())
mysystem.Add(mparallelism);
# ---------------------------------------------------------------------
#
# Create an Irrlicht application to visualize the system
#
myapplication = chronoirr.ChIrrApp(mysystem, 'Import STEP', chronoirr.dimension2du(1024,768))
myapplication.AddTypicalSky()
