def add_θ1r_joint(system,joint):
"""
adds the type of joint (either driven or non-driven) at the θ1r location
"""
a = ALEXR() # contains robot state information
system.refs["GB<->L1r"] = joint # maintain a reference to the joint
local = True # we will use the local frame
GB_j_r = chrono.ChFrameD(chrono.ChVectorD(0,(a.yr - a.yl)/2,.02)) # GB attachment point j represented in ref frame (same as COG frame)
L1r_j_r = chrono.ChFrameD(chrono.ChVectorD(-1*a.L1r/2,0,0)) # L1l attachment point j represented in ref frame
GB = system.refs["GB"] # get a reference to the ground body
L1r = system.refs["L1r"] # get a reference to Link 1 body
GB_j_COG = GB_j_r # COG isn't displaced in GB
L1r_j_COG = L1r_j_r >> L1r.GetFrame_REF_to_COG() # express GB<->L1l joint relative to L1l COG frame
joint.Initialize(GB,L1r,local,GB_j_COG,L1r_j_COG) # init joint
system.Add(joint)
def make_frame_from_name(partname, root_transformation):
shape_marker = TopoDS.TopoDS_Shape()
if (mydoc.GetNamedShape(shape_marker, partname)):
frame_marker = chrono.ChFrameD()
mydoc.FromCascadeToChrono(shape_marker.Location(), frame_marker)
frame_marker.ConcatenatePreTransformation(root_transformation)
return frame_marker
else:
raise ValueError("Warning. Marker part name cannot be found in STEP file.\n")
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 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
#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 ---------------------------
#------------- GB <-> L1l --------------
jt = chrono.ChLinkRevolute() #create revolute joint object
local = True #we will use the local frame
GB_frame = chrono.ChFrameD(chrono.ChVectorD(0, -1 * (yr - yl) / 2,
0.01)) #local frame of attachment
L1l_frame = chrono.ChFrameD(chrono.ChVectorD(-1 * _L1l / 2, 0,
0)) #local frame of attachment
jt.Initialize(GB, L1l, local, GB_frame, L1l_frame) #init joint
mysystem.Add(jt) #add to system
##------------- L1l <-> L2l --------------
jt = chrono.ChLinkRevolute() #create revolute joint object
local = True #we will use the local frame
L1l_frame = chrono.ChFrameD(chrono.ChVectorD(_L1l / 2, 0,
0.01)) #local frame of attachment
L2l_frame = chrono.ChFrameD(chrono.ChVectorD(-1 * _L23l / 2, 0,
0)) #local frame of attachment
jt.Initialize(L1l, L2l, local, L1l_frame, L2l_frame) #init joint
mysystem.Add(jt) #add to system
mysystem.Add(mtrajectory)
#
# EXAMPLE 2:
#
# Create a ChBody that contains the trajectory
mwheel = chrono.ChBody()
mwheel.SetPos(chrono.ChVectorD(-3, 2, 0))
mysystem.Add(mwheel)
# Create a motor that spins the wheel
my_motor = chrono.ChLinkMotorRotationSpeed()
my_motor.Initialize(mwheel, mfloor, chrono.ChFrameD(chrono.ChVectorD(-3, 2,
0)))
my_angularspeed = chrono.ChFunction_Const(chrono.CH_C_PI / 4.0)
my_motor.SetMotorFunction(my_angularspeed)
mysystem.Add(my_motor)
# Create a ChLinePath geometry, and insert sub-paths:
mglyph = chrono.ChLinePath()
ms1 = chrono.ChLineSegment(chrono.ChVectorD(-0.5, -0.5, 0),
chrono.ChVectorD(0.5, -0.5, 0))
mglyph.AddSubLine(ms1)
ma1 = chrono.ChLineArc(chrono.ChCoordsysD(chrono.ChVectorD(0.5, 0, 0)), 0.5,
-chrono.CH_C_PI_2, chrono.CH_C_PI_2, True)
mglyph.AddSubLine(ma1)
ms2 = chrono.ChLineSegment(chrono.ChVectorD(0.5, 0.5, 0),
chrono.ChVectorD(-0.5, 0.5, 0))
mglyph.AddSubLine(ms2)
mysystem.Add(mrod)
# Create a stylized piston
mpiston = chrono.ChBodyEasyCylinder(0.2, 0.3, 1000)
mpiston.SetPos(crank_center + chrono.ChVectorD(crank_rad + rod_length, 0, 0))
mpiston.SetRot(chrono.Q_ROTATE_Y_TO_X)
mysystem.Add(mpiston)
# Now create constraints and motors between the bodies.
# Create crank-truss joint: a motor that spins the crank flywheel
my_motor = chrono.ChLinkMotorRotationSpeed()
my_motor.Initialize(
mcrank, # the first connected body
mfloor, # the second connected body
chrono.ChFrameD(crank_center)) # where to create the motor in abs.space
my_angularspeed = chrono.ChFunction_Const(chrono.CH_C_PI) # ang.speed: 180°/s
my_motor.SetMotorFunction(my_angularspeed)
mysystem.Add(my_motor)
# Create crank-rod joint
mjointA = chrono.ChLinkLockRevolute()
mjointA.Initialize(
mrod, mcrank,
chrono.ChCoordsysD(crank_center + chrono.ChVectorD(crank_rad, 0, 0)))
mysystem.Add(mjointA)
# Create rod-piston joint
mjointB = chrono.ChLinkLockRevolute()
mjointB.Initialize(
mpiston, mrod,
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
material = fea.ChMaterialShellReissnerIsothropic(rho, E, nu, alpha, beta)
alphadamp = 0.1
# elements
elements = []
manager = sens.ChSensorManager(my_rccar.GetSystem())
manager.scene.AddPointLight(chrono.ChVectorF(100,100,100),chrono.ChVectorF(1,1,1),500.0)
manager.scene.AddPointLight(chrono.ChVectorF(-100,100,100),chrono.ChVectorF(1,1,1),500.0)
manager.scene.GetBackground().has_texture = True;
manager.scene.GetBackground().env_tex = "sensor/textures/cloud_layers_8k.hdr";
manager.scene.GetBackground().has_changed = True;
#field of view:
fov = 1.408
camera_3rd = sens.ChCameraSensor(
my_rccar.GetChassisBody(), # body lidar is attached to
60, # scanning rate in Hz
chrono.ChFrameD(chrono.ChVectorD(-2, 0, 1),
chrono.Q_from_AngAxis(.3, chrono.ChVectorD(0, 1, 0))), # offset pose
1920*2, # number of horizontal samples
1080*2, # number of vertical channels
chrono.CH_C_PI / 4 # horizontal field of view # vertical field of view
)
camera_3rd.SetName("Camera Sensor")
if(visualize):
camera_3rd.PushFilter(sens.ChFilterVisualize(1280,720,"Third Person Camera"))
if(save_data):
camera_3rd.PushFilter(sens.ChFilterSave(1280,720,"output/iros/third_person_camera/"))
manager.AddSensor(camera_3rd)
camera_front = sens.ChCameraSensor(
my_rccar.GetChassisBody(), # body lidar is attached to
60, # scanning rate in Hz
chrono.ChFrameD(chrono.ChVectorD(0, 0, .2),
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
# - 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()
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)
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
# 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() cyl_2.GetCylinderGeometry().p1 = chrono.ChVectorD(0, 0, 0) cyl_2.GetCylinderGeometry().p2 = chrono.ChVectorD(2, 0, 0) cyl_2.GetCylinderGeometry().rad = 0.2 pend_2.AddAsset(cyl_2) col_2 = chrono.ChColorAsset(0, 0, 0.6) pend_2.AddAsset(col_2) # In this case, we must specify the centroidal frame, relative to the body # reference frame pend_2.SetFrame_COG_to_REF(chrono.ChFrameD(chrono.ChVectorD(1, 0, 0), chrono.ChQuaternionD(1, 0, 0, 0))) # Specify the initial position of the pendulum (horizontal, pointing towards # positive X). Here, we want to specify the position of the body reference # frame (relative to the absolute frame). Recall that the body reference # frame is located at the pin. pend_2.SetFrame_REF_to_abs(chrono.ChFrameD(chrono.ChVectorD(0, 0, -1))) # Note: Beware of using the method SetPos() to specify the initial position # (as we did for the first pendulum)! SetPos() specifies the position of the # centroidal frame. So one could use it (instead of SetFrame_REF_to_abs) but # using: # pend_2->SetPos(ChVector<>(1, 0, -1)); # However, this defeats the goal of specifying the body through the desired # body reference frame.
# 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()
cyl_2.GetCylinderGeometry().p1 = chrono.ChVectorD(0, 0, 0)
cyl_2.GetCylinderGeometry().p2 = chrono.ChVectorD(2, 0, 0)
cyl_2.GetCylinderGeometry().rad = 0.2
pend_2.AddAsset(cyl_2)
col_2 = chrono.ChColorAsset(0, 0, 0.6)
pend_2.AddAsset(col_2)
# In this case, we must specify the centroidal frame, relative to the body
# reference frame
pend_2.SetFrame_COG_to_REF(
chrono.ChFrameD(chrono.ChVectorD(1, 0, 0),
chrono.ChQuaternionD(1, 0, 0, 0)))
# Specify the initial position of the pendulum (horizontal, pointing towards
# positive X). Here, we want to specify the position of the body reference
# frame (relative to the absolute frame). Recall that the body reference
# frame is located at the pin.
pend_2.SetFrame_REF_to_abs(chrono.ChFrameD(chrono.ChVectorD(0, 0, -1)))
# Note: Beware of using the method SetPos() to specify the initial position
# (as we did for the first pendulum)! SetPos() specifies the position of the
# centroidal frame. So one could use it (instead of SetFrame_REF_to_abs) but
# using:
# pend_2->SetPos(ChVector<>(1, 0, -1));
# However, this defeats the goal of specifying the body through the desired
# body reference frame.
# Alternatively, one could use SetPos() and pass it the position of the body
# Note: this is a rheonomic motor that enforces the motion
# geometrically no compliance is allowed, this means that if the
# rotating body hits some hard contact, the solver might give unpredictable
# oscillatory or diverging results because of the contradiction.
positionA1 = chrono.ChVectorD(-3, 2, -3)
stator1, rotor1 = CreateStatorRotor(material, mphysicalSystem, positionA1)
# Create the motor
rotmotor1 = chrono.ChLinkMotorRotationSpeed()
# Connect the rotor and the stator and add the motor to the system:
rotmotor1.Initialize(
rotor1, # body A (slave)
stator1, # body B (master)
chrono.ChFrameD(positionA1) # motor frame, in abs. coords
)
mphysicalSystem.Add(rotmotor1)
# Create a ChFunction to be used for the ChLinkMotorRotationSpeed
mwspeed = chrono.ChFunction_Const(
chrono.CH_C_PI_2) # constant angular speed, in [rad/s], 1PI/s =180°/s
# Let the motor use this motion function:
rotmotor1.SetSpeedFunction(mwspeed)
# The ChLinkMotorRotationSpeed contains a hidden state that performs the time integration
# of the angular speed setpoint: such angle is then imposed to the
# constraat the positional level too, thus avoiding angle error
# accumulation (angle drift). Optionally, such positional constraint
# level can be disabled as follows:
#
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)
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)
mysystem.Add(mbody2)
mboxasset = chrono.ChBoxShape()
mboxasset.GetBoxGeometry().Size = chrono.ChVectorD(0.2, 0.5, 0.1)
mbody2.AddAsset(mboxasset)
mboxtexture = chrono.ChTexture()
mboxtexture.SetTextureFilename('../../../data/concrete.jpg')
mbody2.GetAssets().push_back(mboxtexture)
# Create a revolute constraint
mlink = chrono.ChLinkRevolute()
# the coordinate system of the constraint reference in abs. space:
mframe = chrono.ChFrameD(chrono.ChVectorD(0.1, 0.5, 0))
# initialize the constraint telling which part must be connected, and where:
mlink.Initialize(mbody1, mbody2, mframe)
mysystem.Add(mlink)
# ---------------------------------------------------------------------
#
# Create an Irrlicht application to visualize the system
#
myapplication = chronoirr.ChIrrApp(mysystem, 'PyChrono example',
chronoirr.dimension2du(1024, 768))
myapplication.AddTypicalSky()
# - 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()
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)
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
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)
my_params = MySoilParams()
# Create sensor manager
manager = sens.ChSensorManager(my_rccar.GetSystem())
f = 3
for i in range(8):
manager.scene.AddPointLight(chrono.ChVectorF(f, 1.25, 2.3),
chrono.ChVectorF(1, 1, 1), 5)
manager.scene.AddPointLight(chrono.ChVectorF(f, 3.75, 2.3),
chrono.ChVectorF(1, 1, 1), 5)
f += 3
factor = 2
cam = sens.ChCameraSensor(
my_rccar.GetChassisBody(), # body lidar is attached to
30, # scanning rate in Hz
chrono.ChFrameD(chrono.ChVectorD(0, 0, .5),
chrono.Q_from_AngAxis(0,
chrono.ChVectorD(0, 1,
0))), # offset pose
1920 * factor, # number of horizontal samples
1080 * factor, # number of vertical channels
chrono.CH_C_PI / 4, # horizontal field of view
(720 / 1280) * chrono.CH_C_PI / 4. # vertical field of view
)
cam.SetName("Camera Sensor")
# cam.FilterList().append(sens.ChFilterImgAlias(factor))
# cam.FilterList().append(sens.ChFilterVisualize(1920, 1080, "Third Person Camera"))
# cam.FilterList().append(sens.ChFilterRGBA8Access())
manager.AddSensor(cam)
cam2 = sens.ChCameraSensor(
my_rccar.GetChassisBody(), # body lidar is attached to
30, # scanning rate in Hz
mysystem.Add(mbody2)
mboxasset = chrono.ChBoxShape()
mboxasset.GetBoxGeometry().Size = chrono.ChVectorD(0.2, 0.5, 0.1)
mbody2.AddAsset(mboxasset)
mboxtexture = chrono.ChTexture()
mboxtexture.SetTextureFilename('../../../data/textures/concrete.jpg')
mbody2.GetAssets().push_back(mboxtexture)
# Create a revolute constraint
mlink = chrono.ChLinkRevolute()
# the coordinate system of the constraint reference in abs. space:
mframe = chrono.ChFrameD(chrono.ChVectorD(0.1, 0.5, 0))
# initialize the constraint telling which part must be connected, and where:
mlink.Initialize(mbody1, mbody2, mframe)
mysystem.Add(mlink)
# ---------------------------------------------------------------------
#
# Create an Irrlicht application to visualize the system
#
myapplication = chronoirr.ChIrrApp(mysystem, 'PyChrono example',
chronoirr.dimension2du(1024, 768))
myapplication.AddTypicalSky()
# - 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()
body_B.SetPos(chrono.ChVectorD(0, 1, 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.0294, 0.1317, -0.0399),
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.GetChronoDataPath() +
'body_1_1.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)
col = chrono.ChColorAsset()
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))
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)
