segwayrmp.cc

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

                                           tireThick, axelHeight + oz ) );
  this->tires[0]->SetRotation( GzQuaternFromAxis( 1, 0, 0, M_PI / 2 ) );
  this->tires[1]->SetPosition( GzVectorSet( 0.00, +0.5 * baseSy + 0.5 *
                                            tireThick, axelHeight + oz ) );
  this->tires[1]->SetRotation( GzQuaternFromAxis( 1, 0, 0, M_PI / 2 ) );

  // Attach the tires to the body
  for (i = 0; i < 2; i++)
  {
    this->tireJoints[i] = new HingeJoint( this->world );
    this->tireJoints[i]->Attach( this->tires[i], this->base );

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

    this->tireJoints[i]->SetParam( dParamFMax, dInfinity ); //5.0);
    this->tireJoints[i]->SetParam(dParamSuspensionERP, 0.8);
    //this->joints[i]->SetParam(dParamSuspensionCFM, 0.8);
  }

  this->AddBody( this->base, true );
  this->AddBody( this->top );
  this->AddBody( this->tires[0] );
  this->AddBody( this->tires[1] );

  // init velocities
  this->curLinVel = 0;
  this->desLinVel = 0;
  this->desAngVel = 0;

  this->maxLinAcc = 1.0;
  this->maxLinVel = 5.0; // not used for now

  return 0;
}



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


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

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

  return 0;
}


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


/////////////////////////////////////////////////////////////////////////////
// Reduces the difference b/n curVel and desVel given maxAcc and time step
// using simple trapezoid velocity profile
// returns new curVel
// TODO: move this into a reusable control object
static double GetTrapVel(double curVel, double desVel, double maxAcc, double step)
{
  double maxVStep = step*maxAcc;

  if (curVel != desVel)
  {
    if (fabs(desVel - curVel) < maxVStep)
    {
      curVel = desVel;
    } else
    {
      int curVelDir = (curVel < 0) ? -1 : 1;
      int desVelDir = (desVel < 0) ? -1 : 1;
      if (curVelDir == desVelDir)
      {
        if  (fabs(curVel) < fabs(desVel))
        {
          curVel += desVelDir*step*maxAcc;
        } else
        if (curVel > desVel)
        {
          curVel -= desVelDir*step*maxAcc;
        }
      } else {
        if (desVelDir > 0)
        {
          curVel += step*maxAcc;
        } else
        {
          curVel -= step*maxAcc;
        }
      }
    }
  }
  return curVel;
}


#ifdef RUN_LOG
FILE* runLog;
double logTime;
#endif

