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 AddContainer(sys):
# Contact material for container
ground_mat = chrono.ChMaterialSurfaceNSC()
# Create the five walls of the rectangular container, using fixed rigid bodies of 'box' type
floorBody = chrono.ChBodyEasyBox(20, 1, 20, 1000, True, True, ground_mat)
floorBody.SetPos(chrono.ChVectorD(0, -5, 0))
floorBody.SetBodyFixed(True)
sys.Add(floorBody)
wallBody1 = chrono.ChBodyEasyBox(1, 10, 20.99, 1000, True, True, ground_mat)
wallBody1.SetPos(chrono.ChVectorD(-10, 0, 0))
wallBody1.SetBodyFixed(True)
sys.Add(wallBody1)
wallBody2 = chrono.ChBodyEasyBox(1, 10, 20.99, 1000, True, True, ground_mat)
wallBody2.SetPos(chrono.ChVectorD(10, 0, 0))
wallBody2.SetBodyFixed(True)
sys.Add(wallBody2)
wallBody3 = chrono.ChBodyEasyBox(20.99, 10, 1, 1000, False, True, ground_mat)
wallBody3.SetPos(chrono.ChVectorD(0, 0, -10))
wallBody3.SetBodyFixed(True)
sys.Add(wallBody3)
wallBody4 = chrono.ChBodyEasyBox(20.99, 10, 1, 1000, True, True, ground_mat)
wallBody4.SetPos(chrono.ChVectorD(0, 0, 10))
wallBody4.SetBodyFixed(True)
sys.Add(wallBody4)
# optional, attach textures for better visualization
mtexturewall = chrono.ChTexture()
mtexturewall.SetTextureFilename(chrono.GetChronoDataFile("textures/concrete.jpg"))
wallBody1.AddAsset(mtexturewall) # note: most assets can be shared
wallBody2.AddAsset(mtexturewall)
wallBody3.AddAsset(mtexturewall)
wallBody4.AddAsset(mtexturewall)
floorBody.AddAsset(mtexturewall)
# Add the rotating mixer
mixer_mat = chrono.ChMaterialSurfaceNSC()
mixer_mat.SetFriction(0.4)
rotatingBody = chrono.ChBodyEasyBox(10, 5, 1, # x,y,z size
4000, # density
True, # visualization?
True, # collision?
mixer_mat) # contact material
rotatingBody.SetPos(chrono.ChVectorD(0, -1.6, 0))
sys.Add(rotatingBody)
# .. a motor between mixer and truss
my_motor = chrono.ChLinkMotorRotationSpeed()
my_motor.Initialize(rotatingBody,
floorBody,
chrono.ChFrameD(chrono.ChVectorD(0, 0, 0),
chrono.Q_from_AngAxis(chrono.CH_C_PI_2, chrono.VECT_X)))
mfun = chrono.ChFunction_Const(chrono.CH_C_PI / 4.0) # speed 45 deg/s
my_motor.SetSpeedFunction(mfun)
sys.AddLink(my_motor)
# NOTE: Instead of creating five separate 'box' bodies to make
# the walls of the container, you could have used a single body
# made of five box shapes, which build a single collision description,
# as in the alternative approach:
"""
# create a plain ChBody (no colliding shape nor visualization mesh is used yet)
mrigidBody = chrono.ChBody()
# set as fixed body, and turn collision ON, otherwise no collide by default
mrigidBody.SetBodyFixed(True)
mrigidBody.SetCollide(True)
# Clear model. The colliding shape description MUST be between ClearModel() .. BuildModel() pair.
mrigidBody.GetCollisionModel().ClearModel()
# Describe the (invisible) colliding shape by adding five boxes (the walls and floor)
mrigidBody.GetCollisionModel().AddBox(ground_mat, 20, 1, 20, chrono.ChVectorD(0, -10, 0))
mrigidBody.GetCollisionModel().AddBox(ground_mat, 1, 40, 20, chrono.ChVectorD(-11, 0, 0))
mrigidBody.GetCollisionModel().AddBox(ground_mat, 1, 40, 20, chrono.ChVectorD(11, 0, 0))
mrigidBody.GetCollisionModel().AddBox(ground_mat, 20, 40, 1, chrono.ChVectorD(0, 0, -11))
mrigidBody.GetCollisionModel().AddBox(ground_mat, 20, 40, 1, chrono.ChVectorD(0, 0, 11))
# Complete the description of collision shape.
mrigidBody.GetCollisionModel().BuildModel()
# Attach some visualization shapes if needed:
vshape = chrono.ChBoxShape()
vshape.GetBoxGeometry().SetLengths(chrono.ChVectorD(20, 1, 20))
vshape.GetBoxGeometry().Pos = chrono.ChVectorD(0, -5, 0)
this.AddAsset(vshape)
# etc. for other 4 box shapes..
"""
return rotatingBody
print("Example: create a slider crank and plot results")
# Change this path to asset path, if running from other working dir.
# It must point to the data folder, containing GUI assets (textures, fonts, meshes, etc.)
chrono.SetChronoDataPath("../../../data/")
# ---------------------------------------------------------------------
#
# Create the simulation system and add items
#
mysystem = chrono.ChSystemNSC()
# Some data shared in the following
crank_center = chrono.ChVectorD(-1, 0.5, 0)
crank_rad = 0.4
crank_thick = 0.1
rod_length = 1.5
# Create four rigid bodies: the truss, the crank, the rod, the piston.
# Create the floor truss
mfloor = chrono.ChBodyEasyBox(3, 1, 3, 1000)
mfloor.SetPos(chrono.ChVectorD(0, -0.5, 0))
mfloor.SetBodyFixed(True)
mysystem.Add(mfloor)
# Create the flywheel crank
mcrank = chrono.ChBodyEasyCylinder(crank_rad, crank_thick, 1000)
mcrank.SetPos(crank_center + chrono.ChVectorD(0, 0, -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.7)
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
tire_vis_type = veh.VisualizationType_MESH
# Collision type for chassis (PRIMITIVES, MESH, or NONE)
chassis_collision_type = veh.ChassisCollisionType_NONE
# Type of tire model (RIGID, TMEASY)
tire_model = veh.TireModelType_TMEASY
my_cylinder = BRepPrimAPI.BRepPrimAPI_MakeCylinder(0.09, 0.1).Shape()
my_shape = BRepAlgoAPI.BRepAlgoAPI_Cut(my_torus, my_cylinder).Shape()
# use it to make a body with proper center of mass and inertia tensor,
# given the CAD shape. Also visualize it.
my_body = cascade.ChBodyEasyCascade(
my_shape, # the CAD shape
1000, # the density
True, # must collide using the triangle mesh geometry?
True) # must be visualized?
mysystem.Add(my_body)
# Create a large cube as a floor.
my_floor = chrono.ChBodyEasyBox(1, 0.2, 1, 1000, True)
my_floor.SetPos(chrono.ChVectorD(0, -0.3, 0))
my_floor.SetBodyFixed(True)
mysystem.Add(my_floor)
my_color = chrono.ChColorAsset(0.2, 0.2, 0.5)
my_floor.AddAsset(my_color)
# ---------------------------------------------------------------------
#
# Create an Irrlicht application to visualize the system
#
myapplication = chronoirr.ChIrrApp(mysystem, 'Use OpenCascade shapes',
chronoirr.dimension2du(1024, 768))
myapplication.AddTypicalSky(chrono.GetChronoDataPath() + 'skybox/')
import numpy as np
# import cv2
save_data = False
visualize = True
"""
!!!! Set this path before running the demo!
"""
chrono.SetChronoDataPath('../../../data/')
veh.SetDataPath('../../../data/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_PRIMITIVES
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.ChassisCollisionType_NONE
# Type of tire model (RIGID, TMEASY)
tire_model = veh.TireModelType_RIGID
# Rigid terrain
sys.GetContactContainer().AddContact(contact, self.m_ball_mat,
self.m_obst_mat)
# ---------------------------------------------------------------------
#
# Create the simulation system and add items
#
# Change use_NSC to specify different contact method
use_NSC = 0
ball_radius = 0.5
obst_radius = 2.0
obst_center = chrono.ChVectorD(2.9, 0, 2.9)
obstacle = MyObstacle(obst_radius, obst_center)
# Create the system and the various contact materials
if use_NSC:
sys = chrono.ChSystemNSC()
g_mat = chrono.ChMaterialSurfaceNSC()
g_mat.SetRestitution(0.9)
g_mat.SetFriction(0.4)
b_mat = chrono.ChMaterialSurfaceNSC()
b_mat.SetRestitution(0.9)
b_mat.SetFriction(0.5)
o_mat = chrono.ChMaterialSurfaceNSC()
o_mat.SetRestitution(0.9)
o_mat.SetFriction(0.4)
# Y pointing to the left.
#
# =============================================================================
import pychrono.core as chrono
import pychrono.irrlicht as irr
import pychrono.vehicle as veh
import pychrono.sensor as sens
import math
"""
!!!! Set this path before running the demo!
"""
chrono.SetChronoDataPath('../../../data/')
veh.SetDataPath('../../../data/vehicle/')
# Initial vehicle location and orientation
initLoc = chrono.ChVectorD(2, 2, 0.5)
initRot = chrono.ChQuaternionD(1, 0, 0, 0)
# Visualization type for vehicle parts (PRIMITIVES, MESH, or NONE)
chassis_vis_type = veh.VisualizationType_PRIMITIVES
suspension_vis_type = veh.VisualizationType_PRIMITIVES
steering_vis_type = veh.VisualizationType_PRIMITIVES
wheel_vis_type = veh.VisualizationType_PRIMITIVES
# Collision type for chassis (PRIMITIVES, MESH, or NONE)
chassis_collision_type = veh.CollisionType_NONE
# Type of tire model (RIGID, TMEASY)
tire_model = veh.TireModelType_RIGID
# Rigid terrain
import pychrono.irrlicht as irr
import pychrono.vehicle as veh
import math
import os
# =============================================================================
# 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.4)
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
tire_vis_type = veh.VisualizationType_MESH
# Type of tire model (RIGID, TMEASY, PAC02)
tire_model = veh.TireModelType_PAC02
# Poon chassis tracked by the camera
trackPoint = chrono.ChVectorD(0.0, 0.0, 1.75)
# Y pointing to the left.
#
# =============================================================================
import pychrono.core as chrono
import pychrono.irrlicht as irr
import pychrono.vehicle as veh
import math
"""
!!!! Set this path before running the demo!
"""
chrono.SetChronoDataPath('../../../../Library/data/')
veh.SetDataPath('../../../../Library/data/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.ChassisCollisionType_NONE
# Type of tire model (RIGID, TMEASY)
tire_model = veh.TireModelType_TMEASY
# Rigid terrain
# and manages its subassembles
mydoc = cascade.ChCascadeDoc()
# load the STEP model using this command:
load_ok = mydoc.Load_STEP(
chrono.GetChronoDataFile('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
if load_ok:
def npvec_to_chvec(npvec):
chvec = chrono.ChVectorD(*npvec.tolist())
return chvec
def AddContainer(sys):
# The fixed body (5 walls)
fixedBody = chrono.ChBody()
fixedBody.SetMass(1.0)
fixedBody.SetBodyFixed(True)
fixedBody.SetPos(chrono.ChVectorD())
fixedBody.SetCollide(True)
# Contact material for container
fixed_mat = chrono.ChMaterialSurfaceSMC()
fixedBody.GetCollisionModel().ClearModel()
AddContainerWall(fixedBody, fixed_mat, chrono.ChVectorD(20, 1, 20),
chrono.ChVectorD(0, -5, 0))
AddContainerWall(fixedBody, fixed_mat, chrono.ChVectorD(1, 10, 20.99),
chrono.ChVectorD(-10, 0, 0))
AddContainerWall(fixedBody, fixed_mat, chrono.ChVectorD(1, 10, 20.99),
chrono.ChVectorD(10, 0, 0))
AddContainerWall(fixedBody, fixed_mat, chrono.ChVectorD(20.99, 10, 1),
chrono.ChVectorD(0, 0, -10), False)
AddContainerWall(fixedBody, fixed_mat, chrono.ChVectorD(20.99, 10, 1),
chrono.ChVectorD(0, 0, 10))
fixedBody.GetCollisionModel().BuildModel()
texture = chrono.ChTexture()
texture.SetTextureFilename(
chrono.GetChronoDataFile("textures/concrete.jpg"))
fixedBody.AddAsset(texture)
sys.AddBody(fixedBody)
# The rotating mixer body
rotatingBody = chrono.ChBody()
rotatingBody.SetMass(10.0)
rotatingBody.SetInertiaXX(chrono.ChVectorD(50, 50, 50))
rotatingBody.SetPos(chrono.ChVectorD(0, -1.6, 0))
rotatingBody.SetCollide(True)
# Contact material for mixer body
rot_mat = chrono.ChMaterialSurfaceSMC()
hsize = chrono.ChVectorD(5, 2.75, 0.5)
rotatingBody.GetCollisionModel().ClearModel()
rotatingBody.GetCollisionModel().AddBox(rot_mat, hsize.x, hsize.y, hsize.z)
rotatingBody.GetCollisionModel().BuildModel()
box = chrono.ChBoxShape()
box.GetBoxGeometry().Size = hsize
rotatingBody.AddAsset(box)
rotatingBody.AddAsset(texture)
sys.AddBody(rotatingBody)
# A motor between the two
my_motor = chrono.ChLinkMotorRotationSpeed()
my_motor.Initialize(
rotatingBody, fixedBody,
chrono.ChFrameD(chrono.ChVectorD(0, 0, 0),
chrono.Q_from_AngAxis(chrono.CH_C_PI_2,
chrono.VECT_X)))
mfun = chrono.ChFunction_Const(chrono.CH_C_PI / 2.0) # speed w=90°/s
my_motor.SetSpeedFunction(mfun)
sys.AddLink(my_motor)
return rotatingBody
# 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')
# ---------------------------------------------------------------------
#
# Create the simulation system and add items
#
mysystem = chrono.ChSystemNSC()
# Create a fixed rigid body
mbody1 = chrono.ChBody()
mbody1.SetBodyFixed(True)
mbody1.SetPos(chrono.ChVectorD(0, 0, -0.2))
mysystem.Add(mbody1)
mboxasset = chrono.ChBoxShape()
mboxasset.GetBoxGeometry().Size = chrono.ChVectorD(0.2, 0.5, 0.1)
mbody1.AddAsset(mboxasset)
# Create a swinging rigid body
mbody2 = chrono.ChBody()
mbody2.SetBodyFixed(False)
mysystem.Add(mbody2)
mboxasset = chrono.ChBoxShape()
mboxasset.GetBoxGeometry().Size = chrono.ChVectorD(0.2, 0.5, 0.1)
mbody2.AddAsset(mboxasset)
size_brick_x = 0.15
size_brick_y = 0.12
size_brick_z = 0.12
density_brick = 1000
# kg/m^3
mass_brick = density_brick * size_brick_x * size_brick_y * size_brick_z
inertia_brick = 2 / 5 * (pow(size_brick_x, 2)) * mass_brick
# to do: compute separate xx,yy,zz inertias
for ix in range(0, nbricks_on_x):
for iy in range(0, nbricks_on_y):
# create it
body_brick = chrono.ChBody()
# set initial position
body_brick.SetPos(
chrono.ChVectorD(ix * size_brick_x, (iy + 0.5) * size_brick_y, 0))
# set mass properties
body_brick.SetMass(mass_brick)
body_brick.SetInertiaXX(
chrono.ChVectorD(inertia_brick, inertia_brick, inertia_brick))
# set collision surface properties
body_brick.SetMaterialSurface(brick_material)
# Collision shape
body_brick.GetCollisionModel().ClearModel()
body_brick.GetCollisionModel().AddBox(size_brick_x / 2,
size_brick_y / 2, size_brick_z /
2) # must set half sizes
body_brick.GetCollisionModel().BuildModel()
body_brick.SetCollide(True)
# Change this path to asset path, if running from other working dir.
# It must point to the data folder, containing GUI assets (textures, fonts, meshes, etc.)
chrono.SetChronoDataPath("../../../data/")
# ---------------------------------------------------------------------
#
# Create the simulation system and add items
#
mysystem = chrono.ChSystemNSC()
# Create a fixed rigid body
mbody1 = chrono.ChBody()
mbody1.SetBodyFixed(True)
mbody1.SetPos(chrono.ChVectorD(0, 0, -0.2))
mysystem.Add(mbody1)
mboxasset = chrono.ChBoxShape()
mboxasset.GetBoxGeometry().Size = chrono.ChVectorD(0.2, 0.5, 0.1)
mbody1.AddAsset(mboxasset)
# Create a swinging rigid body
mbody2 = chrono.ChBody()
mbody2.SetBodyFixed(False)
mysystem.Add(mbody2)
mboxasset = chrono.ChBoxShape()
mboxasset.GetBoxGeometry().Size = chrono.ChVectorD(0.2, 0.5, 0.1)
mbody2.AddAsset(mboxasset)
#
# NOTE: for visualization purposes only, i.e. if you do not use the mesh also for
# collision, the mesh does not need to be watertight.
# Method A:
# - use the ChBodyEasyMesh
# This will automatically create the visualization mesh, the collision mesh,
# and will automatically compute the mass property (COG position respect to REF,
# mass and inertia tensor) given an uniform density.
body_A = chrono.ChBodyEasyMesh(
chrono.GetChronoDataFile('shoe_view.obj'), # mesh filename
7000, # density kg/m^3
True, # use mesh for visualization?
True) # use mesh for collision?
body_A.SetPos(chrono.ChVectorD(0.5, 0.5, 0))
mysystem.Add(body_A)
# Method B:
# - create a ChBodyAuxRef,
# - set mass and inertia tensor as you like
# - set COG center of mass position respect to REF reference as you like
# - 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()
# load the STEP model using this command:
load_ok = mydoc.Load_STEP(chrono.GetChronoDataFile('cascade/IRB7600_23_500_m2000_rev1_01_decorated.stp')) # or specify abs.path: ("C:\\data\\cascade\\assembly.stp");
if not load_ok:
raise ValueError("Warning. Desired STEP file could not be opened/parsed \n")
CH_C_PI = 3.1456
# 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(-CH_C_PI / 2, chrono.ChVectorD(1, 0, 0)) # 1: rotate 90° on X axis
rotation2 = chrono.ChQuaternionD()
rotation2.Q_from_AngAxis(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.
def make_body_from_name(partname, root_transformation):
shape1 = TopoDS.TopoDS_Shape()
if (mydoc.GetNamedShape(shape1, partname)):
# Make a ChBody representing the TopoDS_Shape part from the CAD:
self.m_damping_R = 3e4
else:
self.m_Bekker_Kphi = 5301e3
self.m_Bekker_Kc = 102e3
self.m_Bekker_n = 0.793
self.m_Mohr_cohesion = 1.3e3
self.m_Mohr_friction = 31.1
self.m_Janosi_shear = 1.2e-2
self.m_elastic_K = 4e8
self.m_damping_R = 3e4
# Global parameters for tire
tire_rad = 0.8
tire_vel_z0 = -3
tire_center = chrono.ChVectorD(0, 0.02 + tire_rad, -1.5)
tire_w0 = tire_vel_z0 / tire_rad
# ----------------------------
# Create the mechanical system
# ----------------------------
mysystem = chrono.ChSystemSMC()
# Create the ground
ground = chrono.ChBody()
ground.SetBodyFixed(True)
mysystem.Add(ground)
# Create the rigid body with contact mesh
body = chrono.ChBody()
θ1r, θ2r = IK_RR(Jx - xr, Jy - yr, _L1r, _L2r, e3) #IK of the right robot
#perform a check here, and throw an error if the original configuration can't be solved.
if np.isnan(θ1l and θ2l and θ1r and θ2r):
raise ValueError(
"there is no solution to IK for the end-effector location specified (Xee,Yee)"
)
#--------------- Create the simulation system ---------------------------------
mysystem = chrono.ChSystemNSC()
#--------------- create each link as a rigid body -----------------------------
#------------- ground body ------------
GB = chrono.ChBodyAuxRef()
GB.SetPos(chrono.ChVectorD(0, (yl + yr) / 2, 0))
GB.SetBodyFixed(True)
#set mesh visualization
mesh_for_visualization = chrono.ChTriangleMeshConnected()
mesh_for_visualization.LoadWavefrontMesh(assetsPath + 'ground.obj')
# Optionally: you can scale/shrink/rotate/translate the mesh using this:
meshRotation = chrono.ChMatrix33D(np.pi / 2, chrono.ChVectorD(0, 1, 0))
mesh_for_visualization.Transform(chrono.ChVectorD(0, 0, 0), meshRotation)
# 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)
GB.AddAsset(visualization_shape)
UNIT_FACTOR = 0.01
RING_GAP = 12.5 * UNIT_FACTOR
SHAPE_THICKNESS = 0.01
SHAPE_PATH = 'shapes/printed_April18/Cone_32.obj'
metrics = ['mm', 'cm', 'dm', 'm']
metric = metrics[int(math.fabs(round(math.log(UNIT_FACTOR, 10))))]
HUMAN_DENSITY = 198.5 # Dkg/m^3
# ---------------------------------------------------------------------
#
# Create the simulation system and add items
mysystem = chrono.ChSystemSMC()
contact_method = chrono.ChMaterialSurface.SMC
mysystem.Set_G_acc(chrono.ChVectorD(0., 0., 0.))
filepath = tool.obj_from_millimeter(chrono.GetChronoDataPath() + SHAPE_PATH,
UNIT_FACTOR, f"_{metric}")
# Import the shape
shape = tool.load_shape(filepath, contact_method, 'textures/skin.jpg')
shape.SetDensity(HUMAN_DENSITY)
# Get shape bounding box dimensions
bbmin, bbmax = chrono.ChVectorD(), chrono.ChVectorD()
shape.GetTotalAABB(bbmin, bbmax)
bbmin, bbmax = eval(str(bbmin)), eval(str(bbmax))
bb_dx = bbmax[0] - bbmin[0]
bb_dy = bbmax[1] - bbmin[1]
bb_dz = bbmax[2] - bbmin[2]
def main():
# -----------------
# Create the system
# -----------------
mphysicalSystem = chrono.ChSystemNSC()
# -----------------------------------
# add a mesh to be sensed by a camera
# -----------------------------------
mmesh = chrono.ChTriangleMeshConnected()
mmesh.LoadWavefrontMesh(
chrono.GetChronoDataFile("vehicle/hmmwv/hmmwv_chassis.obj"), False,
True)
# scale to a different size
mmesh.Transform(chrono.ChVectorD(0, 0, 0), chrono.ChMatrix33D(2))
trimesh_shape = chrono.ChTriangleMeshShape()
trimesh_shape.SetMesh(mmesh)
trimesh_shape.SetName("HMMWV Chassis Mesh")
trimesh_shape.SetStatic(True)
mesh_body = chrono.ChBody()
mesh_body.SetPos(chrono.ChVectorD(0, 0, 0))
mesh_body.AddAsset(trimesh_shape)
mesh_body.SetBodyFixed(True)
mphysicalSystem.Add(mesh_body)
# -----------------------
# Create a sensor manager
# -----------------------
manager = sens.ChSensorManager(mphysicalSystem)
intensity = 1.0
manager.scene.AddPointLight(
chrono.ChVectorF(2, 2.5, 100),
chrono.ChVectorF(intensity, intensity, intensity), 500.0)
manager.scene.AddPointLight(
chrono.ChVectorF(9, 2.5, 100),
chrono.ChVectorF(intensity, intensity, intensity), 500.0)
manager.scene.AddPointLight(
chrono.ChVectorF(16, 2.5, 100),
chrono.ChVectorF(intensity, intensity, intensity), 500.0)
manager.scene.AddPointLight(
chrono.ChVectorF(23, 2.5, 100),
chrono.ChVectorF(intensity, intensity, intensity), 500.0)
# manager.SetKeyframeSizeFromTimeStep(.001,1/exposure_time)
# ------------------------------------------------
# Create a camera and add it to the sensor manager
# ------------------------------------------------
offset_pose = chrono.ChFrameD(
chrono.ChVectorD(-5, 0, 2),
chrono.Q_from_AngAxis(2, chrono.ChVectorD(0, 1, 0)))
cam = sens.ChCameraSensor(
mesh_body, # body camera is attached to
update_rate, # update rate in Hz
offset_pose, # offset pose
image_width, # image width
image_height, # image height
fov # camera's horizontal field of view
)
cam.SetName("Camera Sensor")
cam.SetLag(lag)
cam.SetCollectionWindow(exposure_time)
# ------------------------------------------------------------------
# Create a filter graph for post-processing the data from the camera
# ------------------------------------------------------------------
if noise_model == "CONST_NORMAL":
cam.PushFilter(sens.ChFilterCameraNoiseConstNormal(0.0, 0.02))
elif noise_model == "PIXEL_DEPENDENT":
cam.PushFilter(sens.ChFilterCameraNoisePixDep(0, 0.02, 0.03))
elif noise_model == "NONE":
# Don't add any noise models
pass
# Renders the image at current point in the filter graph
if vis:
cam.PushFilter(
sens.ChFilterVisualize(image_width, image_height,
"Before Grayscale Filter"))
# Provides the host access to this RGBA8 buffer
cam.PushFilter(sens.ChFilterRGBA8Access())
# Save the current image to a png file at the specified path
if save:
cam.PushFilter(sens.ChFilterSave(out_dir + "rgb/"))
# Filter the sensor to grayscale
cam.PushFilter(sens.ChFilterGrayscale())
# Render the buffer again to see the new grayscaled image
if vis:
cam.PushFilter(
sens.ChFilterVisualize(int(image_width / 2), int(image_height / 2),
"Grayscale Image"))
# Save the grayscaled image at the specified path
if save:
cam.PushFilter(sens.ChFilterSave(out_dir + "gray/"))
# Resizes the image to the provided width and height
cam.PushFilter(
sens.ChFilterImageResize(int(image_width / 2), int(image_height / 2)))
# Access the grayscaled buffer as R8 pixels
cam.PushFilter(sens.ChFilterR8Access())
# add sensor to manager
manager.AddSensor(cam)
# ---------------
# Simulate system
# ---------------
orbit_radius = 10
orbit_rate = 0.5
ch_time = 0.0
t1 = time.time()
while (ch_time < end_time):
cam.SetOffsetPose(
chrono.ChFrameD(
chrono.ChVectorD(
-orbit_radius * math.cos(ch_time * orbit_rate),
-orbit_radius * math.sin(ch_time * orbit_rate), 1),
chrono.Q_from_AngAxis(ch_time * orbit_rate,
chrono.ChVectorD(0, 0, 1))))
# Access the RGBA8 buffer from the camera
rgba8_buffer = cam.GetMostRecentRGBA8Buffer()
if (rgba8_buffer.HasData()):
rgba8_data = rgba8_buffer.GetRGBA8Data()
print('RGBA8 buffer recieved from cam. Camera resolution: {0}x{1}'\
.format(rgba8_buffer.Width, rgba8_buffer.Height))
print('First Pixel: {0}'.format(rgba8_data[0, 0, :]))
# Update sensor manager
# Will render/save/filter automatically
manager.Update()
# Perform step of dynamics
mphysicalSystem.DoStepDynamics(step_size)
# Get the current time of the simulation
ch_time = mphysicalSystem.GetChTime()
print("Sim time:", end_time, "Wall time:", time.time() - t1)
import pychrono.irrlicht as irr
import pychrono.vehicle as veh
import math
import os
# =============================================================================
# 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.4)
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_NONE
tire_vis_type = veh.VisualizationType_MESH
# Poon chassis tracked by the camera
trackPoint = chrono.ChVectorD(0.0, 0.0, 1.75)
# Simulation step sizes
step_size = 1e-3
tire_step_size = step_size
# Lag time for each sensor cam_lag = 0 lidar_lag = 0 imu_lag = 0 gps_lag = 0 # Collection window for each sensor # Typically 1 / update rate cam_collection_time = 1. / float(cam_update_rate) lidar_collection_time = 1. / float(lidar_update_rate) imu_collection_time = 0 # instant gps_collection_time = 0 # instant # GPS reference point # Located in Madison, WI gps_reference = chrono.ChVectorD(-89.400, 43.070, 260.0) # IMU and GPS noise models # Setting to none (does not affect the data) imu_noise_none = sens.ChIMUNoiseNone() gps_noise_none = sens.ChGPSNoiseNone() # --------------------- # Simulation parameters # --------------------- # Simulation step size step_size = 1e-3 # Simulation end time end_time = 20.0
def stepShape(position_front, direction_front, position_back, direction_back, width, height, clr = [0.5,0.5,0.5]):
################# TRAPPSTEG ###############
# position_front: chrono.ChVectorD, the position of the inner lower front corner
# direction_front: chrono.ChVectorD, normal direction to staircase center that aligns with front of the step
# position_back: chrono.ChVectorD, the position of the inner lower front corner
# direction_back: chrono.ChVectorD, normal direction that aligns with back of the step
# width: double, Width of the step as seen when walking in the staircase
# height: double, Thickness of the stepposition_front = ChVecify(position_front)
position_front = ChVecify(position_front)
direction_front = ChVecify(direction_front)
position_back = ChVecify(position_back)
direction_back = ChVecify(direction_back)
direction_front.SetLength(width)
direction_back.SetLength(width)
# Notation: I = Inner/O = Outer, U = Upper/L = Lower, F = Front/B = Back
# Ex: Step_ILF is the Inner Lower Front corner of the step
Step_ILF = position_front
Step_IUF = position_front + chrono.ChVectorD(0, height, 0)
Step_ILB = position_back
Step_IUB = position_back + chrono.ChVectorD(0, height, 0)
Step_OLF = position_front + direction_front
Step_OUF = position_front + direction_front + chrono.ChVectorD(0, height, 0)
Step_OLB = position_back + direction_back
Step_OUB = position_back + direction_back + chrono.ChVectorD(0, height, 0)
Step_mesh = chrono.ChTriangleMeshConnected()
# inner side
Step_mesh.addTriangle(Step_ILF, Step_ILB, Step_IUF)
Step_mesh.addTriangle(Step_IUB, Step_IUF, Step_ILB)
# outer side
Step_mesh.addTriangle(Step_OLF, Step_OUB, Step_OLB)
Step_mesh.addTriangle(Step_OLF, Step_OUF, Step_OUB)
# top side
Step_mesh.addTriangle(Step_IUF, Step_OUB, Step_OUF)
Step_mesh.addTriangle(Step_IUF, Step_IUB, Step_OUB)
# bottom side
Step_mesh.addTriangle(Step_ILF, Step_OLF, Step_OLB)
Step_mesh.addTriangle(Step_ILF, Step_OLB, Step_ILB)
# back side
Step_mesh.addTriangle(Step_ILB, Step_OLB, Step_IUB)
Step_mesh.addTriangle(Step_OUB, Step_IUB, Step_OLB)
# front side
Step_mesh.addTriangle(Step_ILF, Step_IUF, Step_OLF)
Step_mesh.addTriangle(Step_OUF, Step_OLF, Step_IUF)
Step_mesh.RepairDuplicateVertexes()
Step = chrono.ChBody()
Step.SetBodyFixed(True)
Step_shape = chrono.ChTriangleMeshShape()
Step_shape.SetMesh(Step_mesh)
Step_shape.SetColor(chrono.ChColor(clr[0], clr[1], clr[2]))
Step.GetCollisionModel().ClearModel()
Step.GetCollisionModel().AddTriangleMesh(Step_mesh, True, False)
Step.GetCollisionModel().BuildModel()
Step.SetCollide(True)
# Step_texture = chrono.ChTexture()
# Step_texture.SetTextureFilename(chrono.GetChronoDataFile('textures/red_dot.png'))
# Step_texture.SetTextureScale(10, 10)
# Step.GetAssets().push_back(Step_texture)
Step.GetAssets().push_back(Step_shape)
return Step
def main():
# -----------------
# Create the system
# -----------------
mphysicalSystem = chrono.ChSystemNSC()
mphysicalSystem.Set_G_acc(chrono.ChVectorD(0, 0, -9.81))
# ----------------------------------------
# add a floor, box and sphere to the scene
# ----------------------------------------
phys_mat = chrono.ChMaterialSurfaceNSC()
phys_mat.SetFriction(0.5)
phys_mat.SetDampingF(0.00000)
phys_mat.SetCompliance(1e-9)
phys_mat.SetComplianceT(1e-9)
floor = chrono.ChBodyEasyBox(10, 10, 1, 1000, True, True, phys_mat)
floor.SetPos(chrono.ChVectorD(0, 0, -1))
floor.SetBodyFixed(True)
mphysicalSystem.Add(floor)
box = chrono.ChBodyEasyBox(1, 1, 1, 1000, True, True, phys_mat)
box.SetPos(chrono.ChVectorD(0, 0, 5))
box.SetRot(chrono.Q_from_AngAxis(.2, chrono.ChVectorD(1, 0, 0)))
mphysicalSystem.Add(box)
sphere = chrono.ChBodyEasySphere(.5, 1000, True, True, phys_mat)
sphere.SetPos(chrono.ChVectorD(0, 0, 8))
sphere.SetRot(chrono.Q_from_AngAxis(.2, chrono.ChVectorD(1, 0, 0)))
mphysicalSystem.Add(sphere)
sphere_asset = sphere.GetAssets()[0]
visual_asset = chrono.CastToChVisualization(sphere_asset)
vis_mat = chrono.ChVisualMaterial()
vis_mat.SetAmbientColor(chrono.ChVectorF(0, 0, 0))
vis_mat.SetDiffuseColor(chrono.ChVectorF(.2, .2, .9))
vis_mat.SetSpecularColor(chrono.ChVectorF(.9, .9, .9))
visual_asset.material_list.append(vis_mat)
# -----------------------
# Create a sensor manager
# -----------------------
manager = sens.ChSensorManager(mphysicalSystem)
manager.scene.AddPointLight(chrono.ChVectorF(100, 100, 100),
chrono.ChVectorF(1, 1, 1), 1000.0)
manager.scene.AddPointLight(chrono.ChVectorF(-100, -100, 100),
chrono.ChVectorF(1, 1, 1), 1000.0)
# ------------------------------------------------
# Create a camera and add it to the sensor manager
# ------------------------------------------------
offset_pose = chrono.ChFrameD(
chrono.ChVectorD(-8, 0, 1),
chrono.Q_from_AngAxis(0, chrono.ChVectorD(0, 1, 0)))
cam = sens.ChCameraSensor(
floor, # body camera is attached to
cam_update_rate, # update rate in Hz
offset_pose, # offset pose
image_width, # number of horizontal samples
image_height, # number of vertical channels
cam_fov # vertical field of view
)
cam.SetName("Camera Sensor")
cam.SetLag(cam_lag)
cam.SetCollectionWindow(cam_collection_time)
# ------------------------------------------------------------------
# Create a filter graph for post-processing the data from the camera
# ------------------------------------------------------------------
# Visualizes the image
if vis:
cam.PushFilter(
sens.ChFilterVisualize(image_width, image_height, "RGB Image"))
# Save the current image to a png file at the specified path
if (save):
cam.PushFilter(sens.ChFilterSave(out_dir + "/rgb/"))
# Provides the host access to this RGBA8 buffer
cam.PushFilter(sens.ChFilterRGBA8Access())
# Filter the sensor to grayscale
cam.PushFilter(sens.ChFilterGrayscale())
# Render the buffer again to see the new grayscaled image
if (vis):
cam.PushFilter(
sens.ChFilterVisualize(int(image_width / 2), int(image_height / 2),
"Grayscale Image"))
# Save the grayscaled image at the specified path
if (save):
cam.PushFilter(sens.ChFilterSave(out_dir + "/gray/"))
# Access the grayscaled buffer as R8 pixels
cam.PushFilter(sens.ChFilterR8Access())
# Add a camera to a sensor manager
manager.AddSensor(cam)
# ------------------------------------------------
# Create a lidar and add it to the sensor manager
# ------------------------------------------------
offset_pose = chrono.ChFrameD(
chrono.ChVectorD(-8, 0, 1),
chrono.Q_from_AngAxis(0, chrono.ChVectorD(0, 1, 0)))
lidar = sens.ChLidarSensor(
floor, # body lidar is attached to
lidar_update_rate, # scanning rate in Hz
offset_pose, # offset pose
horizontal_samples, # number of horizontal samples
vertical_samples, # number of vertical channels
horizontal_fov, # horizontal field of view
max_vert_angle,
min_vert_angle, # vertical field of view
100 #max lidar range
)
lidar.SetName("Lidar Sensor")
lidar.SetLag(lidar_lag)
lidar.SetCollectionWindow(lidar_collection_time)
# -----------------------------------------------------------------
# Create a filter graph for post-processing the data from the lidar
# -----------------------------------------------------------------
if vis:
# Randers the raw lidar data
lidar.PushFilter(
sens.ChFilterVisualize(horizontal_samples, vertical_samples,
"Raw Lidar Depth Data"))
# Provides the host access to the Depth,Intensity data
lidar.PushFilter(sens.ChFilterDIAccess())
# Convert Depth,Intensity data to XYZI point cloud data
lidar.PushFilter(sens.ChFilterPCfromDepth())
if vis:
# Visualize the point cloud
lidar.PushFilter(
sens.ChFilterVisualizePointCloud(640, 480, 1.0,
"Lidar Point Cloud"))
# Provides the host access to the XYZI data
lidar.PushFilter(sens.ChFilterXYZIAccess())
# Add the lidar to the sensor manager
manager.AddSensor(lidar)
# ----------------------------------------------
# Create an IMU sensor and add it to the manager
# ----------------------------------------------
offset_pose = chrono.ChFrameD(
chrono.ChVectorD(-8, 0, 1),
chrono.Q_from_AngAxis(0, chrono.ChVectorD(0, 1, 0)))
imu = sens.ChIMUSensor(
box, # body imu is attached to
imu_update_rate, # update rate in Hz
offset_pose, # offset pose
imu_noise_none, # noise model
)
imu.SetName("IMU Sensor")
imu.SetLag(imu_lag)
imu.SetCollectionWindow(imu_collection_time)
# Provides the host access to the imu data
imu.PushFilter(sens.ChFilterIMUAccess())
# Add the imu to the sensor manager
manager.AddSensor(imu)
# ----------------------------------------------
# Create an GPS sensor and add it to the manager
# ----------------------------------------------
offset_pose = chrono.ChFrameD(
chrono.ChVectorD(-8, 0, 1),
chrono.Q_from_AngAxis(0, chrono.ChVectorD(0, 1, 0)))
gps = sens.ChGPSSensor(
box, # body imu is attached to
gps_update_rate, # update rate in Hz
offset_pose, # offset pose
gps_reference,
gps_noise_none # noise model
)
gps.SetName("GPS Sensor")
gps.SetLag(gps_lag)
gps.SetCollectionWindow(gps_collection_time)
# Provides the host access to the gps data
gps.PushFilter(sens.ChFilterGPSAccess())
# Add the gps to the sensor manager
manager.AddSensor(gps)
# ---------------
# Simulate system
# ---------------
t1 = time.time()
ch_time = 0
while (ch_time < end_time):
# Access the sensor data
camera_data_RGBA8 = cam.GetMostRecentRGBA8Buffer()
camera_data_R8 = cam.GetMostRecentR8Buffer()
lidar_data_DI = lidar.GetMostRecentDIBuffer()
lidar_data_XYZI = lidar.GetMostRecentXYZIBuffer()
gps_data = gps.GetMostRecentGPSBuffer()
imu_data = imu.GetMostRecentIMUBuffer()
# Check data is present
# If so, print out the max value
if camera_data_RGBA8.HasData():
print("Camera RGBA8:",
camera_data_RGBA8.GetRGBA8Data().shape, "max:",
np.max(camera_data_RGBA8.GetRGBA8Data()))
if camera_data_R8.HasData():
print("Camera R8:",
camera_data_R8.GetChar8Data().shape, "max:",
np.max(camera_data_R8.GetChar8Data()))
if lidar_data_DI.HasData():
print("Lidar DI:",
lidar_data_DI.GetDIData().shape, "max:",
np.max(lidar_data_DI.GetDIData()))
if lidar_data_XYZI.HasData():
print("Lidar XYZI:",
lidar_data_XYZI.GetXYZIData().shape, "max:",
np.max(lidar_data_XYZI.GetXYZIData()))
if gps_data.HasData():
print("GPS:",
gps_data.GetGPSData().shape, "max:",
np.max(gps_data.GetGPSData()))
if imu_data.HasData():
print("IMU:",
imu_data.GetIMUData().shape, "max:",
np.max(imu_data.GetIMUData()))
# Update sensor manager
# Will render/save/filter automatically
manager.Update()
# Perform step of dynamics
mphysicalSystem.DoStepDynamics(step_size)
# Get the current time of the simulation
ch_time = mphysicalSystem.GetChTime()
print("Sim time:", end_time, "Wall time:", time.time() - t1)
#
# =============================================================================
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
# 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
import math
print("Example: demonstration of a universal joint")
# 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('relative/path/to/data/directory/')
# ---------------------------------------------------------------------
#
# Create the simulation system and add items
#
mysystem = chrono.ChSystemNSC()
mysystem.Set_G_acc(chrono.ChVectorD(0, 0, 0))
# Set the half-length of the two shafts
hl = 2
# Set the bend angle between the two shafts (positive rotation
# about the global X axis)
angle = math.pi / 6.
cosa = math.cos(angle)
sina = math.sin(angle)
rot = chrono.Q_from_AngX(angle)
# Create the ground body
# ----------------------
ground = chrono.ChBody()
ground.SetIdentifier(-1)
size_brick_x = 0.25
size_brick_y = 0.12
size_brick_z = 0.12
density_brick = 1000
# kg/m^3
mass_brick = density_brick * size_brick_x * size_brick_y * size_brick_z
inertia_brick = 2 / 5 * (pow(size_brick_x, 2)) * mass_brick
# to do: compute separate xx,yy,zz inertias
for ix in range(0, nbricks_on_x):
for iy in range(0, nbricks_on_y):
# create it
body_brick = chrono.ChBody()
# set initial position
body_brick.SetPos(
chrono.ChVectorD(ix * size_brick_x, (iy + 0.5) * size_brick_y, 0))
# set mass properties
body_brick.SetMass(mass_brick)
body_brick.SetInertiaXX(
chrono.ChVectorD(inertia_brick, inertia_brick, inertia_brick))
# set collision surface properties
body_brick.SetMaterialSurface(brick_material)
# Collision shape
body_brick.GetCollisionModel().ClearModel()
body_brick.GetCollisionModel().AddBox(size_brick_x / 2,
size_brick_y / 2, size_brick_z /
2) # must set half sizes
body_brick.GetCollisionModel().BuildModel()
body_brick.SetCollide(True)