/**

\page tutorial-franka-pbvs Tutorial: Eye-in-hand PBVS with Panda 7-dof robot from Franka Emika
\tableofcontents

\section franka_intro 1. Introduction

This tutorial explains how to implement eye-in-hand position-based visual servoing (PBVS) with Franka Emika's Panda
7-dof robot equipped with an Intel Realsense camera mounted on its end-effector.

The following video shows the resulting robot trajectory when the robot is achieving a position-based visual servoing
over an Apriltag target.

\htmlonly
<p align="center"><iframe width="560" height="315" src="https://www.youtube.com/embed/7A5cqUEKXHg" frameborder="0" allow="autoplay; encrypted-media" allowfullscreen></iframe></p>
\endhtmlonly

\section franka_prereq 2. Prerequisites

\subsection franka_prereq_hardware 2.1. Hardware

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 attached to the
  robot end-effector.
Note that this tutorial should also work with any other Intel Realsense camera.

\subsection franka_prereq_rt_linux 2.2. Setting up a real-time kernel

In order to control your robot using `libfranka`, the controller program on the workstation PC must run with real-time
priority under a `PREEMPT_RT` kernel.

1. Install real-time kernel<br>
  This [tutorial](https://frankaemika.github.io/docs/installation_linux.html#setting-up-the-real-time-kernel)
  shows how to proceed to install a real-time kernel from Ubuntu 1.04 to Ubuntu 22.04.
  \note For Ubuntu 22.04, we recommend using the Ubuntu Pro real-time kernel.

2. Allow a user to set real-time permissions for its processes
  - After the `PREEMPT_RT` kernel is installed and running, add a group named `realtime` and add the user controlling
    your robot to this group:
\code{.sh}
$ sudo addgroup realtime
$ sudo usermod -a -G realtime $(whoami)
\endcode
  - Afterwards, add the following limits to the realtime group in `/etc/security/limits.conf`:
\code{.sh}
@realtime soft rtprio 99
@realtime soft priority 99
@realtime soft memlock 102400
@realtime hard rtprio 99
@realtime hard priority 99
@realtime hard memlock 102400
\endcode
  The limits will be applied after you log out and in again.

\subsection franka_prereq_libfranka 2.3. Install Franka library

Our robot is a Franka Research (FR1 or Panda) robot with a system firmware 4.2.2 version. This firmware supports
libfranka 0.9.2. Note that ViSP is known to be compatible with Franka Research 3 (FR3) robot. In that case, you may
install `libfranka` latest release.

\subsubsection franka_prereq_libfranka_0_9_2 2.3.1. libfranka 0.9.2 (Ubuntu)

To install `libfranka 0.9.2` compatible with Franka Research (FR1 or Panda) robot, proceed with:

1. Install dependencies
\code{.sh}
$ sudo apt-get update
$ sudo apt install -y build-essential cmake git libpoco-dev libeigen3-dev
\endcode

2. Clone and build the repository
\code{.sh}
$ cd $VISP_WS/3rdparty
$ git clone --branch 0.9.2 --recursive https://github.com/frankaemika/libfranka libfranka-0.9.2
$ mkdir libfranka-0.9.2/build && cd libfranka-0.9.2/build
$ cmake .. -DCMAKE_BUILD_TYPE=Release -DBUILD_TESTS=OFF -DCMAKE_POLICY_VERSION_MINIMUM=3.5
$ make -j$(nproc)
$ sudo make install
\endcode

\subsubsection franka_prereq_libfranka_0_9_2_win 2.3.2. libfranka 0.9.2 (Windows)

1. Install [vcpkg](https://github.com/microsoft/vcpkg) the C++ Library Manager for Windows.
  - Install `vcpkg` in `%%VISP_WS%/3rdparty` folder by clone from github repository or ensure to have the latest
    version with `git pull`.
    To this end open a Command (`cmd`) line terminal and run:
\code{.sh}
C:\\> cd %VISP_WS%\3rdparty
C:\\> git clone https://github.com/microsoft/vcpkg
\endcode
    or if already cloned:
\code{.sh}
C:\\> cd %VISP_WS%\3rdparty\vcpkg
C:\\> git pull
\endcode
  - Boostrap `vcpkg`
\code{.sh}
C:\\> cd %VISP_WS%\3rdparty\vcpkg
C:\\> bootstrap-vcpkg.bat
\endcode
  - Set `VCPKG_ROOT` environment var accordingly to the path to `vcpkg` folder using a Command (`cmd`) line terminal.
    In our case we set this var using:
\code{.sh}
C:\\> setx  VCPKG_ROOT "%VISP_WS%\3rdparty\vcpkg"
\endcode
  - To help finding `vcpkg` executable, add `%%VCPKG_ROOT%` to the Path environment variable.
    It could be added in a Command (`cmd`) line terminal with:
\code{.sh}
C:\\> setx Path "%Path%;%VCPKG_ROOT%"
\endcode
  - Close all the Command line terminals to ensure that `VCPKG_ROOT` and `Path` environment variables are taken
    into account.

2. Install dependencies
  - Open a new Command (`cmd`) line terminal and install `libfranka` dependencies using `vcpkg`:
\code{.sh}
C:\> vcpkg install poco[net]:x64-windows eigen3
\endcode

3. Clone and build the repository
\code{.sh}
C:\\> cd %VISP_WS%\3rdparty
C:\\> git clone --branch 0.9.2 --recursive https://github.com/frankaemika/libfranka libfranka-0.9.2
C:\\> mkdir libfranka-0.9.2\build && cd libfranka-0.9.2\build
C:\\> cmake .. -DCMAKE_BUILD_TYPE=Release -DBUILD_TESTS=OFF -DCMAKE_POLICY_VERSION_MINIMUM=3.5 -DCMAKE_TOOLCHAIN_FILE=%VCPKG_ROOT%\scripts\buildsystems\vcpkg.cmake -DCMAKE_INSTALL_PREFIX=install
C:\\> cmake --build . --config Release --target install
\endcode

4. Help finding `libfranka` dll files<br>
  To help finding `libfranka.dll`, `PocoNet.dll` and `PocoFoundation.dll`, you may update `Path` environment var with
  the following line (we recommand to use the "system settings" pannel):
\code{.sh}
%VISP_WS%\3rdparty\libfranka-0.9.2\build\install\bin;%VCPKG_ROOT%\installed\x64-windows\bin"
\endcode
  The previous line could also be added in a Command (`cmd`) line terminal:
\code{.sh}
C:\\> setx Path "%Path%;%VISP_WS%\3rdparty\libfranka-0.9.2\build\install\bin;%VCPKG_ROOT%\installed\x64-windows\bin"
\endcode
  At this point, be careful since `setx` truncate the `Path` string to 1024 characters.

\subsubsection franka_prereq_libfranka_latest 2.3.3. libfranka latest (Ubuntu)

As described [here](https://github.com/frankaemika/libfranka/blob/main/README.md),
to install the latest libfranka version, compatible with Franka Research 3 (FR3) robot follow the steps:

1. Install dependencies
\code{.sh}
$ sudo apt-get update
$ sudo apt-get install -y build-essential cmake git libpoco-dev libeigen3-dev libfmt-dev
\endcode
  To use libfranka version 0.14.0 or later, you will need to install `pinocchio` and some more dependencies:
\code{.sh}
$ sudo apt-get install -y lsb-release curl
$ sudo mkdir -p /etc/apt/keyrings
$ curl -fsSL http://robotpkg.openrobots.org/packages/debian/robotpkg.asc | sudo tee /etc/apt/keyrings/robotpkg.asc

$ echo "deb [arch=amd64 signed-by=/etc/apt/keyrings/robotpkg.asc] http://robotpkg.openrobots.org/packages/debian/pub $(lsb_release -cs) robotpkg" | sudo tee /etc/apt/sources.list.d/robotpkg.list

$ sudo apt-get update
$ sudo apt-get install -y robotpkg-pinocchio
\endcode

2. Clone and build the repository
\code{.sh}
$ cd $VISP_WS/3rdparty
$ git clone --recursive https://github.com/frankaemika/libfranka
$ mkdir libfranka/build && cd libfranka/build
$ cmake .. -DCMAKE_BUILD_TYPE=Release -DCMAKE_PREFIX_PATH=/opt/openrobots/lib/cmake -DBUILD_TESTS=OFF
$ make -j$(nproc)
$ sudo make install
\endcode

3. Help finding libpinocchio<br>
  To help finding Pinocchio libraries, you may update `LD_LIBRARY_PATH` env var. The following line could also be added
in `${HOME}/.bashrc` file.
\code{.sh}
$ export LD_LIBRARY_PATH=$LD_LIBRARY_PATH:/opt/openrobots/lib
\endcode

\subsection franka_prereq_librealsense 2.4. Install Realsense library

Following the <a href="https://github.com/IntelRealSense/librealsense/blob/master/doc/installation.md">tutorial</a>,
we recall the main steps here:

\note Since our kernel is 4.12+ streaming Depth/IR/Color is supported and is provided out of the box. This means that
the patches are not needed.

1. Unplug any connected Intel RealSense camera.

2. Install the packages required for `librealsense` build:
\code{.sh}
$ sudo apt-get install git libssl-dev libusb-1.0-0-dev pkg-config libgtk-3-dev cmake-curses-gui
$ sudo apt-get install libglfw3-dev libgl1-mesa-dev libglu1-mesa-dev
\endcode

3. Get `librealsense` from github:
\code{.sh}
$ cd $VISP_WS/3rdparty
$ git clone https://github.com/IntelRealSense/librealsense.git
$ cd librealsense
\endcode

4. Run Intel Realsense permissions script located in `librealsense` root directory:
\code{.sh}
$ sudo ./scripts/setup_udev_rules.sh
\endcode

5. Build and install librealsense
\code{.sh}
$ mkdir build && cd build
$ cmake .. -DBUILD_EXAMPLES=ON -DCMAKE_BUILD_TYPE=Release
$ make -j4
$ sudo make install
\endcode

6. Connect your Realsense camera (we are using a D435) and check if you are able to acquire images running:
\code{.sh}
$ ./examples/capture/rs-capture
\endcode

If you are able to visualize the images, it means that you succeed in `librealsense` installation.

\subsection franka_prereq_target 2.5. Print an Apriltag target

We provide a ready to print `36h11` tag that is 12 by 12 cm square
<a href="http://visp-doc.inria.fr/download/apriltag/tag36_11_00000-120x120.pdf">[download]</a> that you may print.

If you are interested to get other tags, follow the steps described in \ref apriltag_detection_print.

\subsection franka_prereq_calib_extrinsic 2.6. Calibrate extrinsic camera parameters

Follow the steps described in \ref tutorial-calibration-extrinsic-eye-in-hand in order to estimate the end-effector to
camera transformation. This step is mandatory to control the robot in cartesian in the camera frame.

\subsection franka_prereq_visp_build 2.7. Configure and build ViSP

Since you installed new `libfranka` and `librealsense` 3rd parties, you need to configure again ViSP with cmake in
order that ViSP is able to use these libraries.

\subsubsection franka_prereq_visp_build_ubuntu 2.7.1. Ubuntu

- We recap hereafter the steps to configure and build ViSP with `libfranka` and `librealsense` on Ubuntu following
  \ref install_ubuntu_visp_config and \ref install_ubuntu_visp_build instructions.
- To configure ViSP with `libfranka` and `librealsense` using CMake, follow the steps:
\code{.sh}
$ cd $VISP_WS
$ git clone https://github.com/lagadic/visp
$ mkdir visp-build && cd visp-build
$ cmake ../visp
\endcode
- Scroll in the terminal to check that both `Franka` and `Realsense2` are found.
- Then build ViSP using:
\code{.sh}
$ cmake --build . --config Release
\endcode

\subsubsection franka_prereq_visp_build_win 2.7.2. Windows

- To configure ViSP with `libfranka` and `librealsense` using CMake for Windows, follow the steps:
\code{.sh}
C:\\> cd %VISP_WS%
C:\\> git clone https://github.com/lagadic/visp
C:\\> mkdir visp-build-vc17 && cd visp-build-vc17
C:\\> cmake ..\visp -G "Visual Studio 17 2022" -A x64 -DFranka_DIR=%VISP_WS%\3rdparty\libfranka\libfranka-0.9.2\build\install\lib\cmake\Franka -DEigen3_DIR=%VCPKG_ROOT%\installed\x64-windows\share\eigen3
\endcode
- Scroll in the terminal to check that both `Franka` and `Realsense2` are found. You should find lines like
\code{.txt}
--     Use Eigen3:                  yes (ver 3.4.1)
--     Use Franka:                  yes (ver 0.9.2)
--     Use Realsense2:              yes (ver 2.57.3)

--     Eigen3 dir:                  C:\visp-ws\3rdparty\vcpkg\installed\x64-windows\share\eigen3
\endcode
- Then build ViSP using:
\code{.sh}
C:\\> cmake --build . --config Release
\endcode
- Now to help finding ViSP DLLs, add `%%VISP_WS%\visp-build-vc17\x64\vc17\bin\Release` folder to the `Path` environment
  variable.

\subsection franka_configure_ethernet 2.8. Configure Ethernet

Our robot controller has by default IP `192.168.1.1`. Here we show how to configure a laptop that is connected with an
Ethernet cable to the robot controller.

Edit Ethernet connections:

\image html img-netwok-connexion.png

Add a new connexion using "Add" button. Choose the default Ethernet connection type:

\image html img-ethernet-connexion-add.png

Click "Create" button in order to create a new Franka controller connection that has a static IPv4 like `192.168.1.10`
and netmask `255.255.255.0`:

\image html img-franka-ethernet-edit-connection.png

Click "Save" button.

\subsection franka_connect_desk 2.9. Connect to Franka desk

Select the new Ethernet Networks connection named "Franka controller". When the connection is established open a web
browser like Firefox or Chromium and enter the address `https://192.168.1.1/desk`. The first time you will be warned
that the connection is not secure. Click "Advanced" and "Add Exception":

\image html img-franka-firefox-exception.png

Then confirm security exception

\image html img-franka-firefox-confirm-exception.png

When connected, you may release the user-stop button and open brakes:

\image html img-franka-firefox-open-brakes.png

\section franka_pbvs 3. Position-based visual servoing

An example of position-based visual servoing using Panda robot equipped with a Realsense camera is available in
servoFrankaPBVS.cpp.

- Attach your Realsense camera to the robot end-effector
- Put an Apriltag in the camera field of view
- If not already done, follow \ref tutorial-calibration-extrinsic-eye-in-hand to estimate \f$^e{\bf M}_c\f$ the
  homogeneous transformation between robot end-effector and camera frame. We suppose here that the file is located
  in `apps/calibration/intrinsic/eMc.yaml`.

Now enter in `example/servo-franka folder` and run `servoFrankaPBVS` binary using `--eMc` to locate the file containing
the \f$^e{\bf M}_c\f$ transformation. Other options are available. Using `--help` show them:

\code{.sh}
$ cd example/servo-franka
$ ./servoFrankaPBVS --help
\endcode

Run the binary activating the plot and using a constant gain:

\code{.sh}
$ ./servoFrankaPBVS --eMc ../../apps/calibration/intrinsic/eMc.yaml --plot
\endcode

\note If you encounter the following error message:
\code{.sh}
$ ./servoFrankaPBVS
Franka network exception: libfranka: Connection to FCI refused. Please install FCI feature or enable FCI mode in Desk.
\endcode
you need to use your navigator to activate FCI. This new security feature was introduced in Franka system between
version 4.0.2 and 4.2.2. See known issue: \ref franka_pbvs_known_issue_activate_FCI.

Now you should see new window that shows the image from the camera like in the next image. In this window, use the left
mouse click to enable the robot controller, and the right click to quit the binary.

\image html img-franka-pbvs-start.png Legend: Example of initial position. The goal is here to bring the RGB frame attached to the tag over the yellow frame corresponding to the desired position of the tag in the camera frame.

\image html img-franka-pbvs-converge.png Legend: Example of final position reached after position-based visual servoing. In green, you can see the trajectories in the image of the tag corners and tag cog. The latest correspond to the trajectory of the projection in the image of the tag frame origin. The 3D trajectory of this frame is a straight line when the camera extrinsic parameters are well calibrated.

\image html img-franka-pbvs-converge-curves.png  Legend: Corresponding visual-features (translation and orientation of the \e cdMc homogeneous matrix corresponding to the transformation between the desired camera pose and the current one) and velocities applied to the robot in the camera frame. You can observe an exponential decrease of the visual features.

You can also activate an adaptive gain that will make the convergence faster:
\code{.sh}
$ ./servoFrankaPBVS --eMc ../../apps/calibration/intrinsic/eMc.yaml --plot --adpative-gain
\endcode
You can also start the robot with a zero velocity at the beginning introducing task sequencing option:
\code{.sh}
$ ./servoFrankaPBVS --eMc ../../apps/calibration/intrinsic/eMc.yaml --plot --task-sequencing
\endcode
And finally you can activate the adaptive gain and task sequencing:
\code{.sh}
$ ./servoFrankaPBVS --eMc ../../apps/calibration/intrinsic/eMc.yaml --plot --adpative-gain --task-sequencing
\endcode
To learn more about adaptive gain and task sequencing see \ref tutorial-boost-vs.

\section franka_pbvs_known_issue 4. Known issues
\subsection franka_pbvs_known_issue_activate_FCI 4.1. libfranka: Connection to FCI refused

When executing one of the binaries that use `libfranka` you may encounter the following error like:
\code{.sh}
$ ./servoFrankaIBVS --ip 192.168.100.2
Franka network exception: libfranka: Connection to FCI refused. Please install FCI feature or enable FCI mode in Desk.
Check if you are connected to the Franka robot or if you specified the right IP using --ip command line option set by
default to 192.168.1.1.
\endcode

This error occurred with our robot after upgrading the Franka system from 4.0.2 to 4.2.2 version.

\image html img-franka-system-version.jpg The Dashboard shows that after a synchonization using Franka World, our
System version is 4.2.2.

To overcome this error, you need:

- First check if FCI is installed:
\image html img-franka-system-window.jpg When FCI is installed, in Desk you should see this image.

- Secondly activate FCI. For that, as shown in the next image click on `"Activate FCI"` in the Desk right column (1).
  It will open the popup (2) that should remain present all the time you want to execute any binary that needs to
  communicate with the robot using `libfranka`.
\image html img-franka-fci-active.jpg Behavior when FCI (Franka Controller Interface) is activated.

\section franka_pbvs_next 5. Next tutorial

You can continue with the \ref tutorial-franka-ibvs that shows how to implement an image-based visual servoing scheme
with the Franka robot or follow \ref tutorial-ibvs that will give some hints on image-based visual servoing in
simulation with a free flying camera.

If you want to achieve a physical simulation of a Franka robot, with a model that has been accurately identified
from a real Franka robot, like in the next video, we recommend to make a tour on \ref tutorial-franka-sim that is
available in [visp_ros](http://wiki.ros.org/visp_ros). Here you will find a ROS package that allows to implement
position, velocity and impedance control of a simulated Franka robot using ROS and
[CoppeliaSim](https://www.coppeliarobotics.com/).

\htmlonly
<p align="center"><iframe width="560" height="315" src="https://www.youtube.com/embed/QQ89sbmufZE" title="YouTube video player" frameborder="0" allow="accelerometer; autoplay; clipboard-write; encrypted-media; gyroscope; picture-in-picture" allowfullscreen></iframe></p>
\endhtmlonly

*/