//////////////////////////////////////////////////////////////////////////////
// Update model
// Inverted pendulum controller
void SegwayRMP::Update( double step )
{
  // Get commands from the external interface
  this->IfaceGetCmd();

  // get velocity from trapezoid profile
  this->curLinVel = GetTrapVel(this->curLinVel, this->desLinVel, this->maxLinAcc, step);

  GzVector rpy = GzQuaternToEuler(this->base->GetRotation());

  // lazy init control states, fill in any current data
  if (this->desState == NULL)
  {
    this->desState = new ControlState;
  }
  if (this->curState == NULL)
  {
    this->curState = new ControlState;
    this->curState->yaw = rpy.z;
  }

  this->desState->dyaw = this->desAngVel;
  this->desState->vel = this->curLinVel; 

  // avg vel
  this->curState->vel = .25*this->wheelDiam*(this->tireJoints[0]->GetAngleRate() + this->tireJoints[1]->GetAngleRate());

  // don't care about distance when moving
  if (!this->desState->vel) this->curState->dist += (this->curState->vel*step);
  else this->curState->dist = 0;

  // yaw w/ wraparound
  double dyaw = rpy.z - this->curState->yaw;
  if (dyaw > M_PI) dyaw += - 2*M_PI;
  else if (dyaw < -M_PI) dyaw += 2*M_PI;
  dyaw /= step;
  this->curState->dyaw = dyaw;
  this->curState->yaw = rpy.z;

  // pitch
  this->curState->dpitch = (rpy.y - this->curState->pitch)/step;
  this->curState->pitch = rpy.y;

  // full state feedback
  double kX = RMP_CTRL_LQR_K1*this->curState->dist +
              RMP_CTRL_LQR_K2*this->curState->vel +
              RMP_CTRL_LQR_K3*this->curState->pitch +
              RMP_CTRL_LQR_K4*this->curState->dpitch;

  // PI for velocity
  if (this->linVelPID == NULL) this->linVelPID = new PID(RMP_CTRL_LIN_KP, RMP_CTRL_LIN_KI, RMP_CTRL_LIN_KD);
  double u = this->linVelPID->s(this->desState->vel - this->curState->vel) + kX;
 
  // PID for steering
  if (this->angVelPID == NULL) this->angVelPID = new PID(RMP_CTRL_ANG_KP, RMP_CTRL_ANG_KI, RMP_CTRL_ANG_KD);
  double yc = (this->desState->dyaw) ? this->angVelPID->s(this->desState->dyaw - this->curState->dyaw) : 0;

  this->tireJoints[0]->SetParam( dParamFMax, 10);
  this->tireJoints[1]->SetParam( dParamFMax, 10);

  this->tireJoints[0]->SetParam( dParamVel, u + yc );
  this->tireJoints[1]->SetParam( dParamVel, u - yc );

  // Update the odometry
  this->UpdateOdometry( step );

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

  // produce some plottable data -- should remove this after model works better
#ifdef RUN_LOG
  if (runLog == NULL) runLog = fopen(RUN_LOG, "w");
  logTime += step;
  fprintf(runLog,"%.2f\t%.4f\t%.4f\t%.4f\t%.4f\t%.2f\t%.2f\n",logTime, curState->dist, curState->vel, curLinVel, curState->pitch, curState->dpitch, curState->dyaw);
#endif

  /*  // debug
  printf("d=%.5f(%.5f)\tdd=%.5f(%.5f)\ta=%.5f\tda=%.5f(%.5f)\tyaw=%.5f\tdyaw=%.5f(%.2f)\tu=%.5f\tyc=%.5f\n",
    curState->dist, desState->dist, curState->vel, desState->vel, curState->pitch, curState->dpitch, desState->dpitch, curState->yaw, curState->dyaw, desState->dyaw, u, yc);
  */


  return;
}

//////////////////////////////////////////////////////////////////////////////
// Update the odometry
// This is probably wrong
void SegwayRMP::UpdateOdometry( double step )
{
  double wd, ws;
  double d1, d2;
  double dr, da;
  GzVector rot;

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

  // Average distance travelled by left and right wheels
  d1 = step * wd / 2 * (this->tireJoints[0]->GetAngleRate());
  d2 = step * wd / 2 * (this->tireJoints[1]->GetAngleRate());

  dr = (d1 + d2) / 2;
  da = (d1 - d2) / ws;

  // Compute odometric pose
  this->odomPose[0] += dr * cos(this->odomPose[2]);
  this->odomPose[1] += dr * sin(this->odomPose[2]);
  this->odomPose[2] += da;

  // Measure robot roll and pitch (determined by onboard IMU)
  rot = GzQuaternToEuler(this->base->GetRotation());
  this->odomOrient[0] = rot.x;
  this->odomOrient[1] = rot.y;

  return;
}


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

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

  return 0;
}


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

  return 0;
}


//////////////////////////////////////////////////////////////////////////////
// Get commands from the external interface
void SegwayRMP::IfaceGetCmd()
{
  this->desLinVel = this->iface->data->cmd_vel_pos[0];
  this->desAngVel = this->iface->data->cmd_vel_rot[2];

  this->wheelSpeed[0] = desLinVel + desAngVel * this->wheelSep;
  this->wheelSpeed[1] = desLinVel - desAngVel * this->wheelSep;
  
  return;
}


//////////////////////////////////////////////////////////////////////////////
// Update the data in the erinterface
void SegwayRMP::IfacePutData()
{
  // Data timestamp
  this->iface->data->time = this->world->GetSimTime();

  this->iface->data->pos[0] = this->odomPose[0];
  this->iface->data->pos[1] = this->odomPose[1];
  this->iface->data->pos[2] = 0.0;
  this->iface->data->rot[0] = this->odomOrient[0];
  this->iface->data->rot[1] = this->odomOrient[1];
  this->iface->data->rot[2] = this->odomPose[2];
  
  // TODO: velocities

  return;
}