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