tutorial-simu-pioneer-pan.cpp

#include <iostream>

#include <visp3/core/vpHomogeneousMatrix.h>
#include <visp3/core/vpVelocityTwistMatrix.h>
#include <visp3/gui/vpPlot.h>
#include <visp3/robot/vpSimulatorPioneerPan.h>
#include <visp3/visual_features/vpFeatureBuilder.h>
#include <visp3/visual_features/vpFeatureDepth.h>
#include <visp3/visual_features/vpFeaturePoint.h>
#include <visp3/vs/vpServo.h>

int main()
{
  try {
    // 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(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

    int iter = 0;
    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::displayText(graph.I, (int)graph.I.getHeight() - 60, (int)graph.I.getWidth() - 150, legend, vpColor::red);
    vpDisplay::flush(graph.I);
    vpDisplay::getClick(graph.I);
#endif

    // Kill the servo task
    task.print();
    task.kill();
  } catch (vpException &e) {
    std::cout << "Catch an exception: " << e << std::endl;
  }
}