tutorial-simu-pioneer-pan.cpp

#include <iostream>
 
#include <visp3/core/vpConfig.h>
#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()
{
#if defined(ENABLE_VISP_NAMESPACE)
  using namespace VISP_NAMESPACE_NAME;
#endif
  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, static_cast<int>(graph.I.getHeight()) - 60, static_cast<int>(graph.I.getWidth()) - 150, legend, vpColor::red);
    vpDisplay::flush(graph.I);
    vpDisplay::getClick(graph.I);
#endif
 
    // Kill the servo task
    task.print();
  }
  catch (const vpException &e) {
    std::cout << "Catch an exception: " << e << std::endl;
  }
}