def CreateTerrain(sys, length, width, height, offset): ground_mat = chrono.ChMaterialSurface.DefaultMaterial( sys.GetContactMethod()) ground_mat.SetFriction(0.8) ground_mat.SetRestitution(0) if sys.GetContactMethod() == chrono.ChContactMethod_SMC: matSMC = chrono.CastToChMaterialSurfaceSMC(ground_mat) matSMC.SetYoungModulus(1e7) ground = robot.GetSystem().NewBody() ground.SetBodyFixed(True) ground.SetCollide(True) ground.GetCollisionModel().ClearModel() ground.GetCollisionModel().AddBox( ground_mat, length / 2, width / 2, 0.1, chrono.ChVectorD(offset, 0, height - 0.1)) ground.GetCollisionModel().BuildModel() box = chrono.ChBoxShape() box.GetBoxGeometry().Size = chrono.ChVectorD(length / 2, width / 2, 0.1) box.GetBoxGeometry().Pos = chrono.ChVectorD(offset, 0, height - 0.1) ground.AddAsset(box) texture = chrono.ChTexture() texture.SetTextureFilename( chrono.GetChronoDataFile("textures/pinkwhite.png")) texture.SetTextureScale(10 * length, 10 * width) ground.AddAsset(texture) ground.AddAsset(chrono.ChColorAsset(0.8, 0.8, 0.8)) sys.AddBody(ground) return ground
def make_box(size, pos, fixed=False, collide=True, euler_angles=(0, 0, 0), color=(1., 0, 1.)): body = chrono.ChBody() body.SetBodyFixed(fixed) body.SetPos(chrono.ChVectorD(*(swap_yz(pos)))) body.SetRot(chrono.Q_from_Euler123(chrono.ChVectorD(*euler_angles))) body.GetCollisionModel().ClearModel() # swap_yz(size) body.GetCollisionModel().AddBox(size[0], size[2], size[1]) # must set half sizes body.GetCollisionModel().BuildModel() body.SetCollide(collide) mboxasset = chrono.ChBoxShape() mboxasset.GetBoxGeometry().Size = chrono.ChVectorD(*(swap_yz(size))) mboxasset.SetColor(chrono.ChColor(color[0], color[1], color[2])) body.AddAsset(mboxasset) # system.Add(body) return body
col_g = chrono.ChColorAsset() col_g.SetColor(chrono.ChColor(0.6, 0.6, 0.2)) ground.AddAsset(col_g) ## Crank crank = chrono.ChBody() system.AddBody(crank) crank.SetIdentifier(1) crank.SetName("crank") crank.SetMass(1.0) crank.SetInertiaXX(chrono.ChVectorD(0.005, 0.1, 0.1)) crank.SetPos(chrono.ChVectorD(-1, 0, 0)) crank.SetRot(chrono.ChQuaternionD(1, 0, 0, 0)) box_c = chrono.ChBoxShape() box_c.GetBoxGeometry().Size = chrono.ChVectorD(0.95, 0.05, 0.05) crank.AddAsset(box_c) cyl_c = chrono.ChCylinderShape() cyl_c.GetCylinderGeometry().p1 = chrono.ChVectorD(1, 0.1, 0) cyl_c.GetCylinderGeometry().p2 = chrono.ChVectorD(1, -0.1, 0) cyl_c.GetCylinderGeometry().rad = 0.05 crank.AddAsset(cyl_c) sph_c = chrono.ChSphereShape() sph_c.GetSphereGeometry().center = chrono.ChVectorD(-1, 0, 0) sph_c.GetSphereGeometry().rad = 0.05 crank.AddAsset(sph_c) col_c = chrono.ChColorAsset()
def reset(self): self.isdone = False self.ant_sys.Clear() self.body_abdomen = chrono.ChBody() self.body_abdomen.SetPos(chrono.ChVectorD(0, self.abdomen_y0, 0 )) self.body_abdomen.SetMass(self.abdomen_mass) self.body_abdomen.SetInertiaXX(self.abdomen_inertia) # set collision surface properties self.body_abdomen.SetMaterialSurface(self.ant_material) abdomen_ellipsoid = chrono.ChEllipsoid(chrono.ChVectorD(0, 0, 0 ), chrono.ChVectorD(self.abdomen_x, self.abdomen_y, self.abdomen_z )) self.abdomen_shape = chrono.ChEllipsoidShape(abdomen_ellipsoid) self.body_abdomen.AddAsset(self.abdomen_shape) self.body_abdomen.SetMaterialSurface(self.ant_material) self.body_abdomen.SetCollide(True) self.body_abdomen.GetCollisionModel().ClearModel() self.body_abdomen.GetCollisionModel().AddEllipsoid(self.abdomen_x, self.abdomen_y, self.abdomen_z, chrono.ChVectorD(0, 0, 0 ) ) self.body_abdomen.GetCollisionModel().BuildModel() self.ant_sys.Add(self.body_abdomen) leg_ang = (1/4)*math.pi+(1/2)*math.pi*np.array([0,1,2,3]) Leg_quat = [chrono.ChQuaternionD() for i in range(len(leg_ang))] self.leg_body = [chrono.ChBody() for i in range(len(leg_ang))] self.leg_pos= [chrono.ChVectorD() for i in range(len(leg_ang))] leg_cyl = chrono.ChCylinder(-chrono.ChVectorD( self.leg_length/2, 0 ,0),chrono.ChVectorD( self.leg_length/2, 0 ,0), self.leg_radius) self.leg_shape = chrono.ChCylinderShape(leg_cyl) ankle_cyl = chrono.ChCylinder(-chrono.ChVectorD( self.ankle_length/2, 0 ,0),chrono.ChVectorD( self.ankle_length/2, 0 ,0), self.ankle_radius) self.ankle_shape = chrono.ChCylinderShape(ankle_cyl) foot_sphere = chrono.ChSphere(chrono.ChVectorD(self.ankle_length/2, 0, 0 ), self.ankle_radius ) self.foot_shape = chrono.ChSphereShape(foot_sphere) Leg_qa = [ chrono.ChQuaternionD() for i in range(len(leg_ang))] Leg_q = [ chrono.ChQuaternionD() for i in range(len(leg_ang))] z2x_leg = [ chrono.ChQuaternionD() for i in range(len(leg_ang))] Leg_rev_pos=[] Leg_chordsys = [] self.legjoint_frame = [] x_rel = [] z_rel = [] self.Leg_rev = [chrono.ChLinkLockRevolute() for i in range(len(leg_ang))] self.leg_motor = [chrono.ChLinkMotorRotationTorque() for i in range(len(leg_ang)) ] #ankle lists anklejoint_chordsys = [] self.anklejoint_frame = [] self.ankleCOG_frame = [] q_ankle_zrot = [ chrono.ChQuaternionD() for i in range(len(leg_ang))] self.ankle_body = [chrono.ChBody() for i in range(len(leg_ang))] self.Ankle_rev = [chrono.ChLinkLockRevolute() for i in range(len(leg_ang))] self.ankle_motor = [chrono.ChLinkMotorRotationTorque() for i in range(len(leg_ang)) ] for i in range(len(leg_ang)): # Legs Leg_quat[i].Q_from_AngAxis(-leg_ang[i] , chrono.ChVectorD(0, 1, 0)) self.leg_pos[i] = chrono.ChVectorD( (0.5*self.leg_length+self.abdomen_x)*math.cos(leg_ang[i]) ,self.abdomen_y0, (0.5*self.leg_length+self.abdomen_z)*math.sin(leg_ang[i])) self.leg_body[i].SetPos(self.leg_pos[i]) self.leg_body[i].SetRot(Leg_quat[i]) self.leg_body[i].AddAsset(self.leg_shape) self.leg_body[i].SetMass(self.leg_mass) self.leg_body[i].SetInertiaXX(self.leg_inertia) self.ant_sys.Add(self.leg_body[i]) x_rel.append( Leg_quat[i].Rotate(chrono.ChVectorD(1, 0, 0))) z_rel.append( Leg_quat[i].Rotate(chrono.ChVectorD(0, 0, 1))) Leg_qa[i].Q_from_AngAxis(-leg_ang[i] , chrono.ChVectorD(0, 1, 0)) z2x_leg[i].Q_from_AngAxis(chrono.CH_C_PI / 2 , x_rel[i]) Leg_q[i] = z2x_leg[i] * Leg_qa[i] Leg_rev_pos.append(chrono.ChVectorD(self.leg_pos[i]-chrono.ChVectorD(math.cos(leg_ang[i])*self.leg_length/2,0,math.sin(leg_ang[i])*self.leg_length/2))) Leg_chordsys.append(chrono.ChCoordsysD(Leg_rev_pos[i], Leg_q[i])) self.legjoint_frame.append(chrono.ChFrameD(Leg_chordsys[i])) self.Leg_rev[i].Initialize(self.body_abdomen, self.leg_body[i],Leg_chordsys[i]) self.ant_sys.Add(self.Leg_rev[i]) self.leg_motor[i].Initialize(self.body_abdomen, self.leg_body[i],self.legjoint_frame[i]) self.ant_sys.Add(self.leg_motor[i]) # Ankles q_ankle_zrot[i].Q_from_AngAxis(-self.ankle_angle , z_rel[i]) anklejoint_chordsys.append(chrono.ChCoordsysD(self.leg_body[i].GetPos()+ self.leg_body[i].GetRot().Rotate(chrono.ChVectorD(self.leg_length/2, 0, 0)) , q_ankle_zrot[i] * self.leg_body[i].GetRot() )) self.anklejoint_frame.append(chrono.ChFrameD(anklejoint_chordsys[i])) self.ankle_body[i].SetPos(self.anklejoint_frame[i].GetPos() + self.anklejoint_frame[i].GetRot().Rotate(chrono.ChVectorD(self.ankle_length/2, 0, 0))) self.ankle_body[i].SetRot( self.anklejoint_frame[i].GetRot() ) self.ankle_body[i].AddAsset(self.ankle_shape) self.ankle_body[i].SetMass(self.ankle_mass) self.ankle_body[i].SetInertiaXX(self.ankle_inertia) self.ant_sys.Add(self.ankle_body[i]) self.Ankle_rev[i].Initialize(self.leg_body[i], self.ankle_body[i], anklejoint_chordsys[i]) self.ant_sys.Add(self.Ankle_rev[i]) self.ankle_motor[i].Initialize(self.leg_body[i], self.ankle_body[i],self.anklejoint_frame[i]) self.ant_sys.Add(self.ankle_motor[i]) # Feet collisions self.ankle_body[i].SetMaterialSurface(self.ant_material) self.ankle_body[i].SetCollide(True) self.ankle_body[i].GetCollisionModel().ClearModel() self.ankle_body[i].GetCollisionModel().AddSphere(self.ankle_radius, chrono.ChVectorD(self.ankle_length/2, 0, 0 ) ) self.ankle_body[i].GetCollisionModel().BuildModel() self.ankle_body[i].AddAsset(self.ankle_shape) self.ankle_body[i].AddAsset(self.foot_shape) self.Leg_rev[i].GetLimit_Rz().SetActive(True) self.Leg_rev[i].GetLimit_Rz().SetMin(-math.pi/3) self.Leg_rev[i].GetLimit_Rz().SetMax(math.pi/3) self.Ankle_rev[i].GetLimit_Rz().SetActive(True) self.Ankle_rev[i].GetLimit_Rz().SetMin(-math.pi/2) self.Ankle_rev[i].GetLimit_Rz().SetMax(math.pi/4) # Create the room floor: a simple fixed rigid body with a collision shape # and a visualization shape self.body_floor = chrono.ChBody() self.body_floor.SetBodyFixed(True) self.body_floor.SetPos(chrono.ChVectorD(0, -1, 0 )) self.body_floor.SetMaterialSurface(self.ant_material) # Floor Collision. self.body_floor.SetMaterialSurface(self.ant_material) self.body_floor.GetCollisionModel().ClearModel() self.body_floor.GetCollisionModel().AddBox(50, 1, 50, chrono.ChVectorD(0, 0, 0 )) self.body_floor.GetCollisionModel().BuildModel() self.body_floor.SetCollide(True) # Visualization shape body_floor_shape = chrono.ChBoxShape() body_floor_shape.GetBoxGeometry().Size = chrono.ChVectorD(5, 1, 5) body_floor_shape.SetColor(chrono.ChColor(0.4,0.4,0.5)) self.body_floor.GetAssets().push_back(body_floor_shape) body_floor_texture = chrono.ChTexture() body_floor_texture.SetTextureFilename(chrono.GetChronoDataFile('vehicle/terrain/textures/grass.jpg')) self.body_floor.GetAssets().push_back(body_floor_texture) self.ant_sys.Add(self.body_floor) #self.body_abdomen.SetBodyFixed(True) if (self.animate): self.myapplication.AssetBindAll() self.myapplication.AssetUpdateAll() self.numsteps= 0 self.step(np.zeros(8)) return self.get_ob()
except ImportError: print("You need NumPy to run this demo!") print("Copyright (c) 2017 projectchrono.org") system = chrono.ChSystemNSC() system.Set_G_acc(chrono.ChVectorD(0, 0, 0)) # Create the ground body ground = chrono.ChBody() system.AddBody(ground) ground.SetIdentifier(-1) ground.SetBodyFixed(True) ground.SetCollide(False) rail1 = chrono.ChBoxShape() rail1.GetBoxGeometry().SetLengths(chrono.ChVectorD(8, 0.1, 0.1)) rail1.GetBoxGeometry().Pos = chrono.ChVectorD(0, 0, -1) ground.AddAsset(rail1) rail2 = chrono.ChBoxShape() rail2.GetBoxGeometry().SetLengths(chrono.ChVectorD(8, 0.1, 0.1)) rail2.GetBoxGeometry().Pos = chrono.ChVectorD(0, 0, 1) ground.AddAsset(rail2) col = chrono.ChColorAsset() col.SetColor(chrono.ChColor(0.6, 0.6, 0.6)) ground.AddAsset(col) # Create the slider bodies slider1 = chrono.ChBody()
def reset(self): self.isdone = False self.hexapod_sys.Clear() self.exported_items = chrono.ImportSolidWorksSystem(self.fpath) self.csys = [] self.frames = [] self.revs = [] self.motors = [] self.limits = [] for con, coi in zip(self.con_link, self.coi_link): indices = [] for i in range(len(self.exported_items)): if con == self.exported_items[i].GetName( ) or coi == self.exported_items[i].GetName(): indices.append(i) rev = self.exported_items[indices[0]] af0 = rev.GetAssetsFrame() # Revolute joints and ChLinkMotorRotation are z oriented, while parallel is x oriented. # Event though this Frame won't be used anymore is good practice to create a copy before editing its value. af = chrono.ChFrameD(af0) af.SetRot(af0.GetRot() % chrono.Q_ROTATE_X_TO_Z) self.frames.append(af) for i in reversed(indices): del self.exported_items[i] # ADD IMPORTED ITEMS TO THE SYSTEM for my_item in self.exported_items: self.hexapod_sys.Add(my_item) """ $$$$$$$$ FIND THE SW DEFINED CONSTRAINTS, GET THEIR MARKERS AND GET RID OF EM $$$$$$$$ """ self.hips = [ self.hexapod_sys.SearchBody(name) for name in self.hip_names ] self.femurs = [ self.hexapod_sys.SearchBody(name) for name in self.femur_names ] self.tibias = [ self.hexapod_sys.SearchBody(name) for name in self.tibia_names ] self.feet = [ self.hexapod_sys.SearchBody(name) for name in self.feet_names ] self.centralbody = self.hexapod_sys.SearchBody('Body-1') # Bodies are used to replace constraints and detect unwanted collision, so feet are excluded self.bodies = [self.centralbody ] + self.hips + self.femurs + self.tibias self.centralbody.SetBodyFixed(False) self.y0 = self.centralbody.GetPos().y """ # SNIPPET FOR COLOR orange = chrono.ChColorAsset() orange.SetColor(chrono.ChColor(255/255,77/255,6/255)) black = chrono.ChColorAsset() black.SetColor(chrono.ChColor(0,0,0)) for body in self.bodies[:-1]: assets = body.GetAssets() for ast in assets: ass_lev = chrono.CastToChAssetLevel(ast) ass_lev.GetAssets().push_back(orange) assets = self.hand.GetAssets() for ast in assets: ass_lev = chrono.CastToChAssetLevel(ast) ass_lev.GetAssets().push_back(black) """ for i in range(len(self.con_link)): revolute = chrono.ChLinkLockRevolute() cs = chrono.ChCoordsysD(self.frames[i].GetPos(), self.frames[i].GetRot()) self.csys.append(cs) if i < 6: j = 0 else: j = i - 5 revolute.Initialize(self.bodies[j], self.bodies[i + 1], self.csys[i]) self.revs.append(revolute) self.hexapod_sys.Add(self.revs[i]) lim = self.revs[i].GetLimit_Rz() self.limits.append(lim) self.limits[i].SetActive(True) self.limits[i].SetMin(self.minRot[i] * (math.pi / 180)) self.limits[i].SetMax(self.maxRot[i] * (math.pi / 180)) m = chrono.ChLinkMotorRotationTorque() m.SetSpindleConstraint(False, False, False, False, False) m.Initialize(self.bodies[j], self.bodies[i + 1], self.frames[i]) self.motors.append(m) self.hexapod_sys.Add(self.motors[i]) self.body_floor = chrono.ChBody() self.body_floor.SetBodyFixed(True) self.body_floor.SetPos(chrono.ChVectorD(0, -1 - 0.128 - 0.0045, 10)) # Floor Collision. self.body_floor.GetCollisionModel().ClearModel() self.body_floor.GetCollisionModel().AddBox(self.my_material, 50, 1, 50, chrono.ChVectorD(0, 0, 0)) self.body_floor.GetCollisionModel().BuildModel() self.body_floor.SetCollide(True) # Visualization shape body_floor_shape = chrono.ChBoxShape() body_floor_shape.GetBoxGeometry().Size = chrono.ChVectorD(4, 1, 15) body_floor_shape.SetColor(chrono.ChColor(0.4, 0.4, 0.5)) self.body_floor.GetAssets().push_back(body_floor_shape) body_floor_texture = chrono.ChTexture() texpath = os.path.join(chrono.GetChronoDataPath(), 'concrete.jpg') body_floor_texture.SetTextureFilename(texpath) self.body_floor.GetAssets().push_back(body_floor_texture) self.hexapod_sys.Add(self.body_floor) self.numsteps = 0 if (self.render_setup): self.myapplication.AssetBindAll() self.myapplication.AssetUpdateAll() return self.get_ob()
# Create the floor: a simple fixed rigid body with a collision shape # and a visualization shape body_floor = chrono.ChBody() body_floor.SetBodyFixed(True) # Collision shape body_floor.GetCollisionModel().ClearModel() body_floor.GetCollisionModel().AddBox(0.1, 0.02, 0.1) # hemi sizes body_floor.GetCollisionModel().BuildModel() body_floor.SetCollide(True) # Visualization shape body_floor_shape = chrono.ChBoxShape() body_floor_shape.GetBoxGeometry().Size = chrono.ChVectorD(0.1, 0.02, 0.1) body_floor_shape.SetColor(chrono.ChColor(0.5,0.5,0.5)) body_floor.GetAssets().push_back(body_floor_shape) my_system.Add(body_floor) # Create boxes that fall # This is just for fun. for ix in range(0,2): for iz in range(0,4): body_brick = chrono.ChBody() body_brick.SetPos(chrono.ChVectorD(0.05+ix*0.021,0.04,0+iz*0.021))
def reset(self): #print("reset") self.isdone = False self.rev_pend_sys.Clear() # create it self.body_rod = chrono.ChBody() # set initial position self.body_rod.SetPos(chrono.ChVectorD(0, self.size_rod_y / 2, 0)) # set mass properties self.body_rod.SetMass(self.mass_rod) self.body_rod.SetInertiaXX( chrono.ChVectorD(self.inertia_rod_x, self.inertia_rod_y, self.inertia_rod_x)) # set collision surface properties self.body_rod.SetMaterialSurface(self.rod_material) # Visualization shape, for rendering animation self.cyl_base1 = chrono.ChVectorD(0, -self.size_rod_y / 2, 0) self.cyl_base2 = chrono.ChVectorD(0, self.size_rod_y / 2, 0) self.body_rod_shape = chrono.ChCylinderShape() self.body_rod_shape.GetCylinderGeometry().p1 = self.cyl_base1 self.body_rod_shape.GetCylinderGeometry().p2 = self.cyl_base2 self.body_rod_shape.GetCylinderGeometry().rad = self.radius_rod self.body_rod.AddAsset(self.body_rod_shape) self.rev_pend_sys.Add(self.body_rod) self.body_floor = chrono.ChBody() self.body_floor.SetBodyFixed(True) self.body_floor.SetPos(chrono.ChVectorD(0, -5, 0)) self.body_floor.SetMaterialSurface(self.rod_material) if self.render: self.body_floor_shape = chrono.ChBoxShape() self.body_floor_shape.GetBoxGeometry().Size = chrono.ChVectorD( 3, 1, 3) self.body_floor.GetAssets().push_back(self.body_floor_shape) self.body_floor_texture = chrono.ChTexture() self.body_floor_texture.SetTextureFilename( '../../../data/concrete.jpg') self.body_floor.GetAssets().push_back(self.body_floor_texture) self.rev_pend_sys.Add(self.body_floor) self.body_table = chrono.ChBody() self.body_table.SetPos(chrono.ChVectorD(0, -self.size_table_y / 2, 0)) self.body_table.SetMaterialSurface(self.rod_material) if self.render: self.body_table_shape = chrono.ChBoxShape() self.body_table_shape.GetBoxGeometry().Size = chrono.ChVectorD( self.size_table_x / 2, self.size_table_y / 2, self.size_table_z / 2) self.body_table_shape.SetColor(chrono.ChColor(0.4, 0.4, 0.5)) self.body_table.GetAssets().push_back(self.body_table_shape) self.body_table_texture = chrono.ChTexture() self.body_table_texture.SetTextureFilename( '../../../data/concrete.jpg') self.body_table.GetAssets().push_back(self.body_table_texture) self.body_table.SetMass(0.1) self.rev_pend_sys.Add(self.body_table) self.link_slider = chrono.ChLinkLockPrismatic() z2x = chrono.ChQuaternionD() z2x.Q_from_AngAxis(-chrono.CH_C_PI / 2, chrono.ChVectorD(0, 1, 0)) self.link_slider.Initialize( self.body_table, self.body_floor, chrono.ChCoordsysD(chrono.ChVectorD(0, 0, 0), z2x)) self.rev_pend_sys.Add(self.link_slider) self.act_initpos = chrono.ChVectorD(0, 0, 0) self.actuator = chrono.ChLinkMotorLinearForce() self.actuator.Initialize(self.body_table, self.body_floor, chrono.ChFrameD(self.act_initpos)) self.rev_pend_sys.Add(self.actuator) self.rod_pin = chrono.ChMarker() self.body_rod.AddMarker(self.rod_pin) self.rod_pin.Impose_Abs_Coord( chrono.ChCoordsysD(chrono.ChVectorD(0, 0, 0))) self.table_pin = chrono.ChMarker() self.body_table.AddMarker(self.table_pin) self.table_pin.Impose_Abs_Coord( chrono.ChCoordsysD(chrono.ChVectorD(0, 0, 0))) self.pin_joint = chrono.ChLinkLockRevolute() self.pin_joint.Initialize(self.rod_pin, self.table_pin) self.rev_pend_sys.Add(self.pin_joint) if self.render: # --------------------------------------------------------------------- # # Create an Irrlicht application to visualize the system # # ==IMPORTANT!== Use this function for adding a ChIrrNodeAsset to all items # in the system. These ChIrrNodeAsset assets are 'proxies' to the Irrlicht meshes. # If you need a finer control on which item really needs a visualization proxy # Irrlicht, just use application.AssetBind(myitem); on a per-item basis. self.myapplication.AssetBindAll() # ==IMPORTANT!== Use this function for 'converting' into Irrlicht meshes the assets # that you added to the bodies into 3D shapes, they can be visualized by Irrlicht! self.myapplication.AssetUpdateAll() self.isdone = False self.steps = 0 self.step(np.array([[0]])) return self.get_ob()
def reset(self): #print("reset") self.isdone = False self.rev_pend_sys.Clear() # create it self.body_rod = chrono.ChBody() # set initial position self.body_rod.SetPos(chrono.ChVectorD(0, self.size_rod_y / 2, 0)) # set mass properties self.body_rod.SetMass(self.mass_rod) self.body_rod.SetInertiaXX( chrono.ChVectorD(self.inertia_rod_x, self.inertia_rod_y, self.inertia_rod_x)) # set collision surface properties self.body_rod.SetMaterialSurface(self.rod_material) self.cyl_base1 = chrono.ChVectorD(0, -self.size_rod_y / 2, 0) self.cyl_base2 = chrono.ChVectorD(0, self.size_rod_y / 2, 0) self.body_rod_shape = chrono.ChCylinderShape() self.body_rod_shape.GetCylinderGeometry().p1 = self.cyl_base1 self.body_rod_shape.GetCylinderGeometry().p2 = self.cyl_base2 self.body_rod_shape.GetCylinderGeometry().rad = self.radius_rod self.body_rod.AddAsset(self.body_rod_shape) self.rev_pend_sys.Add(self.body_rod) self.body_floor = chrono.ChBody() self.body_floor.SetBodyFixed(True) self.body_floor.SetPos(chrono.ChVectorD(0, -5, 0)) self.body_floor.SetMaterialSurface(self.rod_material) self.body_floor_shape = chrono.ChBoxShape() self.body_floor_shape.GetBoxGeometry().Size = chrono.ChVectorD(3, 1, 3) self.body_floor.GetAssets().push_back(self.body_floor_shape) self.body_floor_texture = chrono.ChTexture() self.body_floor_texture.SetTextureFilename(chrono.GetChronoDataPath() + '/concrete.jpg') self.body_floor.GetAssets().push_back(self.body_floor_texture) self.rev_pend_sys.Add(self.body_floor) self.body_table = chrono.ChBody() self.body_table.SetPos(chrono.ChVectorD(0, -self.size_table_y / 2, 0)) self.body_table.SetMaterialSurface(self.rod_material) self.body_table.SetMass(0.1) self.body_table_shape = chrono.ChBoxShape() self.body_table_shape.GetBoxGeometry().Size = chrono.ChVectorD( self.size_table_x / 2, self.size_table_y / 2, self.size_table_z / 2) self.body_table_shape.SetColor(chrono.ChColor(0.4, 0.4, 0.5)) self.body_table.GetAssets().push_back(self.body_table_shape) self.body_table_texture = chrono.ChTexture() self.body_table_texture.SetTextureFilename(chrono.GetChronoDataPath() + '/concrete.jpg') self.body_table.GetAssets().push_back(self.body_table_texture) self.rev_pend_sys.Add(self.body_table) self.link_slider = chrono.ChLinkLockPrismatic() z2x = chrono.ChQuaternionD() z2x.Q_from_AngAxis(-chrono.CH_C_PI / 2, chrono.ChVectorD(0, 1, 0)) self.link_slider.Initialize( self.body_table, self.body_floor, chrono.ChCoordsysD(chrono.ChVectorD(0, 0, 0), z2x)) self.rev_pend_sys.Add(self.link_slider) self.act_initpos = chrono.ChVectorD(0, 0, 0) self.actuator = chrono.ChLinkMotorLinearForce() self.actuator.Initialize(self.body_table, self.body_floor, chrono.ChFrameD(self.act_initpos)) self.rev_pend_sys.Add(self.actuator) self.rod_pin = chrono.ChMarker() self.body_rod.AddMarker(self.rod_pin) self.rod_pin.Impose_Abs_Coord( chrono.ChCoordsysD(chrono.ChVectorD(0, 0, 0))) self.table_pin = chrono.ChMarker() self.body_table.AddMarker(self.table_pin) self.table_pin.Impose_Abs_Coord( chrono.ChCoordsysD(chrono.ChVectorD(0, 0, 0))) self.pin_joint = chrono.ChLinkLockRevolute() self.pin_joint.Initialize(self.rod_pin, self.table_pin) self.rev_pend_sys.Add(self.pin_joint) if self.render_setup: self.myapplication.AssetBindAll() self.myapplication.AssetUpdateAll() self.isdone = False self.steps = 0 self.step(np.array([[0]])) return self.get_ob()
def reset(self): self.isdone = False self.robosystem.Clear() #action = (np.random.rand(6,)-0.5)*2 #torques = np.multiply(action, self.maxT) self.exported_items = chrono.ImportSolidWorksSystem(self.fpath) self.csys = [] self.frames = [] self.revs = [] self.motors = [] self.limits = [] for con, coi in zip(self.con_link, self.coi_link): indices = [] for i in range(len(self.exported_items)): if con == self.exported_items[i].GetName( ) or coi == self.exported_items[i].GetName(): indices.append(i) rev = self.exported_items[indices[0]] af0 = rev.GetAssetsFrame() # Revolute joints and ChLinkMotorRotation are z oriented, while parallel is x oriented. # Event though this Frame won't be used anymore is good practice to create a copy before editing its value. af = chrono.ChFrameD(af0) af.SetRot(af0.GetRot() % chrono.Q_ROTATE_X_TO_Z) self.frames.append(af) for i in indices: del self.exported_items[i] # ADD IMPORTED ITEMS TO THE SYSTEM for my_item in self.exported_items: self.robosystem.Add(my_item) """ $$$$$$$$ FIND THE SW DEFINED CONSTRAINTS, GET THEIR MARKERS AND GET RID OF EM $$$$$$$$ """ self.bodies = [ self.robosystem.SearchBody(name) for name in self.bodiesNames ] self.hand = self.robosystem.SearchBody('Hand_base_and_p07-2') self.base = self.robosystem.SearchBody('Racer3_p01-3') self.biceps = self.robosystem.SearchBody('Racer3_p03-1') self.forearm = self.robosystem.SearchBody('Racer3_p05-1') self.finger1 = self.robosystem.SearchBody('HAND_e_finger-1') self.finger2 = self.robosystem.SearchBody('HAND_e_finger-2') for i in range(len(self.con_link)): revolute = chrono.ChLinkLockRevolute() cs = chrono.ChCoordsysD(self.frames[i].GetPos(), self.frames[i].GetRot()) self.csys.append(cs) revolute.Initialize(self.bodies[i], self.bodies[i + 1], self.csys[i]) self.revs.append(revolute) self.robosystem.Add(self.revs[i]) lim = self.revs[i].GetLimit_Rz() self.limits.append(lim) self.limits[i].SetActive(True) self.limits[i].SetMin(self.minRot[i] * (math.pi / 180)) self.limits[i].SetMax(self.maxRot[i] * (math.pi / 180)) m = chrono.ChLinkMotorRotationTorque() m.Initialize(self.bodies[i], self.bodies[i + 1], self.frames[i]) #self.robosystem.Add(m2) #m2.SetTorqueFunction(chrono.ChFunction_Const(5)) self.motors.append(m) #self.motors[i].SetTorqueFunction(chrono.ChFunction_Const(float(torques[i]))) self.robosystem.Add(self.motors[i]) self.body_floor = chrono.ChBody() self.body_floor.SetBodyFixed(True) self.body_floor.SetPos(chrono.ChVectorD(0, -1, 0)) # Floor Collision. self.body_floor.GetCollisionModel().ClearModel() self.body_floor.GetCollisionModel().AddBox(self.my_material, 5, 1, 5, chrono.ChVectorD(0, 0, 0)) self.body_floor.GetCollisionModel().BuildModel() self.body_floor.SetCollide(True) # Visualization shape body_floor_shape = chrono.ChBoxShape() body_floor_shape.GetBoxGeometry().Size = chrono.ChVectorD(5, 1, 5) body_floor_shape.SetColor(chrono.ChColor(0.4, 0.4, 0.5)) self.body_floor.GetAssets().push_back(body_floor_shape) body_floor_texture = chrono.ChTexture() texpath = os.path.join(chrono.GetChronoDataPath(), 'concrete.jpg') body_floor_texture.SetTextureFilename(texpath) self.body_floor.GetAssets().push_back(body_floor_texture) self.robosystem.Add(self.body_floor) r = np.random.rand(2, ) - np.asarray([0.5, 0.5]) self.targ_init_pos = [ -0.52 + 2 * (r[0] * 0.05), 0.015, 2 * r[1] * 0.05 ] self.targ_box = chrono.ChBody() # UNset to grasp self.targ_box.SetBodyFixed(True) self.targ_box.SetPos( chrono.ChVectorD(self.targ_init_pos[0], self.targ_init_pos[1], self.targ_init_pos[2])) # Floor Collision. self.targ_box.GetCollisionModel().ClearModel() self.targ_box.GetCollisionModel().AddBox(self.my_material, 0.015, 0.015, 0.015, chrono.ChVectorD(0, 0, 0)) self.targ_box.GetCollisionModel().BuildModel() self.targ_box.SetCollide(True) # Visualization shape targ_box_shape = chrono.ChBoxShape() targ_box_shape.GetBoxGeometry().Size = chrono.ChVectorD( 0.015, 0.015, 0.015) col = chrono.ChColorAsset() col.SetColor(chrono.ChColor(1.0, 0, 0)) self.targ_box.GetAssets().push_back(targ_box_shape) self.targ_box.GetAssets().push_back(col) self.robosystem.Add(self.targ_box) self.numsteps = 0 if (self.render_setup): self.myapplication.AssetBindAll() self.myapplication.AssetUpdateAll() return self.get_ob()
def reset(self): #print("reset") self.isdone = False self.rev_pend_sys.Clear() # create it self.body_rod = chrono.ChBody() # set initial position self.body_rod.SetPos(chrono.ChVectorD(0, self.size_rod_y / 2, 0)) # set mass properties self.body_rod.SetMass(self.mass_rod) # una volta che le hao calcolate inserisci inerzie diverse self.body_rod.SetInertiaXX( chrono.ChVectorD(self.inertia_rod_x, self.inertia_rod_y, self.inertia_rod_x)) # set collision surface properties self.body_rod.SetMaterialSurface(self.rod_material) # Visualization shape, for rendering animation # vettori visual cilindro self.cyl_base1 = chrono.ChVectorD(0, -self.size_rod_y / 2, 0) self.cyl_base2 = chrono.ChVectorD(0, self.size_rod_y / 2, 0) #body_rod_shape = chrono.ChCylinder(cyl_base1, cyl_base2, radius_rod) self.body_rod_shape = chrono.ChCylinderShape() self.body_rod_shape.GetCylinderGeometry().p1 = self.cyl_base1 self.body_rod_shape.GetCylinderGeometry().p2 = self.cyl_base2 self.body_rod_shape.GetCylinderGeometry().rad = self.radius_rod #body_rod.GetAssets().push_back(body_rod_shape) self.body_rod.AddAsset(self.body_rod_shape) self.rev_pend_sys.Add(self.body_rod) self.body_floor = chrono.ChBody() self.body_floor.SetBodyFixed(True) self.body_floor.SetPos(chrono.ChVectorD(0, -5, 0)) self.body_floor.SetMaterialSurface(self.rod_material) # Visualization shape if self.render: self.body_floor_shape = chrono.ChBoxShape() self.body_floor_shape.GetBoxGeometry().Size = chrono.ChVectorD( 3, 1, 3) self.body_floor.GetAssets().push_back(self.body_floor_shape) self.body_floor_texture = chrono.ChTexture() self.body_floor_texture.SetTextureFilename( '../../../data/concrete.jpg') self.body_floor.GetAssets().push_back(self.body_floor_texture) self.rev_pend_sys.Add(self.body_floor) self.body_table = chrono.ChBody() self.body_table.SetPos(chrono.ChVectorD(0, -self.size_table_y / 2, 0)) self.body_table.SetMaterialSurface(self.rod_material) # Visualization shape if self.render: self.body_table_shape = chrono.ChBoxShape() self.body_table_shape.GetBoxGeometry().Size = chrono.ChVectorD( self.size_table_x / 2, self.size_table_y / 2, self.size_table_z / 2) self.body_table_shape.SetColor(chrono.ChColor(0.4, 0.4, 0.5)) self.body_table.GetAssets().push_back(self.body_table_shape) self.body_table_texture = chrono.ChTexture() self.body_table_texture.SetTextureFilename( '../../../data/concrete.jpg') self.body_table.GetAssets().push_back(self.body_table_texture) self.body_table.SetMass(0.1) self.rev_pend_sys.Add(self.body_table) # Create a constraint that blocks free 3 x y z translations and 3 rx ry rz rotations # of the table respect to the floor, and impose that the relative imposed position # depends on a specified motion law. self.link_slider = chrono.ChLinkLockPrismatic() z2x = chrono.ChQuaternionD() z2x.Q_from_AngAxis(-chrono.CH_C_PI / 2, chrono.ChVectorD(0, 1, 0)) self.link_slider.Initialize( self.body_table, self.body_floor, chrono.ChCoordsysD(chrono.ChVectorD(0, 0, 0), z2x)) self.rev_pend_sys.Add(self.link_slider) self.link_slider.SetMotion_axis(chrono.ChVectorD(1, 0, 0)) #attuatore lineare self.act_initpos = chrono.ChVectorD(0, 0, 0) self.actuator = chrono.ChLinkMotorLinearForce() self.actuator.Initialize(self.body_table, self.body_floor, chrono.ChFrameD(self.act_initpos)) self.rev_pend_sys.Add(self.actuator) # ..create the function for imposed y vertical motion, etc. # tolta: solo traslazione asse z #mfunY = chrono.ChFunction_Sine(0,1.5,0.001) # phase, frequency, amplitude #link_slider.SetMotion_Y(mfunY) # ..create the function for imposed z horizontal motion, etc. #self.mfunX = chrono.ChFunction_Sine(0,1.5,0.4) # phase, frequency, amplitude #self.link_slider.SetMotion_X(self.action) # Note that you could use other types of ChFunction_ objects, or create # your custom function by class inheritance (see demo_python.py), or also # set a function for table rotation , etc. # REVLOLUTE JOINT: # create frames for the joint self.rod_pin = chrono.ChMarker() self.body_rod.AddMarker(self.rod_pin) self.rod_pin.Impose_Abs_Coord( chrono.ChCoordsysD(chrono.ChVectorD(0, 0, 0))) self.table_pin = chrono.ChMarker() self.body_table.AddMarker(self.table_pin) self.table_pin.Impose_Abs_Coord( chrono.ChCoordsysD(chrono.ChVectorD(0, 0, 0))) self.pin_joint = chrono.ChLinkLockRevolute() self.pin_joint.Initialize(self.rod_pin, self.table_pin) self.rev_pend_sys.Add(self.pin_joint) if self.render: # --------------------------------------------------------------------- # # Create an Irrlicht application to visualize the system # # ==IMPORTANT!== Use this function for adding a ChIrrNodeAsset to all items # in the system. These ChIrrNodeAsset assets are 'proxies' to the Irrlicht meshes. # If you need a finer control on which item really needs a visualization proxy # Irrlicht, just use application.AssetBind(myitem); on a per-item basis. self.myapplication.AssetBindAll() # ==IMPORTANT!== Use this function for 'converting' into Irrlicht meshes the assets # that you added to the bodies into 3D shapes, they can be visualized by Irrlicht! self.myapplication.AssetUpdateAll() # If you want to show shadows because you used "AddLightWithShadow()' # you must remember this: self.isdone = False self.steps = 0 self.step(np.array([[0]])) return self.get_ob()
def reset(self): self.isdone = False self.ant_sys.Clear() self.body_abdomen = chrono.ChBody() self.body_abdomen.SetPos(chrono.ChVectorD(0, self.abdomen_y0, 0)) self.body_abdomen.SetMass(self.abdomen_mass) self.body_abdomen.SetInertiaXX(self.abdomen_inertia) # set collision surface properties self.body_abdomen.SetMaterialSurface(self.ant_material) abdomen_ellipsoid = chrono.ChEllipsoid( chrono.ChVectorD(0, 0, 0), chrono.ChVectorD(self.abdomen_x, self.abdomen_y, self.abdomen_z)) self.abdomen_shape = chrono.ChEllipsoidShape(abdomen_ellipsoid) self.body_abdomen.AddAsset(self.abdomen_shape) self.body_abdomen.SetMaterialSurface(self.ant_material) self.body_abdomen.SetCollide(True) self.body_abdomen.GetCollisionModel().ClearModel() self.body_abdomen.GetCollisionModel().AddEllipsoid( self.abdomen_x, self.abdomen_y, self.abdomen_z, chrono.ChVectorD(0, 0, 0)) self.body_abdomen.GetCollisionModel().BuildModel() self.ant_sys.Add(self.body_abdomen) tilt_factor = 1.1 # no tilt; higher number tilts body toward positive x leg_ang = np.array([ tilt_factor * math.pi / 2, tilt_factor * math.pi / 2 ]) #(1/4)*math.pi+(1/2)*math.pi*np.array([0,1]) Leg_quat = [chrono.ChQuaternionD() for i in range(len(leg_ang))] self.leg_body = [chrono.ChBody() for i in range(len(leg_ang))] self.leg_pos = [chrono.ChVectorD() for i in range(len(leg_ang))] leg_cyl = chrono.ChCylinder( -chrono.ChVectorD(self.leg_length / 2, 0, 0), chrono.ChVectorD(self.leg_length / 2, 0, 0), self.leg_radius) self.leg_shape = chrono.ChCylinderShape(leg_cyl) ankle_cyl = chrono.ChCylinder( -chrono.ChVectorD(self.ankle_length / 2, 0, 0), chrono.ChVectorD(self.ankle_length / 2, 0, 0), self.ankle_radius) self.ankle_shape = chrono.ChCylinderShape(ankle_cyl) foot_sphere = chrono.ChSphere( chrono.ChVectorD(self.ankle_length / 2, 0, 0), self.ankle_radius) self.foot_shape = chrono.ChSphereShape(foot_sphere) Leg_qa = [chrono.ChQuaternionD() for i in range(len(leg_ang))] Leg_q = [chrono.ChQuaternionD() for i in range(len(leg_ang))] z2x_leg = [chrono.ChQuaternionD() for i in range(len(leg_ang))] Leg_rev_pos = [] Leg_chordsys = [] self.legjoint_frame = [] x_rel = [] z_rel = [] self.Leg_rev = [ chrono.ChLinkLockRevolute() for i in range(len(leg_ang)) ] self.leg_motor = [ chrono.ChLinkMotorRotationTorque() for i in range(len(leg_ang)) ] #ankle lists anklejoint_chordsys = [] self.anklejoint_frame = [] self.ankleCOG_frame = [] q_ankle_zrot = [chrono.ChQuaternionD() for i in range(len(leg_ang))] self.ankle_body = [chrono.ChBody() for i in range(len(leg_ang))] self.Ankle_rev = [ chrono.ChLinkLockRevolute() for i in range(len(leg_ang)) ] self.ankle_motor = [ chrono.ChLinkMotorRotationTorque() for i in range(len(leg_ang)) ] for i in range(len(leg_ang)): # Legs Leg_quat[i].Q_from_AngAxis(-leg_ang[i], chrono.ChVectorD(0, 0, 1)) # postion of leg CM from abdomen self.leg_pos[i] = chrono.ChVectorD( (0.5 * self.leg_length * math.cos(leg_ang[i])), (self.abdomen_y0 - self.leg_length / 2 * math.sin(leg_ang[i]) - self.abdomen_y * math.cos(self.CMtohip_angle)), (-1)**i * self.abdomen_z * math.sin(self.CMtohip_angle)) self.leg_body[i].SetPos(self.leg_pos[i]) self.leg_body[i].SetRot(Leg_quat[i]) self.leg_body[i].AddAsset(self.leg_shape) self.leg_body[i].SetMass(self.leg_mass) self.leg_body[i].SetInertiaXX(self.leg_inertia) self.ant_sys.Add(self.leg_body[i]) x_rel.append(Leg_quat[i].Rotate(chrono.ChVectorD(1, 0, 0))) z_rel.append(Leg_quat[i].Rotate(chrono.ChVectorD(0, 0, 1))) Leg_qa[i].Q_from_AngAxis(-leg_ang[i], chrono.ChVectorD(0, 1, 0)) z2x_leg[i].Q_from_AngAxis(chrono.CH_C_PI / 2, x_rel[i]) Leg_q[i] = z2x_leg[i] * Leg_qa[i] Leg_rev_pos.append(chrono.ChVectorD(0, self.abdomen_y0, 0)) # connection to abdomen Leg_chordsys.append(chrono.ChCoordsysD(Leg_rev_pos[i], Leg_q[i])) self.legjoint_frame.append(chrono.ChFrameD(Leg_chordsys[i])) self.Leg_rev[i].Initialize(self.body_abdomen, self.leg_body[i], Leg_chordsys[i]) self.ant_sys.Add(self.Leg_rev[i]) self.leg_motor[i].Initialize(self.body_abdomen, self.leg_body[i], self.legjoint_frame[i]) self.ant_sys.Add(self.leg_motor[i]) # Ankles q_ankle_zrot[i].Q_from_AngAxis(-self.ankle_angle, z_rel[i]) anklejoint_chordsys.append( chrono.ChCoordsysD( self.leg_body[i].GetPos() + self.leg_body[i].GetRot().Rotate( chrono.ChVectorD(self.leg_length / 2, 0, 0)), q_ankle_zrot[i] * self.leg_body[i].GetRot())) self.anklejoint_frame.append( chrono.ChFrameD(anklejoint_chordsys[i])) self.ankle_body[i].SetPos( self.anklejoint_frame[i].GetPos() + self.anklejoint_frame[i].GetRot().Rotate( chrono.ChVectorD(self.ankle_length / 2, 0, 0))) self.ankle_body[i].SetRot(self.anklejoint_frame[i].GetRot()) self.ankle_body[i].AddAsset(self.ankle_shape) self.ankle_body[i].SetMass(self.ankle_mass) self.ankle_body[i].SetInertiaXX(self.ankle_inertia) self.ant_sys.Add(self.ankle_body[i]) self.Ankle_rev[i].Initialize(self.leg_body[i], self.ankle_body[i], anklejoint_chordsys[i]) self.ant_sys.Add(self.Ankle_rev[i]) self.ankle_motor[i].Initialize(self.leg_body[i], self.ankle_body[i], self.anklejoint_frame[i]) self.ant_sys.Add(self.ankle_motor[i]) # Feet collisions self.ankle_body[i].SetMaterialSurface(self.ant_material) self.ankle_body[i].SetCollide(True) self.ankle_body[i].GetCollisionModel().ClearModel() self.ankle_body[i].GetCollisionModel().AddSphere( self.ankle_radius, chrono.ChVectorD(self.ankle_length / 2, 0, 0)) self.ankle_body[i].GetCollisionModel().BuildModel() self.ankle_body[i].AddAsset(self.ankle_shape) self.ankle_body[i].AddAsset(self.foot_shape) # Limits need to turn back to true self.Leg_rev[i].GetLimit_Rz().SetActive(True) self.Leg_rev[i].GetLimit_Rz().SetMin(-math.pi / 3) self.Leg_rev[i].GetLimit_Rz().SetMax(math.pi / 3) self.Ankle_rev[i].GetLimit_Rz().SetActive(True) self.Ankle_rev[i].GetLimit_Rz().SetMin(-math.pi / 3) self.Ankle_rev[i].GetLimit_Rz().SetMax(math.pi / 3) # Create the room floor: a simple fixed rigid body with a collision shape # and a visualization shape self.body_floor = chrono.ChBody() self.body_floor.SetBodyFixed(True) self.body_floor.SetPos( chrono.ChVectorD( 0, -self.abdomen_y0 - self.abdomen_y * math.cos(self.CMtohip_angle) - self.ankle_radius, 0)) self.body_floor.SetMaterialSurface(self.ant_material) # Floor Collision. self.body_floor.SetMaterialSurface(self.ant_material) self.body_floor.GetCollisionModel().ClearModel() self.body_floor.GetCollisionModel().AddBox(50, 1, 50, chrono.ChVectorD(0, 0, 0)) self.body_floor.GetCollisionModel().BuildModel() self.body_floor.SetCollide(True) # Visualization shape body_floor_shape = chrono.ChBoxShape() body_floor_shape.GetBoxGeometry().Size = chrono.ChVectorD(5, 1, 5) body_floor_shape.SetColor(chrono.ChColor(0.4, 0.4, 0.5)) self.body_floor.GetAssets().push_back(body_floor_shape) body_floor_texture = chrono.ChTexture() body_floor_texture.SetTextureFilename('../data/grass.jpg') self.body_floor.GetAssets().push_back(body_floor_texture) self.ant_sys.Add(self.body_floor) # this was commented out to not fix the segments # this is helpful to check geometry of humanoid #self.body_abdomen.SetBodyFixed(True) #self.leg_body[0].SetBodyFixed(True) #self.leg_body[1].SetBodyFixed(True) #self.ankle_body[0].SetBodyFixed(True) #self.ankle_body[1].SetBodyFixed(True) if (self.animate): self.myapplication.AssetBindAll() self.myapplication.AssetUpdateAll() self.numsteps = 0 self.step(np.zeros(4)) return self.get_ob()
# Create the room floor: a simple fixed rigid body with a collision shape # and a visualization shape body_floor = chrono.ChBody() body_floor.SetBodyFixed(True) body_floor.SetPos(chrono.ChVectorD(0, -1, 0 )) body_floor.SetMaterialSurface(pebble_material) # Collision shape (shared by all particle clones) body_floor.GetCollisionModel().ClearModel() body_floor.GetCollisionModel().AddBox(3, 1, 3) # hemi sizes body_floor.GetCollisionModel().BuildModel() body_floor.SetCollide(True) # Visualization shape (shared by all particle clones) body_floor_shape = chrono.ChBoxShape() body_floor_shape.GetBoxGeometry().Size = chrono.ChVectorD(3, 1, 3) body_floor.GetAssets().push_back(body_floor_shape) body_floor_texture = chrono.ChTexture() body_floor_texture.SetTextureFilename('concrete.jpeg') body_floor.GetAssets().push_back(body_floor_texture) my_system.Add(body_floor) if True: # m_visualization == "irrlicht": # --------------------------------------------------------------------- # # Create an Irrlicht application to visualize the system
ground.AddAsset(sph_2) # Create a body suspended through a ChLinkTSDA (default linear) # ------------------------------------------------------------- body_1 = chrono.ChBody() system.AddBody(body_1) body_1.SetPos(chrono.ChVectorD(-1, -3, 0)) body_1.SetIdentifier(1) body_1.SetBodyFixed(False) body_1.SetCollide(False) body_1.SetMass(1) body_1.SetInertiaXX(chrono.ChVectorD(1, 1, 1)) # Attach a visualization asset. box_1 = chrono.ChBoxShape() box_1.GetBoxGeometry().SetLengths(chrono.ChVectorD(1, 1, 1)) body_1.AddAsset(box_1) col_1 = chrono.ChColorAsset() col_1.SetColor(chrono.ChColor(0.6, 0, 0)) body_1.AddAsset(col_1) # Create the spring between body_1 and ground. The spring end points are # specified in the body relative frames. spring_1 = chrono.ChLinkTSDA() spring_1.Initialize(body_1, ground, True, chrono.ChVectorD(0, 0, 0), chrono.ChVectorD(-1, 0, 0), False, rest_length) spring_1.SetSpringCoefficient(spring_coef) spring_1.SetDampingCoefficient(damping_coef) system.AddLink(spring_1)
H = 0.25 K = 0.05 vA = chrono.ChVectorD(0, 0, 0) vC = chrono.ChVectorD(L, 0, 0) vB = chrono.ChVectorD(L, -H, 0) vG = chrono.ChVectorD(L - K, -H, 0) vd = chrono.ChVectorD(0, 0, 0.0001) # Create a truss: body_truss = chrono.ChBody() body_truss.SetBodyFixed(True) my_system.AddBody(body_truss) # Attach a 'box' shape asset for visualization. mboxtruss = chrono.ChBoxShape() mboxtruss.GetBoxGeometry().Pos = chrono.ChVectorD(-0.01, 0, 0) mboxtruss.GetBoxGeometry().SetLengths(chrono.ChVectorD(0.02, 0.2, 0.1)) body_truss.AddAsset(mboxtruss) # Create body for crank body_crank = chrono.ChBody() body_crank.SetPos((vB + vG) * 0.5) my_system.AddBody(body_crank) # Attach a 'box' shape asset for visualization. mboxcrank = chrono.ChBoxShape() mboxcrank.GetBoxGeometry().Pos = chrono.ChVectorD(0, 0, 0) mboxcrank.GetBoxGeometry().SetLengths(chrono.ChVectorD(K, 0.02, 0.02)) body_crank.AddAsset(mboxcrank)