def resetPos(self):
self.mbody1.SetPos(chrono.ChVectorD(0, 0, -0.2))
self.mbody1.SetRot(chrono.ChQuaternionD(0, 0, 0, 1))
self.mbody1.SetNoSpeedNoAcceleration()
self.Body_wheel_L.SetPos(chrono.ChVectorD(0, -0.5, 0))
self.Body_wheel_L.SetRot(chrono.ChQuaternionD(0.707, -0.707, 0, 0))
self.Body_wheel_L.SetNoSpeedNoAcceleration()
self.Body_wheel_R.SetPos(chrono.ChVectorD(0, -0.5, -0.3))
self.Body_wheel_R.SetRot(chrono.ChQuaternionD(0.707, -0.707, 0, 0))
self.Body_wheel_R.SetNoSpeedNoAcceleration()
def rotateVector(vec, rotAngle=0, rotAxis=[0,1,0], rotDegrees=True):
if(rotDegrees):
rotAngle = np.deg2rad(rotAngle)
ChRotAxis = ChVecify(rotAxis).GetNormalized()
q = chrono.ChQuaternionD()
q.Q_from_AngAxis(rotAngle, ChRotAxis)
rotvec = q.Rotate(ChVecify(vec))
rotvec = [rotvec.x, rotvec.y, rotvec.z]
return np.array(rotvec)
def createWheels(self):
# Left Wheel
self.Body_wheel_L = chrono.ChBody()
self.Body_wheel_L.SetBodyFixed(False)
self.Body_wheel_L.SetPos(chrono.ChVectorD(0, -0.5, 0))
self.Body_wheel_L.SetRot(chrono.ChQuaternionD(0.707, -0.707, 0, 0))
self.mysystem.Add(self.Body_wheel_L)
self.Shape_Wheel_L = chrono.ChCylinderShape()
self.Shape_Wheel_L.GetCylinderGeometry().radius = 0.03
self.Shape_Wheel_L.GetCylinderGeometry().p1 = chrono.ChVectorD(
0, 0.1, 0)
self.Shape_Wheel_L.GetCylinderGeometry().p2 = chrono.ChVectorD(
0, -0.01, 0)
self.Body_wheel_L.AddAsset(self.Shape_Wheel_L)
self.motor_L = chrono.ChLinkMotorRotationSpeed()
self.motorpos_L = chrono.ChFrameD(chrono.ChVectorD(0, -0.5, 0))
self.motor_L.Initialize(self.mbody1, self.Body_wheel_L,
self.motorpos_L)
self.mysystem.Add(self.motor_L)
# Right Wheel
self.Body_wheel_R = chrono.ChBody()
self.Body_wheel_R.SetBodyFixed(False)
self.Body_wheel_R.SetPos(chrono.ChVectorD(0, -0.5, -0.3))
self.Body_wheel_R.SetRot(chrono.ChQuaternionD(0.707, -0.707, 0, 0))
self.mysystem.Add(self.Body_wheel_R)
self.Shape_Wheel_R = chrono.ChCylinderShape()
self.Shape_Wheel_R.GetCylinderGeometry().radius = 0.03
self.Shape_Wheel_R.GetCylinderGeometry().p1 = chrono.ChVectorD(
0, 0.1, 0)
self.Shape_Wheel_R.GetCylinderGeometry().p2 = chrono.ChVectorD(
0, -0.01, 0)
self.Body_wheel_R.AddAsset(self.Shape_Wheel_R)
self.motor_R = chrono.ChLinkMotorRotationSpeed()
self.motorpos_R = chrono.ChFrameD(chrono.ChVectorD(0, -0.5, 0))
self.motor_R.Initialize(self.mbody1, self.Body_wheel_R,
self.motorpos_R)
self.mysystem.Add(self.motor_R)
def rotateBody(body, rotX=0, rotY=0, rotZ=0, rotOrder=['x', 'y', 'z'], rotAngle=0, rotAxis=[0,1,0], rotDegrees=True):
ChRotAxis = ChVecify(rotAxis)
if(rotDegrees):
rotX = np.deg2rad(rotX)
rotY = np.deg2rad(rotY)
rotZ = np.deg2rad(rotZ)
rotAngle = np.deg2rad(rotAngle)
if rotAngle:
q = chrono.ChQuaternionD()
q.Q_from_AngAxis(rotAngle, ChRotAxis.GetNormalized())
body.SetRot(q*body.GetRot())
for dim in rotOrder:
angle = (dim == 'x')*rotX + (dim == 'y')*rotY + (dim == 'z')*rotZ
if angle:
axis = chrono.ChVectorD((dim == 'x')*1, (dim == 'y')*1, (dim == 'z')*1)
q = chrono.ChQuaternionD()
q.Q_from_AngAxis(angle, axis)
body.SetRot(q*body.GetRot())
def createBody(self):
# Create a fixed rigid body
self.mbody1 = chrono.ChBody()
self.mbody1.SetBodyFixed(False)
self.mbody1.SetIdentifier(1)
self.mbody1.SetPos(chrono.ChVectorD(0, 0, -0.2))
self.mbody1.SetRot(chrono.ChQuaternionD(0, 0, 0, 1))
self.mysystem.Add(self.mbody1)
self.mboxasset = chrono.ChBoxShape()
self.mboxasset.GetBoxGeometry().Size = chrono.ChVectorD(0.2, 0.5, 0.1)
self.mbody1.AddAsset(self.mboxasset)
def LinkBodies_Hinge(body1, body2, link_position, link_direction, MiroSystem=False):
linkdir = ChVecify(link_direction)
linkpos = ChVecify(link_position)
# Get the quaternion that represents the rotation of the global z-axis to the link direction
z_ax = chrono.ChVectorD(0,0,1)
q = chrono.ChQuaternionD(1 + z_ax.Dot(linkdir), z_ax.Cross(linkdir))
q.Normalize()
# Create a new ChFrame coordinate system at the link position and with the global_z-to-linkdir rotation
mframe = chrono.ChFrameD(linkpos, q)
# Create a revolute link between the components at the coordinate system
mlink = chrono.ChLinkRevolute()
mlink.Initialize(body1, body2, mframe)
if MiroSystem:
MiroSystem.Add(mlink)
return mlink
cyl_2.GetCylinderGeometry().p2 = chrono.ChVectorD(0, -(hl + 0.2) * sina,
(hl + 0.2) * cosa)
cyl_2.GetCylinderGeometry().rad = 0.3
ground.AddAsset(cyl_2)
# Create the first shaft body
# ---------------------------
shaft_1 = chrono.ChBody()
mysystem.AddBody(shaft_1)
shaft_1.SetIdentifier(1)
shaft_1.SetBodyFixed(False)
shaft_1.SetCollide(False)
shaft_1.SetMass(1)
shaft_1.SetInertiaXX(chrono.ChVectorD(1, 1, 0.2))
shaft_1.SetPos(chrono.ChVectorD(0, 0, -hl))
shaft_1.SetRot(chrono.ChQuaternionD(1, 0, 0, 0))
# Add visualization assets to represent the shaft (a box) and the arm of the
# universal joint's cross associated with this shaft (a cylinder)
box_1 = chrono.ChBoxShape()
box_1.GetBoxGeometry().Size = chrono.ChVectorD(0.15, 0.15, 0.9 * hl)
shaft_1.AddAsset(box_1)
cyl_2 = chrono.ChCylinderShape()
cyl_2.GetCylinderGeometry().p1 = chrono.ChVectorD(-0.2, 0, hl)
cyl_2.GetCylinderGeometry().p2 = chrono.ChVectorD(0.2, 0, hl)
cyl_2.GetCylinderGeometry().rad = 0.05
shaft_1.AddAsset(cyl_2)
col = chrono.ChColorAsset()
col.SetColor(chrono.ChColor(0.9, 0.4, 0.1))
# =============================================================================
import pychrono.core as chrono
import pychrono.irrlicht as irr
import pychrono.vehicle as veh
import math
# The path to the Chrono data directory containing various assets (meshes, textures, data files)
# is automatically set, relative to the default location of this demo.
# If running from a different directory, you must change the path to the data directory with:
#chrono.SetChronoDataPath('path/to/data')
veh.SetDataPath(chrono.GetChronoDataPath() + 'vehicle/')
# Initial vehicle location and orientation
initLoc = chrono.ChVectorD(0, 0, 0.5)
initRot = chrono.ChQuaternionD(1, 0, 0, 0)
# Visualization type for vehicle parts (PRIMITIVES, MESH, or NONE)
chassis_vis_type = veh.VisualizationType_MESH
suspension_vis_type = veh.VisualizationType_PRIMITIVES
steering_vis_type = veh.VisualizationType_PRIMITIVES
wheel_vis_type = veh.VisualizationType_MESH
# Collision type for chassis (PRIMITIVES, MESH, or NONE)
chassis_collision_type = veh.CollisionType_NONE
# Type of tire model (RIGID, TMEASY)
tire_model = veh.TireModelType_TMEASY
# Rigid terrain
terrainHeight = 0 # terrain height (FLAT terrain only)
mysystem.Add(shape)
# Reconstruct 3D shape from sensors
na = 8
nl = 8
nodes, edges = reconstruct_shape(na,
nl,
UNIT_FACTOR,
ring_gap=RING_GAP,
shift_z=-RING_GAP * nl / 2.0)
# mesh
mesh = fea.ChMesh()
# nodes
noderot = chrono.ChQuaternionD(chrono.QUNIT)
fea_nodes = []
for n in nodes:
fn = fea.ChNodeFEAxyzrot(
chrono.ChFrameD(chrono.ChVectorD(n[0], n[1], n[2]), noderot))
fn.SetMass(0.1)
fn.SetFixed(False)
mesh.AddNode(fn)
fea_nodes.append(fn)
# material of each element
rho = 152.2 # 152.2 material density
E = 8e20 # Young's modulus 8e4
nu = 0.5 # 0.5 # Poisson ratio
alpha = 1.0 # 0.3 # shear factor
beta = 0.2 # torque factor
# - attach a visualization shape based on a .obj triangle mesh
# - add contact shape based on a .obj triangle mesh
# This is more complicate than method A, yet this can be still preferred if you
# need deeper control, ex. you want to provide two different meshes, one
# with high level of detail just for the visualization and a coarse one for
# collision, or if you want to set custom COG and inertia values, etc.
# Rigid body part
body_B = chrono.ChBodyAuxRef()
body_B.SetPos(chrono.ChVectorD(0, 0.5, 0))
body_B.SetMass(16)
body_B.SetInertiaXX(chrono.ChVectorD(0.270, 0.400, 0.427))
body_B.SetInertiaXY(chrono.ChVectorD(0.057, 0.037, -0.062))
body_B.SetFrame_COG_to_REF(
chrono.ChFrameD(chrono.ChVectorD(0.12, 0.0, 0),
chrono.ChQuaternionD(1, 0, 0, 0)))
# Attach a visualization shape .
# First load a .obj from disk into a ChTriangleMeshConnected:
mesh_for_visualization = chrono.ChTriangleMeshConnected()
mesh_for_visualization.LoadWavefrontMesh(
chrono.GetChronoDataFile('shoe_view.obj'))
# Optionally: you can scale/shrink/rotate the mesh using this:
mesh_for_visualization.Transform(chrono.ChVectorD(0.01, 0, 0),
chrono.ChMatrix33D(1))
# Now the triangle mesh is inserted in a ChTriangleMeshShape visualization asset,
# and added to the body
visualization_shape = chrono.ChTriangleMeshShape()
visualization_shape.SetMesh(mesh_for_visualization)
body_B.AddAsset(visualization_shape)
#
# Create the ChCascadeDoc, a container that loads the STEP model
# and manages its subassembles
mydoc = cascade.ChCascadeDoc()
# load the STEP model using this command:
load_ok = mydoc.Load_STEP(chrono.GetChronoDataPath() + "cascade/assembly.stp")
# or specify abs.path: ("C:\\data\\cascade\\assembly.stp");
# print the contained shapes
#mydoc.Dump(chrono.GetLog())
# In most CADs the Y axis is horizontal, but we want it vertical.
# So define a root transformation for rotating all the imported objects.
rotation1 = chrono.ChQuaternionD()
rotation1.Q_from_AngAxis(-chrono.CH_C_PI / 2, chrono.ChVectorD(1, 0, 0))
# 1: rotate 90° on X axis
rotation2 = chrono.ChQuaternionD()
rotation2.Q_from_AngAxis(chrono.CH_C_PI, chrono.ChVectorD(0, 1, 0))
# 2: rotate 180° on vertical Y axis
tot_rotation = chrono.ChQuaternionD()
tot_rotation = rotation2 % rotation1 # rotate on 1 then on 2, using quaternion product
root_frame = chrono.ChFrameMovingD(chrono.ChVectorD(0, 0, 0), tot_rotation)
# Retrieve some sub shapes from the loaded model, using
# the GetNamedShape() function, that can use path/subpath/subsubpath/part
# syntax and * or ? wildcards, etc.
mrigidBody1 = 0
mrigidBody2 = 0
# Create a fixed rigid body chrono.ChCollisionModel.SetDefaultSuggestedEnvelope(0.001) chrono.ChCollisionModel.SetDefaultSuggestedMargin(0.001) mysystem.SetMaxItersSolverSpeed(70) brick_material = chrono.ChMaterialSurfaceNSC() brick_material.SetFriction(0.5) brick_material.SetDampingF(0.2) brick_material.SetCompliance(0.0000001) brick_material.SetComplianceT(0.0000001) mbody1 = chrono.ChBody() mbody1.SetBodyFixed(True) mbody1.SetPos(chrono.ChVectorD(0, 0, 0)) q = chrono.ChQuaternionD() q.Q_from_AngAxis(math.pi / 8, chrono.ChVectorD(0, 1, 0)) mbody1.SetRot(q) mbody1.SetMass(1) mbody1.SetMaterialSurface(brick_material) mbody1.GetCollisionModel().ClearModel() mbody1.GetCollisionModel().AddBox(1, 1, 1) # must set half sizes mbody1.GetCollisionModel().BuildModel() mbody1.SetCollide(True) ChronoPandaInterface.PandaCube(cpi, mbody1, 2, WHITE) mysystem.Add(mbody1) # Create the falling rigid body mbody2 = chrono.ChBody()
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()
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)
