C++ (Cpp) GetPositionCmd Examples

Example #1

File: diffdrive_plugin_6w.cpp Project: capparell/thinc_simulator

// Update the controller
void DiffDrivePlugin6W::Update()
{
  // TODO: Step should be in a parameter of this function
  double d1, d2;
  double dr, da;
  common::Time stepTime;

  GetPositionCmd();

  //stepTime = World::Instance()->GetPhysicsEngine()->GetStepTime();
  stepTime = world->GetSimTime() - prevUpdateTime;
  prevUpdateTime = world->GetSimTime();

  // Distance travelled by front wheels
  d1 = stepTime.Double() * wheelDiam / 2 * joints[MID_LEFT]->GetVelocity(0);
  d2 = stepTime.Double() * wheelDiam / 2 * joints[MID_RIGHT]->GetVelocity(0);

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

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

  // Compute odometric instantaneous velocity
  odomVel[0] = dr / stepTime.Double();
  odomVel[1] = 0.0;
  odomVel[2] = da / stepTime.Double();

  if (enableMotors)
  {
    joints[FRONT_LEFT]->SetVelocity(0, wheelSpeed[0] / (wheelDiam / 2.0));
    joints[MID_LEFT]->SetVelocity(0, wheelSpeed[0] / (wheelDiam / 2.0));
    joints[REAR_LEFT]->SetVelocity(0, wheelSpeed[0] / (wheelDiam / 2.0));

    joints[FRONT_RIGHT]->SetVelocity(0, wheelSpeed[1] / (wheelDiam / 2.0));
    joints[MID_RIGHT]->SetVelocity(0, wheelSpeed[1] / (wheelDiam / 2.0));
    joints[REAR_RIGHT]->SetVelocity(0, wheelSpeed[1] / (wheelDiam / 2.0));

    joints[FRONT_LEFT]->SetMaxForce(0, torque);
    joints[MID_LEFT]->SetMaxForce(0, torque);
    joints[REAR_LEFT]->SetMaxForce(0, torque);

    joints[FRONT_RIGHT]->SetMaxForce(0, torque);
    joints[MID_RIGHT]->SetMaxForce(0, torque);
    joints[REAR_RIGHT]->SetMaxForce(0, torque);
  }

  //publish_odometry();
}

Example #2

File: diffdrive_plugin.cpp Project: Chriscuit/igvc

// Update the controller
  void DiffDrivePlugin::UpdateChild()
  {
    // TODO: Step should be in a parameter of this function
    double wd, ws;
    double d1, d2;
    double dr, da;
    double stepTime = this->world->GetPhysicsEngine()->GetUpdatePeriod();
    
    GetPositionCmd();
    
    wd = wheelDiameter;
    ws = wheelSeparation;
    
    // Distance travelled by front wheels
    d1 = stepTime * wd / 2 * joints[LEFT]->GetVelocity(0);
    d2 = stepTime * wd / 2 * joints[RIGHT]->GetVelocity(0);
    
    dr = (d1 + d2) / 2;
    da = (d1 - d2) / ws;
    
    // Compute odometric pose
    odomPose[0] += dr * cos(odomPose[2]);
    odomPose[1] += dr * sin(odomPose[2]);
    odomPose[2] += da;
    
    // Compute odometric instantaneous velocity
    odomVel[0] = dr / stepTime;
    odomVel[1] = 0.0;
    odomVel[2] = da / stepTime;
    
    joints[LEFT]->SetVelocity(0, wheelSpeed[LEFT] / (wheelDiameter / 2.0));
    joints[RIGHT]->SetVelocity(0, wheelSpeed[RIGHT] / (wheelDiameter / 2.0));
    
    joints[LEFT]->SetMaxForce(0, torque);
    joints[RIGHT]->SetMaxForce(0, torque);
    
    write_position_data();
    publish_odometry();
  }