clodbuster.cc

机器人人3D仿真工具,可以加入到Simbad仿真环境下应用。

    geom->SetMass( 0.001 );
    geom->SetColor( GzColor(0.1, 0.1, 0.1) );
    //this->stubs[i]->AttachGeom( geom );
    this->AddBody(this->stubs[i]);  
	
  }

  this->stubs[0]->SetPosition(GzVectorSet(0.4*length, +0.5*width-.03, -0.1));
  this->stubs[1]->SetPosition(GzVectorSet(0.4*length, -0.5*width+.03, -0.1));
  this->stubs[2]->SetPosition(GzVectorSet(-0.4*length, +0.5*width-.03, -0.1));
  this->stubs[3]->SetPosition(GzVectorSet(-0.4*length, -0.5*width+.03, -0.1));

  // Attach the tires to the body
  for (i = 0; i < 4; i++)
  {

    this->sjoints[i] = new HingeJoint( this->world );

    this->sjoints[i]->Attach(this->stubs[i], this->body );
        
    GzVector ab = stubs[i]->GetPosition();
    this->sjoints[i]->SetAnchor( ab );
    this->sjoints[i]->SetAxis(GzVectorSet(0,0,1));

    this->wjoints[i] = new HingeJoint( this->world );

    this->wjoints[i]->Attach(this->tires[i],this->stubs[i]);
        
    GzVector a = tires[i]->GetPosition();
    this->wjoints[i]->SetAnchor( a );
    this->wjoints[i]->SetAxis(GzVectorSet(0,1,0));

    this->wjoints[i]->SetParam( dParamSuspensionERP, 0.4 ); // 0.4
    this->wjoints[i]->SetParam( dParamSuspensionCFM, 0.8 ); //0.8
      
    this->sjoints[i]->SetParam( dParamLoStop,-M_PI/6);
    this->sjoints[i]->SetParam( dParamHiStop,+M_PI/6);
  }
  // fix rear wheels for now
  this->sjoints[2]->SetParam(dParamLoStop,0);
  this->sjoints[2]->SetParam(dParamHiStop,0);
  this->sjoints[3]->SetParam(dParamLoStop,0);
  this->sjoints[3]->SetParam(dParamHiStop,0);

  this->body->SetFiniteRotationMode(1);  

  return 0;
}


//////////////////////////////////////////////////////////////////////////////
// Initialize ODE
int ClodBuster::OdeInit( WorldFile *file, WorldFileNode *node )
{
  return 0;
}


//////////////////////////////////////////////////////////////////////////////
// Finalize the model
int ClodBuster::Fini()
{
  // Finalize external interface  
  this->IfaceFini();

  // Finalize ODE objects
  this->OdeFini();

  return 0;
}


//////////////////////////////////////////////////////////////////////////////
// Finalize ODE
int ClodBuster::OdeFini()
{
  return 0;
}


//////////////////////////////////////////////////////////////////////////////
// Update model
void ClodBuster::Update( double step )
{
  // Get commands from the external interface
  this->IfaceGetCmd();
   
  double length = 0.33;
  double v = this->wheelSpeed[0];// + this->wheelSpeed[1] * this->wheelSep;
;
  double rTurn;
  // Radius of the Turn
  if(this->wheelSpeed[1]==0.0 || this->wheelSpeed[0]==0.0)
    rTurn=RBIG;
  else
    rTurn= this->wheelSpeed[0]/this->wheelSpeed[1];

  if(fabs(rTurn)>RBIG)
    rTurn=fabs(rTurn)*RBIG/rTurn; //ftb
 
  if(fabs(rTurn) < RMIN)
    rTurn=fabs(rTurn)*RMIN/rTurn;
  
  //double w0=this->wheelSpeed[1]*(fabs(rTurn)/(rTurn+this->wheelSep/2.0));
  //  double w1=this->wheelSpeed[1]*(fabs(rTurn)/(rTurn-this->wheelSep/2.0));
  double d=this->wheelSep/2.0;

  //  double steerR=atan2(length,rTurn-d);
  //  double steerL=atan2(length,rTurn+d);
  double steerR=atan(length/(rTurn-d));
  double steerL=atan(length/(rTurn+d));

  double vLB = v* ( rTurn-d)/(rTurn);
  double vRB=  v* ( rTurn+d)/(rTurn);
  double vLF = v* sqrt(( rTurn-d)*(rTurn-d)+length*length)/fabs(rTurn);
  double vRF = v* sqrt(( rTurn+d)*(rTurn+d)+length*length)/fabs(rTurn);
  
  //  printf("vr= %f va= %f , rTurn=%f w0=%f w1=%f\n",this->wheelSpeed[0],this->wheelSpeed[1],rTurn,w0,w1);
 
  
   double angleL = this->sjoints[0]->GetAngle();
   
   double angleR = this->sjoints[1]->GetAngle();
   //  printf("steerL=%f angleL=%f steerR=%f angleR=%f\n",steerL,angleL,steerR,angleR);
  
  this->sjoints[0]->SetParam(dParamVel,kSteer*(steerL-angleL));//w0 );
  this->sjoints[1]->SetParam(dParamVel,kSteer*(steerR-angleR));// w1 );

   this->sjoints[2]->SetParam(dParamVel, 0);
   this->sjoints[3]->SetParam(dParamVel, 0);

  this->sjoints[0]->SetParam( dParamFMax, 1.1 ); 
  this->sjoints[1]->SetParam( dParamFMax, 1.1 ); 
   this->sjoints[2]->SetParam( dParamFMax, 1.8 ); 
  this->sjoints[3]->SetParam( dParamFMax, 1.8 );

  this->wjoints[0]->SetParam( dParamVel,vLF / this->wheelDiam);//vright / this->wheelDiam * 2 );
this->wjoints[1]->SetParam( dParamVel,vRF / this->wheelDiam);//vleft / this->wheelDiam * 2 );

 this->wjoints[2]->SetParam( dParamVel,vLB / this->wheelDiam);//vright / this->wheelDiam * 2 ); 
 this->wjoints[3]->SetParam( dParamVel,vRB / this->wheelDiam);//vleft / this->wheelDiam * 2 ); 
 

  // printf("angle rates: axis1 %f %f",this->joints[0]->GetAngle1Rate(),this->joints[2]->GetAngle1Rate());
 
  // printf(" axis2 %f %f\n",this->joints[0]->GetAngle2Rate(),this->joints[3]->GetAngle2Rate());
  this->wjoints[0]->SetParam( dParamFMax, 1.1 ); //1.1
  this->wjoints[1]->SetParam( dParamFMax, 1.1 ); // 1.1
  this->wjoints[2]->SetParam( dParamFMax, 1.1 ); // 1.1
  this->wjoints[3]->SetParam( dParamFMax, 1.1 ); // 1.1
  
  // Update the odometry
  this->UpdateOdometry( step );

  // Update the interface
  this->IfacePutData();

  return;
}


