The Mobile Robot Programming Toolkit

The model that transform left+right encoder ticks into an increment in the robot 2D pose (x,y,phi) is implemented in CActionRobotMovement2D::computeFromEncoders.

If the corresponding CActionRobotMovement2D object contains encoders information (i.e. only if it was gathered from a real robot!), this method performs a simulation by dividing the encoder ticks into a number of smaller steps (e.g. 100) and by updating the 2D pose as described by the kinematic model.

Finally, it uses the new 2D pose increment as input to the selected probabilistic motion model.

The source code of the implementation is:

void  CActionRobotMovement2D::computeFromEncoders(
  double K_left,
  double K_right,
  double D )
{
 if (hasEncodersInfo)
 {
   int  nSteps = 100;
   double  factor = 1.0 / nSteps;

   double As = factor * 0.5* ( K_right*encoderRightTicks + K_left*encoderLeftTicks );
   double Aphi = factor * ( K_right*encoderRightTicks - K_left*encoderLeftTicks ) / D;
   double  x=0,y=0,phi=0;

   for (int step=0;step<nSteps;step++)
   {
    x += cos(phi)*As;
    y += sin(phi)*As;
    phi += Aphi;
   }

   CPose2D  incrPose(x,y,phi);

   // Build the whole PDF with the current parameters:
   computeFromOdometry( incrPose, motionModelConfiguration );
 }
}