tutorial-simu-pioneer-pan.cpp

#include <iostream>

#include <visp/vpFeatureBuilder.h>
#include <visp/vpFeatureDepth.h>
#include <visp/vpFeaturePoint.h>
#include <visp/vpHomogeneousMatrix.h>
#include <visp/vpPlot.h>
#include <visp/vpServo.h>
#include <visp/vpSimulatorPioneerPan.h>
#include <visp/vpVelocityTwistMatrix.h>

int main()
{
  // Set the position the camera has to reach
  vpHomogeneousMatrix cdMo ;
  cdMo[1][3] = 1.2; // t_y should be different from zero to be non singular
  cdMo[2][3] = 0.5;

  // Set the initial camera position
  vpHomogeneousMatrix cMo;
  cMo[0][3] = 0.3;
  cMo[1][3] = cdMo[1][3];
  cMo[2][3] = 1.;
  vpRotationMatrix cdRo(0, atan2(cMo[0][3], cMo[1][3]), 0);
  cMo.insert(cdRo);

  vpSimulatorPioneerPan robot ;
  robot.setSamplingTime(0.04);
  vpHomogeneousMatrix wMc, wMo;

  // Get robot position world frame
  robot.getPosition(wMc);

  // Compute the position of the object in the world frame
  wMo = wMc * cMo;

  // Define the target
  vpPoint point;
  point.setWorldCoordinates(0,0,0); // Coordinates in the object frame
  point.track(cMo);

  vpServo task;
  task.setServo(vpServo::EYEINHAND_L_cVe_eJe);
  task.setInteractionMatrixType(vpServo::CURRENT, vpServo::PSEUDO_INVERSE);
  task.setLambda(0.2);

  vpVelocityTwistMatrix cVe;
  cVe = robot.get_cVe();
  task.set_cVe(cVe);

  vpMatrix eJe;
  robot.get_eJe(eJe);
  task.set_eJe(eJe);

  // Current and desired visual feature associated later to the x coordinate of the point
  vpFeaturePoint s_x, s_xd;

  // Create the current x visual feature
  vpFeatureBuilder::create(s_x, point);

  // Create the desired x* visual feature
  s_xd.buildFrom(0, 0, cdMo[2][3]);

  // Add the feature
  task.addFeature(s_x, s_xd, vpFeaturePoint::selectX());

  // Create the current and desired log(Z/Z*) visual feature
  vpFeatureDepth s_Z, s_Zd;
  // Initial depth of the target in front of the camera
  double Z = point.get_Z();
  // Desired depth Z* of the target.
  double Zd = cdMo[2][3];
  s_Z.buildFrom(s_x.get_x(), s_x.get_y(), Z, log(Z/Zd));
  s_Zd.buildFrom(0, 0, Zd, 0); // log(Z/Z*) = 0 that's why the last parameter is 0

  // Add the feature
  task.addFeature(s_Z, s_Zd);

#ifdef VISP_HAVE_DISPLAY
  // Create a window (800 by 500) at position (400, 10) with 3 graphics
  vpPlot graph(3, 800, 500, 400, 10, "Curves...");

  // Init the curve plotter
  graph.initGraph(0,3);
  graph.initGraph(1,2);
  graph.initGraph(2,1);
  graph.setTitle(0, "Velocities");
  graph.setTitle(1, "Error s-s*");
  graph.setTitle(2, "Depth");
  graph.setLegend(0, 0, "vx");
  graph.setLegend(0, 1, "wz");
  graph.setLegend(0, 2, "qdot_pan");
  graph.setLegend(1, 0, "x");
  graph.setLegend(1, 1, "log(Z/Z*)");
  graph.setLegend(2, 0, "Z");
#endif

  try
  {
    int iter = 1;
    for (; ;)
    {
      robot.getPosition(wMc) ;
      cMo = wMc.inverse() * wMo;

      point.track(cMo);

      // Update the current x feature
      vpFeatureBuilder::create(s_x, point);

      // Update log(Z/Z*) feature. Since the depth Z change, we need to update the intection matrix
      Z = point.get_Z() ;
      s_Z.buildFrom(s_x.get_x(), s_x.get_y(), Z, log(Z/Zd));

      robot.get_cVe(cVe);
      task.set_cVe(cVe);
      robot.get_eJe(eJe);
      task.set_eJe(eJe);

      // Compute the control law. Velocities are computed in the mobile robot reference frame
      vpColVector v = task.computeControlLaw();

      // Send the velocity to the robot
      robot.setVelocity(vpRobot::ARTICULAR_FRAME, v);

#ifdef VISP_HAVE_DISPLAY
      graph.plot(0, iter, v); // plot velocities applied to the robot
      graph.plot(1, iter, task.getError()); // plot error vector
      graph.plot(2, 0, iter, Z); // plot the depth
#endif
      iter ++;

      if (task.getError().sumSquare() < 0.0001) {
        std::cout << "Reached a small error. We stop the loop... " << std::endl;
        break;
      }
    }
#ifdef VISP_HAVE_DISPLAY
    const char *legend = "Click to quit...";
    vpDisplay::displayCharString(graph.I, graph.I.getHeight()-60, graph.I.getWidth()-150, legend, vpColor::red);
    vpDisplay::flush(graph.I);
    vpDisplay::getClick(graph.I);
#endif
  }
  catch(...)
  {
  }

  // Kill the servo task
  task.print();
  task.kill();
}