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)