visp实例学习---视觉伺服控制Viper850机械臂

处于摸索阶段，记录学习防忘记，实例来源于visp视觉伺服平台，现在看一下实例程序1
servoViper850FourPoints2DArtVelocityLs_cur.cpp

/****************************************************************************
 *
 * Description:
 *   tests the control law
 *   eye-in-hand control
 *   velocity computed in the 关节框架
 *
 
  \example servoViper850FourPoints2DArtVelocityLs_cur.cpp

  \brief Example of eye-in-hand control law. We control here a real robot, the
  Viper S850 robot (arm with 6 degrees of freedom). The velocities resulting
  from visual servo are here joint velocities. Visual features are the image
  coordinates of 4 points. The target is made of 4 dots arranged as a 10cm by
  10cm square.

*****************************************************************************/

#include 
#include  // 调试跟踪

#include 
#include 
#include 
#include 
#include 
#if (defined(VISP_HAVE_VIPER850) && defined(VISP_HAVE_DC1394))

#include 
#include 
#include 
#include 
#include 
#include 
#include 
#include 
#include 
#include 
#include 
#include 
#include 
#include 
#include 
#include 
#include 

#define L 0.05 // 处理一个10厘米乘10厘米的正方形

/*!

  Compute the pose \e cMo from the 3D coordinates of the points \e point and
  their corresponding 2D coordinates \e dot. The pose is computed using a Lowe
  non linear method.
  根据点\ e点的3D坐标及其对应的2D坐标\ e点计算姿态\ e cMo。
  使用Lowe非线性方法计算姿势。

  \param point : 3D coordinates of the points.
  点的3D坐标。

  \param dot : 2D coordinates of the points.
  点的2D坐标。

  \param ndot : Number of points or dots used for the pose estimation.
  用于姿势估计的点或点数。

  \param cam : Intrinsic camera parameters.
  固有摄像机参数

  \param cMo : Homogeneous matrix in output describing the transformation
  between the camera and object frame.
  输出中的齐次矩阵，描述了相机和对象框架之间的转换。

  \param cto : Translation in ouput extracted from \e cMo.
  从\ e cMo提取的输出中的转换。

  \param cro : Rotation in ouput extracted from \e cMo.
  从\ e cMo提取的输出中的旋转。

  \param init : Indicates if the we have to estimate an initial pose with
  Lagrange or Dementhon methods.
  指示我们是否必须使用Lagrange或Dementhon方法估计初始姿势。

*/
void compute_pose(vpPoint point[], vpDot2 dot[], int ndot, vpCameraParameters cam, vpHomogeneousMatrix &cMo,
                  vpTranslationVector &cto, vpRxyzVector &cro, bool init)
{
  vpHomogeneousMatrix cMo_dementhon; // 计算位姿 with dementhon
  vpHomogeneousMatrix cMo_lagrange;  // 计算位姿 with dementhon
  vpRotationMatrix cRo;
  vpPose pose;
  vpImagePoint cog;
  for (int i = 0; i < ndot; i++) {

    double x = 0, y = 0;
    cog = dot[i].getCog();
    vpPixelMeterConversion::convertPoint(cam, cog, x,
                                         y); // 像素到米转换
    point[i].set_x(x);                       // projection perspective          p    投影角度
    point[i].set_y(y);
    pose.addPoint(point[i]);
  }

  if (init == true) {
    pose.computePose(vpPose::DEMENTHON, cMo_dementhon);
    // Compute and return the residual expressed in meter for the pose matrix
    //计算并返回以米表示的姿势矩阵的残差
	// 'cMo'
    double residual_dementhon = pose.computeResidual(cMo_dementhon);
    pose.computePose(vpPose::LAGRANGE, cMo_lagrange);
    double residual_lagrange = pose.computeResidual(cMo_lagrange);

    // Select the best pose to initialize the lowe pose computation
    //选择最佳姿势以初始化Lowe姿势计算
	if (residual_lagrange < residual_dementhon)
      cMo = cMo_lagrange;
    else
      cMo = cMo_dementhon;

  } else { // init = false; 使用之前的姿势初始化LOWE use of the previous pose to initialise LOWE
    cRo.buildFrom(cro);
    cMo.buildFrom(cto, cRo);
  }
  pose.computePose(vpPose::LOWE, cMo);
  cMo.extract(cto);
  cMo.extract(cRo);
  cro.buildFrom(cRo);
}

int main()
{
  // Log file creation in /tmp/$USERNAME/log.dat
  // This file contains by line:
  // - the 6 computed joint velocities (m/s, rad/s) to achieve the task
  // - the 6 mesured joint velocities (m/s, rad/s)
  // - the 6 mesured joint positions (m, rad)
  // - the 8 values of s - s*
  std::string username;
  // Get the user login name
  vpIoTools::getUserName(username);

  // Create a log filename to save velocities...
  std::string logdirname;
  logdirname = "/tmp/" + username;

  // Test if the output path exist. If no try to create it
  if (vpIoTools::checkDirectory(logdirname) == false) {
    try {
      // Create the dirname
      vpIoTools::makeDirectory(logdirname);
    } catch (...) {
      std::cerr << std::endl << "ERROR:" << std::endl;
      std::cerr << "  Cannot create " << logdirname << std::endl;
      return (-1);
    }
  }
  std::string logfilename;
  logfilename = logdirname + "/log.dat";

  // Open the log file name
  std::ofstream flog(logfilename.c_str());

  try {
    vpRobotViper850 robot;
    // Load the end-effector to camera frame transformation obtained
    // using a camera intrinsic model with distortion
	//将末端执行器加载到获得的相机帧转换中
	//使用带失真的相机固有模型
    vpCameraParameters::vpCameraParametersProjType projModel = vpCameraParameters::perspectiveProjWithDistortion;
    robot.init(vpRobotViper850::TOOL_PTGREY_FLEA2_CAMERA, projModel);

    vpServo task;

    vpImage<unsigned char> I;
    int i;

    bool reset = false;
    vp1394TwoGrabber g(reset);
    g.setVideoMode(vp1394TwoGrabber::vpVIDEO_MODE_640x480_MONO8);
    g.setFramerate(vp1394TwoGrabber::vpFRAMERATE_60);
    g.open(I);

    g.acquire(I);

#ifdef VISP_HAVE_X11
    vpDisplayX display(I, 100, 100, "Current image");
#elif defined(VISP_HAVE_OPENCV)
    vpDisplayOpenCV display(I, 100, 100, "Current image");
#elif defined(VISP_HAVE_GTK)
    vpDisplayGTK display(I, 100, 100, "Current image");
#endif

    vpDisplay::display(I);
    vpDisplay::flush(I);

    std::cout << std::endl;
    std::cout << "-------------------------------------------------------" << std::endl;
    std::cout << " Test program for vpServo " << std::endl; //vpServo的测试程序
    std::cout << " Eye-in-hand task control, velocity computed in the joint space" << std::endl;//手眼任务控制，在关节空间中计算速度
    std::cout << " Use of the Afma6 robot " << std::endl;
    std::cout << " task : servo 4 points on a square with dimention " << L << " meters" << std::endl;
    std::cout << "-------------------------------------------------------" << std::endl;
    std::cout << std::endl;

    vpDot2 dot[4];
    vpImagePoint cog;

    std::cout << "Click on the 4 dots clockwise starting from upper/left dot..." << std::endl;

    for (i = 0; i < 4; i++) {
      dot[i].setGraphics(true);
      dot[i].initTracking(I);
      cog = dot[i].getCog();
      vpDisplay::displayCross(I, cog, 10, vpColor::blue);
      vpDisplay::flush(I);
    }

    vpCameraParameters cam;

    // Update camera parameters
    robot.getCameraParameters(cam, I);

    cam.printParameters();

    // Sets the current position of the visual feature
	//设置视觉特征的当前位置
    vpFeaturePoint p[4];
    for (i = 0; i < 4; i++)
      vpFeatureBuilder::create(p[i], cam, dot[i]); // retrieve x,y  of the vpFeaturePoint structure

    // Set the position of the square target in a frame which origin is
    // centered in the middle of the square
	//设置正方形目标在原点居中于正方形中间的框架中的位置
    vpPoint point[4];
    point[0].setWorldCoordinates(-L, -L, 0);
    point[1].setWorldCoordinates(L, -L, 0);
    point[2].setWorldCoordinates(L, L, 0);
    point[3].setWorldCoordinates(-L, L, 0);

    // Initialise a desired pose to compute s*, the desired 2D point features
	//初始化所需的姿势以计算s*，所需的2D点特征
    vpHomogeneousMatrix cMo;
    vpTranslationVector cto(0, 0, 0.5); // tz = 0.5 meter
    vpRxyzVector cro(vpMath::rad(0), vpMath::rad(10), vpMath::rad(20));
    vpRotationMatrix cRo(cro); // Build the rotation matrix
    cMo.buildFrom(cto, cRo);   // Build the homogeneous matrix

    // Sets the desired position of the 2D visual feature
	//设置2D视觉特征的所需位置
    vpFeaturePoint pd[4];
    // Compute the desired position of the features from the desired pose
    //从所需姿势计算特征的所需位置
	for (int i = 0; i < 4; i++) {
      vpColVector cP, p;
      point[i].changeFrame(cMo, cP);
      point[i].projection(cP, p);

      pd[i].set_x(p[0]);
      pd[i].set_y(p[1]);
      pd[i].set_Z(cP[2]);
    }

    // We want to see a point on a point
    for (i = 0; i < 4; i++)
      task.addFeature(p[i], pd[i]);

    // Set the proportional gain
	//设定比例增益
    task.setLambda(0.3);

    // Display task information
    task.print();

    // Define the task
    // - we want an eye-in-hand control law
    // - articular velocity are computed  计算关节速度
    task.setServo(vpServo::EYEINHAND_L_cVe_eJe);
    task.setInteractionMatrixType(vpServo::CURRENT, vpServo::PSEUDO_INVERSE);
    task.print();

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

    // Set the Jacobian (expressed in the end-effector frame)
	//设置雅可比行列式（在末端执行器框中表示）
    vpMatrix eJe;
    robot.get_eJe(eJe);
    task.set_eJe(eJe);
    task.print();

    // Initialise the velocity control of the robot
	//初始化机器人的速度控制
    robot.setRobotState(vpRobot::STATE_VELOCITY_CONTROL);

    std::cout << "\nHit CTRL-C to stop the loop...\n" << std::flush;
    for (;;) {
      // Acquire a new image from the camera
	  //从相机获取新图像
      g.acquire(I);

      // Display this image
      vpDisplay::display(I);

      try {
        // For each point...
        for (i = 0; i < 4; i++) {
          // Achieve the tracking of the dot in the image
		  //实现图像中点的跟踪
          dot[i].track(I);
          // Display a green cross at the center of gravity position in the image
		  //在图像的重心位置显示绿色十字
          cog = dot[i].getCog();
          vpDisplay::displayCross(I, cog, 10, vpColor::green);
        }
      } catch (...) {
        flog.close(); // Close the log file
        vpTRACE("Error detected while tracking visual features");
        robot.stopMotion();
        return (1);
      }

      // During the servo, we compute the pose using LOWE method. For the
      // initial pose used in the non linear minimisation we use the pose
      // computed at the previous iteration.
	  //在伺服期间，我们使用LOWE方法计算姿势。 
	  //对于非线性最小化中使用的初始姿态，
	  //我们使用在上一次迭代中计算出的姿态。
      compute_pose(point, dot, 4, cam, cMo, cto, cro, false);

      for (i = 0; i < 4; i++) {
        // Update the point feature from the dot location
		//从点位置更新点特征
        vpFeatureBuilder::create(p[i], cam, dot[i]);
        // Set the feature Z coordinate from the pose
		//根据姿势设置要素Z坐标
        vpColVector cP;
        point[i].changeFrame(cMo, cP);

        p[i].set_Z(cP[2]);
      }

      // Get the jacobian of the robot
      robot.get_eJe(eJe);
      // Update this jacobian in the task structure. It will be used to
      // compute the velocity skew (as an articular velocity) qdot = -lambda *
      // L^+ * cVe * eJe * (s-s*)
	  //在任务结构中更新此jacobian。 将用于计算速度偏斜（作为关节速度）
      task.set_eJe(eJe);

      vpColVector v;
      // Compute the visual servoing skew vector
	  //计算视觉伺服偏斜向量
      v = task.computeControlLaw();

      // Display the current and desired feature points in the image display
	  //在图像显示中显示当前和所需的特征点
      vpServoDisplay::display(task, cam, I);

      // Apply the computed joint velocities to the robot
	  //将计算出的关节速度应用于机器人
      robot.setVelocity(vpRobot::ARTICULAR_FRAME, v);

      // Save velocities applied to the robot in the log file
      // v[0], v[1], v[2] correspond to joint translation velocities in m/s
      // v[3], v[4], v[5] correspond to joint rotation velocities in rad/s
	  //在日志文件中保存应用于机器人的速度
	  // v [0]，v [1]，v [2]对应于以m / s为单位的联合平移速度
	  // v [3]，v [4]，v [5]对应于关节旋转速度，单位为rad / s
      flog << v[0] << " " << v[1] << " " << v[2] << " " << v[3] << " " << v[4] << " " << v[5] << " ";

      // Get the measured joint velocities of the robot
	  //获取测得的机器人关节速度
      vpColVector qvel;
      robot.getVelocity(vpRobot::ARTICULAR_FRAME, qvel);
      // Save measured joint velocities of the robot in the log file:
      // - qvel[0], qvel[1], qvel[2] correspond to measured joint translation
      //   velocities in m/s
      // - qvel[3], qvel[4], qvel[5] correspond to measured joint rotation
      //   velocities in rad/s
	  //将测得的机器人关节速度保存在日志文件中：
	  //-qvel [0]，qvel [1]，qvel [2]对应于测得的关节平移速度，以m / s为单位
	  //-qvel [3]，qvel [4]，qvel [5]对应于测得的关节旋转速度的弧度/秒
      flog << qvel[0] << " " << qvel[1] << " " << qvel[2] << " " << qvel[3] << " " << qvel[4] << " " << qvel[5] << " ";

      // Get the measured joint positions of the robot
	  //获取测量的机器人关节位置
      vpColVector q;
      robot.getPosition(vpRobot::ARTICULAR_FRAME, q);
      // Save measured joint positions of the robot in the log file
      // - q[0], q[1], q[2] correspond to measured joint translation
      //   positions in m
      // - q[3], q[4], q[5] correspond to measured joint rotation
      //   positions in rad
      flog << q[0] << " " << q[1] << " " << q[2] << " " << q[3] << " " << q[4] << " " << q[5] << " ";

      // Save feature error (s-s*) for the 4 feature points. For each feature
      // point, we have 2 errors (along x and y axis).  This error is
      // expressed in meters in the camera frame
      flog << (task.getError()).t() << std::endl;

      // Flush the display
      vpDisplay::flush(I);

      // std::cout << "|| s - s* || = "  << ( task.getError() ).sumSquare() <<
      // std::endl;
    }

    std::cout << "Display task information: " << std::endl;
    task.print();
    task.kill();
    flog.close(); // Close the log file
    return EXIT_SUCCESS;
  }
  catch (const vpException &e) {
    flog.close(); // Close the log file
    std::cout << "Catch an exception: " << e.getMessage() << std::endl;
    return EXIT_FAILURE;
  }
}

#else
int main()
{
  std::cout << "You do not have an Viper 850 robot connected to your computer..." << std::endl;
  return EXIT_SUCCESS;
}
#endif