vis_shape = chrono.ChTriangleMeshShape()
vis_shape.SetMesh(mesh)
body.AddAsset(vis_shape)
body.AddAsset(chrono.ChColorAsset(0.3, 0.3, 0.3))
# Set collision shape
material = chrono.ChMaterialSurfaceSMC()
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
# ------------------------
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)
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)
# Add the collision shape.
# Again load a .obj file in Wavefront file format. NOTE: in this
# example we use the same .obj file as for visualization, but here one
# could do a better thing: using a different low-level-of-detail mesh for the
# collision, so the simulation performance is not affected by many details such
# as bolts and chamfers that may be wanted only for visualization.
mesh_for_collision = chrono.ChTriangleMeshConnected()
mesh_for_collision.LoadWavefrontMesh(chrono.GetChronoDataFile('shoe_view.obj'))
# Optionally: you can scale/shrink/rotate the mesh using this:
#--------------- 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) mysystem.Add(GB) #--------- coordinate frame --------------- coord = chrono.ChBodyAuxRef() coord.SetPos(chrono.ChVectorD(0, 0, 0)) coord.SetBodyFixed(True) mesh_for_visualization = chrono.ChTriangleMeshConnected()
mysystem = chrono.ChSystemNSC()
#--------------- create each link as a rigid body -----------------------------
#------------- ground body ------------
GB = chrono.ChBodyAuxRef()
GB.SetName("GB")
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)
mysystem.Add(GB)
#--------- coordinate frame ---------------
coord = chrono.ChBodyAuxRef()
coord.SetPos(chrono.ChVectorD(0,0,0))
coord.SetBodyFixed(True)
# 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('models/bulldozer/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)
# Add the collision shape.
# Again load a .obj file in Wavefront file format. NOTE: in this
# example we use the same .obj file as for visualization, but here one
# could do a better thing: using a different low-level-of-detail mesh for the
# collision, so the simulation performance is not affected by many details such
# as bolts and chamfers that may be wanted only for visualization.
mesh_for_collision = chrono.ChTriangleMeshConnected()
mesh_for_collision.LoadWavefrontMesh(chrono.GetChronoDataFile('models/bulldozer/shoe_view.obj'))
def buildALEXR(system,
Xee = .5,
Yee = -.5,
eeMass = 1,
side = "right"):
"""
public interface for building the ALEXR
:param system: the Chrono system to which the ALEXR robot is added
"""
#--------------- state infromation for initialization ---------------------
#initial location of the end effector (EE) of the ALEX Robot
#Xee = .5
#Yee = -.5
#specify mass properties of the payload at the end effector.
#eeMass = 1
#set the elbow discrete vars this will flip for right vs. left side useage
#side = "right" # "left"
#----------- Calculate IK angles using custom Library ---------------------
a = ALEXR() #encodes robot state info
system.refs = {} #setup the dictionary of object references
#ALEXR link lengths and origins
xl = a.xl ; yl = a.yl
xr = a.xr ; yr = a.yr
#left robot
_L1l = a.L1l
_L2l = a.L2l
_L3l = a.L3l
_L23l= a.L23l
#right robot
_L1r = a.L1r
_L2r = a.L2r
#calculate IK robot angles
θ1l , θ2l, θ1r , θ2r = a.IK_2DOF(Xee,Yee,side)
#perform a check here, and throw an error if the original configuration can't be solved.
if not a.feasable_point(Xee, Yee,side):
raise ValueError("there is no solution to IK for the end-effector location specified (Xee,Yee)")
#--------------- create each link as a rigid body -------------------------
# chrono uses a right handed coordinate system, and the model is constructed to look like the plotly model,
# but the irrelict visualizer operates in the mirror world - with a left handed coordinate system
# the long axis of the link to the right is the x axis, up is y, z is link rotation direction out of the page
# R is the rotation matrix between the default link frame orientation, and the solidworks coordinate system
# R (solidworks - > link frame)
#------------- 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)
system.Add(GB)
system.refs["GB"] = GB
#--------- coordinate frame ---------------
coord = chrono.ChBodyAuxRef()
coord.SetPos(chrono.ChVectorD(0,0,0))
coord.SetBodyFixed(True)
mesh_for_visualization = chrono.ChTriangleMeshConnected()
mesh_for_visualization.LoadWavefrontMesh(assetsPath +'coords.obj')
mesh_for_visualization.Transform(chrono.ChVectorD(0,0,0), chrono.ChMatrix33D(.01))
visualization_shape = chrono.ChTriangleMeshShape()
visualization_shape.SetMesh(mesh_for_visualization)
coord.AddAsset(visualization_shape)
system.Add(coord)
#----------- left link 1 ------------------
#add body
L1l = chrono.ChBodyAuxRef()
L1l.SetBodyFixed(False)
system.Add(L1l)
system.refs["L1l"] = L1l
#set position,orientation with FK (while REF frame and COG frame are coincident)
x = xl + (_L1l/2)*np.cos(θ1l)
y = yl + (_L1l/2)*np.sin(θ1l)
L1l.SetPos(chrono.ChVectorD(x,y,.01))
L1l.SetRot(chrono.ChMatrix33D(θ1l,chrono.ChVectorD(0,0,1)))
#add visualization
mesh_for_visualization = chrono.ChTriangleMeshConnected()
mesh_for_visualization.LoadWavefrontMesh(assetsPath +'_L1l.obj')
meshRotation = chrono.ChMatrix33D(np.pi/2,chrono.ChVectorD(0,1,0))
mesh_for_visualization.Transform(chrono.ChVectorD(0,0,0), meshRotation)
visualization_shape = chrono.ChTriangleMeshShape()
visualization_shape.SetMesh(mesh_for_visualization)
L1l.AddAsset(visualization_shape)
texture = chrono.ChTexture()
texture.SetTextureFilename(assetsPath + 'blue.png')
L1l.GetAssets().push_back(texture)
#set the mass and inertial properties
# xs ys zs
R1 = np.array([[ 0, 0, -1], #xl #found by hand
[ 0, -1, 0], #yl
[-1, 0, 0]]) #zl
L1l.SetMass(3.642)
Is = np.array([[ 0.13286460, -0.00001280, 0.03326759], # centroidal moment of inertia
[-0.00001280, 0.15328523,-0.00014996],
[ 0.03326759, -0.00014996, 0.03071782]])
Il = R1 @ Is @ np.linalg.inv(R1) # rotate the inertia tensor into the link frame
Ilch = chrono.ChMatrix33D()
Ilch.SetMatr(Il.tolist())
L1l.SetInertia(Ilch)
# move the COG frame
c = R1 @ np.array([[.0904],[-.0004],[.1461]])
L1l.SetFrame_COG_to_REF(chrono.ChFrameD(chrono.ChVectorD(c[0,0],c[1,0],c[2,0])))
#----------- left link 2 ------------------
#add body
L2l = chrono.ChBodyAuxRef()
L2l.SetBodyFixed(False)
system.Add(L2l)
system.refs["L2l"] = L2l
#set position,orientation with FK
x = xl + (_L1l)*np.cos(θ1l) + (_L23l/2)*np.cos(θ1l + θ2l)
y = yl + (_L1l)*np.sin(θ1l) + (_L23l/2)*np.sin(θ1l + θ2l)
L2l.SetPos(chrono.ChVectorD(x,y,.02))
L2l.SetRot(chrono.ChMatrix33D(θ1l + θ2l,chrono.ChVectorD(0,0,1)))
#add visualization
mesh_for_visualization = chrono.ChTriangleMeshConnected()
mesh_for_visualization.LoadWavefrontMesh(assetsPath +'_L2l.obj')
meshRotation = chrono.ChMatrix33D(np.pi/2,chrono.ChVectorD(0,1,0))
#mesh origin was slightly off, so I hand tuned it
mesh_for_visualization.Transform(chrono.ChVectorD(-.00775,0,0), meshRotation)
visualization_shape = chrono.ChTriangleMeshShape()
visualization_shape.SetMesh(mesh_for_visualization)
L2l.AddAsset(visualization_shape)
texture = chrono.ChTexture()
texture.SetTextureFilename(assetsPath + 'blue.png')
L2l.GetAssets().push_back(texture)
#set the mass and inertial properties
# xs ys zs
R2 = np.array([[ 0, 0, 1], #xl #found by hand
[ 0, 1, 0], #yl
[-1, 0, 0]]) #zl
L2l.SetMass(1.158)
Is = np.array([[ 0.04061717, 0.00000000, 0.00000000], # centroidal moment of inertia
[ 0.00000000, 0.04040908, 0.00000000],
[ 0.00000000, 0.00000000, 0.00072961]])
Il = R2 @ Is @ np.linalg.inv(R2) # rotate the inertia tensor into the link frame
Ilch = chrono.ChMatrix33D()
Ilch.SetMatr(Il.tolist())
L2l.SetInertia(Ilch)
# move the COG frame
c = R2 @ np.array([[0],[0],[-.1192]])
L2l.SetFrame_COG_to_REF(chrono.ChFrameD(chrono.ChVectorD(c[0,0],c[1,0],c[2,0])))
#----------- right link 1 -----------------
#add body
L1r = chrono.ChBodyAuxRef()
L1r.SetBodyFixed(False)
system.Add(L1r)
system.refs["L1r"] = L1r
#set position,orientation with FK
x = xr + (_L1r/2)*np.cos(θ1r)
y = yr + (_L1r/2)*np.sin(θ1r)
L1r.SetPos(chrono.ChVectorD(x,y,.02))
L1r.SetRot(chrono.ChMatrix33D(θ1r,chrono.ChVectorD(0,0,1)))
#add visualization
mesh_for_visualization = chrono.ChTriangleMeshConnected()
mesh_for_visualization.LoadWavefrontMesh(assetsPath +'_L1r.obj')
meshRotation = chrono.ChMatrix33D(np.pi/2,chrono.ChVectorD(0,1,0))
mesh_for_visualization.Transform(chrono.ChVectorD(0,0,0), meshRotation)
visualization_shape = chrono.ChTriangleMeshShape()
visualization_shape.SetMesh(mesh_for_visualization)
L1r.AddAsset(visualization_shape)
texture = chrono.ChTexture()
texture.SetTextureFilename(assetsPath + 'red.png')
L1r.GetAssets().push_back(texture)
#set the mass and inertial properties
L1r.SetMass(4.1637)
Is = np.array([[ 0.05261769, -0.00006255, 0.02546226], # centroidal moment of inertia
[-0.00006255, 0.09428792,-0.00007718],
[ 0.02546226, -0.00007718, 0.05243999]])
Il = R1 @ Is @ np.linalg.inv(R1) # rotate the inertia tensor into the link frame
Ilch = chrono.ChMatrix33D()
Ilch.SetMatr(Il.tolist())
L1r.SetInertia(Ilch)
# move the COG frame
c = R1 @ np.array([[0.1222],[-0.0004],[.0927]])
L1r.SetFrame_COG_to_REF(chrono.ChFrameD(chrono.ChVectorD(c[0,0],c[1,0],c[2,0])))
#----------- right link 2 -----------------
#add body
L2r = chrono.ChBodyAuxRef()
L2r.SetBodyFixed(False)
system.Add(L2r)
system.refs["L2r"] = L2r
#set position,orientation with FK
x = xr + (_L1r)*np.cos(θ1r) + (_L2r/2)*np.cos(θ1r + θ2r)
y = yr + (_L1r)*np.sin(θ1r) + (_L2r/2)*np.sin(θ1r + θ2r)
L2r.SetPos(chrono.ChVectorD(x,y,.03))
L2r.SetRot(chrono.ChMatrix33D(θ1r + θ2r,chrono.ChVectorD(0,0,1)))
#add visualization
mesh_for_visualization = chrono.ChTriangleMeshConnected()
mesh_for_visualization.LoadWavefrontMesh(assetsPath +'_L2r.obj')
meshRotation = chrono.ChMatrix33D(np.pi/2,chrono.ChVectorD(0,1,0))
mesh_for_visualization.Transform(chrono.ChVectorD(0,0,0), meshRotation)
visualization_shape = chrono.ChTriangleMeshShape()
visualization_shape.SetMesh(mesh_for_visualization)
L2r.AddAsset(visualization_shape)
texture = chrono.ChTexture()
texture.SetTextureFilename(assetsPath + 'red.png')
L2r.GetAssets().push_back(texture)
#set the mass and inertial properties
L2r.SetMass(1.1947)
Is = np.array([[ 0.06453132, 0.00000000, 0.00101029], # centroidal moment of inertia
[ 0.00000000, 0.06454599, 0.00000000],
[ 0.00101029, 0.00000000, 0.00093856]])
Il = R1 @ Is @ np.linalg.inv(R1) #R1 is correct here, I checked, rotate the inertia tensor into the link frame
Ilch = chrono.ChMatrix33D()
Ilch.SetMatr(Il.tolist())
L2r.SetInertia(Ilch)
# move the COG frame
c = R1 @ np.array([[-0.0041],[0.0000],[-0.0499]])
L2r.SetFrame_COG_to_REF(chrono.ChFrameD(chrono.ChVectorD(c[0,0],c[1,0],c[2,0])))
#----------- end effector payload ---------
#add body
ee = chrono.ChBodyAuxRef()
ee.SetBodyFixed(False)
system.Add(ee)
system.refs["EE"] = ee
#add mass properties //improve these based on actual data...
ee.SetMass(eeMass)
#can leave the inertia large, as this frame doesn't rotate (it can be thought of as on a bearing)
#set position,orientation with FK
x = xl + (_L1l)*np.cos(θ1l) + (_L23l)*np.cos(θ1l + θ2l)
y = yl + (_L1l)*np.sin(θ1l) + (_L23l)*np.sin(θ1l + θ2l)
ee.SetPos(chrono.ChVectorD(x,y,.03))
ee.SetRot(chrono.ChMatrix33D(0,chrono.ChVectorD(0,0,1)))
#add visualization
mesh_for_visualization = chrono.ChTriangleMeshConnected()
mesh_for_visualization.LoadWavefrontMesh(assetsPath +'_EE.obj')
meshRotation = chrono.ChMatrix33D(np.pi/2,chrono.ChVectorD(0,1,0))
mesh_for_visualization.Transform(chrono.ChVectorD(0,0,0), meshRotation)
visualization_shape = chrono.ChTriangleMeshShape()
visualization_shape.SetMesh(mesh_for_visualization)
ee.AddAsset(visualization_shape)
#----------------------- create the revolute joints ---------------------------
# joint frame naming conventions
# X_c_a
# X - body the frame is attached to in the joint
# c_a - c frame represented in the a frame
# potential frames
# j - the joint frame - where the joint marker and frame is located
# ref - the reference frame located at the center of the link
# cog - the cog of the link, which is offset from the reference frame in an ChAuxRefBody()
# example L1l_j_cog
# this refers to a frame on body L1l, attached at the joint location, represented
# relative to the COG of L1l. this is the objective, as joints must be formed relative to
# a bodies COG frame, not it's auxillary reference frame
#------------- GB <-> L1l --------------
# jt = chrono.ChLinkRevolute() #set higher up
# add_θ1l_joint(system,jt)
#
##------------- L1l <-> L2l --------------
jt = chrono.ChLinkRevolute() # create revolute joint object
local = True # we will use the local frame
L1l_j_r = chrono.ChFrameD(chrono.ChVectorD(_L1l/2,0,0.01)) # local frame of attachment
L2l_j_r = chrono.ChFrameD(chrono.ChVectorD(-1*_L23l/2,0,0)) # local frame of attachment
L1l_j_COG = L1l_j_r >> L1l.GetFrame_REF_to_COG() # express L1l <-> L2l joint relative to L1l COG frame
L2l_j_COG = L2l_j_r >> L2l.GetFrame_REF_to_COG() # express L1l <-> L2l joint relative to L12 COG frame
jt.Initialize(L1l,L2l,local,L1l_j_COG,L2l_j_COG) # init joint
system.Add(jt) # add to system
system.refs["L1l<->L2l"] = jt # maintain a reference to the joint
##------------- GB <-> L1r --------------
# jt = chrono.ChLinkRevolute()
# add_θ1r_joint(system,jt)
##------------- L1r <-> L2r --------------
jt = chrono.ChLinkRevolute() # create revolute joint object
local = True # we will use the local frame
L1r_j_r = chrono.ChFrameD(chrono.ChVectorD(_L1r/2,0,.01)) # local frame of attachment
L2r_j_r = chrono.ChFrameD(chrono.ChVectorD(-1*_L2r/2,0,0)) # local frame of attachment
L1r_j_COG = L1r_j_r >> L1r.GetFrame_REF_to_COG() # express L1l <-> L2l joint relative to L1l COG frame
L2r_j_COG = L2r_j_r >> L2r.GetFrame_REF_to_COG() # express L1l <-> L2l joint relative to L12 COG frame
jt.Initialize(L1r,L2r,local,L1r_j_COG,L2r_j_COG) # init joint
system.Add(jt) # add to system
system.refs["L1r<->L2r"] = jt # maintain a reference to the joint
##------------- L2l <-> L2r --------------
jt = chrono.ChLinkRevolute() # create revolute joint object
local = True # we will use the local frame
dj = -1*(_L23l/2 - _L2l) # distance from center to joint point
L2l_j_r = chrono.ChFrameD(chrono.ChVectorD(dj,0,.01)) # local frame of attachment
L2r_j_r = chrono.ChFrameD(chrono.ChVectorD(_L2r/2,0,0)) # local frame of attachment
L2l_j_COG = L2l_j_r >> L2l.GetFrame_REF_to_COG() # express L2l <-> L2r joint relative to L2l COG frame
L2r_j_COG = L2r_j_r >> L2r.GetFrame_REF_to_COG() # express L2l <-> L2r joint relative to L2r COG frame
jt.Initialize(L2l,L2r,local,L2l_j_COG,L2r_j_COG) # init joint
system.Add(jt) # add to system
system.refs["L2l<->L2r"] = jt # maintain a reference to the joint
#
##------------- ee <-> L2l --------------
jt = chrono.ChLinkRevolute() # create revolute joint object
local = True # we will use the local frame
L2l_j_r = chrono.ChFrameD(chrono.ChVectorD(_L23l/2,0,.01)) # local frame of attachment
ee_j_r = chrono.ChFrameD(chrono.ChVectorD(0,0,0)) # local frame of attachment # COG isn't displaced in GB
L2l_j_COG = L2l_j_r >> L2l.GetFrame_REF_to_COG() # express ee <-> L2l joint relative to L1l COG frame
ee_j_COG = ee_j_r # COG isn't displaced in ee frame
jt.Initialize(L2l,ee,local,L2l_j_COG,ee_j_COG) # init joint
system.Add(jt) # add to system
system.refs["EE<->L2l"] = jt # maintain a reference to the joint
#no need to return, as system is passed by reference and then modified.
return system
def main():
chrono.SetChronoDataPath("../../../data/")
# -----------------
# Create the system
# -----------------
mphysicalSystem = chrono.ChSystemNSC()
# ----------------------------------
# add a mesh to be sensed by a lidar
# ----------------------------------
mmesh = chrono.ChTriangleMeshConnected()
mmesh.LoadWavefrontMesh(
chrono.GetChronoDataFile("vehicle/hmmwv/hmmwv_chassis.obj"), False,
True)
mmesh.Transform(chrono.ChVectorD(0, 0, 0),
chrono.ChMatrix33D(2)) # scale to a different size
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)
manager.SetKeyframeSizeFromTimeStep(.001, 1 / collection_time)
# ------------------------------------------------
# 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(
mesh_body, # body lidar is attached to
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, # vertical field of view
min_vert_angle,
100.0, #max lidar range
sample_radius, # sample radius
divergence_angle, # divergence angle
return_mode, # return mode for the lidar
lens_model # method/model to use for generating data
)
lidar.SetName("Lidar Sensor")
lidar.SetLag(lag)
lidar.SetCollectionWindow(collection_time)
# -----------------------------------------------------------------
# Create a filter graph for post-processing the data from the lidar
# -----------------------------------------------------------------
if noise_model == "CONST_NORMAL_XYZI":
lidar.PushFilter(sens.ChFilterLidarNoiseXYZI(0.01, 0.001, 0.001, 0.01))
elif noise_model == "NONE":
# Don't add any noise models
pass
if vis:
# Visualize 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)
# ---------------
# Simulate system
# ---------------
orbit_radius = 5
orbit_rate = 0.2
ch_time = 0.0
render_time = 0
t1 = time.time()
while (ch_time < end_time):
lidar.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 XYZI buffer from lidar
xyzi_buffer = lidar.GetMostRecentXYZIBuffer()
if xyzi_buffer.HasData():
xyzi_data = xyzi_buffer.GetXYZIData()
print('XYZI buffer recieved from lidar. Lidar resolution: {0}x{1}'\
.format(xyzi_buffer.Width, xyzi_buffer.Height))
print('Max Value: {0}'.format(np.max(xyzi_data)))
# 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)
