void sODEJoint::SetHinge2Limit( float axis1low, float axis1high, float axis2low, float axis2high ) { dJointSetHinge2Param( oJoint, dParamLoStop, axis1low ); dJointSetHinge2Param( oJoint, dParamHiStop, axis1high ); dJointSetHinge2Param( oJoint, dParamLoStop2, axis2low ); dJointSetHinge2Param( oJoint, dParamHiStop2, axis2high ); }
void Machine::adjuststeer(void) { dReal v = steer - dJointGetHinge2Angle1 (wheeljoint[0]); if (v > 0.1) v = 0.1; if (v < -0.1) v = -0.1; v *= 50.0; dJointSetHinge2Param (wheeljoint[0],dParamVel,v); dJointSetHinge2Param (wheeljoint[0],dParamFMax,8000); dJointSetHinge2Param (wheeljoint[0],dParamLoStop,-0.6); dJointSetHinge2Param (wheeljoint[0],dParamHiStop,0.666666); dJointSetHinge2Param (wheeljoint[0],dParamFudgeFactor,0.1); }
void WheelPiece::activate() { switch(activationDirection) { case 0: dJointSetHinge2Param (connectingJoint,dParamVel2,-2); break; case 1: dJointSetHinge2Param (connectingJoint,dParamVel2,2); break; } dJointSetHinge2Param (connectingJoint,dParamVel2,-2); dJointSetHinge2Param (connectingJoint,dParamFMax2,0.7); }
void Machine::setengine(int mode) { int i; if(mode==MACHINE_ENGINE_COAST) { for(i=0; i<3; i++) dJointSetHinge2Param(wheeljoint[i], dParamFMax2, 0); } if(mode==MACHINE_ENGINE_BRAKE) { for(i=0; i<3; i++) { dJointSetHinge2Param(wheeljoint[i], dParamVel2, 0); dJointSetHinge2Param(wheeljoint[i], dParamFMax2, 10000); } } if(mode==MACHINE_ENGINE_FORWARD) { for(i=0; i<3; i++) { dJointSetHinge2Param(wheeljoint[i], dParamVel2, 30); dJointSetHinge2Param(wheeljoint[i], dParamFMax2, 1500); } } if(mode==MACHINE_ENGINE_REVERSE) { for(i=0; i<3; i++) { dJointSetHinge2Param(wheeljoint[i], dParamVel2, -20); dJointSetHinge2Param(wheeljoint[i], dParamFMax2, 1500); } } }
bool Simulator::SimulateOneStep(const double speed, const double steer) { //set desired speed dJointSetHinge2Param(m_robotJoints[0], dParamVel2, -speed); // hinge-2 velocity = -speed dJointSetHinge2Param(m_robotJoints[0], dParamFMax2, 10); // maximum torque = 0.1 //set desired steering angle dReal v = steer - dJointGetHinge2Angle1 (m_robotJoints[0]); //if (v > 0.1) v = 0.1; //if (v < -0.1) v = -0.1; v *= 10.0; dJointSetHinge2Param(m_robotJoints[0], dParamVel, v); dJointSetHinge2Param(m_robotJoints[0], dParamFMax, 0.2); dJointSetHinge2Param(m_robotJoints[0], dParamLoStop, -0.75); dJointSetHinge2Param(m_robotJoints[0], dParamHiStop, 0.75); dJointSetHinge2Param(m_robotJoints[0], dParamFudgeFactor, 0.1); //simulate the dynamics for one time step (set to 0.05) m_collision = false; dSpaceCollide(m_space, reinterpret_cast<void *>(this), &CollisionCheckingCallbackFn); dWorldStep(m_world, 0.2); // setting time step for integral dJointGroupEmpty(m_contacts); return !m_collision; }
void WheelPiece::attachToBase(dBodyID otherBody, dWorldID world, dJointGroupID jointGroup, dReal x, dReal y, dReal z, const dMatrix3 rotationMatrix) { // set this piece position and rotation dBodySetPosition(body, x, y, z); dBodySetRotation(body, rotationMatrix); // create and connect connectingJoint = dJointCreateHinge2(world,0); dJointAttach (connectingJoint,otherBody,body); // set anchor, axes, and lo and hi stops const dReal *a = dBodyGetPosition (body); dJointSetHinge2Anchor (connectingJoint,a[0],a[1],a[2]); dJointSetHinge2Axis1 (connectingJoint,rotationMatrix[1],rotationMatrix[5],rotationMatrix[9]); dJointSetHinge2Axis2 (connectingJoint,rotationMatrix[2],rotationMatrix[6],rotationMatrix[10]); dJointSetHinge2Param (connectingJoint,dParamLoStop,0); dJointSetHinge2Param (connectingJoint,dParamHiStop,0); }
static void set_phys_joint_attr(dJointID j, int p, float v) { switch (dJointGetType(j)) { case dJointTypeHinge: dJointSetHingeParam (j, p, v); break; case dJointTypeSlider: dJointSetSliderParam (j, p, v); break; case dJointTypeHinge2: dJointSetHinge2Param (j, p, v); break; case dJointTypeUniversal: dJointSetUniversalParam(j, p, v); break; default: break; } }
void Buggy::doDynamics(dBodyID body) { //dReal *v = (dReal *)dBodyGetLinearVel(body); //dJointSetHinge2Param (carjoint[0],dParamVel,getThrottle()); //dJointSetHinge2Param (carjoint[0],dParamFMax,1000); dJointSetHinge2Param (carjoint[0],dParamVel2,yRotAngle); dJointSetHinge2Param (carjoint[0],dParamFMax2,1000); dJointSetHinge2Param (carjoint[1],dParamVel2,yRotAngle); dJointSetHinge2Param (carjoint[1],dParamFMax2,1000); // steering dReal v = dJointGetHinge2Angle1 (carjoint[0]); if (v > 0.1) v = 0.1; if (v < -0.1) v = -0.1; v *= 10.0; //dJointSetHinge2Param (carjoint[0],dParamVel,0); //dJointSetHinge2Param (carjoint[0],dParamFMax,1000); //dJointSetHinge2Param (carjoint[0],dParamLoStop,-0.75); //dJointSetHinge2Param (carjoint[0],dParamHiStop,0.75); //dJointSetHinge2Param (carjoint[0],dParamFudgeFactor,0.1); //dBodyAddRelForce (body,0, 0,getThrottle()); // This should be after the world step /// stuff dVector3 result; dBodyVectorToWorld(body, 0,0,1,result); setForward(result[0],result[1],result[2]); const dReal *dBodyPosition = dBodyGetPosition(body); const dReal *dBodyRotation = dBodyGetRotation(body); setPos(dBodyPosition[0],dBodyPosition[1],dBodyPosition[2]); setLocation((float *)dBodyPosition, (float *)dBodyRotation); }
void Robots::conectarChassiRodas(dWorldID world) { const dReal *a; for (int i = 0; i < 2; i++) { this->joint[i] = dJointCreateHinge2(world,0); dJointAttach(this->joint[i],this->body[0],this->wheel[i]); a = dBodyGetPosition (this->wheel[i]); dJointSetHinge2Anchor(this->joint[i],a[0],a[1],a[2]); dJointSetHinge2Axis1(this->joint[i],0,0,1); dJointSetHinge2Axis2(this->joint[i],0,1,0); // set joint suspension dJointSetHinge2Param(this->joint[i],dParamSuspensionERP,0.4); //set stops to make sure wheels always stay in alignment dJointSetHinge2Param(this->joint[i],dParamLoStop,0); dJointSetHinge2Param(this->joint[i],dParamHiStop,0); } }
static void simLoop (int pause) { int i; if (!pause) { // motor dJointSetHinge2Param (joint[0],dParamVel2,-speed); dJointSetHinge2Param (joint[0],dParamFMax2,0.1); // steering dReal v = steer - dJointGetHinge2Angle1 (joint[0]); if (v > 0.1) v = 0.1; if (v < -0.1) v = -0.1; v *= 10.0; dJointSetHinge2Param (joint[0],dParamVel,v); dJointSetHinge2Param (joint[0],dParamFMax,0.2); dJointSetHinge2Param (joint[0],dParamLoStop,-0.75); dJointSetHinge2Param (joint[0],dParamHiStop,0.75); dJointSetHinge2Param (joint[0],dParamFudgeFactor,0.1); dSpaceCollide (space,0,&nearCallback); dWorldStep (world,0.05); // remove all contact joints dJointGroupEmpty (contactgroup); } dsSetColor (0,1,1); dsSetTexture (DS_WOOD); dReal sides[3] = {LENGTH,WIDTH,HEIGHT}; dsDrawBox (dBodyGetPosition(body[0]),dBodyGetRotation(body[0]),sides); dsSetColor (1,1,1); for (i=1; i<=3; i++) dsDrawCylinder (dBodyGetPosition(body[i]), dBodyGetRotation(body[i]),0.02f,RADIUS); dVector3 ss; dGeomBoxGetLengths (ground_box,ss); dsDrawBox (dGeomGetPosition(ground_box),dGeomGetRotation(ground_box),ss); /* printf ("%.10f %.10f %.10f %.10f\n", dJointGetHingeAngle (joint[1]), dJointGetHingeAngle (joint[2]), dJointGetHingeAngleRate (joint[1]), dJointGetHingeAngleRate (joint[2])); */ }
void CCar::SWheelSteer::Init() { CKinematics* pKinematics=smart_cast<CKinematics*>(pwheel->car->Visual()); pwheel->Init(); (bone_map.find(pwheel->bone_id))->second.joint->GetLimits(lo_limit,hi_limit,0); CBoneData& bone_data= pKinematics->LL_GetData(u16(pwheel->bone_id)); switch(bone_data.IK_data.type) { case jtWheel: pos_right=bone_map.find(pwheel->bone_id)->second.element->mXFORM.i.y;//.dotproduct(pwheel->car->m_root_transform.j); break; default: NODEFAULT; } pos_right=pos_right>0.f ? -1.f : 1.f; float steering_torque=pKinematics->LL_UserData()->r_float("car_definition","steering_torque"); pwheel->ApplySteerAxisTorque(steering_torque); dJointSetHinge2Param(pwheel->joint->GetDJoint(), dParamFudgeFactor, 0.005f/steering_torque); pwheel->ApplySteerAxisVel(0.f); limited=false; }
void TSRODESteeringJoint::SetMaxForce( float _fForceMax ) { dJointSetHinge2Param( m_ODEJoint.m_JointID, dParamFMax, _fForceMax ); }
static void simLoop (int pause) { int i, j; dsSetTexture (DS_WOOD); if (!pause) { #ifdef BOX dBodyAddForce(body[bodies-1],lspeed,0,0); #endif for (j = 0; j < joints; j++) { dReal curturn = dJointGetHinge2Angle1 (joint[j]); //dMessage (0,"curturn %e, turn %e, vel %e", curturn, turn, (turn-curturn)*1.0); dJointSetHinge2Param(joint[j],dParamVel,(turn-curturn)*1.0); dJointSetHinge2Param(joint[j],dParamFMax,dInfinity); dJointSetHinge2Param(joint[j],dParamVel2,speed); dJointSetHinge2Param(joint[j],dParamFMax2,FMAX); dBodyEnable(dJointGetBody(joint[j],0)); dBodyEnable(dJointGetBody(joint[j],1)); } if (doFast) { dSpaceCollide (space,0,&nearCallback); #if defined(QUICKSTEP) dWorldQuickStep (world,0.05); #elif defined(STEPFAST) dWorldStepFast1 (world,0.05,ITERS); #endif dJointGroupEmpty (contactgroup); } else { dSpaceCollide (space,0,&nearCallback); dWorldStep (world,0.05); dJointGroupEmpty (contactgroup); } for (i = 0; i < wb; i++) { b = dGeomGetBody(wall_boxes[i]); if (dBodyIsEnabled(b)) { bool disable = true; const dReal *lvel = dBodyGetLinearVel(b); dReal lspeed = lvel[0]*lvel[0]+lvel[1]*lvel[1]+lvel[2]*lvel[2]; if (lspeed > DISABLE_THRESHOLD) disable = false; const dReal *avel = dBodyGetAngularVel(b); dReal aspeed = avel[0]*avel[0]+avel[1]*avel[1]+avel[2]*avel[2]; if (aspeed > DISABLE_THRESHOLD) disable = false; if (disable) wb_stepsdis[i]++; else wb_stepsdis[i] = 0; if (wb_stepsdis[i] > DISABLE_STEPS) { dBodyDisable(b); dsSetColor(0.5,0.5,1); } else dsSetColor(1,1,1); } else dsSetColor(0.4,0.4,0.4); dVector3 ss; dGeomBoxGetLengths (wall_boxes[i], ss); dsDrawBox(dGeomGetPosition(wall_boxes[i]), dGeomGetRotation(wall_boxes[i]), ss); } } else { for (i = 0; i < wb; i++) { b = dGeomGetBody(wall_boxes[i]); if (dBodyIsEnabled(b)) dsSetColor(1,1,1); else dsSetColor(0.4,0.4,0.4); dVector3 ss; dGeomBoxGetLengths (wall_boxes[i], ss); dsDrawBox(dGeomGetPosition(wall_boxes[i]), dGeomGetRotation(wall_boxes[i]), ss); } } dsSetColor (0,1,1); dReal sides[3] = {LENGTH,WIDTH,HEIGHT}; for (i = 0; i < boxes; i++) dsDrawBox (dGeomGetPosition(box[i]),dGeomGetRotation(box[i]),sides); dsSetColor (1,1,1); for (i=0; i< spheres; i++) dsDrawSphere (dGeomGetPosition(sphere[i]), dGeomGetRotation(sphere[i]),RADIUS); // draw the cannon dsSetColor (1,1,0); dMatrix3 R2,R3,R4; dRFromAxisAndAngle (R2,0,0,1,cannon_angle); dRFromAxisAndAngle (R3,0,1,0,cannon_elevation); dMultiply0 (R4,R2,R3,3,3,3); dReal cpos[3] = {CANNON_X,CANNON_Y,1}; dReal csides[3] = {2,2,2}; dsDrawBox (cpos,R2,csides); for (i=0; i<3; i++) cpos[i] += 1.5*R4[i*4+2]; dsDrawCylinder (cpos,R4,3,0.5); // draw the cannon ball dsDrawSphere (dBodyGetPosition(cannon_ball_body),dBodyGetRotation(cannon_ball_body), CANNON_BALL_RADIUS); }
void resetSimulation() { int i; i = 0; // destroy world if it exists if (bodies) { dJointGroupDestroy (contactgroup); dSpaceDestroy (space); dWorldDestroy (world); } for (i = 0; i < 1000; i++) wb_stepsdis[i] = 0; // recreate world world = dWorldCreate(); // space = dHashSpaceCreate( 0 ); // space = dSimpleSpaceCreate( 0 ); space = dSweepAndPruneSpaceCreate( 0, dSAP_AXES_XYZ ); contactgroup = dJointGroupCreate (0); dWorldSetGravity (world,0,0,-1.5); dWorldSetCFM (world, 1e-5); dWorldSetERP (world, 0.8); dWorldSetQuickStepNumIterations (world,ITERS); ground = dCreatePlane (space,0,0,1,0); bodies = 0; joints = 0; boxes = 0; spheres = 0; wb = 0; #ifdef CARS for (dReal x = 0.0; x < COLS*(LENGTH+RADIUS); x += LENGTH+RADIUS) for (dReal y = -((ROWS-1)*(WIDTH/2+RADIUS)); y <= ((ROWS-1)*(WIDTH/2+RADIUS)); y += WIDTH+RADIUS*2) makeCar(x, y, bodies, joints, boxes, spheres); #endif #ifdef WALL bool offset = false; for (dReal z = WBOXSIZE/2.0; z <= WALLHEIGHT; z+=WBOXSIZE) { offset = !offset; for (dReal y = (-WALLWIDTH+z)/2; y <= (WALLWIDTH-z)/2; y+=WBOXSIZE) { wall_bodies[wb] = dBodyCreate (world); dBodySetPosition (wall_bodies[wb],-20,y,z); dMassSetBox (&m,1,WBOXSIZE,WBOXSIZE,WBOXSIZE); dMassAdjust (&m, WALLMASS); dBodySetMass (wall_bodies[wb],&m); wall_boxes[wb] = dCreateBox (space,WBOXSIZE,WBOXSIZE,WBOXSIZE); dGeomSetBody (wall_boxes[wb],wall_bodies[wb]); //dBodyDisable(wall_bodies[wb++]); wb++; } } dMessage(0,"wall boxes: %i", wb); #endif #ifdef BALLS for (dReal x = -7; x <= -4; x+=1) for (dReal y = -1.5; y <= 1.5; y+=1) for (dReal z = 1; z <= 4; z+=1) { b = dBodyCreate (world); dBodySetPosition (b,x*RADIUS*2,y*RADIUS*2,z*RADIUS*2); dMassSetSphere (&m,1,RADIUS); dMassAdjust (&m, BALLMASS); dBodySetMass (b,&m); sphere[spheres] = dCreateSphere (space,RADIUS); dGeomSetBody (sphere[spheres++],b); } #endif #ifdef ONEBALL b = dBodyCreate (world); dBodySetPosition (b,0,0,2); dMassSetSphere (&m,1,RADIUS); dMassAdjust (&m, 1); dBodySetMass (b,&m); sphere[spheres] = dCreateSphere (space,RADIUS); dGeomSetBody (sphere[spheres++],b); #endif #ifdef BALLSTACK for (dReal z = 1; z <= 6; z+=1) { b = dBodyCreate (world); dBodySetPosition (b,0,0,z*RADIUS*2); dMassSetSphere (&m,1,RADIUS); dMassAdjust (&m, 0.1); dBodySetMass (b,&m); sphere[spheres] = dCreateSphere (space,RADIUS); dGeomSetBody (sphere[spheres++],b); } #endif #ifdef CENTIPEDE dBodyID lastb = 0; for (dReal y = 0; y < 10*LENGTH; y+=LENGTH+0.1) { // chassis body b = body[bodies] = dBodyCreate (world); dBodySetPosition (body[bodies],-15,y,STARTZ); dMassSetBox (&m,1,WIDTH,LENGTH,HEIGHT); dMassAdjust (&m,CMASS); dBodySetMass (body[bodies],&m); box[boxes] = dCreateBox (space,WIDTH,LENGTH,HEIGHT); dGeomSetBody (box[boxes++],body[bodies++]); for (dReal x = -17; x > -20; x-=RADIUS*2) { body[bodies] = dBodyCreate (world); dBodySetPosition(body[bodies], x, y, STARTZ); dMassSetSphere(&m, 1, RADIUS); dMassAdjust(&m, WMASS); dBodySetMass(body[bodies], &m); sphere[spheres] = dCreateSphere (space, RADIUS); dGeomSetBody (sphere[spheres++], body[bodies]); joint[joints] = dJointCreateHinge2 (world,0); if (x == -17) dJointAttach (joint[joints],b,body[bodies]); else dJointAttach (joint[joints],body[bodies-2],body[bodies]); const dReal *a = dBodyGetPosition (body[bodies++]); dJointSetHinge2Anchor (joint[joints],a[0],a[1],a[2]); dJointSetHinge2Axis1 (joint[joints],0,0,1); dJointSetHinge2Axis2 (joint[joints],1,0,0); dJointSetHinge2Param (joint[joints],dParamSuspensionERP,1.0); dJointSetHinge2Param (joint[joints],dParamSuspensionCFM,1e-5); dJointSetHinge2Param (joint[joints],dParamLoStop,0); dJointSetHinge2Param (joint[joints],dParamHiStop,0); dJointSetHinge2Param (joint[joints],dParamVel2,-10.0); dJointSetHinge2Param (joint[joints++],dParamFMax2,FMAX); body[bodies] = dBodyCreate (world); dBodySetPosition(body[bodies], -30 - x, y, STARTZ); dMassSetSphere(&m, 1, RADIUS); dMassAdjust(&m, WMASS); dBodySetMass(body[bodies], &m); sphere[spheres] = dCreateSphere (space, RADIUS); dGeomSetBody (sphere[spheres++], body[bodies]); joint[joints] = dJointCreateHinge2 (world,0); if (x == -17) dJointAttach (joint[joints],b,body[bodies]); else dJointAttach (joint[joints],body[bodies-2],body[bodies]); const dReal *b = dBodyGetPosition (body[bodies++]); dJointSetHinge2Anchor (joint[joints],b[0],b[1],b[2]); dJointSetHinge2Axis1 (joint[joints],0,0,1); dJointSetHinge2Axis2 (joint[joints],1,0,0); dJointSetHinge2Param (joint[joints],dParamSuspensionERP,1.0); dJointSetHinge2Param (joint[joints],dParamSuspensionCFM,1e-5); dJointSetHinge2Param (joint[joints],dParamLoStop,0); dJointSetHinge2Param (joint[joints],dParamHiStop,0); dJointSetHinge2Param (joint[joints],dParamVel2,10.0); dJointSetHinge2Param (joint[joints++],dParamFMax2,FMAX); } if (lastb) { dJointID j = dJointCreateFixed(world,0); dJointAttach (j, b, lastb); dJointSetFixed(j); } lastb = b; } #endif #ifdef BOX body[bodies] = dBodyCreate (world); dBodySetPosition (body[bodies],0,0,HEIGHT/2); dMassSetBox (&m,1,LENGTH,WIDTH,HEIGHT); dMassAdjust (&m, 1); dBodySetMass (body[bodies],&m); box[boxes] = dCreateBox (space,LENGTH,WIDTH,HEIGHT); dGeomSetBody (box[boxes++],body[bodies++]); #endif #ifdef CANNON cannon_ball_body = dBodyCreate (world); cannon_ball_geom = dCreateSphere (space,CANNON_BALL_RADIUS); dMassSetSphereTotal (&m,CANNON_BALL_MASS,CANNON_BALL_RADIUS); dBodySetMass (cannon_ball_body,&m); dGeomSetBody (cannon_ball_geom,cannon_ball_body); dBodySetPosition (cannon_ball_body,CANNON_X,CANNON_Y,CANNON_BALL_RADIUS); #endif }
void makeCar(dReal x, dReal y, int &bodyI, int &jointI, int &boxI, int &sphereI) { int i; dMass m; // chassis body body[bodyI] = dBodyCreate (world); dBodySetPosition (body[bodyI],x,y,STARTZ); dMassSetBox (&m,1,LENGTH,WIDTH,HEIGHT); dMassAdjust (&m,CMASS/2.0); dBodySetMass (body[bodyI],&m); box[boxI] = dCreateBox (space,LENGTH,WIDTH,HEIGHT); dGeomSetBody (box[boxI],body[bodyI]); // wheel bodies for (i=1; i<=4; i++) { body[bodyI+i] = dBodyCreate (world); dQuaternion q; dQFromAxisAndAngle (q,1,0,0,M_PI*0.5); dBodySetQuaternion (body[bodyI+i],q); dMassSetSphere (&m,1,RADIUS); dMassAdjust (&m,WMASS); dBodySetMass (body[bodyI+i],&m); sphere[sphereI+i-1] = dCreateSphere (space,RADIUS); dGeomSetBody (sphere[sphereI+i-1],body[bodyI+i]); } dBodySetPosition (body[bodyI+1],x+0.4*LENGTH-0.5*RADIUS,y+WIDTH*0.5,STARTZ-HEIGHT*0.5); dBodySetPosition (body[bodyI+2],x+0.4*LENGTH-0.5*RADIUS,y-WIDTH*0.5,STARTZ-HEIGHT*0.5); dBodySetPosition (body[bodyI+3],x-0.4*LENGTH+0.5*RADIUS,y+WIDTH*0.5,STARTZ-HEIGHT*0.5); dBodySetPosition (body[bodyI+4],x-0.4*LENGTH+0.5*RADIUS,y-WIDTH*0.5,STARTZ-HEIGHT*0.5); // front and back wheel hinges for (i=0; i<4; i++) { joint[jointI+i] = dJointCreateHinge2 (world,0); dJointAttach (joint[jointI+i],body[bodyI],body[bodyI+i+1]); const dReal *a = dBodyGetPosition (body[bodyI+i+1]); dJointSetHinge2Anchor (joint[jointI+i],a[0],a[1],a[2]); dJointSetHinge2Axis1 (joint[jointI+i],0,0,(i<2 ? 1 : -1)); dJointSetHinge2Axis2 (joint[jointI+i],0,1,0); dJointSetHinge2Param (joint[jointI+i],dParamSuspensionERP,0.8); dJointSetHinge2Param (joint[jointI+i],dParamSuspensionCFM,1e-5); dJointSetHinge2Param (joint[jointI+i],dParamVel2,0); dJointSetHinge2Param (joint[jointI+i],dParamFMax2,FMAX); } //center of mass offset body. (hang another copy of the body COMOFFSET units below it by a fixed joint) dBodyID b = dBodyCreate (world); dBodySetPosition (b,x,y,STARTZ+COMOFFSET); dMassSetBox (&m,1,LENGTH,WIDTH,HEIGHT); dMassAdjust (&m,CMASS/2.0); dBodySetMass (b,&m); dJointID j = dJointCreateFixed(world, 0); dJointAttach(j, body[bodyI], b); dJointSetFixed(j); //box[boxI+1] = dCreateBox(space,LENGTH,WIDTH,HEIGHT); //dGeomSetBody (box[boxI+1],b); bodyI += 5; jointI += 4; boxI += 1; sphereI += 4; }
void CCar::SWheel::ApplySteerAxisTorque(float torque) { if(!joint) return; dJointSetHinge2Param(joint->GetDJoint(), dParamFMax, torque); }
// NOTE: it is important that rigid bodies are added // (happens in CJoint::Attach()) before joint transforms are set!!! void CHinge2Joint::Attach(dWorldID WorldID, dJointGroupID GroupID, const matrix44& ParentTfm) { ODEJointID = dJointCreateHinge2(WorldID, GroupID); for (int i = 0; i < 2; i++) { const CJointAxis& CurrAxis = AxisParams[i]; if (CurrAxis.IsLoStopEnabled) dJointSetHinge2Param(ODEJointID, dParamLoStop + dParamGroup * i, CurrAxis.LoStop); if (CurrAxis.IsHiStopEnabled) dJointSetHinge2Param(ODEJointID, dParamHiStop + dParamGroup * i, CurrAxis.HiStop); dJointSetHinge2Param(ODEJointID, dParamVel + dParamGroup * i, CurrAxis.Velocity); dJointSetHinge2Param(ODEJointID, dParamFMax + dParamGroup * i, CurrAxis.FMax); dJointSetHinge2Param(ODEJointID, dParamFudgeFactor + dParamGroup * i, CurrAxis.FudgeFactor); dJointSetHinge2Param(ODEJointID, dParamBounce + dParamGroup * i, CurrAxis.Bounce); dJointSetHinge2Param(ODEJointID, dParamCFM + dParamGroup * i, CurrAxis.CFM); dJointSetHinge2Param(ODEJointID, dParamStopERP + dParamGroup * i, CurrAxis.StopERP); dJointSetHinge2Param(ODEJointID, dParamStopCFM + dParamGroup * i, CurrAxis.StopCFM); } dJointSetHinge2Param(ODEJointID, dParamSuspensionERP, SuspensionERP); dJointSetHinge2Param(ODEJointID, dParamSuspensionCFM, SuspensionCFM); CJoint::Attach(WorldID, GroupID, ParentTfm); UpdateTransform(ParentTfm); }
void CCar::SWheel::SetSteerHiLimit(float hi) { if(!joint) return; dJointSetHinge2Param(joint->GetDJoint(), dParamHiStop, hi); }
void TSRODESteeringJoint::SetLowStop( float _fLowStop ) { dJointSetHinge2Param( m_ODEJoint.m_JointID, dParamLoStop, _fLowStop ); }
void TSRODESteeringJoint::SetVelocity( float _fAngularVelocity ) { dJointSetHinge2Param( m_ODEJoint.m_JointID, dParamVel, _fAngularVelocity ); }
int setupTest (int n) { switch (n) { // ********** fixed joint case 0: { // 2 body constructWorldForTest (0,2, 0.5*SIDE,0.5*SIDE,1, -0.5*SIDE,-0.5*SIDE,1, 1,1,0, 1,1,0, 0.25*M_PI,0.25*M_PI); joint = dJointCreateFixed (world,0); dJointAttach (joint,body[0],body[1]); dJointSetFixed (joint); return 1; } case 1: { // 1 body to static env constructWorldForTest (0,1, 0.5*SIDE,0.5*SIDE,1, 0,0,0, 1,0,0, 1,0,0, 0,0); joint = dJointCreateFixed (world,0); dJointAttach (joint,body[0],0); dJointSetFixed (joint); return 1; } case 2: { // 2 body with relative rotation constructWorldForTest (0,2, 0.5*SIDE,0.5*SIDE,1, -0.5*SIDE,-0.5*SIDE,1, 1,1,0, 1,1,0, 0.25*M_PI,-0.25*M_PI); joint = dJointCreateFixed (world,0); dJointAttach (joint,body[0],body[1]); dJointSetFixed (joint); return 1; } case 3: { // 1 body to static env with relative rotation constructWorldForTest (0,1, 0.5*SIDE,0.5*SIDE,1, 0,0,0, 1,0,0, 1,0,0, 0.25*M_PI,0); joint = dJointCreateFixed (world,0); dJointAttach (joint,body[0],0); dJointSetFixed (joint); return 1; } // ********** hinge joint case 200: // 2 body constructWorldForTest (0,2, 0.5*SIDE,0.5*SIDE,1, -0.5*SIDE,-0.5*SIDE,1, 1,1,0, 1,1,0, 0.25*M_PI,0.25*M_PI); joint = dJointCreateHinge (world,0); dJointAttach (joint,body[0],body[1]); dJointSetHingeAnchor (joint,0,0,1); dJointSetHingeAxis (joint,1,-1,1.41421356); return 1; case 220: // hinge angle polarity test case 221: // hinge angle rate test constructWorldForTest (0,2, 0.5*SIDE,0.5*SIDE,1, -0.5*SIDE,-0.5*SIDE,1, 1,0,0, 1,0,0, 0,0); joint = dJointCreateHinge (world,0); dJointAttach (joint,body[0],body[1]); dJointSetHingeAnchor (joint,0,0,1); dJointSetHingeAxis (joint,0,0,1); max_iterations = 50; return 1; case 230: // hinge motor rate (and polarity) test case 231: // ...with stops constructWorldForTest (0,2, 0.5*SIDE,0.5*SIDE,1, -0.5*SIDE,-0.5*SIDE,1, 1,0,0, 1,0,0, 0,0); joint = dJointCreateHinge (world,0); dJointAttach (joint,body[0],body[1]); dJointSetHingeAnchor (joint,0,0,1); dJointSetHingeAxis (joint,0,0,1); dJointSetHingeParam (joint,dParamFMax,1); if (n==231) { dJointSetHingeParam (joint,dParamLoStop,-0.5); dJointSetHingeParam (joint,dParamHiStop,0.5); } return 1; case 250: // limit bounce test (gravity down) case 251: { // ...gravity up constructWorldForTest ((n==251) ? 0.1 : -0.1, 2, 0.5*SIDE,0,1+0.5*SIDE, -0.5*SIDE,0,1-0.5*SIDE, 1,0,0, 1,0,0, 0,0); joint = dJointCreateHinge (world,0); dJointAttach (joint,body[0],body[1]); dJointSetHingeAnchor (joint,0,0,1); dJointSetHingeAxis (joint,0,1,0); dJointSetHingeParam (joint,dParamLoStop,-0.9); dJointSetHingeParam (joint,dParamHiStop,0.7854); dJointSetHingeParam (joint,dParamBounce,0.5); // anchor 2nd body with a fixed joint dJointID j = dJointCreateFixed (world,0); dJointAttach (j,body[1],0); dJointSetFixed (j); return 1; } // ********** slider case 300: // 2 body constructWorldForTest (0,2, 0,0,1, 0.2,0.2,1.2, 0,0,1, -1,1,0, 0,0.25*M_PI); joint = dJointCreateSlider (world,0); dJointAttach (joint,body[0],body[1]); dJointSetSliderAxis (joint,1,1,1); return 1; case 320: // slider angle polarity test case 321: // slider angle rate test constructWorldForTest (0,2, 0,0,1, 0,0,1.2, 1,0,0, 1,0,0, 0,0); joint = dJointCreateSlider (world,0); dJointAttach (joint,body[0],body[1]); dJointSetSliderAxis (joint,0,0,1); max_iterations = 50; return 1; case 330: // slider motor rate (and polarity) test case 331: // ...with stops constructWorldForTest (0, 2, 0,0,1, 0,0,1.2, 1,0,0, 1,0,0, 0,0); joint = dJointCreateSlider (world,0); dJointAttach (joint,body[0],body[1]); dJointSetSliderAxis (joint,0,0,1); dJointSetSliderParam (joint,dParamFMax,100); if (n==331) { dJointSetSliderParam (joint,dParamLoStop,-0.4); dJointSetSliderParam (joint,dParamHiStop,0.4); } return 1; case 350: // limit bounce tests case 351: { constructWorldForTest ((n==351) ? 0.1 : -0.1, 2, 0,0,1, 0,0,1.2, 1,0,0, 1,0,0, 0,0); joint = dJointCreateSlider (world,0); dJointAttach (joint,body[0],body[1]); dJointSetSliderAxis (joint,0,0,1); dJointSetSliderParam (joint,dParamLoStop,-0.5); dJointSetSliderParam (joint,dParamHiStop,0.5); dJointSetSliderParam (joint,dParamBounce,0.5); // anchor 2nd body with a fixed joint dJointID j = dJointCreateFixed (world,0); dJointAttach (j,body[1],0); dJointSetFixed (j); return 1; } // ********** hinge-2 joint case 420: // hinge-2 steering angle polarity test case 421: // hinge-2 steering angle rate test constructWorldForTest (0,2, 0.5*SIDE,0,1, -0.5*SIDE,0,1, 1,0,0, 1,0,0, 0,0); joint = dJointCreateHinge2 (world,0); dJointAttach (joint,body[0],body[1]); dJointSetHinge2Anchor (joint,-0.5*SIDE,0,1); dJointSetHinge2Axis1 (joint,0,0,1); dJointSetHinge2Axis2 (joint,1,0,0); max_iterations = 50; return 1; case 430: // hinge 2 steering motor rate (+polarity) test case 431: // ...with stops case 432: // hinge 2 wheel motor rate (+polarity) test constructWorldForTest (0,2, 0.5*SIDE,0,1, -0.5*SIDE,0,1, 1,0,0, 1,0,0, 0,0); joint = dJointCreateHinge2 (world,0); dJointAttach (joint,body[0],body[1]); dJointSetHinge2Anchor (joint,-0.5*SIDE,0,1); dJointSetHinge2Axis1 (joint,0,0,1); dJointSetHinge2Axis2 (joint,1,0,0); dJointSetHinge2Param (joint,dParamFMax,1); dJointSetHinge2Param (joint,dParamFMax2,1); if (n==431) { dJointSetHinge2Param (joint,dParamLoStop,-0.5); dJointSetHinge2Param (joint,dParamHiStop,0.5); } return 1; // ********** angular motor joint case 600: // test euler angle calculations constructWorldForTest (0,2, -SIDE*0.5,0,1, SIDE*0.5,0,1, 0,0,1, 0,0,1, 0,0); joint = dJointCreateAMotor (world,0); dJointAttach (joint,body[0],body[1]); dJointSetAMotorNumAxes (joint,3); dJointSetAMotorAxis (joint,0,1, 0,0,1); dJointSetAMotorAxis (joint,2,2, 1,0,0); dJointSetAMotorMode (joint,dAMotorEuler); max_iterations = 200; return 1; // ********** universal joint case 700: // 2 body case 701: case 702: constructWorldForTest (0,2, 0.5*SIDE,0.5*SIDE,1, -0.5*SIDE,-0.5*SIDE,1, 1,1,0, 1,1,0, 0.25*M_PI,0.25*M_PI); joint = dJointCreateUniversal (world,0); dJointAttach (joint,body[0],body[1]); dJointSetUniversalAnchor (joint,0,0,1); dJointSetUniversalAxis1 (joint, 1, -1, 1.41421356); dJointSetUniversalAxis2 (joint, 1, -1, -1.41421356); return 1; case 720: // universal transmit torque test case 721: case 722: case 730: // universal torque about axis 1 case 731: case 732: case 740: // universal torque about axis 2 case 741: case 742: constructWorldForTest (0,2, 0.5*SIDE,0.5*SIDE,1, -0.5*SIDE,-0.5*SIDE,1, 1,0,0, 1,0,0, 0,0); joint = dJointCreateUniversal (world,0); dJointAttach (joint,body[0],body[1]); dJointSetUniversalAnchor (joint,0,0,1); dJointSetUniversalAxis1 (joint,0,0,1); dJointSetUniversalAxis2 (joint, 1, -1,0); max_iterations = 100; return 1; } return 0; }
void CCar::SWheel::ApplyDriveAxisVel(float vel) { if(!joint) return; dJointSetHinge2Param(joint->GetDJoint(), dParamVel2, vel); }
void CCar::SWheel::ApplyDriveAxisTorque(float torque) { if(!joint) return; dJointSetHinge2Param(joint->GetDJoint(), dParamFMax2,torque);//car->m_axle_friction }
void CCar::SWheel::SetSteerLoLimit(float lo) { if(!joint) return; dJointSetHinge2Param(joint->GetDJoint(), dParamLoStop, lo); }
void TSRODESteeringJoint::SetHighStop( float _fHighStop ) { dJointSetHinge2Param( m_ODEJoint.m_JointID, dParamHiStop, _fHighStop ); }
void WorldPhysics::mainLoop () { for (int i=0;i<4;i++) { if (i%2) { dJointSetHinge2Param (wheelJoints[i],dParamVel2,-speed+dspeed); dJointSetHinge2Param (wheelJoints[i],dParamFMax2,10); } else { dJointSetHinge2Param (wheelJoints[i],dParamVel2,-speed-dspeed); dJointSetHinge2Param (wheelJoints[i],dParamFMax2,10); } if (i>=2) { dJointSetHinge2Param (wheelJoints[i],dParamVel,0); dJointSetHinge2Param (wheelJoints[i],dParamFMax,100); dJointSetHinge2Param (wheelJoints[i],dParamLoStop,0); dJointSetHinge2Param (wheelJoints[i],dParamHiStop,0); dJointSetHinge2Param (wheelJoints[i],dParamFudgeFactor,0.1); } if (i<2) { // steering dReal v = steer - dJointGetHinge2Angle1 (wheelJoints[i]); if (v > 0.1) v = 0.1; if (v < -0.1) v = -0.1; v *= 10.0; dJointSetHinge2Param (wheelJoints[i],dParamVel,v); dJointSetHinge2Param (wheelJoints[i],dParamFMax,5); dJointSetHinge2Param (wheelJoints[i],dParamLoStop,-1); dJointSetHinge2Param (wheelJoints[i],dParamHiStop,1); dJointSetHinge2Param (wheelJoints[i],dParamFudgeFactor,0.1); } } dSpaceCollide (space,this,&nearCallback); dWorldStep (world,0.05); // remove all contact joints dJointGroupEmpty (contactgroup); }
WorldPhysics::WorldPhysics() { enable_complex=0; bulldozer_state=1; tmp_scalar=0; tmp_wait=0; qsrand(QTime::currentTime().msec()); dInitODE(); world = dWorldCreate(); space = dHashSpaceCreate (0); contactgroup = dJointGroupCreate (0); dWorldSetGravity (world,0,0,-9.81); //ground_cheat = dCreatePlane (space,0,0,1,0); wall1=dCreatePlane (space,-1,0,0,-100); wall2=dCreatePlane (space,1,0,0,0); wall3=dCreatePlane (space,0,-1,0,-100); wall4=dCreatePlane (space,0,1,0,0); // our heightfield floor dHeightfieldDataID heightid = dGeomHeightfieldDataCreate(); // Create an finite heightfield. dGeomHeightfieldDataBuildCallback( heightid, NULL, near_heightfield_callback, HFIELD_WIDTH, HFIELD_DEPTH, HFIELD_WSTEP, HFIELD_DSTEP, REAL( 1.0 ), REAL( 0.0 ), REAL( 0.0 ), 0 ); // Give some very bounds which, while conservative, // makes AABB computation more accurate than +/-INF. //dGeomHeightfieldDataSetBounds( heightid, REAL( -4.0 ), REAL( +6.0 ) ); gheight = dCreateHeightfield( space, heightid, 1 ); // Rotate so Z is up, not Y (which is the default orientation) dMatrix3 R; dRSetIdentity( R ); dRFromAxisAndAngle( R, 1, 0, 0, DEGTORAD * 90 ); dGeomSetRotation( gheight, R ); dGeomSetPosition( gheight, 50,50,0 ); // for (int i=0;i<MAX_ITEMS;i++) { // items.push_back(generateItem()); //} generateItems(); bulldozer_space = dSimpleSpaceCreate(space); dSpaceSetCleanup (bulldozer_space,0); bulldozer=new BoxItem(world,bulldozer_space,LENGTH,WIDTH,HEIGHT,CMASS); bulldozer->setPosition(STARTX,STARTY,STARTZ); bulldozer_cabin=new BoxItem(world,bulldozer_space,LENGTH/2,WIDTH/2,2*HEIGHT,CMASS/3); bulldozer_cabin->setPosition(-LENGTH/4+STARTX,STARTY,3.0/2.0*HEIGHT+STARTZ); bulldozer_bucket_c=new BoxItem(world,bulldozer_space,BUCKET_LENGTH,BUCKET_WIDTH,BUCKET_HEIGHT,CMASS/10); bulldozer_bucket_c->setPosition(LENGTH/2+BUCKET_LENGTH/2+RADIUS+STARTX,STARTY,STARTZ); bulldozer_bucket_l=new BoxItem(world,bulldozer_space,BUCKET_WIDTH/5,BUCKET_LENGTH,BUCKET_HEIGHT,CMASS/20); bulldozer_bucket_l->setPosition(LENGTH/2+BUCKET_LENGTH+RADIUS+BUCKET_WIDTH/10+STARTX,-BUCKET_WIDTH/2+BUCKET_LENGTH/2+STARTY,STARTZ); bulldozer_bucket_r=new BoxItem(world,bulldozer_space,BUCKET_WIDTH/5,BUCKET_LENGTH,BUCKET_HEIGHT,CMASS/20); bulldozer_bucket_r->setPosition(LENGTH/2+BUCKET_LENGTH+RADIUS+BUCKET_WIDTH/10+STARTX,BUCKET_WIDTH/2-BUCKET_LENGTH/2+STARTY,STARTZ); for (int i=0; i<4; i++) { dQuaternion q; dQFromAxisAndAngle(q,1,0,0,M_PI*0.5); wheels[i] = new WheelItem(world,bulldozer_space,q,RADIUS,WMASS); } dBodySetPosition (wheels[0]->body,0.5*LENGTH+STARTX,WIDTH*0.5+STARTY,STARTZ-HEIGHT*0.5); dBodySetPosition (wheels[1]->body,0.5*LENGTH+STARTX,-WIDTH*0.5+STARTY,STARTZ-HEIGHT*0.5); dBodySetPosition (wheels[2]->body,-0.5*LENGTH+STARTX, WIDTH*0.5+STARTY,STARTZ-HEIGHT*0.5); dBodySetPosition (wheels[3]->body,-0.5*LENGTH+STARTX,-WIDTH*0.5+STARTY,STARTZ-HEIGHT*0.5); cabin_joint=dJointCreateSlider(world,0); dJointAttach(cabin_joint,bulldozer->body,bulldozer_cabin->body); dJointSetSliderAxis(cabin_joint,0,0,1); dJointSetSliderParam(cabin_joint,dParamLoStop,0); dJointSetSliderParam(cabin_joint,dParamHiStop,0); bucket_joint_c=dJointCreateSlider(world,0); dJointAttach(bucket_joint_c,bulldozer->body,bulldozer_bucket_c->body); dJointSetSliderAxis(bucket_joint_c,0,0,1); dJointSetSliderParam(bucket_joint_c,dParamLoStop,0); dJointSetSliderParam(bucket_joint_c,dParamHiStop,0); bucket_joint_l=dJointCreateSlider(world,0); dJointAttach(bucket_joint_l,bulldozer->body,bulldozer_bucket_l->body); dJointSetSliderAxis(bucket_joint_l,0,0,1); dJointSetSliderParam(bucket_joint_l,dParamLoStop,0); dJointSetSliderParam(bucket_joint_l,dParamHiStop,0); bucket_joint_r=dJointCreateSlider(world,0); dJointAttach(bucket_joint_r,bulldozer->body,bulldozer_bucket_r->body); dJointSetSliderAxis(bucket_joint_r,0,0,1); dJointSetSliderParam(bucket_joint_r,dParamLoStop,0); dJointSetSliderParam(bucket_joint_r,dParamHiStop,0); // front and back wheel hinges for (int i=0; i<4; i++) { wheelJoints[i] = dJointCreateHinge2 (world,0); dJointAttach (wheelJoints[i],bulldozer->body,wheels[i]->body); const dReal *a = dBodyGetPosition (wheels[i]->body); dJointSetHinge2Anchor (wheelJoints[i],a[0],a[1],a[2]); dJointSetHinge2Axis1 (wheelJoints[i],0,0,1); dJointSetHinge2Axis2 (wheelJoints[i],0,1,0); } // seeting ERP & CRM for (int i=0; i<4; i++) { dJointSetHinge2Param (wheelJoints[i],dParamSuspensionERP,0.5); dJointSetHinge2Param (wheelJoints[i],dParamSuspensionCFM,0.8); } // block back axis !!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!! for (int i=0; i<2; i++) { dJointSetHinge2Param (wheelJoints[i],dParamLoStop,0); dJointSetHinge2Param (wheelJoints[i],dParamHiStop,0); } }
Simulator::Simulator(const double posx, const double posy) { m_collision = false; //Open Dynamics Engine stuff m_world = dWorldCreate(); m_space = dHashSpaceCreate(0); m_contacts = dJointGroupCreate(0); m_ground = dCreatePlane(m_space, 0, 0, 1, 0); dGeomSetData(m_ground, (void *) &TYPE_TERRAIN); dWorldSetGravity(m_world, 0, 0, -0.81); //create robot const double LENGTH = 2.50; // chassis length 2.50; const double WIDTH = 1.00; // chassis width const double HEIGHT = 0.40; // chassis height const double RADIUS = 0.45 * WIDTH;//0.45 * WIDTH; // wheel radius const double STARTZ = 0.05; dMass m; dQuaternion q; double car_center_x= posx + 1.5; //1.5 double car_center_y= posy + 5.3; // chassis body m_robotBodyChassis = dBodyCreate(m_world); dBodySetPosition(m_robotBodyChassis, car_center_x, car_center_y, 0.85 * RADIUS + 0.5 * HEIGHT + STARTZ); dMassSetBox(&m, 1, LENGTH, WIDTH, HEIGHT); dBodySetMass(m_robotBodyChassis,&m); // chassis geometry m_robotGeomChassis = dCreateBox(m_space, LENGTH, WIDTH, HEIGHT); dGeomSetBody(m_robotGeomChassis, m_robotBodyChassis); m_robotBodies.push_back(m_robotBodyChassis); dGeomSetData(m_robotGeomChassis, (void *) &TYPE_ROBOT); // wheel bodies for(int i= 0; i < 3; i++) { m_robotBodyWheels[i] = dBodyCreate(m_world); dQFromAxisAndAngle(q, 1, 0, 0, M_PI * 0.5); dBodySetQuaternion(m_robotBodyWheels[i], q); dMassSetSphere(&m, 1, RADIUS); dBodySetMass(m_robotBodyWheels[i], &m); m_robotGeomWheels[i] = dCreateSphere(m_space, RADIUS); dGeomSetBody(m_robotGeomWheels[i], m_robotBodyWheels[i]); m_robotBodies.push_back(m_robotBodyWheels[i]); dGeomSetData(m_robotGeomWheels[i], (void *) &TYPE_ROBOT); } dBodySetPosition(m_robotBodyWheels[0], car_center_x + 0.5 * LENGTH, car_center_y, RADIUS + STARTZ); dBodySetPosition(m_robotBodyWheels[1], car_center_x - 0.5 * LENGTH, car_center_y + 0.5 * WIDTH, RADIUS + STARTZ); dBodySetPosition(m_robotBodyWheels[2], car_center_x - 0.5 * LENGTH, car_center_y - 0.5 * WIDTH, RADIUS + STARTZ); // front and back wheel hinges for (int i = 0; i < 3; i++) { m_robotJoints[i] = dJointCreateHinge2(m_world, 0); // creat hign-2 joint as wheels dJointAttach(m_robotJoints[i], m_robotBodyChassis, m_robotBodyWheels[i]); const dReal *a = dBodyGetPosition(m_robotBodyWheels[i]); dJointSetHinge2Anchor(m_robotJoints[i], a[0], a[1], a[2]); dJointSetHinge2Axis1(m_robotJoints[i], 0, 0, 1); dJointSetHinge2Axis2(m_robotJoints[i], 0, 1, 0); // set joint suspension dJointSetHinge2Param(m_robotJoints[i], dParamSuspensionERP, 0.04); dJointSetHinge2Param(m_robotJoints[i], dParamSuspensionCFM, 0.08); } // lock back wheels along the steering axis for (int i = 1; i < 3; i++) { // set stops to make sure wheels always stay in alignment dJointSetHinge2Param (m_robotJoints[i], dParamLoStop, 0); dJointSetHinge2Param (m_robotJoints[i], dParamHiStop, 0); } }
dReal doStuffAndGetError (int n) { switch (n) { // ********** fixed joint case 0: { // 2 body addOscillatingTorque (0.1); dampRotationalMotion (0.1); // check the orientations are the same const dReal *R1 = dBodyGetRotation (body[0]); const dReal *R2 = dBodyGetRotation (body[1]); dReal err1 = dMaxDifference (R1,R2,3,3); // check the body offset is correct dVector3 p,pp; const dReal *p1 = dBodyGetPosition (body[0]); const dReal *p2 = dBodyGetPosition (body[1]); for (int i=0; i<3; i++) p[i] = p2[i] - p1[i]; dMULTIPLY1_331 (pp,R1,p); pp[0] += 0.5; pp[1] += 0.5; return (err1 + length (pp)) * 300; } case 1: { // 1 body to static env addOscillatingTorque (0.1); // check the orientation is the identity dReal err1 = cmpIdentity (dBodyGetRotation (body[0])); // check the body offset is correct dVector3 p; const dReal *p1 = dBodyGetPosition (body[0]); for (int i=0; i<3; i++) p[i] = p1[i]; p[0] -= 0.25; p[1] -= 0.25; p[2] -= 1; return (err1 + length (p)) * 1e6; } case 2: { // 2 body addOscillatingTorque (0.1); dampRotationalMotion (0.1); // check the body offset is correct // Should really check body rotation too. Oh well. const dReal *R1 = dBodyGetRotation (body[0]); dVector3 p,pp; const dReal *p1 = dBodyGetPosition (body[0]); const dReal *p2 = dBodyGetPosition (body[1]); for (int i=0; i<3; i++) p[i] = p2[i] - p1[i]; dMULTIPLY1_331 (pp,R1,p); pp[0] += 0.5; pp[1] += 0.5; return length(pp) * 300; } case 3: { // 1 body to static env with relative rotation addOscillatingTorque (0.1); // check the body offset is correct dVector3 p; const dReal *p1 = dBodyGetPosition (body[0]); for (int i=0; i<3; i++) p[i] = p1[i]; p[0] -= 0.25; p[1] -= 0.25; p[2] -= 1; return length (p) * 1e6; } // ********** hinge joint case 200: // 2 body addOscillatingTorque (0.1); dampRotationalMotion (0.1); return dInfinity; case 220: // hinge angle polarity test dBodyAddTorque (body[0],0,0,0.01); dBodyAddTorque (body[1],0,0,-0.01); if (iteration == 40) { dReal a = dJointGetHingeAngle (joint); if (a > 0.5 && a < 1) return 0; else return 10; } return 0; case 221: { // hinge angle rate test static dReal last_angle = 0; dBodyAddTorque (body[0],0,0,0.01); dBodyAddTorque (body[1],0,0,-0.01); dReal a = dJointGetHingeAngle (joint); dReal r = dJointGetHingeAngleRate (joint); dReal er = (a-last_angle)/STEPSIZE; // estimated rate last_angle = a; return fabs(r-er) * 4e4; } case 230: // hinge motor rate (and polarity) test case 231: { // ...with stops static dReal a = 0; dReal r = dJointGetHingeAngleRate (joint); dReal err = fabs (cos(a) - r); if (a==0) err = 0; a += 0.03; dJointSetHingeParam (joint,dParamVel,cos(a)); if (n==231) return dInfinity; return err * 1e6; } // ********** slider joint case 300: // 2 body addOscillatingTorque (0.05); dampRotationalMotion (0.1); addSpringForce (0.5); return dInfinity; case 320: // slider angle polarity test dBodyAddForce (body[0],0,0,0.1); dBodyAddForce (body[1],0,0,-0.1); if (iteration == 40) { dReal a = dJointGetSliderPosition (joint); if (a > 0.2 && a < 0.5) return 0; else return 10; return a; } return 0; case 321: { // slider angle rate test static dReal last_pos = 0; dBodyAddForce (body[0],0,0,0.1); dBodyAddForce (body[1],0,0,-0.1); dReal p = dJointGetSliderPosition (joint); dReal r = dJointGetSliderPositionRate (joint); dReal er = (p-last_pos)/STEPSIZE; // estimated rate (almost exact) last_pos = p; return fabs(r-er) * 1e9; } case 330: // slider motor rate (and polarity) test case 331: { // ...with stops static dReal a = 0; dReal r = dJointGetSliderPositionRate (joint); dReal err = fabs (0.7*cos(a) - r); if (a < 0.04) err = 0; a += 0.03; dJointSetSliderParam (joint,dParamVel,0.7*cos(a)); if (n==331) return dInfinity; return err * 1e6; } // ********** hinge-2 joint case 420: // hinge-2 steering angle polarity test dBodyAddTorque (body[0],0,0,0.01); dBodyAddTorque (body[1],0,0,-0.01); if (iteration == 40) { dReal a = dJointGetHinge2Angle1 (joint); if (a > 0.5 && a < 0.6) return 0; else return 10; } return 0; case 421: { // hinge-2 steering angle rate test static dReal last_angle = 0; dBodyAddTorque (body[0],0,0,0.01); dBodyAddTorque (body[1],0,0,-0.01); dReal a = dJointGetHinge2Angle1 (joint); dReal r = dJointGetHinge2Angle1Rate (joint); dReal er = (a-last_angle)/STEPSIZE; // estimated rate last_angle = a; return fabs(r-er)*2e4; } case 430: // hinge 2 steering motor rate (+polarity) test case 431: { // ...with stops static dReal a = 0; dReal r = dJointGetHinge2Angle1Rate (joint); dReal err = fabs (cos(a) - r); if (a==0) err = 0; a += 0.03; dJointSetHinge2Param (joint,dParamVel,cos(a)); if (n==431) return dInfinity; return err * 1e6; } case 432: { // hinge 2 wheel motor rate (+polarity) test static dReal a = 0; dReal r = dJointGetHinge2Angle2Rate (joint); dReal err = fabs (cos(a) - r); if (a==0) err = 0; a += 0.03; dJointSetHinge2Param (joint,dParamVel2,cos(a)); return err * 1e6; } // ********** angular motor joint case 600: { // test euler angle calculations // desired euler angles from last iteration static dReal a1,a2,a3; // find actual euler angles dReal aa1 = dJointGetAMotorAngle (joint,0); dReal aa2 = dJointGetAMotorAngle (joint,1); dReal aa3 = dJointGetAMotorAngle (joint,2); // printf ("actual = %.4f %.4f %.4f\n\n",aa1,aa2,aa3); dReal err = dInfinity; if (iteration > 0) { err = dFabs(aa1-a1) + dFabs(aa2-a2) + dFabs(aa3-a3); err *= 1e10; } // get random base rotation for both bodies dMatrix3 Rbase; dRFromAxisAndAngle (Rbase, 3*(dRandReal()-0.5), 3*(dRandReal()-0.5), 3*(dRandReal()-0.5), 3*(dRandReal()-0.5)); dBodySetRotation (body[0],Rbase); // rotate body 2 by random euler angles w.r.t. body 1 a1 = 3.14 * 2 * (dRandReal()-0.5); a2 = 1.57 * 2 * (dRandReal()-0.5); a3 = 3.14 * 2 * (dRandReal()-0.5); dMatrix3 R1,R2,R3,Rtmp1,Rtmp2; dRFromAxisAndAngle (R1,0,0,1,-a1); dRFromAxisAndAngle (R2,0,1,0,a2); dRFromAxisAndAngle (R3,1,0,0,-a3); dMultiply0 (Rtmp1,R2,R3,3,3,3); dMultiply0 (Rtmp2,R1,Rtmp1,3,3,3); dMultiply0 (Rtmp1,Rbase,Rtmp2,3,3,3); dBodySetRotation (body[1],Rtmp1); // printf ("desired = %.4f %.4f %.4f\n",a1,a2,a3); return err; } // ********** universal joint case 700: { // 2 body: joint constraint dVector3 ax1, ax2; addOscillatingTorque (0.1); dampRotationalMotion (0.1); dJointGetUniversalAxis1(joint, ax1); dJointGetUniversalAxis2(joint, ax2); return fabs(10*dDOT(ax1, ax2)); } case 701: { // 2 body: angle 1 rate static dReal last_angle = 0; addOscillatingTorque (0.1); dampRotationalMotion (0.1); dReal a = dJointGetUniversalAngle1(joint); dReal r = dJointGetUniversalAngle1Rate(joint); dReal diff = a - last_angle; if (diff > M_PI) diff -= 2*M_PI; if (diff < -M_PI) diff += 2*M_PI; dReal er = diff / STEPSIZE; // estimated rate last_angle = a; // I'm not sure why the error is so large here. return fabs(r - er) * 1e1; } case 702: { // 2 body: angle 2 rate static dReal last_angle = 0; addOscillatingTorque (0.1); dampRotationalMotion (0.1); dReal a = dJointGetUniversalAngle2(joint); dReal r = dJointGetUniversalAngle2Rate(joint); dReal diff = a - last_angle; if (diff > M_PI) diff -= 2*M_PI; if (diff < -M_PI) diff += 2*M_PI; dReal er = diff / STEPSIZE; // estimated rate last_angle = a; // I'm not sure why the error is so large here. return fabs(r - er) * 1e1; } case 720: { // universal transmit torque test: constraint error dVector3 ax1, ax2; addOscillatingTorqueAbout (0.1, 1, 1, 0); dampRotationalMotion (0.1); dJointGetUniversalAxis1(joint, ax1); dJointGetUniversalAxis2(joint, ax2); return fabs(10*dDOT(ax1, ax2)); } case 721: { // universal transmit torque test: angle1 rate static dReal last_angle = 0; addOscillatingTorqueAbout (0.1, 1, 1, 0); dampRotationalMotion (0.1); dReal a = dJointGetUniversalAngle1(joint); dReal r = dJointGetUniversalAngle1Rate(joint); dReal diff = a - last_angle; if (diff > M_PI) diff -= 2*M_PI; if (diff < -M_PI) diff += 2*M_PI; dReal er = diff / STEPSIZE; // estimated rate last_angle = a; return fabs(r - er) * 1e10; } case 722: { // universal transmit torque test: angle2 rate static dReal last_angle = 0; addOscillatingTorqueAbout (0.1, 1, 1, 0); dampRotationalMotion (0.1); dReal a = dJointGetUniversalAngle2(joint); dReal r = dJointGetUniversalAngle2Rate(joint); dReal diff = a - last_angle; if (diff > M_PI) diff -= 2*M_PI; if (diff < -M_PI) diff += 2*M_PI; dReal er = diff / STEPSIZE; // estimated rate last_angle = a; return fabs(r - er) * 1e10; } case 730:{ dVector3 ax1, ax2; dJointGetUniversalAxis1(joint, ax1); dJointGetUniversalAxis2(joint, ax2); addOscillatingTorqueAbout (0.1, ax1[0], ax1[1], ax1[2]); dampRotationalMotion (0.1); return fabs(10*dDOT(ax1, ax2)); } case 731:{ dVector3 ax1; static dReal last_angle = 0; dJointGetUniversalAxis1(joint, ax1); addOscillatingTorqueAbout (0.1, ax1[0], ax1[1], ax1[2]); dampRotationalMotion (0.1); dReal a = dJointGetUniversalAngle1(joint); dReal r = dJointGetUniversalAngle1Rate(joint); dReal diff = a - last_angle; if (diff > M_PI) diff -= 2*M_PI; if (diff < -M_PI) diff += 2*M_PI; dReal er = diff / STEPSIZE; // estimated rate last_angle = a; return fabs(r - er) * 2e3; } case 732:{ dVector3 ax1; static dReal last_angle = 0; dJointGetUniversalAxis1(joint, ax1); addOscillatingTorqueAbout (0.1, ax1[0], ax1[1], ax1[2]); dampRotationalMotion (0.1); dReal a = dJointGetUniversalAngle2(joint); dReal r = dJointGetUniversalAngle2Rate(joint); dReal diff = a - last_angle; if (diff > M_PI) diff -= 2*M_PI; if (diff < -M_PI) diff += 2*M_PI; dReal er = diff / STEPSIZE; // estimated rate last_angle = a; return fabs(r - er) * 1e10; } case 740:{ dVector3 ax1, ax2; dJointGetUniversalAxis1(joint, ax1); dJointGetUniversalAxis2(joint, ax2); addOscillatingTorqueAbout (0.1, ax2[0], ax2[1], ax2[2]); dampRotationalMotion (0.1); return fabs(10*dDOT(ax1, ax2)); } case 741:{ dVector3 ax2; static dReal last_angle = 0; dJointGetUniversalAxis2(joint, ax2); addOscillatingTorqueAbout (0.1, ax2[0], ax2[1], ax2[2]); dampRotationalMotion (0.1); dReal a = dJointGetUniversalAngle1(joint); dReal r = dJointGetUniversalAngle1Rate(joint); dReal diff = a - last_angle; if (diff > M_PI) diff -= 2*M_PI; if (diff < -M_PI) diff += 2*M_PI; dReal er = diff / STEPSIZE; // estimated rate last_angle = a; return fabs(r - er) * 1e10; } case 742:{ dVector3 ax2; static dReal last_angle = 0; dJointGetUniversalAxis2(joint, ax2); addOscillatingTorqueAbout (0.1, ax2[0], ax2[1], ax2[2]); dampRotationalMotion (0.1); dReal a = dJointGetUniversalAngle2(joint); dReal r = dJointGetUniversalAngle2Rate(joint); dReal diff = a - last_angle; if (diff > M_PI) diff -= 2*M_PI; if (diff < -M_PI) diff += 2*M_PI; dReal er = diff / STEPSIZE; // estimated rate last_angle = a; return fabs(r - er) * 1e4; } } return dInfinity; }
void Machine::init() { int i; pushtime=0; energy=4; dMass m; for(i=0; i<3; i++) { wheel[i] = dBodyCreate(world); dMassSetSphere(&m, 1, 5); dMassAdjust(&m, 2); dBodySetMass(wheel[i], &m); sphere[i] = dCreateSphere(0, 5); dGeomSetBody(sphere[i], wheel[i]); } dBodySetPosition(wheel[0], 0, 12, 6); dBodySetPosition(wheel[1], -6, -7, 6); dBodySetPosition(wheel[2], 6, -7, 6); body[0] = dBodyCreate(world); dMassSetBox(&m, 1, 20, 80, 5); dMassAdjust(&m, 5); dBodySetMass(body[0], &m); dBodySetPosition(body[0], 0, 0, 6.5); geom[0] = dCreateBox(0, 19, 27, 10); dGeomSetBody(geom[0], body[0]); body[1] = dBodyCreate(world); dMassSetBox(&m, 1, 11, 5, 10); dMassAdjust(&m, 0.3); dBodySetMass(body[1], &m); dBodySetPosition(body[1], 0, 17, 6.5); geom[1] = dCreateBox(0, 11, 5, 10); dGeomSetBody(geom[1], body[1]); joint = dJointCreateSlider(world, 0); dJointAttach(joint, body[0], body[1]); dJointSetSliderAxis(joint, 0, 1, 0); dJointSetSliderParam(joint, dParamLoStop, -9); dJointSetSliderParam(joint, dParamHiStop, 0); for(i=0; i<2; i++) { geom[i+2] = dCreateGeomTransform(0); dGeomTransformSetCleanup(geom[i+2], 1); finE[i] = dCreateBox(0, 7, 5, 10); dGeomSetPosition(finE[i], i==0?-6.3:6.3, -2, 0); dMatrix3 R; dRFromAxisAndAngle(R, 0, 0, 1, i==0?M_PI/4:-M_PI/4); dGeomSetRotation(finE[i], R); dGeomTransformSetGeom(geom[i+2], finE[i]); dGeomSetBody(geom[i+2], body[1]); } for(i=0; i<3; i++) { wheeljoint[i] = dJointCreateHinge2(world, 0); dJointAttach(wheeljoint[i], body[0], wheel[i]); const dReal *wPos = dBodyGetPosition(wheel[i]); dJointSetHinge2Anchor(wheeljoint[i], wPos[0], wPos[1], wPos[2]); dJointSetHinge2Axis1(wheeljoint[i], 0, 0, 1); dJointSetHinge2Axis2(wheeljoint[i], 1, 0, 0); dJointSetHinge2Param(wheeljoint[i], dParamSuspensionERP, 0.8); dJointSetHinge2Param(wheeljoint[i], dParamSuspensionCFM, 0.01); dJointSetHinge2Param(wheeljoint[i], dParamLoStop, 0); dJointSetHinge2Param(wheeljoint[i], dParamHiStop, 0); dJointSetHinge2Param(wheeljoint[i], dParamCFM, 0.0001); dJointSetHinge2Param(wheeljoint[i], dParamStopERP, 0.8); dJointSetHinge2Param(wheeljoint[i], dParamStopCFM, 0.0001); } reset(); }