//////////////////////////////////////////////////////////////////////////////
// Update the odometry

void ClodBuster::UpdateOdometry( double step )
{
  double wd, ws;
  double d1, d2;
  double dr, da;

  wd = this->wheelDiam;
  ws = this->wheelSep;

  // Average distance travelled by left and right wheels
  //d1 = step * wd * (joints[0]->GetAngle2Rate() + joints[2]->GetAngle2Rate()) / 2;
  //d2 = step * wd * (joints[1]->GetAngle2Rate() + joints[3]->GetAngle2Rate()) / 2;
  d1 = step * wd * wjoints[2]->GetAngleRate() / 2;
  d2 = step * wd * wjoints[3]->GetAngleRate() / 2;

  dr = (d1 + d2) / 2;
  da = (d1-d2)/2*ws;  
  // Compute odometric pose
  this->odomPose[0] += dr * cos( this->odomPose[2] );
  this->odomPose[1] += dr * sin( this->odomPose[2] );
  this->odomPose[2] += da;
 
  //update encoder counts
  this->encoders[0] += d1*408/M_PI/wd;
  this->encoders[1] += d2*408/M_PI/wd;

  /*
    GzVector pos;
    
    pos=this->body->GetRelPointPos(0,0,0);
    //  GzVector p0,p1;
    //  p0=this->body->GetRelPointPos(0,0,0);
    //  p1=this->body->GetRelPointPos(1,0,0);
    
    GzQuatern angles=this->body->GetRotation();
    double r,p,y;
    GzQuaternToEuler(&r,&p,&y,angles);
    
    // if(y<0)
    //  y=y+2*M_PI;
    // printf(" dy=%f dx=%f\n",(p1.y-p0.y),(p1.x-p0.x));
    
    this->odomPose[0] =pos.x;
    this->odomPose[1] =pos.y;
    this->odomPose[2] =y; //rad? yep
    //(180/M_PI)*(atan2((p1.y-p0.y),(p1.x-p0.x)));
    //printf("yaw = %f, %f\n",(180/M_PI)*y,this->odomPose[2]);
  */
  return;
}


// Initialize the external interface
int ClodBuster::IfaceInit()
{
  this->iface = gz_position_alloc();
  assert(this->iface);

  if (gz_position_create(this->iface, this->world->gz_server, this->GetId(),
        "ClobBuster", this->GetIntId(), this->GetParentIntId()) != 0)
    return -1;
  
  return 0;
}


// Finalize the external interface
int ClodBuster::IfaceFini()
{
  gz_position_destroy( this->iface );
  gz_position_free( this->iface );
  this->iface = NULL;
  
  return 0;
}


// Get commands from the external interface
void ClodBuster::IfaceGetCmd()
{
  double vr, va;

  vr = this->iface->data->cmd_vel_pos[0];
  va = this->iface->data->cmd_vel_rot[2];

  // printf("vr=%f va=%f \n",vr,va);  
  this->wheelSpeed[0] = vr ;
  this->wheelSpeed[1] = va;
  
  return;
}

// Update the data in the erinterface
void ClodBuster::IfacePutData()
{
  // Data timestamp
  this->iface->data->time = this->world->GetSimTime();
  if(this->raw_encoder_position)
  {
    this->iface->data->pos[0] = this->encoders[0]*1e-3;
    this->iface->data->pos[1] = this->encoders[1]*1e-3;
    this->iface->data->rot[2] = NAN;
  }
  else 
  {
    this->iface->data->pos[0] = ( this->odomPose[0]);
    this->iface->data->pos[1] = ( this->odomPose[1]);
    this->iface->data->rot[2] = ( this->odomPose[2]);
    // printf("CLOD DUMP %f,%f,%f\n",this->odomPose[0],this->odomPose[1],this->odomPose[2]);
  }
  return;
}