/*!
\page tutorial-franka-ibvs-eth Tutorial: Eye-to-hand IBVS with Panda 7-dof robot from Franka Emika
\tableofcontents

\section franka_eth_ibvs_intro 1. Introduction

This tutorial explains how to achieve an eye-to-hand image-based visual servoing (IBVS) with Franka Emika's Panda 7-dof
robot, with a fixed Realsense camera mounted on a tripod and observing the robot's end-effector, to which we've
attached an Apriltag.

\image html img-franka-calib-eye-to-hand-setup.jpg Panda robot in eye-to-hand configuration.

The proposed image-based visual servoing will consider the four 2D image point visual features corresponding to the
corners of the Apritag.

Let us note:
- \f$ {^e}{\bf J}_e \f$ the robot Jacobian in the end-effector frame relative to the end-effector frame
- \f$ {^c}{\bf V}_e \f$ the 6-by-6 velocity twist matrix corresponding to the \f$ {^c}{\bf M}_e \f$ camera to
  end-effector frames transformation that allows to transform the velocity from point e in end-effector frame to
  point c in camera frame
- \f$ {^c}{\bf V}_f \f$ the 6-by-6 velocity twist matrix corresponding to the \f$ {^c}{\bf M}_f \f$ camera to
  robot reference frames transformation that allows to transform the velocity from point f in robot reference frame to
  point c in camera frame
- \f$ {^f}{\bf V}_e \f$ the 6-by-6 velocity twist matrix corresponding to the \f$ {^f}{\bf M}_e \f$ robot reference to
  robot end-effector frames transformation that allows to transform the velocity from point e in robot end-effector
  frame to point f in robot reference frame
- \f$ {\bf s}(t) \f$ current image point visual features vector
- \f$ {\bf s}^{*} \f$ desired current image point visual features vector
- \f$ {\widehat {\bf L}}_{s} \f$ the interaction matrix corresponding to image point visual features that can be the
  current one updated at each iteration of the loop \f$ {\bf s}(t) \f$, or the desired one \f$ {\bf s}^{*} \f$
- \f$ \lambda \f$ is the gain of the controller
- \f$ {\dot {\bf q}} \f$ the joint velocities to apply tho the robot to achieve the eye-to-hand visual servoing.

In this tutorial we will consider two different eye-to-hand control laws:
- The first one called `"L_cVe_eJe"` implemented in servoFrankaIBVS-EyeToHand-Lcur_cVe_eJe.cpp where joint velocities are
  computed thanks to:
  \f[{\dot {\bf q}} = \lambda \left( {{\widehat {\bf L}}_{e} {^c}{\bf V}_e {^e}{\bf J}_e} \right)^{+} {\bf e}\f]
  Here to build \f$ {^c}{\bf V}_e \f$ there is the need to compute \f$ {^c}{\bf M}_e = {^c}{\bf M}_o \; {^o}{\bf M}_e\f$
  where \f$ {^c}{\bf M}_o \f$ is the pose of the object in the camera frame and \f$ {^o}{\bf M}_e \f$ constant
  transformation is obtained by extrinsic calibration.
- The second one called `"L_cVf_fVe_eJe"` implemented in servoFrankaIBVS-EyeToHand-Ldes_cVf_fVe_eJe.cpp where joint
  velocities are computed thanks to:
  \f[{\dot {\bf q}} = \lambda \left( {{\widehat {\bf L}}_{s=s(t)} {^c}{\bf V}_f {^f}{\bf V}_e {^e}{\bf J}_e} \right)^{+} ({\bf s}(t) - {\bf s}^*)\f]
  This control law is recommended since it doesn't need to compute the pose of the object \f$ {^c}{\bf M}_o \f$.
  Here we will build \f$ {^c}{\bf V}_f \f$ thanks to constant \f$ {^c}{\bf M}_f \f$ transformation obtained by
  extrinsic calibration, while \f$ {^f}{\bf V}_e \f$ is here build thanks to \f$ {^f}{\bf M}_e \f$ given by the
  robot SDK.
- The third one called `"Ldes_cVf_fVe_eJe"` implemented in servoFrankaIBVS-EyeToHand-Ldes_cVf_fVe_eJe.cpp where joint
  velocities are computed thanks to:
  \f[{\dot {\bf q}} = \lambda \left( {{\widehat {\bf L}}_{s=s^{*}} {^c}{\bf V}_f {^f}{\bf V}_e {^e}{\bf J}_e} \right)^{+} ({\bf s}(t) - {\bf s}^*)\f]
  Unlike previous control laws, this control law doesn't need to compute the pose of the object \f$ {^c}{\bf M}_o \f$.
  Here we will build \f$ {^c}{\bf V}_f \f$ thanks to constant \f$ {^c}{\bf M}_f \f$ transformation obtained by
  extrinsic calibration, while \f$ {^f}{\bf V}_e \f$ is here build thanks to \f$ {^f}{\bf M}_e \f$ given by the
  robot odometry.

We suppose here that you have:
- a Panda robot in its research version from <a href="https://franka.de/">Franka Emika</a> that will be controlled throw
  vpRobotFranka class.
- an Intel Realsense <a href="https://www.intelrealsense.com/depth-camera-d435/">D345</a> camera
  mounted on a tripod. Images will be acquired thanks to vpRealSense2 class. Note that this tutorial should also work
  with any other Intel Realsense camera.
- an Apriltag 36h11 attached to the robot end-effector. If needed follow \ref apriltag_detection_print section.

\section franka_eth_prereq 2. Prerequisites

\subsection franka_eth_prereq_data_folder 2.1. Create a folder to host data

The binaries corresponding to servoFrankaIBVS-EyeToHand-Lcur_cVe_eJe.cpp and servoFrankaIBVS-EyeToHand-Ldes_cVf_fVe_eJe.cpp
are build and available in `"${VISP_WS}/visp-build/example/servo-franka/"` folder.
Inside this folder we recommand to create a folder (let say `"data-calib"`) in which we will record data camera
intrinsic and extrinsic calibration

\code{.sh}
$ cd ${VISP_WS}/visp-build/example/servo-franka/
$ mkdir data-calib
\endcode

\subsection franka_eth_prereq_intrinsics 2.2. Calibrate camera intrinsics

Even though the Realsense camera can retrieve intrinsic factory parameters, we recommend calibrating your camera to
obtain more accurate results. To this end follow \ref tutorial-calibration-intrinsic.

To resume:
- Print an OpenCV chessboard [OpenCV_Chessboard.pdf](http://visp-doc.inria.fr/download/calib-grid/OpenCV_Chessboard.pdf)
- Acquire images of the chessboard
\code{.sh}
$ cd ${VISP_WS}/visp-build/example/servo-franka/data-calib
$ ../../../tutorial/grabber/tutorial-grabber-realsense --seqname chessboard-%02d.jpg --record 1
\endcode
- In `"${VISP_WS}/visp-build/example/servo-franka/data-calib/"` folder create `calib-chessboard.cfg` file with
  the following content
\code{.sh}
# Number of inner corners per a item row and column. (square, circle)
BoardSize_Width: 9
BoardSize_Height: 6

# The size of a square in meters
Square_Size: 0.025

# The type of pattern used for camera calibration.
# One of: CHESSBOARD or CIRCLES_GRID
Calibrate_Pattern: CHESSBOARD

# The input image sequence to use for calibration
Input: chessboard-%02d.jpg

# Tempo in seconds between two images. If > 10 wait a click to continue
Tempo: 1
\endcode
- In this file update the `"Square_Size"` parameter set by default to 0.025 m to the size of your printed chessboard
- Run the intrinsic calibration app
\code{.sh}
$ cd ${VISP_WS}/visp-build/example/servo-franka/data-calib/
$ ../../../apps/calibration/intrinsic/visp-calibrate-camera calib-chessboard.cfg --output franka_camera_visp.xml
\endcode
- Results are saved in `"${VISP_WS}/visp-build/example/servo-franka/data-calib/franka_camera_visp.xml"`

\subsection franka_eth_prereq_extrinsics 2.3. Calibrate camera extrinsics

To achieve an eye-to-hand visual servoing depending on the visual-servoing scheme, there is the need to know the
transformation between the robot reference and the camera frames or the transformation between the robot end-effector
and the object (in our case the Apriltag) frames.

The \ref tutorial-calibration-extrinsic-eye-to-hand explains how to estimate both transformations. Follow the steps
corresponding to our use case \ref calib_eth_usecase_franka. To resume:

- The first step is to aquire images of your object, the Apriltag, for different robot end-effector positions.
  Usually we acquire between 6 and 10 images. By default the robot controller IP is `192.168.1.1`. If your Franka has
  an other IP (let say 192.168.30.10) use `"--ip"` option like:
\code{.sh}
$ cd ${VISP_WS}/visp-build/example/servo-franka/data-calib/
$ ../../../apps/calibration/hand-eye/visp-acquire-franka-calib-data --ip 192.168.30.10 --output-folder .
\endcode
- It records the following outputs in `"${VISP_WS}/visp-build/example/servo-franka/data-calib/"` folder:
  - `franka_camera.xml`: XML file that contains the intrinsic camera parameters extracted from camera firmware
  - couples of `franka_image-<number>.png` + `franka_pose_rPe-<number>.txt` with number
    starting from 1. `franka_pose_rPe-<number>.yaml` is the pose of the end-effector expressed in the robot reference
    frame \f$^r{\bf M}_e\f$, while `franka_image-<number>.png` is the image captured at the corresponding robot position.
- Given the camera intrinsic parameters estimated previously and the set of images, compute now the tag poses running
  (adapt the tag size parameter to your use case):
\code{.sh}
$ cd ${VISP_WS}/visp-build/example/servo-franka/data-calib/
$ ../../../apps/calibration/hand-eye/visp-compute-apriltag-poses \
    --tag-size  0.048                   \
    --input     franka_image-%d.png     \
    --intrinsic franka_camera_visp.xml  \
    --output    franka_pose_cPo_%d.yaml
\endcode
- Finally estimate the extrinsic parameters, running:
\code{.sh}
$ cd ${VISP_WS}/visp-build/example/servo-franka/data-calib/
$ ../../../apps/calibration/hand-eye/visp-compute-eye-to-hand-calibration \
    --data-path  .                       \
    --rPe        franka_pose_rPe_%d.yaml \
    --cPo        franka_pose_cPo_%d.yaml \
    --output-rPc franka_rPc.yaml         \
    --output-ePo franka_ePo.yaml
\endcode
- Extrinsic parameters will be saved in `"${VISP_WS}/visp-build/example/servo-franka/data-calib/"` folder where
  you will find
  - `franka_rPc.yaml` file that contains the transformation between the robot reference and the camera frames
  - `franka_ePo.yaml` file that contains the transformation between the end-effector and the apriltag frames.

\subsection franka_eth_prereq_learn 2.4. Learn desired visual features

To ensure that the visual servo loop converges to a desired position within the robot's joint limits, the desired
visual features are learned. More precisely, for each tag corner we will save \f$(x, y, Z)\f$.
Since we are using a 0.048 m large Apriltag we will run
`servoFrankaIBVS-EyeToHand-Lcur_cVe_eJe` binary corresponding to servoFrankaIBVS-EyeToHand-Lcur_cVe_eJe.cpp source code.
\note You can also use the `servoFrankaIBVS-EyeToHand-Ldes_cVf_fVe_eJe` binary corresponding to
servoFrankaIBVS-EyeToHand-Ldes_cVf_fVe_eJe source code with the same command line options.

\warning In our case, we are using an Apriltag 0.048 m wide. Adapt the value of parameter `”--tag-size”` to the
dimensions of your tag.
\code{.sh}
$ cd ${VISP_WS}/visp-build/example/servo-franka/
$ ./servoFrankaIBVS-EyeToHand-Lcur_cVe_eJe \
    --learn-desired-features                               \
    --intrinsic          data-calib/franka_camera_visp.xml \
    --camera-name        Camera                            \
    --tag-size           0.048
\endcode
- It will produce the following printings
\code{.sh}
Parameters:
  Apriltag
    Size [m]              : 0.048
    Z aligned             : false
  Camera intrinsics
    Factory parameters    : no
    Param file name [.xml]: data-calib/franka_camera.xml
    Camera name           : Camera
Found camera with name: "Camera"
Camera parameters used to compute the pose:
Camera parameters for perspective projection with distortion:
  px = 601.0670294	 py = 602.4186662
  u0 = 321.3921431	 v0 = 236.8025954
  kud = 0.05663697965
  kdu = -0.05584121348
\endcode
- Move the robot arm to a desired position in which the Apriltag is visible like the following
\image html img-franka-eth-desired.jpg
- Save the corresponding visual features with a left-click in the image, and then use a right-click to quit the app.
\code{.sh}
Desired visual features saved in: learned_desired_features.txt
\endcode
- The content of the `"learned_desired_features.txt"` file looks like the following in which we have the
  \f$(x, y, Z)\f$ values for each tag corner.
\code{.sh}
$ cat learned_desired_features.txt
 0.243153  -0.152012 0.155126
-0.0636572 -0.146852 0.158983
-0.0669343  0.158418 0.153581
 0.250862   0.161218 0.149723
\endcode

\subsection franka_eth_prereq_resume 2.5. Summary of prerequisite data

At this point in `"${VISP_WS}/visp-build/example/servo-franka/"` folder you should have the following data files
- The learned desired position of the tag
\code{.sh}
$ cd ${VISP_WS}/visp-build/example/servo-franka
$ ls
...
learned_desired_features.txt
\endcode
- The intrinsic and extrinsic parameters:
\code{.sh}
$ ls data-calib
...
franka_camera_visp.xml   franka_ePo.yaml   franka_rPc.yaml
\endcode

\section franka_eth_ibvs 3. Eye-to-hand IBVS

\subsection franka_eth_ibvs_Lcur_cVe_eJe 3.1 Using Lcur_cVe_eJe controller

- You are now ready to start the eye-in-hand image-based visual servoing using `"L_cVe_eJe"` controller implemented
  in servoFrankaIBVS-EyeToHand-Lcur_cVe_eJe.cpp
\code{.sh}
$ cd ${VISP_WS}/visp-build/example/servo-franka/
$ ./servoFrankaIBVS-EyeToHand-Lcur_cVe_eJe \
    --ip          192.168.30.10                     \
    --intrinsic   data-calib/franka_camera_visp.xml \
    --eMo         data-calib/franka_ePo.yaml        \
    --camera-name Camera                            \
    --tag-size    0.048                             \
    --no-convergence-threshold
\endcode

- Visual servoing will start and converge to the desired features like in the following video:
\htmlonly
<p align="center">
<iframe width="560" height="315" src="https://www.youtube.com/embed/81HIbcsaW8A?si=lxVjoCJsh9EU8KBq" title="YouTube video player" frameborder="0" allow="accelerometer; autoplay; clipboard-write; encrypted-media; gyroscope; picture-in-picture; web-share" referrerpolicy="strict-origin-when-cross-origin" allowfullscreen></iframe>
</p>
\endhtmlonly
- Here you can notice the straight line trajectories of the image points.
- The following video shows the corresponding trajectory of the robot:
\htmlonly
<p align="center">
<iframe width="560" height="315" src="https://www.youtube.com/embed/hgIO3csJrvA?si=yeEL7x_wssyuABn0" title="YouTube video player" frameborder="0" allow="accelerometer; autoplay; clipboard-write; encrypted-media; gyroscope; picture-in-picture; web-share" referrerpolicy="strict-origin-when-cross-origin" allowfullscreen></iframe>
</p>
\endhtmlonly

<b>Code explanation</b>
- If you take a look to the source code, you will see that we create a desired feature point vector `p_d` updated by
  reading the values \f$ (x^*, y^*, Z^*) \f$ of the four points from `"learned_desired_features.txt"` file
\snippet servoFrankaIBVS-EyeToHand-Lcur_cVe_eJe.cpp Read desired features
- Then we create the servo task by adding the four current and desired visual feature points
\snippet servoFrankaIBVS-EyeToHand-Lcur_cVe_eJe.cpp Create task
- Next we set the servo task type
\snippet servoFrankaIBVS-EyeToHand-Lcur_cVe_eJe.cpp Set servo task
- Indicating that the current inteaction matrix will be used
\snippet servoFrankaIBVS-EyeToHand-Lcur_cVe_eJe.cpp Set interaction type task
- Thus in the servo while loop the interaction matrix will be computed at each iteration of the servo loop.
  To this end we need to update \f$ (x, y, Z) \f$ for each of the 4 points. For updating \f$ Z \f$ coordinate of each
  point there is the need to compute the pose \f$ {^c}{\bf M}_o \f$ of the object.
\snippet servoFrankaIBVS-EyeToHand-Lcur_cVe_eJe.cpp Detect and estimate tag pose
  Here the tag pose is available in `c_M_o_vec[0]` as an homogeneous matrix
\snippet servoFrankaIBVS-EyeToHand-Lcur_cVe_eJe.cpp Get tag pose
- Updating current visual features `p[i]` is done with
\snippet servoFrankaIBVS-EyeToHand-Lcur_cVe_eJe.cpp Update current visual features
- This pose is here also used to compute
  \f$ {^c}{\bf M}_e = {^c}{\bf M}_o \; {^e}{\bf M}{_o}{^{-1}} \f$ where \f$ {^e}{\bf M}_o \f$ constant homogeneous
  matrix is given by extrinsic calibration and read thanks to `"--eMo"` command line option.
\snippet servoFrankaIBVS-EyeToHand-Lcur_cVe_eJe.cpp Set the camera to end-effector velocity twist matrix transformation
- The Jacobian \f$ {^e}{\bf J}_e \f$ is updated with
\snippet servoFrankaIBVS-EyeToHand-Lcur_cVe_eJe.cpp Set the Jacobian (expressed in the end-effector frame)
- We can now compute the control law corresponding to the joint velocities `q_dot` that we have to apply to the robot
\snippet servoFrankaIBVS-EyeToHand-Lcur_cVe_eJe.cpp Compute basic control law


\subsection franka_eth_ibvs_Lcur_cVf_fVe_eJe 3.2 Using Lcur_cVf_fVe_eJe controller

- To use rather `"Lcur_cVf_fVe_eJe"` controller implemented in servoFrankaIBVS-EyeToHand-Lcur_cVf_fVe_eJe, you may run:
\code{.sh}
$ cd ${VISP_WS}/visp-build/example/servo-franka/
$ ./servoFrankaIBVS-EyeToHand-Lcur_cVf_fVe_eJe \
    --ip          192.168.30.10                     \
    --intrinsic   data-calib/franka_camera_visp.xml \
    --rMc         data-calib/franka_rPc.yaml        \
    --camera-name Camera                            \
    --tag-size    0.048                             \
    --no-convergence-threshold
\endcode

- As in the previous section, we use the current interaction matrix computed at each iteration of the servo loop.
\code
    task.setInteractionMatrixType(vpServo::CURRENT);
\endcode
- That's why we need to update \f$ (x, y, Z) \f$ for each of the 4 feature points. For updating \f$ Z \f$ coordinate
  there is the need to compute the pose \f$ {^c}{\bf M}_o \f$ of the object.
- Visual servoing will start and converge to the desired features like in the following video:
\htmlonly
<p align="center">
<iframe width="560" height="315" src="https://www.youtube.com/embed/CQpHW40h6h4?si=y63FRAFJWB_4epoC" title="YouTube video player" frameborder="0" allow="accelerometer; autoplay; clipboard-write; encrypted-media; gyroscope; picture-in-picture; web-share" referrerpolicy="strict-origin-when-cross-origin" allowfullscreen></iframe></p>
\endhtmlonly
- Here you can also notice the straight line trajectories of the image points.

\subsection franka_eth_ibvs_Ldes_cVf_fVe_eJe 3.3 Using Ldes_cVf_fVe_eJe controller

- To use rather `"Ldes_cVf_fVe_eJe"` controller implemented in servoFrankaIBVS-EyeToHand-Ldes_cVf_fVe_eJe, you may run:
\code{.sh}
$ cd ${VISP_WS}/visp-build/example/servo-franka/
$ ./servoFrankaIBVS-EyeToHand-Ldes_cVf_fVe_eJe \
    --ip          192.168.30.10                     \
    --intrinsic   data-calib/franka_camera_visp.xml \
    --rMc         data-calib/franka_rPc.yaml        \
    --camera-name Camera                            \
    --tag-size    0.048                             \
    --no-convergence-threshold
\endcode

- As explained in \ref franka_eth_ibvs_intro, with this controller you don't need to estimate the pose
  \f$ {^c}{\bf M}_o \f$ of the object. That's why in the source code we will use the interaction matrix at the desired
  position by calling:
\code
    task.setInteractionMatrixType(vpServo::DESIRED);
\endcode

- Visual servoing will start and converge to the desired features like in the following video. As the interaction
  matrix is computed with the desired visual features, you may notice that the trajectories of the points in the image
  are no longer straight lines.
\htmlonly
<p align="center">
<iframe width="560" height="315" src="https://www.youtube.com/embed/HZnH8C4ku3Y?si=MRHGcF7Mqo_g2TkK" title="YouTube video player" frameborder="0" allow="accelerometer; autoplay; clipboard-write; encrypted-media; gyroscope; picture-in-picture; web-share" referrerpolicy="strict-origin-when-cross-origin" allowfullscreen></iframe>
</p>
\endhtmlonly
- Note that with this controller, there's no guarantee that the 4 points used as visual features will remain visible
  in the image.

*/
