def Transform(self, pos, scale=1, ang=0):
self.mesh.body.SetPos(pos)
self.mesh.mesh.Transform(chrono.ChVectorD(0, 0, 0), chrono.ChMatrix33D(scale))
self.mesh.body.SetRot(chrono.Q_from_AngAxis(ang, chrono.ChVectorD(0, 0, 1)))
self.pos = pos
self.scale = scale
self.ang = ang
self.rot = chrono.Q_from_AngAxis(ang, chrono.ChVectorD(0, 0, 1))
self.mesh.Scale(scale)
chrono.ChVectorD(-1.29340068058665e-17,
4.10138104133823e-17, 0.00633921901294084),
chrono.ChQuaternionD(1, 0, 0, 0)))
# Visualization shape
body_1_1_shape = chrono.ChObjShapeFile()
body_1_1_shape.SetFilename(shapes_dir + 'body_1_1.obj')
body_1_1_level = chrono.ChAssetLevel()
body_1_1_level.GetFrame().SetPos(chrono.ChVectorD(0, 0, 0))
body_1_1_level.GetFrame().SetRot(chrono.ChQuaternionD(1, 0, 0, 0))
body_1_1_level.GetAssets().push_back(body_1_1_shape)
body_1.GetAssets().push_back(body_1_1_level)
# Collision shape(s)
body_1.GetCollisionModel().ClearModel()
mr = chrono.ChMatrix33D()
mr[0, 0] = 0
mr[1, 0] = 0
mr[2, 0] = -1
mr[0, 1] = -0.947036648244772
mr[1, 1] = 0.321125500204061
mr[2, 1] = 0
mr[0, 2] = 0.321125500204061
mr[1, 2] = 0.947036648244772
mr[2, 2] = 0
body_1.GetCollisionModel().AddBox(
0.0025, 0.00328030945702576, 0.002,
chrono.ChVectorD(0.0120114645617556, -0.0887792305476897, 0.0025), mr)
mr = chrono.ChMatrix33D()
mr[0, 0] = 0
mr[1, 0] = 0
def Transform(self):
self.mesh.Transform(chrono.ChVectorD(0,0,0), chrono.ChMatrix33D(self.scale))
print(" Quaternion from Euler angles:")
eu.x = alpha1
eu.y = beta1
eu.z = gamma1
print(" Input = {", eu.x, ";", eu.y, ";", eu.z, "}")
q.Q_from_Euler123(eu * chrono.CH_C_DEG_TO_RAD)
eu = q.Q_to_Euler123() * chrono.CH_C_RAD_TO_DEG
print(" Output = {", eu.x, ";", eu.y, ";", eu.z, "}")
eu.x = alpha2
eu.y = beta2
eu.z = gamma2
print(" Input = {", eu.x, ";", eu.y, ";", eu.z, "}")
q.Q_from_Euler123(eu * chrono.CH_C_DEG_TO_RAD)
eu = q.Q_to_Euler123() * chrono.CH_C_RAD_TO_DEG
print(" Output = {", eu.x, ";", eu.y, ";", eu.z, "}")
# --------
print(" Rotation matrix for sequence (90, 90, 90)")
eu.x = math.pi / 2
eu.y = math.pi / 2
eu.z = math.pi / 2
q.Q_from_Euler123(eu)
R = chrono.ChMatrix33D(q)
# Create a 2D npy array from the list extracted from ChMatrix33
npmat = np.asarray(R.GetMatr())
np.set_printoptions(suppress=True)
print(npmat)
#### -------------------------------------------------------------------------
#### EXERCISE 2.1
#### Enable contact on the slider body and specify contact geometry
#### The contact shape attached to the slider body should be a box with the
#### same dimensions as the visualization asset, centered at the body origin.
#### Use a coefficient of friction of 0.4.
#### -------------------------------------------------------------------------
slider.SetCollide(True)
slider_mat = chrono.ChMaterialSurfaceNSC()
slider_mat.SetFriction(0.4)
slider.GetCollisionModel().ClearModel()
slider.GetCollisionModel().AddBox(slider_mat, 0.2, 0.1, 0.1, chrono.VNULL,
chrono.ChMatrix33D(chrono.QUNIT))
slider.GetCollisionModel().BuildModel()
#### -------------------------------------------------------------------------
#### EXERCISE 2.2
#### Create a new body, with a spherical shape (radius 0.2), used both as
#### visualization asset and contact shape (mu = 0.4). This body should have:
#### mass: 1
#### moments of inertia: I_xx = I_yy = I_zz = 0.02
#### initial location: (5.5, 0, 0)
#### -------------------------------------------------------------------------
ball = chrono.ChBody()
system.AddBody(ball)
ball.SetIdentifier(4)
ball.SetName("ball")
mmeshbox = chrono.ChTriangleMeshConnected()
mmeshbox.LoadWavefrontMesh(chrono.GetChronoDataFile("models/cube.obj"), True,
True)
if (do_mesh_collision_floor):
# floor as a triangle mesh surface:
mfloor = chrono.chronoChBody()
mfloor.SetPos(chrono.ChVectorD(0, -1, 0))
mfloor.SetBodyFixed(True)
my_system.Add(mfloor)
mfloor.GetCollisionModel().ClearModel()
mfloor.GetCollisionModel().AddTriangleMesh(mysurfmaterial, mmeshbox, False,
False, chrono.VNULL,
chrono.ChMatrix33D(1),
sphere_swept_thickness)
mfloor.GetCollisionModel().BuildModel()
mfloor.SetCollide(True)
masset_meshbox = chrono.ChTriangleMeshShape()
masset_meshbox.SetMesh(mmeshbox)
mfloor.AddAsset(masset_meshbox)
masset_texture = chrono.ChTexture()
masset_texture.SetTextureFilename(
chrono.GetChronoDataFile("textures/concrete.jpg"))
mfloor.AddAsset(masset_texture)
else:
# floor as a simple collision primitive:
