def AddFallingItems(sys): # Shared contact materials for falling objects mat = chrono.ChMaterialSurfaceSMC() # Create falling rigid bodies (spheres and boxes etc.) for ix in range(-2, 3): for iz in range(-2, 3): # add spheres mass = 1 radius = 1.1 body = chrono.ChBody() comp = (2.0 / 5.0) * mass * radius**2 body.SetInertiaXX(chrono.ChVectorD(comp, comp, comp)) body.SetMass(mass) body.SetPos(chrono.ChVectorD(4.0 * ix + 0.1, 4.0, 4.0 * iz)) body.GetCollisionModel().ClearModel() body.GetCollisionModel().AddSphere(mat, radius) body.GetCollisionModel().BuildModel() body.SetCollide(True) sphere = chrono.ChSphereShape() sphere.GetSphereGeometry().rad = radius body.AddAsset(sphere) texture = chrono.ChTexture() texture.SetTextureFilename( chrono.GetChronoDataFile("textures/bluewhite.png")) body.AddAsset(texture) sys.AddBody(body) # add boxes mass = 1 hsize = chrono.ChVectorD(0.75, 0.75, 0.75) body = chrono.ChBody() body.SetMass(mass) body.SetPos(chrono.ChVectorD(4.0 * ix, 6.0, 4.0 * iz)) body.GetCollisionModel().ClearModel() body.GetCollisionModel().AddBox(mat, hsize.x, hsize.y, hsize.z) body.GetCollisionModel().BuildModel() body.SetCollide(True) box = chrono.ChBoxShape() box.GetBoxGeometry().Size = hsize body.AddAsset(box) texture = chrono.ChTexture() texture.SetTextureFilename( chrono.GetChronoDataFile("textures/pinkwhite.png")) body.AddAsset(texture) sys.AddBody(body)
def add_ellisoidShape(MiroSystem, radius_x, radius_y, radius_z, pos, texture='test.jpg', density=1000, scale=[1,1], Collide=True, Fixed=True, rotX=0, rotY=0, rotZ=0, rotOrder=['x','y','z'], rotAngle=0, rotAxis=[1,0,0], rotDegrees=True, color=[0.5, 0.5, 0.5]): # Convert position to chrono vector, supports using chvector as input as well ChPos = ChVecify(pos) ChRotAxis = ChVecify(rotAxis) # Create a cylinder body_ball = chrono.ChBodyEasyEllipsoid(chrono.ChVectorD(radius_x, radius_y, radius_z), density) body_ball.SetBodyFixed(Fixed) body_ball.SetPos(ChPos) rotateBody(body_ball, rotX, rotY, rotZ, rotOrder, rotAngle, ChRotAxis, rotDegrees) # Collision shape if(Collide): body_ball.GetCollisionModel().ClearModel() body_ball.GetCollisionModel().AddEllipsoid(radius_x, radius_y, radius_z) # hemi sizes body_ball.GetCollisionModel().BuildModel() body_ball.SetCollide(Collide) # Body texture if texture: # Filter 'textures/' out of the texture name, it's added later if len(texture) > len('textures/'): if texture[0:len('textures/')] == 'textures/': texture = texture[len('textures/'):] body_texture = chrono.ChTexture() body_texture.SetTextureFilename(chrono.GetChronoDataFile('textures/'+texture)) body_texture.SetTextureScale(scale[0], scale[1]) body_ball.GetAssets().push_back(body_texture) if MiroSystem: ChSystem = MiroSystem.Get_APIsystem()[0] ChSystem.Add(body_ball) return body_ball
def AddFallingItems(sys): # Shared contact materials for falling objects sph_mat = chrono.ChMaterialSurfaceNSC() sph_mat.SetFriction(0.2) box_mat = chrono.ChMaterialSurfaceNSC() cyl_mat = chrono.ChMaterialSurfaceNSC() # Create falling rigid bodies (spheres and boxes etc.) for bi in range(29): msphereBody = chrono.ChBodyEasySphere(1.1, # radius size 1000, # density True, # visualization? True, # collision? sph_mat) # contact material msphereBody.SetPos(chrono.ChVectorD(-5 + chrono.ChRandom() * 10, 4 + bi * 0.05, -5 + chrono.ChRandom() * 10)) sys.Add(msphereBody) mtexture = chrono.ChTexture() mtexture.SetTextureFilename(chrono.GetChronoDataFile("textures/bluewhite.png")) msphereBody.AddAsset(mtexture) mboxBody = chrono.ChBodyEasyBox(1.5, 1.5, 1.5, # x,y,z size 100, # density True, # visualization? True, # collision? box_mat) # contact material mboxBody.SetPos(chrono.ChVectorD(-5 + chrono.ChRandom() * 10, 4 + bi * 0.05, -5 + chrono.ChRandom() * 10)) sys.Add(mboxBody) mtexturebox = chrono.ChTexture() mtexturebox.SetTextureFilename(chrono.GetChronoDataFile("textures/cubetexture_bluewhite.png")) mboxBody.AddAsset(mtexturebox) mcylBody = chrono.ChBodyEasyCylinder(0.75, 0.5, # radius, height 100, # density True, # visualization? True, # collision? cyl_mat) # contact material mcylBody.SetPos(chrono.ChVectorD(-5 + chrono.ChRandom() * 10, 4 + bi * 0.05, -5 + chrono.ChRandom() * 10)) sys.Add(mcylBody) # optional, attach a texture for better visualization mtexturecyl = chrono.ChTexture() mtexturecyl.SetTextureFilename(chrono.GetChronoDataFile("textures/pinkwhite.png")) mcylBody.AddAsset(mtexturecyl)
def add_boxShape(MiroSystem, size_x, size_y, size_z, pos, texture='test.jpg', scale=[4,3], Collide=True, Fixed=True, rotX=0, rotY=0, rotZ=0, rotOrder=['x','y','z'], rotAngle=0, rotAxis=[1,0,0], rotDegrees=True, mass=False, density=1000, dynamic=False, color=[0.5, 0.5, 0.5]): '''system, size_x, size_y, size_z, pos, texture, scale = [5,5], hitbox = True/False''' # Convert position to chrono vector, supports using chvector as input as well ChPos = ChVecify(pos) ChRotAxis = ChVecify(rotAxis) # Create a box body_box = chrono.ChBody() body_box.SetBodyFixed(Fixed) body_box.SetPos(ChPos) if not mass: mass = density * size_x * size_y * size_z inertia_brick_xx = (size_y**2 + size_z**2)*mass/3 inertia_brick_yy = (size_x**2 + size_z**2)*mass/3 inertia_brick_zz = (size_x**2 + size_y**2)*mass/3 body_box.SetMass(mass) body_box.SetInertiaXX(chrono.ChVectorD(inertia_brick_xx, inertia_brick_yy, inertia_brick_zz)) # Collision shape body_box.GetCollisionModel().ClearModel() body_box.GetCollisionModel().AddBox(size_x/2, size_y/2, size_z/2) # hemi sizes body_box.GetCollisionModel().BuildModel() body_box.SetCollide(Collide) # Visualization shape body_box_shape = chrono.ChBoxShape() body_box_shape.GetBoxGeometry().Size = chrono.ChVectorD(size_x/2, size_y/2, size_z/2) body_box_shape.SetColor(chrono.ChColor(0.4,0.4,0.5)) body_box.GetAssets().push_back(body_box_shape) # Body texture if texture: # Filter 'textures/' out of the texture name, it's added later if len(texture) > len('textures/'): if texture[0:len('textures/')] == 'textures/': texture = texture[len('textures/'):] body_box_texture = chrono.ChTexture() body_box_texture.SetTextureFilename(chrono.GetChronoDataFile('textures/'+texture)) body_box_texture.SetTextureScale(scale[0], scale[1]) body_box.GetAssets().push_back(body_box_texture) rotateBody(body_box, rotX, rotY, rotZ, rotOrder, rotAngle, rotAxis, rotDegrees) if MiroSystem: ChSystem = MiroSystem.Get_APIsystem()[0] ChSystem.Add(body_box) return body_box
def enableContact(self): self.mbody1.GetCollisionModel().ClearModel() self.mbody1.GetCollisionModel().AddBox(0.2, 0.5, 0.1) # hemi sizes self.mbody1.GetCollisionModel().BuildModel() self.mbody1.SetCollide(True) self.mbody1.SetMass(20) self.Body_wheel_L.GetCollisionModel().ClearModel() self.Body_wheel_L.GetCollisionModel().AddCylinder(0.2, 0.5, 0.1) # hemi sizes self.Body_wheel_L.GetCollisionModel().BuildModel() self.Body_wheel_L.SetCollide(True) self.Body_wheel_L.SetMaterialSurface(self.wheelMaterial) self.Body_wheel_L.SetMass(1) self.Body_wheel_L.AddAsset(chrono.ChTexture("Wheel_texture.png")) self.Body_wheel_R.GetCollisionModel().ClearModel() self.Body_wheel_R.GetCollisionModel().AddCylinder(0.2, 0.5, 0.1) # hemi sizes self.Body_wheel_R.GetCollisionModel().BuildModel() self.Body_wheel_R.SetCollide(True) self.Body_wheel_R.SetMaterialSurface(self.wheelMaterial) self.Body_wheel_R.SetMass(1) self.Body_wheel_R.AddAsset(chrono.ChTexture("Wheel_texture.png"))
def ChangeBodyTexture(body, texture_file, scale=[1,1]): texture = chrono.ChTexture() texture.SetTextureFilename(chrono.GetChronoDataFile(texture_file)) texture.SetTextureScale(scale[0], scale[1]) body.AddAsset(texture)
vshape_3 = chrono.ChBoxShape() vshape_3.GetBoxGeometry().SetLengths(chrono.ChVectorD(0.2, 2, 10.2)) vshape_3.GetBoxGeometry().Pos = chrono.ChVectorD(5, 0, 0) ground.AddAsset(vshape_3) vshape_4 = chrono.ChBoxShape() vshape_4.GetBoxGeometry().SetLengths(chrono.ChVectorD(10.2, 2, 0.2)) vshape_4.GetBoxGeometry().Pos = chrono.ChVectorD(0, 0, -5) ground.AddAsset(vshape_4) vshape_5 = chrono.ChBoxShape() vshape_5.GetBoxGeometry().SetLengths(chrono.ChVectorD(10.2, 2, 0.2)) vshape_5.GetBoxGeometry().Pos = chrono.ChVectorD(0, 0, 5) ground.AddAsset(vshape_5) ground.AddAsset(chrono.ChTexture( chrono.GetChronoDataFile("textures/blue.png"))) # Add obstacle visualization (in a separate level with a different color). ground.AddAsset(obstacle.GetVisualization()) # Create the falling ball ball = chrono.ChBody() sys.AddBody(ball) ball.SetMass(10) comp = 4 * ball_radius * ball_radius ball.SetInertiaXX(chrono.ChVectorD(comp, comp, comp)) ball.SetPos(chrono.ChVectorD(-3, 1.2 * ball_radius, -3)) ball.SetPos_dt(chrono.ChVectorD(5, 0, 5)) ball.SetCollide(True) ball.GetCollisionModel().ClearModel()
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) #----------- left link 2 ------------------ #add body L2l = chrono.ChBodyAuxRef() L2l.SetBodyFixed(False) mysystem.Add(L2l) #add mass properties //improve these based on actual data... m = .266 + .274 L2l.SetMass(m) #L2l.SetInertiaXX(chrono.ChVectorD(.00005,.02053,.02057)) #from solidworks #set position,orientation with FK
mboxasset = chrono.ChBoxShape() mboxasset.GetBoxGeometry().Size = chrono.ChVectorD(0.2, 0.5, 0.1) mbody1.AddAsset(mboxasset) # Create a swinging rigid body mbody2 = chrono.ChBody() mbody2.SetBodyFixed(False) 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)
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
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
body_floor.SetBodyFixed(True) body_floor.SetPos(chrono.ChVectorD(0, -2, 0)) body_floor.SetMaterialSurface(brick_material) # Collision shape body_floor.GetCollisionModel().ClearModel() body_floor.GetCollisionModel().AddBox(3, 1, 3) # hemi sizes body_floor.GetCollisionModel().BuildModel() body_floor.SetCollide(True) # Visualization shape 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(chrono.GetChronoDataPath() + 'concrete.jpg') body_floor.GetAssets().push_back(body_floor_texture) my_system.Add(body_floor) # Create the shaking table, as a box size_table_x = 1 size_table_y = 0.2 size_table_z = 1 body_table = chrono.ChBody() body_table.SetPos(chrono.ChVectorD(0, -size_table_y / 2, 0)) body_table.SetMaterialSurface(brick_material)
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
# If running from a different directory, you must change the path to the data directory with: # chrono.SetChronoDataPath('relative/path/to/data/directory/') # --------------------------------------------------------------------- # # Create the simulation system and add items # mphysicalSystem = chrono.ChSystemNSC() # Create all the rigid bodies. mradius = 0.5 density = 1000 # Create a texture asset. It can be shared between bodies. textureasset = chrono.ChTexture( chrono.GetChronoDataFile("textures/bluewhite.png")) # Create some spheres that roll horizontally, with increasing rolling friction values for bi in range(10): mat = chrono.ChMaterialSurfaceNSC() mat.SetFriction(0.4) mat.SetRollingFriction((float(bi) / 10.) * 0.05) msphereBody = chrono.ChBodyEasySphere( mradius, # radius size 1000, # density True, # visualization? True, # collision? mat) # contact material # Set some properties
def run_sim(traits, trial_num, gen_num, difficulty_level): my_system = chrono.ChSystemNSC() # Set the default outward/inward shape margins for collision detection chrono.ChCollisionModel.SetDefaultSuggestedEnvelope(0.001) chrono.ChCollisionModel.SetDefaultSuggestedMargin(0.001) # Sets simulation precision my_system.SetMaxItersSolverSpeed(70) # Create a contact material (surface property)to share between all objects. rollfrict_param = 0.5 / 10.0 * 0.05 brick_material = chrono.ChMaterialSurfaceNSC() brick_material.SetFriction(0.5) brick_material.SetDampingF(0.2) brick_material.SetCompliance(0.0000001) brick_material.SetComplianceT(0.0000001) brick_material.SetRollingFriction(rollfrict_param) brick_material.SetSpinningFriction(0.00000001) brick_material.SetComplianceRolling(0.0000001) brick_material.SetComplianceSpinning(0.0000001) # Create the set of bricks in a vertical stack, along Y axis block_bodies = [] # visualizes bodies block_shapes = [] # geometry purposes current_y = 0 for block_index in range(0, 4): size_brick_x = traits[block_index][0] size_brick_z = traits[block_index][1] size_brick_y = traits[block_index][2] if size_brick_y < settings.MIN_DIMENSIONS_THRESHOLD or size_brick_x < settings.MIN_DIMENSIONS_THRESHOLD or size_brick_z < settings.MIN_DIMENSIONS_THRESHOLD: return [-50, traits] mass_brick = settings.BLOCK_MASS inertia_brick_xx = 1 / 12 * mass_brick * (pow(size_brick_z, 2) + pow(size_brick_y, 2)) inertia_brick_yy = 1 / 12 * mass_brick * (pow(size_brick_x, 2) + pow(size_brick_z, 2)) inertia_brick_zz = 1 / 12 * mass_brick * (pow(size_brick_x, 2) + pow(size_brick_y, 2)) body_brick = chrono.ChBody() body_brick.SetPos(chrono.ChVectorD(0, current_y + 0.5 * size_brick_y, 0)) # set initial position current_y += size_brick_y # set tower block positions # setting mass properties body_brick.SetMass(mass_brick) body_brick.SetInertiaXX(chrono.ChVectorD(inertia_brick_xx, inertia_brick_yy, inertia_brick_zz)) # set collision surface properties body_brick.SetMaterialSurface(brick_material) # Collision shape body_brick.GetCollisionModel().ClearModel() body_brick.GetCollisionModel().AddBox(size_brick_x / 2, size_brick_y / 2, size_brick_z / 2) # must set half sizes body_brick.GetCollisionModel().BuildModel() body_brick.SetCollide(True) # Visualization shape, for rendering animation body_brick_shape = chrono.ChBoxShape() body_brick_shape.GetBoxGeometry().Size = chrono.ChVectorD(size_brick_x / 2, size_brick_y / 2, size_brick_z / 2) if block_index % 2 == 0: body_brick_shape.SetColor(chrono.ChColor(0.65, 0.65, 0.6)) # set gray color only for odd bricks body_brick.GetAssets().push_back(body_brick_shape) my_system.Add(body_brick) block_bodies.append(body_brick) block_shapes.append(body_brick_shape); # Create the room floor body_floor = chrono.ChBody() body_floor.SetBodyFixed(True) body_floor.SetPos(chrono.ChVectorD(0, -2, 0)) body_floor.SetMaterialSurface(brick_material) # Floor's collision shape body_floor.GetCollisionModel().ClearModel() body_floor.GetCollisionModel().AddBox(3, 1, 3) # hemi sizes default: 3,1,3 body_floor.GetCollisionModel().BuildModel() body_floor.SetCollide(True) # Visualization shape 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(chrono.GetChronoDataPath() + 'concrete.jpg') body_floor.GetAssets().push_back(body_floor_texture) my_system.Add(body_floor) # Create the shaking table, as a box size_table_x = 1 size_table_y = 0.2 size_table_z = 1 body_table = chrono.ChBody() body_table.SetPos(chrono.ChVectorD(0, -size_table_y / 2, 0)) body_table.SetMaterialSurface(brick_material) # Collision shape body_table.GetCollisionModel().ClearModel() body_table.GetCollisionModel().AddBox(size_table_x / 2, size_table_y / 2, size_table_z / 2) # hemi sizes body_table.GetCollisionModel().BuildModel() body_table.SetCollide(True) # Visualization shape body_table_shape = chrono.ChBoxShape() body_table_shape.GetBoxGeometry().Size = chrono.ChVectorD(size_table_x / 2, size_table_y / 2, size_table_z / 2) body_table_shape.SetColor(chrono.ChColor(0.4, 0.4, 0.5)) body_table.GetAssets().push_back(body_table_shape) body_table_texture = chrono.ChTexture() # body_table_texture.SetTextureFilename(chrono.GetChronoDataPath() + 'concrete.jpg') body_table.GetAssets().push_back(body_table_texture) my_system.Add(body_table) # Makes the table shake link_shaker = chrono.ChLinkLockLock() # link_shaker.SetMotion_X() link_shaker.Initialize(body_table, body_floor, chrono.CSYSNORM) my_system.Add(link_shaker) # ..create the function for imposed y vertical motion, etc. mfunY = chrono.ChFunction_Sine(0, settings.TABLE_FREQ_Y, settings.TABLE_AMP_Y) # phase, frequency, amplitude # ..create the function for imposed z horizontal motion, etc. mfunZ = chrono.ChFunction_Sine(0, settings.TABLE_FREQ_Z, settings.TABLE_AMP_Z) # phase, frequency, amplitude # ..create the function for imposed x horizontal motion, etc. mfunX = chrono.ChFunction_Sine(2, 0, 0) # phase, frequency, amplitude print("Sim env_level " + str(difficulty_level)) if difficulty_level == settings.SHAKE_IN_X_AXIS_LEVEL: mfunX = chrono.ChFunction_Sine(2, settings.TABLE_FREQ_X, settings.TABLE_AMP_X) # phase, frequency, amplitude elif difficulty_level >= settings.SHAKE_IN_X_AND_Z_AXIS_LEVEL: increment = 0.25 * difficulty_level mfunX = chrono.ChFunction_Sine(2, settings.TABLE_FREQ_X + increment, settings.TABLE_AMP_X) # phase, frequency, amplitude mfunZ = chrono.ChFunction_Sine(0, settings.TABLE_FREQ_Z + increment, settings.TABLE_AMP_Z) # phase, frequency, amplitude link_shaker.SetMotion_Y(mfunY) link_shaker.SetMotion_Z(mfunZ) link_shaker.SetMotion_X(mfunX) # --------------------------------------------------------------------- # # Create an Irrlicht application to visualize the system window_name = "Tower Trial: " + str(trial_num) + " Gen: " + str(gen_num) if trial_num == -1: window_name = "Initializing Population..." app = chronoirr.ChIrrApp(my_system, window_name, chronoirr.dimension2du(settings.SCREEN_WIDTH, settings.SCREEN_HEIGHT)) app.AddTypicalSky() app.AddTypicalLogo(chrono.GetChronoDataPath() + 'logo_pychrono_alpha.png') app.AddTypicalCamera(chronoirr.vector3df(settings.CAMERA_X, settings.CAMERA_Y, settings.CAMERA_Z)) app.AddLightWithShadow(chronoirr.vector3df(2, 4, 2), # point chronoirr.vector3df(0, 0, 0), # aimpoint 9, # radius (power) 1, 9, # near, far 30) # Committing visualization app.AssetBindAll() app.AssetUpdateAll(); app.AddShadowAll(); # --------------------------------------------------------------------- # # Run the simulation. This is where all of the constraints are set # app.SetTimestep(settings.SPEED) app.SetTryRealtime(True) app.GetDevice().run() fitness = 0 brick1_init = block_bodies[0].GetPos().y brick2_init = block_bodies[1].GetPos().y brick3_init = block_bodies[2].GetPos().y brick4_init = block_bodies[3].GetPos().y # Highest while True: brick1_curr_height = block_bodies[0].GetPos().y brick2_curr_height = block_bodies[1].GetPos().y brick3_curr_height = block_bodies[2].GetPos().y brick4_curr_height = block_bodies[3].GetPos().y # Highest # Break conditions if my_system.GetChTime() > settings.SIMULATION_RUNTIME: break if my_system.GetChTime() > settings.SIMULATION_RUNTIME / 2: mfunX = chrono.ChFunction_Sine(2, 0, 0) mfunZ = chrono.ChFunction_Sine(2, 0, 0) link_shaker.SetMotion_Z(mfunZ) link_shaker.SetMotion_X(mfunX) # If the blocks fall out of line if brick1_init - brick1_curr_height > settings.CANCEL_SIM_THRESHOLD or \ brick2_init - brick2_curr_height > settings.CANCEL_SIM_THRESHOLD or \ brick3_init - brick3_curr_height > settings.CANCEL_SIM_THRESHOLD or \ brick4_init - brick4_curr_height > settings.CANCEL_SIM_THRESHOLD: break # Record fitness every 1/1000 of the runtime if 0.01 > my_system.GetChTime() % ((1 / 1000) * settings.SIMULATION_RUNTIME) > 0: if settings.FITNESS_FUNCTION == settings.Fitness.SumLengths: # Sum of size_y fitness += block_shapes[0].GetBoxGeometry().GetLengths().y + \ block_shapes[1].GetBoxGeometry().GetLengths().y + \ block_shapes[2].GetBoxGeometry().GetLengths().y + \ block_shapes[3].GetBoxGeometry().GetLengths().y elif settings.FITNESS_FUNCTION == settings.Fitness.MaxPosition: # Max of y positions fitness += max(block_bodies[0].GetPos().y, block_bodies[1].GetPos().y, block_bodies[2].GetPos().y, block_bodies[3].GetPos().y) elif settings.FITNESS_FUNCTION == settings.Fitness.MaxPositionSumLengths: # Max * sum of sizes fitness += max(block_bodies[0].GetPos().y, block_bodies[1].GetPos().y, block_bodies[2].GetPos().y, block_bodies[3].GetPos().y) * \ block_shapes[0].GetBoxGeometry().GetLengths().y + \ block_shapes[1].GetBoxGeometry().GetLengths().y + \ block_shapes[2].GetBoxGeometry().GetLengths().y + \ block_shapes[3].GetBoxGeometry().GetLengths().y app.BeginScene() app.DrawAll() for substep in range(0, 5): app.DoStep() app.EndScene() app.GetDevice().closeDevice() print("Fitness: " + str(fitness) + " Gen: " + str(gen_num)) return [fitness, traits]