Here's an example of simulating "motors" in ODE. I'm not using the AMotor at all, just applying a velocity on a hinge joint...
In this demo some robotic critters try to crawl over obstacles including (breakable) tables! :)
Danny
In this demo some robotic critters try to crawl over obstacles including (breakable) tables! :)
Include "JV-ODE.bb" ; DANNY's robotic crawlers test Const ANGTHRESH# = 0.1 ;default auto disable angular threshold Const LINTHRESH# = 0.1 ;default auto disable linear threshold Const STEPTHRESH = 1 Graphics3D 800,600,0,2 Type odeGeom Field body Field geom Field mesh Field isCrawler End Type Type odeJoint Field joint Field speed# End Type ; ################################################################################################### ; ### Setup ODE Global World=dWorldCreate() Global Space=dHashSpaceCreate(0) Global ContactGroup=dJointGroupCreate(0) dWorldSetAutoDisableFlag(World,1) dWorldSetGravity(World,0,-0.48,0) dWorldSetERP World, 0.25 dContactSetMaxContacts 64 dContactSetMode dContactBounce dContactSetBounce 0.1 dContactSetMu 30 ; High Performance Settings / causes objects to settle down and sleep faster dWorldSetAutoDisableLinearThreshold World, .05 dWorldSetAutoDisableAngularThreshold World, .05 dWorldSetAutoDisableSteps World,20 dWorldSetQuickStepNumIterations World,2 SeedRnd 666 Global camera, camerapivot, Crawler Create3D() ;add some tables For i = -6 To 6 Step 2 addTable i,1,0, 1.5, 0.5, 0.75 Next ;add some obstacles For i = 1 To 50 AddObject Rand(-8,8),Rand(2,10) Next ;pre-roll physics-get everything on the floor! ClsColor 0,0,0 Cls Color 255,255,255 Print "Pre-rolling physics..." For i = 0 To 400 dSpaceCollide(Space,World,ContactGroup) dWorldQuickStep(World,0.025) dJointGroupEmpty(ContactGroup) Next ;add some crawlers w# = 0.5 h# = 0.15 d# = 2 isFirst = True numSegments=4 For i = -2 To 6 Step 3 numSegments = numSegments+1 c = AddCrawler(i,1,-2, w#,h#,d#, numSegments, 1) If isFirst Then isFirst = False crawler = c EndIf Next ;reset mouse position MoveMouse GraphicsWidth()/2, GraphicsHeight()/2 ; ################################################################################################### ; Main Loop While Not KeyHit(1) ;---------------------------------------------------------- ; Keyboard UpdateControls() ;----------------------------------------------- ; Physics ; ; Run Physics every 20ms ( 50 Times/sec ) T=MilliSecs() If T > Time3 + 10 Then Time3=T PTime=MilliSecs() UpdateGeoms() For x=1 To 2 ; Increase to improve resolution dSpaceCollide(Space,World,ContactGroup) dWorldQuickStep(World,0.05) dJointGroupEmpty(ContactGroup) Next PhysicsTime#=MilliSecs()-PTime UpdateJointlimits(3) End If ;----------------------------------------------- ; FPS If T >= Time4 + 1000 Then Time4=T FPS=Frames frames=0 End If frames=frames+1 ;----------------------------------------------- ; render world RenderWorld ;----------------------------------------------- ; debug Color 50,255,50 Text 5,0,"JV-ODE Version "+dGetVersion() Text 5,15,"Physics Time:"+PhysicsTime Text 5,30,"FPS: "+FPS Text GraphicsWidth()/2,GraphicsHeight()-20,"Use mouse to turn camera, use cursor keys to move", 1 Text GraphicsWidth()/2,GraphicsHeight()-35,"Press ENTER to dump more boxes on the crawlers", 1 Text GraphicsWidth()/2,GraphicsHeight()-50,"Press SPACEBAR to kick the crawlers", 1 ;show Flip 0 Wend dJointGroupDestroy(ContactGroup) dSpaceDestroy(Space) dWorldDestroy(World) dCloseODE() End ; ################################################################################################### Function create3D() ;| Default 3d scene ; ### Create light Light=CreateLight() RotateEntity Light,45,-90,0 LightColor Light,255,255,255 AmbientLight 130,130,130 ; ### Create camera camerapivot = CreatePivot() Camera=CreateCamera(camerapivot) CameraZoom camera, 1.5 CameraClsColor Camera,0,0,0 CameraRange Camera,0.01,250 PositionEntity Camera,-5,7,-7 PositionEntity camerapivot, 0,1,0 PointEntity camera, camerapivot ;make top text readable CameraFogColor camera, 0,0,0 CameraFogRange camera, 5,100 CameraFogMode camera, 1 ; ### Create plane dCreatePlane(Space,0,1,0,0) Plane=CreatePlane() EntityAlpha Plane,0.90 PlaneTexture=CreateTexture(128,128,9) ClsColor 0,0,0 Cls Color 240,240,240 Rect 0,0,64,64,1 Rect 64,64,64,64,1 CopyRect 0,0,128,128,0,0,BackBuffer(),TextureBuffer(PlaneTexture) ScaleTexture PlaneTexture,5,5 EntityTexture Plane,PlaneTexture,0,0 Mirror=CreateMirror() ClsColor 0,0,0 Color 255,255,255 End Function ; ################################################################################################### Function addCrawler( x#,y#,z#, w#,h#,d#, segments=2, speed#=1.25 ) ;segment depth# sd# = d#/segments ; space between body segments - the larger this is, the more shaky and springy the bodies will be segSpace# = 0.005 ;foot dimensions# fw# = w# * 0.50 fh# = h# * 0.50 fd# = sd# * 2.00 isFirst = True For i = 0 To segments-1 ;create main body segment Segment.odeGeom = AddRigidBox( x#,y#,z#-sd#*i, w#,h#,sd#-segSpace#*2, 0,0,0, 255,240,200, 0.25) ;remember the first segment as 'the head' If isFirst Then isFirst = False MainBody = Segment\body MainMesh = Segment\mesh Segment\isCrawler = True PrevBody.odeGeom = Segment ;color the head EntityColor Segment\mesh, 255,50,0 Else ;if not the head, connect this segment to the previous one! joint = dJointCreateHinge(World, 0) dJointAttach joint, prevBody\body, Segment\body dJointSetHingeAnchor joint, x#, y#, z#-sd#*i+sd#/2 dJointSetHingeAxis joint, 1,0,0 dJointSetHingeParam joint, dParamLoStop, -0.05 * 3.141 ; limit the angle between segments dJointSetHingeParam joint, dParamHiStop, 0.10 * 3.141 ; else he'll break his back ;for the next connection PrevBody.odeGeom = Segment EndIf ;add feet If i=0 Or i=segments-1 Then ;add feet LeftFoot.odeGeom = AddRigidBox( x#-w#/2-fw#/2-segSpace#*2,y#,z#-sd#*i-sd#/2, fw#,fh#,fd#, 0,0,0, 220,160,52, 0.25) RightFoot.odeGeom = AddRigidBox( x#+w#/2+fw#/2+segSpace#*2,y#,z#-sd#*i-sd#/2, fw#,fh#,fd#, 0,0,0, 220,160,52, 0.25) ;add joints AddMotorJoint segment\body, leftFoot\body, x#-w#/2-segSpace#, y#, z#-sd#*i-sd#/2, speed# AddMotorJoint segment\body, rightFoot\body, x#+w#/2+segSpace#, y#, z#-sd#*i-sd#/2, speed# EndIf Next ;return front mesh for camera control Return MainMesh End Function ; ################################################################################################### Function addMotorJoint( BodyA, BodyB, x#,y#,z#, speed#) j.odeJoint = New odeJoint j\speed# = speed# j\joint = dJointCreateHinge(World,0) dJointAttach j\joint, bodyA, bodyB dJointSetHingeAnchor j\joint, x#, y#, z# dJointSetHingeAxis j\joint, -1,0,0 End Function ; ################################################################################################### Function addRigidBox.odeGeom( x#=0,y#=0,z#=0, w#=1,h#=1,d#=1, xr#=0,yr#=0,zr#=0, r=200,g=200,b=200, weight#=0) ;add rigid box ode.odeGeom = New odeGeom ode\body=dBodyCreate(World) dBodySetPosition ode\body, x#,y#,z# dBodySetRotation ode\body, xr#,yr#,zr# ode\geom = dCreateBox(Space, w#,h#,d#) dGeomSetBody ode\geom, ode\body ode\mesh = CreateCube() ScaleMesh ode\mesh, w#/2,h#/2,d#/2 PositionEntity ode\mesh, x#,y#,z# EntityColor ode\mesh, r,g,b If weight#<>0 Then mass = dMassCreate() dMassSetBoxTotal mass, weight#, w#,h#,d# dBodySetMass ode\body, mass dMassDestroy mass EndIf Return ode End Function ; ################################################################################################### Function addObject(xp#=666,zp#=666) size# = Rnd#(0.25,1.00) range# = 0.5 yp#=Rand(5,10) If xp#=666 Then xp#=Rand(-range#,range#) zp#=Rand(-range#,range#) EndIf c=Rand(150)+105 addRigidBox(xp#,yp#,zp#, size#,size#/2,size#, c,c,c, c/2,0,c, 5) End Function ; ################################################################################################### Function addTable(xp#, yp#, zp#, w#,h#,d#) ;# Creates a table with 4 legs - breakable legs! :) w# = Rnd(w#*0.75,w#*1.25) ;add some variation to the table's dimensions h# = Rnd(h#*0.75,h#*1.25) d# = Rnd(d#*0.75,d#*1.25) ;table top dimensions th# = h# * 0.1 ;leg dimensions lw# = w#*0.1 lh# = h#*0.9 ld# = d#*0.1 ;create table top ode.odeGeom=addRigidBox( xp#,yp#,zp#, w#,th#,d#, 0,0,0, 100,050,000, 0.5) tableBody = ode\body ;create leg 1 ode.odeGeom=addRigidBox( xp#-w#/2+lw#/2, yp#-lh#/2-th#/2, zp#-d#/2+ld#/2, lw#,lh#,ld#, 0,0,0, 200,100,000, 0.5) ;join the legs to the table joint = dJointCreateFixed(world,0) dJointAttach joint, tableBody, ode\body dJointSetFixed joint dJointFeedbackEnable(Joint) ; Allow feedback for breakable joints ;create leg 2 ode.odeGeom=addRigidBox( xp#+w#/2-lw#/2, yp#-lh#/2-th#/2, zp#-d#/2+ld#/2, lw#,lh#,ld#, 0,0,0, 200,100,000, 0.5) ;join the legs to the table joint = dJointCreateFixed(world,0) dJointAttach joint, tableBody, ode\body dJointSetFixed joint dJointFeedbackEnable(Joint) ; Allow feedback for breakable joints ;create leg 3 ode.odeGeom=addRigidBox( xp#+w#/2-lw#/2, yp#-lh#/2-th#/2, zp#+d#/2-ld#/2, lw#,lh#,ld#, 0,0,0, 200,100,000, 0.5) ;join the legs to the table joint = dJointCreateFixed(world,0) dJointAttach joint, tableBody, ode\body dJointSetFixed joint dJointFeedbackEnable(Joint) ; Allow feedback for breakable joints ;create leg 4 ode.odeGeom=addRigidBox( xp#-w#/2+lw#/2, yp#-lh#/2-th#/2, zp#+d#/2-ld#/2, lw#,lh#,ld#, 0,0,0, 200,100,000, 0.5) ;join the legs to the table joint = dJointCreateFixed(world,0) dJointAttach joint, tableBody, ode\body dJointSetFixed joint dJointFeedbackEnable(Joint) ; Allow feedback for breakable joints End Function ; ################################################################################################### Function UpdateGeoms() For ode.ODEGeom=Each ODEGeom RotateEntity ode\mesh, dGeomGetPitch#(ode\geom),dGeomGetYaw#(ode\geom),dGeomGetRoll#(ode\geom) PositionEntity ode\mesh, dGeomGetPositionX#(ode\geom),dGeomGetPositionY#(ode\geom),dGeomGetPositionZ#(ode\geom) Next ;motors For joint.odeJoint = Each odeJoint If joint\speed# <> 0 Then ; make sure it has more energy than it needs to get to speed# ; (to give it strength enough when burried under stuff or when in tricky situations) dJointSetHingeParam joint\joint, dparamFMax, joint\speed# ; desired velocity (angular) dJointSetHingeParam joint\joint, dparamVel, joint\speed# EndIf Next End Function ; ################################################################################################### Function UpdateControls() ;SPACE - kick crawlers! If KeyHit(57) Then For ode.odeGeom = Each odeGeom If ode\isCrawler Then x#=Rnd( 5,15) y#=Rnd(10,50) z#=Rnd( 5,15) dBodyAddForce ode\body, x#,y#,z# dBodyEnable ode\body EndIf Next EndIf ;ENTER - dump more bricks on crawler(s) :) If KeyDown(28) Then For ode.odeGeom = Each odeGeom If ode\isCrawler Then crawler = ode\mesh x# = Rand(EntityX(crawler)-1, EntityX(crawler)+1) z# = Rand(EntityZ(crawler)-1, EntityZ(crawler)+1) addObject x#,z# EndIf Next EndIf ; cursor - Forward If KeyDown(17) Or KeyDown(200) Then MoveEntity camera,0,0,1 ; cursor - Reverse If KeyDown(31) Or KeyDown(208) Then MoveEntity camera,0,0,-1 ; cursor - Left If KeyDown(30) Or KeyDown(203) Then MoveEntity camera,-1,0,0 ; cursor - Right If KeyDown(32) Or KeyDown(205) Then MoveEntity camera,1,0,0 If EntityY#(camera,1)<0.10 Then PositionEntity camera, EntityX#(camera,1), 0.10, EntityZ#(camera,1),1 ;----------------------------------------------- ; Mouse look around mx_speed# = MouseXSpeed() * 0.1250 my_speed# = MouseYSpeed() * 0.0625 If mx_speed#<>0 Or my_speed#<>0 Then TurnEntity camera, my_speed, 0, 0, 0 TurnEntity camera, 0, -mx_speed, 0, 1 MoveMouse GraphicsWidth()/2, GraphicsHeight()/2 EndIf End Function ; ################################################################################################### Function UpdateJointLimits(limit#) For ode.ODEGeom=Each ODEGeom If dGeomIsEnabled(ode\geom) numjoints=dBodyGetNumJoints(ode\body) If numjoints>0 For joints=0 To numjoints-1 joint=dBodyGetJoint(ode\body,joints) If joint<>0 ; valid joints only If dJointFeedbackIsEnabled(Joint)=1 ; Get linear velocity vector dJointGetFeedbackForce1(Joint) totForce# = Abs(dVectorX()) + Abs(dVectorY()) + Abs(dVectorZ()) If totForce#>limit dJointAttach(joint,0,0) ; break joint Else dJointGetFeedbackForce2(Joint) totForce# = Abs(dVectorX()) + Abs(dVectorY()) + Abs(dVectorZ()) If totForce#>limit dJointAttach(joint,0,0) ; Break joint Else dJointGetFeedbackTorque1(Joint) totForce# = Abs(dVectorX()) + Abs(dVectorY()) + Abs(dVectorZ()) If totForce#>limit dJointAttach(joint,0,0) ; Break joint Else dJointGetFeedbackTorque2(Joint) totForce# = Abs(dVectorX()) + Abs(dVectorY()) + Abs(dVectorZ()) If totForce#>limit dJointAttach(joint,0,0) ; Break joint End If End If End If End If End If End If Next End If End If Next End Function
Danny
