diff --git a/.buildinfo b/.buildinfo
new file mode 100644
index 0000000..a816615
--- /dev/null
+++ b/.buildinfo
@@ -0,0 +1,4 @@
+# Sphinx build info version 1
+# This file hashes the configuration used when building these files. When it is not found, a full rebuild will be done.
+config: 17960178d1506ace463f1e6609b84fff
+tags: 645f666f9bcd5a90fca523b33c5a78b7
diff --git a/.nojekyll b/.nojekyll
new file mode 100644
index 0000000..e69de29
diff --git a/_images/accessing-admin.png b/_images/accessing-admin.png
new file mode 100644
index 0000000..7a3ab23
Binary files /dev/null and b/_images/accessing-admin.png differ
diff --git a/_images/activate_fci.png b/_images/activate_fci.png
new file mode 100644
index 0000000..f4f2fa3
Binary files /dev/null and b/_images/activate_fci.png differ
diff --git a/_images/apply_control.png b/_images/apply_control.png
new file mode 100644
index 0000000..676796a
Binary files /dev/null and b/_images/apply_control.png differ
diff --git a/_images/apply_control_settings.png b/_images/apply_control_settings.png
new file mode 100644
index 0000000..8ef2306
Binary files /dev/null and b/_images/apply_control_settings.png differ
diff --git a/_images/collision-coarse.png b/_images/collision-coarse.png
new file mode 100644
index 0000000..31379e4
Binary files /dev/null and b/_images/collision-coarse.png differ
diff --git a/_images/collision-fine.png b/_images/collision-fine.png
new file mode 100644
index 0000000..f6d4c98
Binary files /dev/null and b/_images/collision-fine.png differ
diff --git a/_images/control-static-ip.png b/_images/control-static-ip.png
new file mode 100644
index 0000000..ee4cc79
Binary files /dev/null and b/_images/control-static-ip.png differ
diff --git a/_images/control.png b/_images/control.png
new file mode 100644
index 0000000..c4e1f1b
Binary files /dev/null and b/_images/control.png differ
diff --git a/_images/demos_files.png b/_images/demos_files.png
new file mode 100644
index 0000000..53704fa
Binary files /dev/null and b/_images/demos_files.png differ
diff --git a/_images/dh-diagram.png b/_images/dh-diagram.png
new file mode 100644
index 0000000..34bfab4
Binary files /dev/null and b/_images/dh-diagram.png differ
diff --git a/_images/edit-connections.png b/_images/edit-connections.png
new file mode 100644
index 0000000..8b063d4
Binary files /dev/null and b/_images/edit-connections.png differ
diff --git a/_images/fci-architecture-non-realtime.png b/_images/fci-architecture-non-realtime.png
new file mode 100644
index 0000000..cf37d8e
Binary files /dev/null and b/_images/fci-architecture-non-realtime.png differ
diff --git a/_images/fci-architecture.png b/_images/fci-architecture.png
new file mode 100644
index 0000000..695ec18
Binary files /dev/null and b/_images/fci-architecture.png differ
diff --git a/_images/fci-on.png b/_images/fci-on.png
new file mode 100644
index 0000000..6f4b7e6
Binary files /dev/null and b/_images/fci-on.png differ
diff --git a/_images/frames.svg b/_images/frames.svg
new file mode 100644
index 0000000..151096c
--- /dev/null
+++ b/_images/frames.svg
@@ -0,0 +1,449 @@
+
+
+
+
+
+
diff --git a/_images/franka-gazebo-example-grasp.png b/_images/franka-gazebo-example-grasp.png
new file mode 100644
index 0000000..5dd2ae8
Binary files /dev/null and b/_images/franka-gazebo-example-grasp.png differ
diff --git a/_images/franka-gazebo-example.png b/_images/franka-gazebo-example.png
new file mode 100644
index 0000000..172dfd5
Binary files /dev/null and b/_images/franka-gazebo-example.png differ
diff --git a/_images/get_duration.png b/_images/get_duration.png
new file mode 100644
index 0000000..8f689b3
Binary files /dev/null and b/_images/get_duration.png differ
diff --git a/_images/get_gripper_state.png b/_images/get_gripper_state.png
new file mode 100644
index 0000000..472fd15
Binary files /dev/null and b/_images/get_gripper_state.png differ
diff --git a/_images/get_model.png b/_images/get_model.png
new file mode 100644
index 0000000..c815fb3
Binary files /dev/null and b/_images/get_model.png differ
diff --git a/_images/get_robot_state.png b/_images/get_robot_state.png
new file mode 100644
index 0000000..24fd259
Binary files /dev/null and b/_images/get_robot_state.png differ
diff --git a/_images/get_robot_state_init.png b/_images/get_robot_state_init.png
new file mode 100644
index 0000000..9bf8d1d
Binary files /dev/null and b/_images/get_robot_state_init.png differ
diff --git a/_images/get_robot_state_init_settings.png b/_images/get_robot_state_init_settings.png
new file mode 100644
index 0000000..bfab4b2
Binary files /dev/null and b/_images/get_robot_state_init_settings.png differ
diff --git a/_images/libfranka-architecture.png b/_images/libfranka-architecture.png
new file mode 100644
index 0000000..9a1286b
Binary files /dev/null and b/_images/libfranka-architecture.png differ
diff --git a/_images/matlab_command_window_error_message.png b/_images/matlab_command_window_error_message.png
new file mode 100644
index 0000000..606a79e
Binary files /dev/null and b/_images/matlab_command_window_error_message.png differ
diff --git a/_images/move-groups.png b/_images/move-groups.png
new file mode 100644
index 0000000..e2b84d4
Binary files /dev/null and b/_images/move-groups.png differ
diff --git a/_images/pbv_equations_max.svg b/_images/pbv_equations_max.svg
new file mode 100644
index 0000000..26cba4d
--- /dev/null
+++ b/_images/pbv_equations_max.svg
@@ -0,0 +1,465 @@
+
+
+
\ No newline at end of file
diff --git a/_images/pbv_equations_min.svg b/_images/pbv_equations_min.svg
new file mode 100644
index 0000000..1b9665b
--- /dev/null
+++ b/_images/pbv_equations_min.svg
@@ -0,0 +1,474 @@
+
+
+
\ No newline at end of file
diff --git a/_images/pbv_limits_j1.svg b/_images/pbv_limits_j1.svg
new file mode 100644
index 0000000..4edacf1
--- /dev/null
+++ b/_images/pbv_limits_j1.svg
@@ -0,0 +1,302 @@
+
+
diff --git a/_images/pbv_limits_j2.svg b/_images/pbv_limits_j2.svg
new file mode 100644
index 0000000..906af35
--- /dev/null
+++ b/_images/pbv_limits_j2.svg
@@ -0,0 +1,302 @@
+
+
diff --git a/_images/pbv_limits_j3.svg b/_images/pbv_limits_j3.svg
new file mode 100644
index 0000000..909901f
--- /dev/null
+++ b/_images/pbv_limits_j3.svg
@@ -0,0 +1,302 @@
+
+
diff --git a/_images/pbv_limits_j4.svg b/_images/pbv_limits_j4.svg
new file mode 100644
index 0000000..7c41c43
--- /dev/null
+++ b/_images/pbv_limits_j4.svg
@@ -0,0 +1,302 @@
+
+
diff --git a/_images/pbv_limits_j5.svg b/_images/pbv_limits_j5.svg
new file mode 100644
index 0000000..4f30758
--- /dev/null
+++ b/_images/pbv_limits_j5.svg
@@ -0,0 +1,302 @@
+
+
diff --git a/_images/pbv_limits_j6.svg b/_images/pbv_limits_j6.svg
new file mode 100644
index 0000000..5279f37
--- /dev/null
+++ b/_images/pbv_limits_j6.svg
@@ -0,0 +1,302 @@
+
+
diff --git a/_images/pbv_limits_j7.svg b/_images/pbv_limits_j7.svg
new file mode 100644
index 0000000..dc985ce
--- /dev/null
+++ b/_images/pbv_limits_j7.svg
@@ -0,0 +1,302 @@
+
+
diff --git a/_images/pop_up_fci.png b/_images/pop_up_fci.png
new file mode 100644
index 0000000..c43bee0
Binary files /dev/null and b/_images/pop_up_fci.png differ
diff --git a/_images/ros-architecture.png b/_images/ros-architecture.png
new file mode 100644
index 0000000..41000cf
Binary files /dev/null and b/_images/ros-architecture.png differ
diff --git a/_images/rt-interfaces.png b/_images/rt-interfaces.png
new file mode 100644
index 0000000..f40e894
Binary files /dev/null and b/_images/rt-interfaces.png differ
diff --git a/_images/rt-loop.png b/_images/rt-loop.png
new file mode 100644
index 0000000..0cf9d5d
Binary files /dev/null and b/_images/rt-loop.png differ
diff --git a/_images/simulink_apps_pane.png b/_images/simulink_apps_pane.png
new file mode 100644
index 0000000..59acd50
Binary files /dev/null and b/_images/simulink_apps_pane.png differ
diff --git a/_images/simulink_hardware_pane.png b/_images/simulink_hardware_pane.png
new file mode 100644
index 0000000..9698080
Binary files /dev/null and b/_images/simulink_hardware_pane.png differ
diff --git a/_images/simulink_library_browser.png b/_images/simulink_library_browser.png
new file mode 100644
index 0000000..70e4629
Binary files /dev/null and b/_images/simulink_library_browser.png differ
diff --git a/_images/simulink_view_diagnostics.png b/_images/simulink_view_diagnostics.png
new file mode 100644
index 0000000..bd89565
Binary files /dev/null and b/_images/simulink_view_diagnostics.png differ
diff --git a/_images/simulink_view_errors.png b/_images/simulink_view_errors.png
new file mode 100644
index 0000000..25f36ac
Binary files /dev/null and b/_images/simulink_view_errors.png differ
diff --git a/_images/static-ip-ubuntu.png b/_images/static-ip-ubuntu.png
new file mode 100644
index 0000000..401884b
Binary files /dev/null and b/_images/static-ip-ubuntu.png differ
diff --git a/_images/terminal_error_message.png b/_images/terminal_error_message.png
new file mode 100644
index 0000000..08b194c
Binary files /dev/null and b/_images/terminal_error_message.png differ
diff --git a/_images/visual.png b/_images/visual.png
new file mode 100644
index 0000000..65bddd6
Binary files /dev/null and b/_images/visual.png differ
diff --git a/_images/vscode.png b/_images/vscode.png
new file mode 100644
index 0000000..5eb8d58
Binary files /dev/null and b/_images/vscode.png differ
diff --git a/_images/workspace_parameters.png b/_images/workspace_parameters.png
new file mode 100644
index 0000000..0c750ed
Binary files /dev/null and b/_images/workspace_parameters.png differ
diff --git a/_sources/compatibility.rst.txt b/_sources/compatibility.rst.txt
new file mode 100644
index 0000000..33b4f43
--- /dev/null
+++ b/_sources/compatibility.rst.txt
@@ -0,0 +1,63 @@
+Compatible versions
+===================
+
+.. _compatibility-libfranka:
+
+Compatibility with libfranka
+----------------------------
+
+Different versions of open source components compatible.
+The following table provides an overview. It is recommended to always work with up-to-date versions.
+The ``>=`` indicates that it is not tested against newer robot system versions, meaning
+compatibility is not guaranteed (i.e. libfranka 0.2.0 is not compatible with the robot
+system version 4.0.0).
+
+The Robot system versions 2.x.x are not listed in the table below,
+but are included as compatible with the Robot system version >= 1.3.0. Therefore, these are
+compatible with the libfranka version 0.4.0 and 0.5.0.
+
++----------------------+-------------------+-----------------+--------------------+-----------------+
+| Robot system version | libfranka version | Robot / Gripper | franka_ros version | Ubuntu / ROS |
+| | | Server version | | |
++======================+===================+=================+====================+=================+
+| >= 5.2.0 (FR3) | >= 0.10.0 | 6 / 3 | >= 0.10.0 | 20.04 / noetic |
++----------------------+-------------------+-----------------+--------------------+-----------------+
+| >= 4.2.1 (FER) | >= 0.9.1 < 0.10.0 | 5 / 3 | >= 0.8.0 | 20.04 / noetic |
++----------------------+-------------------+-----------------+--------------------+-----------------+
+| >= 4.0.0 (FER) | >= 0.8.0 | 4 / 3 | >= 0.8.0 | 20.04 / noetic |
++----------------------+-------------------+-----------------+--------------------+-----------------+
+| >= 3.0.0 (FER) | 0.7.1 | 3 / 3 | 0.7.0 | 18.04 / melodic |
++----------------------+-------------------+-----------------+--------------------+-----------------+
+| >= 1.3.0 (FER) | 0.5.0 | 3 / 2 | 0.6.0 | 16.04 / kinetic |
++----------------------+-------------------+-----------------+--------------------+-----------------+
+| >= 1.2.0 (FER) | 0.3.0 | 2 / 2 | 0.4.0 | 16.04 / kinetic |
++----------------------+-------------------+-----------------+--------------------+-----------------+
+| >= 1.1.0 (FER) | 0.2.0 | 2 / 1 | | |
++----------------------+-------------------+-----------------+--------------------+-----------------+
+
+`Robot version line 17
+`_
+and `Gripper version line 17
+`_
+are part of libfranka-common repository, a submodule of libfranka repository.
+
+Franka Matlab compatible versions are located :ref:`here`.
+
+.. caution::
+ Franka Emika currently does not provide any support for Windows or Arm
+
+Compatibility with the kernel
+-----------------------------
+
+There are different kernels, which are compatible with different Ubuntu system versions.
+The following table gives an overview of recommended Kernels.
+
++----------------+----------------------+
+| Kernel version | Ubuntu |
++================+======================+
+| 5.9.1 | 20.04 (Focal Fossa) |
++----------------+----------------------+
+| 5.4.19 | 18.04 (Bionic) |
++----------------+----------------------+
+| 4.14.12 | 16.04 (Xenial Xerus) |
++----------------+----------------------+
diff --git a/_sources/control_parameters.rst.txt b/_sources/control_parameters.rst.txt
new file mode 100644
index 0000000..eb29a50
--- /dev/null
+++ b/_sources/control_parameters.rst.txt
@@ -0,0 +1,275 @@
+.. _control_parameters_specifications:
+
+Robot and interface specifications
+===================================
+Realtime control commands sent to the robot should fulfill *recommended* and *necessary*
+conditions. Recommended conditions should be fulfilled to ensure optimal operation of the
+robot. If necessary conditions are not met then the motion will be aborted.
+
+The final robot trajectory is the result of processing the user-specified trajectory ensuring
+that recommended conditions are fulfilled. As long as necessary conditions are met, the robot
+will try to follow the user-provided trajectory but it will only match the final trajectory
+if it also fulfills recommended conditions. If the necessary conditions are violated, an error
+will abort the motion: if, for instance, the first point of the user defined joint trajectory
+is very different from robot start position (:math:`q(t=0) \neq q_c(t=0)`) a ``start_pose_invalid`` error
+will abort the motion.
+
+Values for the constants used in the equations below are shown in the `Limits for Panda`_ and `Limits for Franka Research 3`_ section.
+
+Joint trajectory requirements
+-----------------------------
+
+Necessary conditions
+********************
+
+- :math:`q_{min} < q_c < q_{max}`
+- :math:`-\dot{q}_{max} < \dot{q}_c < \dot{q}_{max}`
+- :math:`-\ddot{q}_{max} < \ddot{q}_c < \ddot{q}_{max}`
+- :math:`-\dddot{q}_{max} < \dddot{q}_c < \dddot{q}_{max}`
+
+Recommended conditions
+**********************
+
+- :math:`-{\tau_j}_{max} < {\tau_j}_d < {\tau_j}_{max}`
+- :math:`-\dot{\tau_j}_{max} < \dot{\tau_j}_d < \dot{\tau_j}_{max}`
+
+At the beginning of the trajectory, the following conditions should be fulfilled:
+
+- :math:`q = q_c`
+- :math:`\dot{q}_{c} = 0`
+- :math:`\ddot{q}_{c} = 0`
+
+At the end of the trajectory, the following conditions should be fulfilled:
+
+- :math:`\dot{q}_{c} = 0`
+- :math:`\ddot{q}_{c} = 0`
+
+Cartesian trajectory requirements
+---------------------------------
+
+Necessary conditions
+********************
+
+- :math:`T` is proper transformation matrix
+- :math:`-\dot{p}_{max} < \dot{p_c} < \dot{p}_{max}` (Cartesian velocity)
+- :math:`-\ddot{p}_{max} < \ddot{p_c} < \ddot{p}_{max}` (Cartesian acceleration)
+- :math:`-\dddot{p}_{max} < \dddot{p_c} < \dddot{p}_{max}` (Cartesian jerk)
+
+Conditions derived from inverse kinematics:
+
+- :math:`q_{min} < q_c < q_{max}`
+- :math:`-\dot{q}_{max} < \dot{q_c} < \dot{q}_{max}`
+- :math:`-\ddot{q}_{max} < \ddot{q_c} < \ddot{q}_{max}`
+
+Recommended conditions
+**********************
+
+Conditions derived from inverse kinematics:
+
+- :math:`-{\tau_j}_{max} < {\tau_j}_d < {\tau_j}_{max}`
+- :math:`-\dot{\tau_j}_{max} < \dot{{\tau_j}_d} < \dot{\tau_j}_{max}`
+
+At the beginning of the trajectory, the following conditions should be fulfilled:
+
+- :math:`{}^OT_{EE} = {{}^OT_{EE}}_c`
+- :math:`\dot{p}_{c} = 0` (Cartesian velocity)
+- :math:`\ddot{p}_{c} = 0` (Cartesian acceleration)
+
+At the end of the trajectory, the following conditions should be fulfilled:
+
+- :math:`\dot{p}_{c} = 0` (Cartesian velocity)
+- :math:`\ddot{p}_{c} = 0` (Cartesian acceleration)
+
+Controller requirements
+-----------------------
+
+Necessary conditions
+********************
+
+- :math:`-\dot{\tau_j}_{max} < \dot{{\tau_j}_d} < \dot{\tau_j}_{max}`
+
+Recommended conditions
+**********************
+
+- :math:`-{\tau_j}_{max} < {\tau_j}_d < {\tau_j}_{max}`
+
+At the beginning of the trajectory, the following conditions should be fulfilled:
+
+- :math:`{\tau_j}_{d} = 0`
+
+.. _limit_table:
+
+Limits for Panda
+----------------
+
+Limits in the Cartesian space are as follows:\
+
++------------------------+-----------------------------------------------+--------------------------------------------------+--------------------------------------------+
+| Name | Translation | Rotation | Elbow |
++========================+===============================================+==================================================+============================================+
+| :math:`\dot{p}_{max}` | 1.7 :math:`\frac{\text{m}}{\text{s}}` | 2.5 :math:`\frac{\text{rad}}{\text{s}}` | 2.1750 :math:`\frac{rad}{\text{s}}` |
++------------------------+-----------------------------------------------+--------------------------------------------------+--------------------------------------------+
+| :math:`\ddot{p}_{max}` | 13.0 :math:`\frac{\text{m}}{\text{s}^2}` | 25.0 :math:`\frac{\text{rad}}{\text{s}^2}` | 10.0 :math:`\;\frac{rad}{\text{s}^2}` |
++------------------------+-----------------------------------------------+--------------------------------------------------+--------------------------------------------+
+| :math:`\dddot{p}_{max}`| 6500.0 :math:`\frac{\text{m}}{\text{s}^3}` | 12500.0 :math:`\frac{\text{rad}}{\text{s}^3}` | 5000.0 :math:`\;\frac{rad}{\text{s}^3}` |
++------------------------+-----------------------------------------------+--------------------------------------------------+--------------------------------------------+
+
+Joint space limits are:
+
+.. csv-table::
+ :header-rows: 1
+ :file: control-parameters-joint-panda.csv
+
+The arm can reach its maximum extension when joint 4 has angle :math:`q_{elbow-flip}`, where :math:`q_{elbow-flip} = -0.467002423653011\:rad`.
+This parameter is used to determine the flip direction of the elbow.
+
+Limits for Franka Research 3
+----------------------------
+
+Limits in the Cartesian space are as follows:\
+
++------------------------+-----------------------------------------------+--------------------------------------------------+--------------------------------------------+
+| Name | Translation | Rotation | Elbow |
++========================+===============================================+==================================================+============================================+
+| :math:`\dot{p}_{max}` | 3.0 :math:`\frac{\text{m}}{\text{s}}` | 2.5 :math:`\frac{\text{rad}}{\text{s}}` | 2.620 :math:`\frac{rad}{\text{s}}` |
++------------------------+-----------------------------------------------+--------------------------------------------------+--------------------------------------------+
+| :math:`\ddot{p}_{max}` | 9.0 :math:`\frac{\text{m}}{\text{s}^2}` | 17.0 :math:`\frac{\text{rad}}{\text{s}^2}` | 10.0 :math:`\;\frac{rad}{\text{s}^2}` |
++------------------------+-----------------------------------------------+--------------------------------------------------+--------------------------------------------+
+| :math:`\dddot{p}_{max}`| 4500.0 :math:`\frac{\text{m}}{\text{s}^3}` | 8500.0 :math:`\frac{\text{rad}}{\text{s}^3}` | 5000.0 :math:`\;\frac{rad}{\text{s}^3}` |
++------------------------+-----------------------------------------------+--------------------------------------------------+--------------------------------------------+
+
+Joint space limits are:
+
+.. csv-table::
+ :header-rows: 1
+ :file: control-parameters-joint-fr3.csv
+
+The arm can reach its maximum extension when joint 4 has angle :math:`q_{elbow-flip}`, where :math:`q_{elbow-flip} = -0.467002423653011\:rad`.
+This parameter is used to determine the flip direction of the elbow.
+
+
+.. important::
+
+ Note that the maximum joint velocity depends on the joint position. The maximum and minimum joint velocities at a certain joint position are calculated as:
+
+ .. list-table::
+ :class: borderless
+
+ * - .. figure:: _static/pbv_equations_max.svg
+ :align: center
+ :figclass: align-center
+
+ Maximum velocities
+
+ * - .. figure:: _static/pbv_equations_min.svg
+ :align: center
+ :figclass: align-center
+
+ Minimum velocities
+
+
+As most motion planners cannot deal with those functions for describing the velocity limits of each joint but they only deal with
+fixed velocity limits (rectangular limits), we are providing here a suggestion on which values to use for them.
+
+In the figures below the system velocity limits are visualized by the red and blue thresholds while the suggested
+"position-velocity rectangular limits" are visualized in black.
+
+.. list-table:: Visualization of the joint limits of FR3
+ :class: borderless
+
+ * - .. figure:: _static/pbv_limits_j1.svg
+ :align: center
+ :figclass: align-center
+
+ Velocity limits of Joint 1
+
+ - .. figure:: _static/pbv_limits_j2.svg
+ :align: center
+ :figclass: align-center
+
+ Velocity limits of Joint 2
+
+ * - .. figure:: _static/pbv_limits_j3.svg
+ :align: center
+ :figclass: align-center
+
+ Velocity limits of Joint 3
+
+ - .. figure:: _static/pbv_limits_j4.svg
+ :align: center
+ :figclass: align-center
+
+ Velocity limits of Joint 4
+
+ * - .. figure:: _static/pbv_limits_j5.svg
+ :align: center
+ :figclass: align-center
+
+ Velocity limits of Joint 5
+
+ - .. figure:: _static/pbv_limits_j6.svg
+ :align: center
+ :figclass: align-center
+
+ Velocity limits of Joint 6
+
+ * - .. figure:: _static/pbv_limits_j7.svg
+ :align: center
+ :figclass: align-center
+
+ Velocity limits of Joint 7
+ -
+
+
+Here are the parameters describing the suggested position-velocity rectangular limits:
+
+.. csv-table::
+ :header-rows: 1
+ :file: control-parameters-joint-fr3-rectangular.csv
+
+.. important::
+
+ These limits are the values that are used by default in the rate limiter and in the URDF inside :doc:`franka_ros`.
+ However, these are only a suggestion, you are free to define your own rectangles within the specification accordingly to your needs.
+
+ Since FR3 does not inherently implement any restriction to the system limits (red and blue line in the plots above), you are also free
+ to implement your own motion generator to exploit the HW capabilities of FR3 beyond the rectangular limits imposed by existing motion generators.
+
+
+Denavit–Hartenberg parameters
+-----------------------------
+
+The Denavit–Hartenberg parameters for the Panda's kinematic chain are derived following Craig's convention and are as follows:
+
+.. figure:: _static/dh-diagram.png
+ :align: center
+ :figclass: align-center
+
+ Panda's kinematic chain.
+
++-------------+-----------------------+-----------------------+------------------------------+------------------------------+
+| Joint | :math:`a\;(\text{m})` | :math:`d\;(\text{m})` | :math:`\alpha\;(\text{rad})` | :math:`\theta\;(\text{rad})` |
++=============+=======================+=======================+==============================+==============================+
+| Joint 1 | 0 | 0.333 | 0 | :math:`\theta_1` |
++-------------+-----------------------+-----------------------+------------------------------+------------------------------+
+| Joint 2 | 0 | 0 | :math:`-\frac{\pi}{2}` | :math:`\theta_2` |
++-------------+-----------------------+-----------------------+------------------------------+------------------------------+
+| Joint 3 | 0 | 0.316 | :math:`\frac{\pi}{2}` | :math:`\theta_3` |
++-------------+-----------------------+-----------------------+------------------------------+------------------------------+
+| Joint 4 | 0.0825 | 0 | :math:`\frac{\pi}{2}` | :math:`\theta_4` |
++-------------+-----------------------+-----------------------+------------------------------+------------------------------+
+| Joint 5 | -0.0825 | 0.384 | :math:`-\frac{\pi}{2}` | :math:`\theta_5` |
++-------------+-----------------------+-----------------------+------------------------------+------------------------------+
+| Joint 6 | 0 | 0 | :math:`\frac{\pi}{2}` | :math:`\theta_6` |
++-------------+-----------------------+-----------------------+------------------------------+------------------------------+
+| Joint 7 | 0.088 | 0 | :math:`\frac{\pi}{2}` | :math:`\theta_7` |
++-------------+-----------------------+-----------------------+------------------------------+------------------------------+
+| Flange | 0 | 0.107 | 0 | 0 |
++-------------+-----------------------+-----------------------+------------------------------+------------------------------+
+
+
+.. note::
+
+ :math:`{}^0T_{1}` is the transformation matrix which describes the position and orientation of
+ `frame 1` in `frame 0`. A kinematic chain can be calculated like the following:
+ :math:`{}^0T_{2} = {}^0T_{1} * {}^1T_{2}`
diff --git a/_sources/faq.rst.txt b/_sources/faq.rst.txt
new file mode 100644
index 0000000..107d164
--- /dev/null
+++ b/_sources/faq.rst.txt
@@ -0,0 +1,30 @@
+FAQ
+===
+
+How can I use libfranka in my own CMake-based project?
+------------------------------------------------------
+
+Add the following to your ``CMakeLists.txt``:
+
+.. code-block:: cmake
+
+ find_package(Franka REQUIRED)
+
+ # ...
+
+ target_link_libraries( Franka::Franka)
+
+
+To build against a custom ``libfranka`` location, compile
+your project with::
+
+ # Build against libfranka build directory
+ cmake -DFranka_DIR=/path/to/libfranka/build
+
+ # Build against custom libfranka installation
+ cmake -DFranka_DIR=/path/to/usr/lib/cmake/Franka
+
+.. important::
+
+ Don't forget to build your project in release mode
+ (``cmake -DCMAKE_BUILD_TYPE=Release``)!
diff --git a/_sources/fr3-certification-remarks.rst.txt b/_sources/fr3-certification-remarks.rst.txt
new file mode 100644
index 0000000..153f00a
--- /dev/null
+++ b/_sources/fr3-certification-remarks.rst.txt
@@ -0,0 +1,16 @@
+FR3 certification remarks
+=========================
+
+Congratulations on your new Franka Research 3.
+As part of the package you received the documentation including Product Manual Franka Production 3 (110010/1.5/EN).
+Even though its title indicates it is for the other Franka robot arm - FP3,
+it is in general also valid for Franka Research 3.
+We recommend reading this product manual before using the robot. Please be aware that especially
+when using the FCI and in general use cases in research applications there might be some deviations from this manual.
+Therefore, please consult the following table, where the applicability is stated and check the
+referred documentation like the datasheet.
+
+
+.. csv-table::
+ :header-rows: 1
+ :file: fr3-certification-remarks.csv
diff --git a/_sources/franka_matlab/CHANGELOG.md.txt b/_sources/franka_matlab/CHANGELOG.md.txt
new file mode 100644
index 0000000..b2a5b32
--- /dev/null
+++ b/_sources/franka_matlab/CHANGELOG.md.txt
@@ -0,0 +1,46 @@
+# Franka Matlab changelog
+
+## 0.3.1 (07-06-2023)
+
+ - Bugfix. Properly setting the collision threshold values in Simulink.
+
+## 0.3.0 (31-09-2022)
+
+ - Windows 10 support (Experimental mainly due to the non Real-Time nature of the default Windows system).
+ - Project now relies on the leaner "Generic Real-Time" .tlc (grt.tlc) target framework.
+ - Support for XCP communication protocol (vehicle network communication). Data inspector is now enabled!
+ - Support for "Run on custom Hardware" Simulink App for controlling the "Build-deploy-execute-connect" workflow.
+ - Project back-end overall simplification with modified rt_main.cpp for handling the external mode as a separate thread.
+ - **BREAKING** all the Franka Matlab functions are starting with the `franka_` prefix.
+ - Expansion of Matlab library with the functions `franka_communication_test()`, `franka_joint_poses()`, `franka_robot_state()` and `franka_joint_point_to_point_motion()`.
+ - Addition of the Simulink demo, "joint_impedance_control.slx".
+ - Fixing the bug when utilizing the Control Modes "Torque Control - X".
+
+## 0.2.1 (29-04-2022)
+
+ - Adding support for all versions from Matlab2019a till Matlab2021a with libfranka 0.9.0.
+
+## 0.2.0 (31-05-2021)
+
+ - franka_matlab upgrade, supports Matlab2019a, Matlab2019b, Matlab2020a, Matlab2020b, Matlab2021a & libfranka 0.8.0.
+
+## 0.1.1 (01-07-2020)
+
+ - Any dependencies that lead to source code linking for the Simulink Franka Library during mexing removed. That fixes the memory corruption
+ bug when shutting down Matlab.
+ - Simulink Franka Library sFunctions C++ and tlc implementations decoupled from src code necessary for automatic code gen. SRC Folder can be treated separately as a C++ project.
+
+## 0.1.0 (21-01-2020)
+
+ - Features:
+ - **Simulink Library** for **Panda Robot**, includes the following blocks:
+ - **Franka Simulink Interface** for applying the desired control, plus additional parameters.
+ - **Read Initial Robot State** for reading initial values during the first execution step for any desirable signal. The set of the desired signals can be set through the mask in free text form.
+ - **Read Robot State** for reading the values of any desirable signal during execution. The set of the desired signals can be set through the mask in free text form.
+ - **Panda Model** for reading the values of all Model parameters of the Panda Robot during execution.
+ - **Duration Period** for reading the current step, sample time. If communication is not secured during the 1ms, the block will return the value of 2ms or 3ms etc.
+ - **Gripper Read State** for reading the current values out of the Panda Gripper.
+ - **franka_emika_panda.tlc** & **franka_emika_panda_shrlib.tlc** custom linux targets, based on ert, that offer ext mode that is real time capable(package drop in case of main step frame loss).
+ - **Matlab Library**(Experimental, limited support), includes the following command:
+ - **automatic_error_recovery(robot_ip)**. Execute through a matlab command line for getting automatically out of an error state.
+ - **Simulink Library misc**(Experimental, limited support) that includes a set of UI buttons with callback scripts with the potential to automate some of the dev. workflow.
\ No newline at end of file
diff --git a/_sources/franka_matlab/compatibility.rst.txt b/_sources/franka_matlab/compatibility.rst.txt
new file mode 100644
index 0000000..1c1b344
--- /dev/null
+++ b/_sources/franka_matlab/compatibility.rst.txt
@@ -0,0 +1,24 @@
+Compatible versions
+===================
+
+.. _compatibility-franka-matlab:
+
++-----------------------+-------------------+----------------------------+
+| Franka Matlab version | libfranka version | Matlab Version |
++=======================+===================+============================+
+| 0.3.0 | 0.9.x & 0.10.x | :math:`\geq` R2019a |
++-----------------------+-------------------+----------------------------+
+| 0.2.1 | 0.9.x | R2019a to R2021a |
++-----------------------+-------------------+----------------------------+
+| 0.2.0 | 0.8.0 | R2019a to R2021a |
++-----------------------+-------------------+----------------------------+
+| 0.1.1 | 0.7.1 | R2019a |
++-----------------------+-------------------+----------------------------+
+| 0.1.0 | 0.7.1 | R2019a |
++-----------------------+-------------------+----------------------------+
+
+`libfranka and robot system compatibility `_
+
+.. important::
+ Make sure that you've installed and set up Matlab with a compatible compiler version! You can find the list of
+ compatible compilers to Matlab in `this link `_.
\ No newline at end of file
diff --git a/_sources/franka_matlab/franka_matlab_changelog.rst.txt b/_sources/franka_matlab/franka_matlab_changelog.rst.txt
new file mode 100644
index 0000000..04977b9
--- /dev/null
+++ b/_sources/franka_matlab/franka_matlab_changelog.rst.txt
@@ -0,0 +1,2 @@
+.. include:: ./CHANGELOG.md
+ :parser: myst_parser.sphinx_
diff --git a/_sources/franka_matlab/getting_started.rst.txt b/_sources/franka_matlab/getting_started.rst.txt
new file mode 100644
index 0000000..e2dd819
--- /dev/null
+++ b/_sources/franka_matlab/getting_started.rst.txt
@@ -0,0 +1,145 @@
+Getting started
+===============
+
+Overview
+--------
+
+In the demos folder a set of Simulink examples with various implementations is located. Feel free to experiment, adjust
+them and expand them to fit your project needs!
+
+.. figure:: _static/demos_files.png
+ :align: center
+ :figclass: align-center
+
+ Simulink Demos from franka_matlab.
+
+Initialization
+--------------
+
+After opening, double clicking on any of the simulink models a set of parameters will be loaded automatically in the
+workspace.
+
+.. hint::
+
+ The Simulink models are delivered in R2019a version. They will convert automatically to your Matlab version
+ when you try to save the model.
+
+.. figure:: _static/workspace_parameters.png
+ :align: center
+ :figclass: align-center
+
+ Working space after loading a Simulink demo.
+
+The robot_ip is set to 172.16.0.2 by default after loading the demos. Make sure that the robot_ip parameters matches your
+setup, either by modifying it in the `demos/demos_common_config.m` matlab script file or from the cmd line after the
+simulink demo is loaded, like:
+
+.. code-block:: shell
+
+ >> robot_ip =
+
+At this point we can start the building & deployment of the Simulink model.
+
+Execution
+---------
+
+.. hint::
+
+ The current workflow presented is utilizing the "Run on Custom Hardware" Simulink App, present in Matlab versions
+ :math:`\geq` R2020a. In case of Matlab 2019a you can build the model normally by using the "Build Model" button.
+ You then need to run the executable from a terminal as described below.
+
+Let's start by selecting the `Run on custom hardware` App from the Apps pane in Simulink.
+
+.. figure:: _static/simulink_apps_pane.png
+ :align: center
+ :figclass: align-center
+
+ Run on custom hardware Simulink App.
+
+.. important::
+
+ Before executing make sure that the brakes of the robot are disengaged and that the robot is in execution mode!
+
+You can then select from the Hardware tab either `Monitor & Tune` in case monitoring through the external mode is
+desired or `Build, Deploy & Start` for just executing the application without monitoring.
+
+.. figure:: _static/simulink_hardware_pane.png
+ :align: center
+ :figclass: align-center
+
+ Hardware Simulink App.
+
+For running the generated executable manually from a terminal make sure that you've first exported the full libfranka
+build path, in case of Linux:
+
+.. code-block:: shell
+
+ >> $ export LD_LIBRARY_PATH=$LD_LIBRARY_PATH:
+
+or in case of Windows, that you've included the full path of libfranka build directory in the PATH environment variable.
+
+.. hint::
+
+ As a reminder, in case of a robot with system version 4.2.0 or greater, the FCI control mode needs to be explicitly enabled through Desk --> Sidebar menu --> Activate FCI.
+
+.. caution::
+
+ The robot will move! Make sure that you are monitoring the situation, ready to take action if necessary!
+
+You can then run the executable, which is located in the current working space.
+
+In case of Linux:
+
+.. code-block:: shell
+
+ $ ./
+
+or in case of Windows:
+
+.. code-block:: shell
+
+ > .exe
+
+You can manually choose the simple tcpip from the Simulink model settings.
+
+Automatic error recovery
+------------------------
+
+.. figure:: _static/simulink_view_diagnostics.png
+ :align: center
+ :figclass: align-center
+
+ View diagnostic messages during runtime in Simulink.
+
+.. figure:: _static/simulink_view_errors.png
+ :align: center
+ :figclass: align-center
+
+ View error messages in Simulink.
+
+.. figure:: _static/matlab_command_window_error_message.png
+ :align: center
+ :figclass: align-center
+
+ Error message displayed in Matlab Command Window.
+
+.. figure:: _static/terminal_error_message.png
+ :align: center
+ :figclass: align-center
+
+ Error message displayed in terminal in case of manual execution.
+
+If the robot encounters an error state and transitions to reflex mode, you may attempt to recover by executing the `franka_automatic_error_recovery` command in Matlab.
+
+.. code-block:: shell
+
+ >> franka_automatic_error_recovery();
+
+In case the command fails and the robot remains in the erroneous state try using the guiding mode to manually bring
+back the robot to a valid configuration.
+
+.. hint::
+
+ Checkout the :ref:`matlab library ` for a set of helper
+ functions that can help to optimize your workflow.
diff --git a/_sources/franka_matlab/index.rst.txt b/_sources/franka_matlab/index.rst.txt
new file mode 100644
index 0000000..9f9312c
--- /dev/null
+++ b/_sources/franka_matlab/index.rst.txt
@@ -0,0 +1,42 @@
+Franka Matlab
+=============
+
+.. todolist::
+
+.. important::
+ Franka Matlab artifacts can be obtained at `Franka World `_.
+
+.. important::
+ Franka Matlab is based on the `Franka Control Interface (FCI) `_ and
+ specifically the `libfranka C++ interface `_,
+ therefore all the same
+ `system and network requirements `_ apply.
+
+.. hint::
+ The design of the current libraries and tools in the package take as a given that the **build & execution will be performed in the same machine that is running the current Matlab session**. Therefore when we use the term
+ **"deployment", the local PC is targeted**. Manual deployment to other target PCs is of course possible.
+
+Franka Matlab contains the following main components for interfacing the Franka Emika Robot through Matlab & Simulink:
+
+* ``Simulink Franka Library``, a set of simulink blocks for interfacing the Franka Emika Robot, for rapid-prototyping and evaluation of control algorithms.
+
+.. figure:: _static/simulink_library_browser.png
+ :align: center
+ :figclass: align-center
+
+ Simulink Library for rapid-prototyping of controllers for the Franka Emika robot.
+
+* ``Matlab Franka Library``, a set of helper matlab functions for reading current the robot state and applying some simple open loop motion commands.
+
+.. toctree::
+ :maxdepth: 2
+ :caption: Contents:
+
+ requirements
+ compatibility
+ installation
+ getting_started
+ simulink_library
+ matlab_library
+ franka_matlab_changelog
+ troubleshooting
\ No newline at end of file
diff --git a/_sources/franka_matlab/installation.rst.txt b/_sources/franka_matlab/installation.rst.txt
new file mode 100644
index 0000000..e5bc486
--- /dev/null
+++ b/_sources/franka_matlab/installation.rst.txt
@@ -0,0 +1,108 @@
+Installation
+============
+
+Franka Matlab can be downloaded from the `Franka World Hub `_.
+
+Installation on Linux
+---------------------
+
+.. important::
+ Franka Matlab is based on the `Franka Control Interface (FCI) `_ and
+ specifically the `libfranka C++ interface `_,
+ therefore all the same `system and network requirements `_
+ apply.
+
+.. important::
+ Also, make sure that the Real Time Kernel is properly installed as described in the
+ `libfranka documentation `_.
+
+Same as with libfranka the following dependencies are required:
+
+.. code-block:: shell
+
+ $ sudo apt install build-essential cmake git libpoco-dev libeigen3-dev
+
+Add the 1st level of the franka_matlab to the path, by right clicking on the franka_matlab folder and selecting Add to Path >
+Selected Folders.
+
+Alternatively, the following command can be executed:
+
+.. code-block:: shell
+
+ >> addpath();
+
+Initialize the project with:
+
+.. code-block:: shell
+
+ >> init_franka_matlab();
+
+In case no libfranka build is found in the franka_matlab directory, the init script will try to install it automatically.
+Just press `Y` and enter when prompted. By default the init function will try to install libfranka 0.10.0 which is compatible with the
+FR3 robot. For installing the previous libfranka version you can initialize the project with:
+
+.. code-block:: shell
+
+ >> init_franka_matlab('0.9.2');
+
+.. warning::
+ In case you are installing libfranka 0.9.x it is also necessary to mex the Matlab API once:
+
+ .. code-block:: shell
+
+ >> mex_franka_matlab_library();
+
+After the init script is completed you can start utilizing the Simulink and Matlab scripts for the Franka Emika robot.
+
+.. hint::
+ It is highly recommended to start with the provided Simulink demos before writing custom applications from scratch.
+
+Installation on Windows
+-----------------------
+
+.. warning::
+ Support for Windows is still experimental. Issues could arise due to a lack of real-time kernel capabilities.
+ Make sure that your windows system is powerful enough and in top condition.
+
+Please make sure first to install the Visual Studio 2017 community edition (English Version) on a Windows 10 PC.
+
+Make sure that vcpkg is also installed.
+
+.. important::
+ Make sure that the vcpkg path is exposed through the PATH environment variable.
+ You can modify the PATH environment variable in Windows 10:
+
+ 1. Open the Start Search, type in “env”, and choose “Edit the system environment variables”.
+ Alternatively hit *Windows Key + R* at the same time to get command prompt. Then type 'sysdm.cpl',
+ go to advanced and select Environmental Variables.
+ 2. Click the “Environment Variables” button.
+ 3. Under the “System Variables” section, find the row with “Path” in the first column, and click edit.
+ 4. Add the path to the vcpkg executable, like e.g C:\src\vcpkg
+ 5. Verify by opening a terminal and hitting `vcpkg`.
+
+You can then install the 64bit! versions of eigen3 and poco packages:
+
+.. code-block:: shell
+
+ vcpkg install eigen3:x64-windows
+ vcpkg install poco:x64-windows
+
+Same as with linux you can execute in Matlab:
+
+.. code-block:: shell
+
+ >> addpath();
+
+Initialize the project with:
+
+.. code-block:: shell
+
+ >> init_franka_matlab();
+
+or for the previous libfranka version:
+
+.. code-block:: shell
+
+ >> init_franka_matlab('0.9.2');
+
+Press `Y` when prompted for automatically installing libfranka.
\ No newline at end of file
diff --git a/_sources/franka_matlab/matlab_library.rst.txt b/_sources/franka_matlab/matlab_library.rst.txt
new file mode 100644
index 0000000..b4098c1
--- /dev/null
+++ b/_sources/franka_matlab/matlab_library.rst.txt
@@ -0,0 +1,45 @@
+.. _matlab-library:
+
+Matlab Library
+==============
+
+Automatic Error Recovery
+------------------------
+
+.. code-block:: shell
+
+ >> franka_automatic_error_recovery();
+
+Communication Test
+------------------
+
+.. code-block:: shell
+
+ >> franka_communication_test();
+
+Will return a struct with the communication test results.
+
+Joint Point to Point Motion
+---------------------------
+
+.. code-block:: shell
+
+ >> franka_joint_point_to_point_motion(,<7 element double array with target configuration>, <0 to 1 scalar speed factor>);
+
+Get Joint Poses
+---------------
+
+.. code-block:: shell
+
+ >> franka_joint_poses();
+
+Will return a 7 element cell array with the current robot joint poses.
+
+Get Robot State
+---------------
+
+.. code-block:: shell
+
+ >> franka_robot_state();
+
+Will return a struct will the current robot state.
\ No newline at end of file
diff --git a/_sources/franka_matlab/requirements.rst.txt b/_sources/franka_matlab/requirements.rst.txt
new file mode 100644
index 0000000..28856a1
--- /dev/null
+++ b/_sources/franka_matlab/requirements.rst.txt
@@ -0,0 +1,14 @@
+Matlab toolbox dependencies
+===========================
+
+The following Mathworks products are required:
+
+* `Matlab `_
+* `Simulink `_
+* `Simulink Coder `_
+
+The following Mathworks products are required only for being able to build and
+execute all the simulink demos provided:
+
+* `Matlab Coder `_
+* `Stateflow `_
diff --git a/_sources/franka_matlab/simulink_library.rst.txt b/_sources/franka_matlab/simulink_library.rst.txt
new file mode 100644
index 0000000..c0ada49
--- /dev/null
+++ b/_sources/franka_matlab/simulink_library.rst.txt
@@ -0,0 +1,119 @@
+Simulink Library
+================
+
+.. hint::
+ Regarding the inputs/outputs signals nomenclature datatypes and sizes the libfranka definitions
+ have been fully adopted. You can check the list of signals here -->
+ `Robot State Attributes `_.
+ Column-major format for the signal has been adopted as well.
+
+Apply Control
+-------------
+
+.. figure:: _static/apply_control.png
+ :align: center
+ :figclass: align-center
+
+ Apply Control Simulink Block.
+
+This is the main block of the franka simulink library and it is responsible for applying the desired parameters and
+control signals to the robot.
+
+Additionally a set of parameters for the robot can be applied through this block main pane based mostly
+on the desired control mode that is selected from the drop-down list `Control Mode`.
+
+.. figure:: _static/apply_control_settings.png
+ :align: center
+ :figclass: align-center
+
+ Apply Control Simulink Block Settings.
+
+.. hint::
+ If desirable, an initial robot configuration can be applied **!!before!!** the main execution of the control loop.
+ Namely the robot will move to the desirable configuration and only then the main execution of the Simulink model
+ will take place. You can define that in the `Initial Configuration` section of the block settings.
+
+Get Initial Robot State
+-----------------------
+
+.. figure:: _static/get_robot_state_init.png
+ :align: center
+ :figclass: align-center
+
+ Get initial robot state Simulink Block.
+
+This Simulink block will only capture and deliver the initial robot state values which are present during the
+initial execution step.
+
+You can write the signals that you wish to capture in free text form in the `Parameters` section.
+For the set of available signals --> `Robot State Attributes `_
+
+.. figure:: _static/get_robot_state_init_settings.png
+ :align: center
+ :figclass: align-center
+
+ Get initial robot state Simulink Block Settings.
+
+Get Current Robot State
+-----------------------
+
+.. figure:: _static/get_robot_state.png
+ :align: center
+ :figclass: align-center
+
+ Get current robot state Simulink Block.
+
+This Simulink block will capture and deliver the current robot state values.
+
+You can write the signals that you wish to capture in free text form in the `Parameters` section.
+For the set of available signals -->
+`Robot State Attributes `_
+
+Get Duration(Sample Time)
+-------------------------
+
+.. figure:: _static/get_duration.png
+ :align: center
+ :figclass: align-center
+
+ Get duration from last main callback(Sample Time) Simulink Block.
+
+This Simulink block outputs the duration from the last execution step in seconds. Ideally this should be always
+0.001 seconds but due to lost packages during communication errors 0.002 secs or 0.003 secs could be seen.
+
+.. warning::
+ The step count of the Simulink model **does not change** during these communication mishaps!
+ It just continues incrementally although an execution step in reality has been lost!
+ Special design considerations should be therefore demanded especially in the case of
+ sensitive position motion generators.
+ Have a look at, e.g., the generate_cartesian_pose_motion.slx demo to see how the
+ main "clock" of the application has been designed.
+
+Get Model
+---------
+
+.. figure:: _static/get_model.png
+ :align: center
+ :figclass: align-center
+
+ Get robot model numeric values Simulink Block.
+
+This Simulink block will deliver the numerical values of the Model of the Franka Emika robot. For an analytical
+description of the available signals see -->
+`Robot Model Signals `_
+
+Get Gripper State
+-----------------
+
+.. figure:: _static/get_gripper_state.png
+ :align: center
+ :figclass: align-center
+
+ Get current gripper state Simulink Block.
+
+The get gripper state block will inform the application about the current gripper state.
+
+.. hint::
+ Highly recommended to have a look at the
+ `GripperState Struct Reference `_
+ for the list of available signals and the demo `grasp_objects.slx` which is provided for getting started.
\ No newline at end of file
diff --git a/_sources/franka_matlab/troubleshooting.rst.txt b/_sources/franka_matlab/troubleshooting.rst.txt
new file mode 100644
index 0000000..7f8caae
--- /dev/null
+++ b/_sources/franka_matlab/troubleshooting.rst.txt
@@ -0,0 +1,56 @@
+Troubleshooting
+===============
+
+.. hint::
+ Checkout the `Franka Community `_ and the
+ `franka_matlab category `_ for relevant posts or for creating new ones!
+
+libfranka reference
+-------------------
+.. hint::
+ Same error messages and advised troubleshooting as `libfranka `_.
+
+Issues with the graphics driver in Linux
+----------------------------------------
+
+NVIDIA's graphics driver's are not officially supported in Linux with Real-Time Kernel. This could cause issues in graphics renderings in Matlab
+and Simulink, e.g with figures and scopes respectively. We would then recommend starting matlab with the `-softwareopengl` for avoiding these issues:
+
+.. code-block:: shell
+
+ $ matlab -softwareopengl
+
+Mexing Simulink & Matlab library
+--------------------------------
+
+Simulink & Matlab libraries are delivered with compiled binaries. In case issues arise re-mexing is advised.
+
+You can mex the libraries by running:
+
+.. code-block:: shell
+
+ >> mex_simulink_library();
+ >> mex_matlab_library();
+
+Issues with libstdc++.so
+------------------------
+
+It could be that Matlab throws an error related to libstdc++.so.6 after calling one of the matlab scripts of the franka-matlab library in a
+Linux environment. If that's the case our current working solution involves renaming the precompiled libstdc++ library in the Matlab installation,
+which forces Matlab to look in the system for the proper dynamic standard library.
+
+This can be performed with e.g:
+
+.. code-block:: shell
+
+ $ mv matlabroot/sys/os/glnx64/libstdc++.so.6 matlabroot/sys/os/glnx64/libstdc++.so.6.off
+
+Restarting Matlab is then recommended.
+
+Franka Simulink library number of block instances
+-------------------------------------------------
+
+.. important::
+ Multiple instances for the Apply Control block are NOT allowed** in the same system (multiple instances can be
+ inserted in corresponding enabled subsystems). One-and-only-one Apply Control block is necessary for all the other
+ blocks to work. Multiple instances of the rest of the Franka Simulink library blocks ARE allowed.
\ No newline at end of file
diff --git a/_sources/franka_ros.rst.txt b/_sources/franka_ros.rst.txt
new file mode 100644
index 0000000..318af8c
--- /dev/null
+++ b/_sources/franka_ros.rst.txt
@@ -0,0 +1,1254 @@
+franka_ros
+==========
+.. note::
+
+ ``franka_ros`` is not supported on Windows.
+
+Before continuing with this chapter, please :doc:`install or compile franka_ros `.
+
+.. figure:: _static/ros-architecture.png
+ :align: center
+ :figclass: align-center
+
+ Schematic overview of the ``franka_ros`` packages.
+
+The ``franka_ros`` metapackage integrates ``libfranka`` into ROS and ROS control.
+Here, we introduce its packages and
+we also give a short how-to for :ref:`writing controllers `.
+
+All parameters passed to launch files in this section come with default values, so they
+can be omitted if using the default network addresses and ROS namespaces.
+Make sure the ``source`` command was called with the setup script from your workspace:
+
+.. code-block:: shell
+
+ source /path/to/catkin_ws/devel/setup.sh
+
+
+franka_description
+------------------
+
+This package contains the description of our robots and end effectors in terms of kinematics, joint
+limits, visual surfaces and collision space. The collision space is a simplified version of the
+visual description used to improve performance of collision checks. The descriptions are based on
+the URDF format according to the `URDF XML documentation `_ .
+
+In case you want to simulate the Panda robot, you can pass a ``gazebo`` argument to the XACRO file.
+Then the URDF will contain *estimated* inertias values, i.e. link masses and inertia tensors based
+on the `Dynamic Identification of the Franka Emika Panda Robot With Retrieval of Feasible Parameters Using
+Penalty-Based Optimization `_.
+
+.. code-block:: shell
+
+ xacro $(rospack find franka_description)/robots/panda/panda.urdf.xacro gazebo:=true
+
+The same works for FR3:
+
+.. code-block:: shell
+
+ xacro $(rospack find franka_description)/robots/fr3/fr3.urdf.xacro gazebo:=true
+
+
+Collisions Volumes
+""""""""""""""""""
+
+The URDF defines two types of collision types:
+
+ * **Fine**: These collision volumes are made from convex meshes which are approximated and
+ drastically simplified versions of the visual meshes (.dae) of each link. The fine volumes
+ should be used for simulating robot collisions in :ref:`Gazebo `
+ * **Coarse**: These collision volumes are simply capsules (a cylinder with two semispherical
+ end caps) attached to each link and inflated by a certain safety distance. These volumes
+ are more efficient to compute and are used internally in the robot for self-collision
+ avoidance. Use these geometries when you are planning motions e.g. with `MoveIt `_.
+
+
+.. table::
+ :widths: 1 1 1
+
+ +-------------------------------+---------------------------------------+-----------------------------------------+
+ | Visual | Collision (Fine) | Collision (Coarse) |
+ +-------------------------------+---------------------------------------+-----------------------------------------+
+ | .. image:: _static/visual.png | .. image:: _static/collision-fine.png | .. image:: _static/collision-coarse.png |
+ +-------------------------------+---------------------------------------+-----------------------------------------+
+
+To distinguish between the two types of collision models artificial links are inserted in the URDF
+with an ``*_sc`` suffix (for self-collision):
+
+| **panda_link0**
+| ├─ **panda_link0_sc**
+| └─ **panda_link1**
+| ├─ **panda_link1_sc**
+| └─ **panda_link2**
+| ├─ **panda_link2_sc**
+| └─ **panda_link3**
+| ├─ **panda_link3_sc**
+| └─ **panda_link4**
+| ├─ **panda_link4_sc**
+| └─ **panda_link5**
+| ├─ **panda_link5_sc**
+| └─ **panda_link6**
+| ├─ **panda_link6_sc**
+| └─ **panda_link7**
+| ├─ **panda_link7_sc**
+| └─ **panda_link8**
+| ├─ **panda_link8_sc**
+| └─ **panda_hand**
+| ├─ **panda_leftfinger**
+| ├─ **panda_rightfinger**
+| ├─ **panda_hand_sc**
+| └─ **panda_hand_tcp**
+
+You can control which collision model is loaded into your URDF via the ``gazebo`` XACRO argument:
+
+ * ``xacro ... panda.urdf.xacro gazebo:=false``: This will use *both* the fine and coarse collision model.
+ This is also the default if you omit the arg entirely. Use this when you want to use MoveIt
+ * ``xacro ... panda.urdf.xacro gazebo:=true``: This will use *only* the fine collision model. Use
+ this when you want a simulatable URDF i.e. for Gazebo. When using the coarse collision model the robot
+ will of course be in constant collision with the capsules of the next link.
+
+
+.. _franka_gripper:
+
+franka_gripper
+--------------
+This package implements the ``franka_gripper_node`` for interfacing a gripper from ROS.
+The node publishes the state of the gripper and offers the following `actions servers`:
+
+ * ``franka_gripper::MoveAction(width, speed)``: moves to a target ``width`` with the defined
+ ``speed``.
+ * ``franka_gripper::GraspAction(width, epsilon_inner, epsilon_outer, speed, force)``: tries to
+ grasp at the desired ``width`` with a desired ``force`` while closing with the given ``speed``. The
+ operation is successful if the distance :math:`d` between the gripper fingers is:
+ :math:`\text{width} - \epsilon_\text{inner} < d < \text{width} + \epsilon_\text{outer}`.
+ * ``franka_gripper::HomingAction()``: homes the gripper and updates the maximum width given the
+ mounted fingers.
+ * ``franka_gripper::StopAction()``: aborts a running action. This can be used to stop applying
+ forces after grasping.
+ * ``control_msgs::GripperCommandAction(width, max_effort)``: A standard gripper action
+ recognized by MoveIt!. If the argument ``max_effort`` is greater than zero, the gripper
+ will try to grasp an object of the desired ``width``. On the other hand, if ``max_effort`` is
+ zero (:math:`\text{max_effort} < 1^{-4}`), the gripper will move to the desired ``width``.
+
+ .. note::
+
+ Use the argument ``max_effort`` only when grasping an object, otherwise, the gripper will
+ close ignoring the ``width`` argument.
+
+
+You can launch the ``franka_gripper_node`` with:
+
+.. code-block:: shell
+
+ roslaunch franka_gripper franka_gripper.launch robot_ip:=
+
+.. hint::
+
+ Starting with ``franka_ros`` 0.6.0, specifying ``load_gripper:=true`` for
+ ``roslaunch franka_control franka_control.launch`` will start a ``franka_gripper_node`` as well.
+
+
+.. _franka_hw:
+
+franka_hw
+---------
+This package contains the hardware abstraction of the robot for the ROS control framework
+based on the ``libfranka`` API. The hardware class ``franka_hw::FrankaHW`` is implemented in this
+package offering the following interfaces to controllers:
+
++-------------------------------------------------+----------------------------------------------+
+| Interface | Function |
++=================================================+==============================================+
+| ``hardware_interface::JointStateInterface`` | Reads joint states. |
++-------------------------------------------------+----------------------------------------------+
+| ``hardware_interface::VelocityJointInterface`` | Commands joint velocities and reads joint |
+| | states. |
++-------------------------------------------------+----------------------------------------------+
+| ``hardware_interface::PositionJointInterface`` | Commands joint positions and reads joint |
+| | states. |
++-------------------------------------------------+----------------------------------------------+
+| ``hardware_interface::EffortJointInterface`` | Commands joint-level torques and reads |
+| | joint states. |
++-------------------------------------------------+----------------------------------------------+
+| ``franka_hw::FrankaStateInterface`` | Reads the full robot state. |
++-------------------------------------------------+----------------------------------------------+
+| ``franka_hw::FrankaPoseCartesianInterface`` | Commands Cartesian poses and reads the full |
+| | robot state. |
++-------------------------------------------------+----------------------------------------------+
+| ``franka_hw::FrankaVelocityCartesianInterface`` | Commands Cartesian velocities and reads the |
+| | full robot state. |
++-------------------------------------------------+----------------------------------------------+
+| ``franka_hw::FrankaModelInterface`` | Reads the dynamic and kinematic model of the |
+| | robot. |
++-------------------------------------------------+----------------------------------------------+
+
+To use ROS control interfaces, you have to retrieve resource handles by name:
+
++-------------------------------------------------+----------------------------------------+
+| Interface | Resource handle name |
++=================================================+========================================+
+| ``hardware_interface::JointStateInterface`` | "_joint1" to "_joint7" |
++-------------------------------------------------+----------------------------------------+
+| ``hardware_interface::VelocityJointInterface`` | "_joint1" to "_joint7" |
++-------------------------------------------------+----------------------------------------+
+| ``hardware_interface::PositionJointInterface`` | "_joint1" to "_joint7" |
++-------------------------------------------------+----------------------------------------+
+| ``hardware_interface::EffortJointInterface`` | "_joint1" to "_joint7" |
++-------------------------------------------------+----------------------------------------+
+| ``franka_hw::FrankaStateInterface`` | "_robot" |
++-------------------------------------------------+----------------------------------------+
+| ``franka_hw::FrankaPoseCartesianInterface`` | "_robot" |
++-------------------------------------------------+----------------------------------------+
+| ``franka_hw::FrankaVelocityCartesianInterface`` | "_robot" |
++-------------------------------------------------+----------------------------------------+
+| ``franka_hw::FrankaModelInterface`` | "_robot" |
++-------------------------------------------------+----------------------------------------+
+
+.. hint::
+
+ By default, is set to "panda".
+
+The ``franka_hw::FrankaHW`` class also implements the starting, stopping and switching of
+controllers.
+
+The ``FrankaHW`` class also serves as base class for ``FrankaCombinableHW``, a hardware class that
+can be combined with others to control multiple robots from a single controller. The combination of
+an arbitrary number of Panda robots (number configured by parameters) based on ``FrankaCombinableHW``
+for the ROS control framework ``_ is implemented
+in ``FrankaCombinedHW``. The key-difference between ``FrankaHW`` and ``FrankaCombinedHW`` is
+that the latter supports torque control only.
+
+.. important::
+
+ The ``FrankaCombinableHW`` class is available from version 0.7.0 and allows torque/effort control only.
+
+The ROS parameter server is used to determine at runtime which robots are loaded in the combined
+class. For an example on how to configure the ``FrankaCombinedHW`` in the according hardware node,
+see :ref:`franka_control `.
+
+.. note::
+
+ The approach of ``FrankaHW`` is optimal for controlling single robots. Thus we recommend using
+ the ``FrankaCombinableHW``/``FrankaCombinedHW`` classes only for controlling multiple robots.
+
+The interfaces offered by the ``FrankaCombinableHW``/``FrankaCombinedHW`` classes are the following:
+
++-------------------------------------------------+----------------------------------------------+
+| Interface | Function |
++=================================================+==============================================+
+| ``hardware_interface::EffortJointInterface`` | Commands joint-level torques and reads |
+| | joint states. |
++-------------------------------------------------+----------------------------------------------+
+| ``hardware_interface::JointStateInterface`` | Reads joint states. |
++-------------------------------------------------+----------------------------------------------+
+| ``franka_hw::FrankaStateInterface`` | Reads the full robot state. |
++-------------------------------------------------+----------------------------------------------+
+| ``franka_hw::FrankaModelInterface`` | Reads the dynamic and kinematic model of the |
+| | robot. |
++-------------------------------------------------+----------------------------------------------+
+
+The only admissible command interface claim is the ``EffortJointInterface`` which can be combined
+with any set of read-only-interfaces (``FrankaModelInterface``, ``JointStateInterface``,
+``FrankaStateInterface``). The resource handles offered by all interfaces are claimed by name and
+follow the same naming conventions as described for `FrankaHW`. Every instance of
+``FrankaCombinableHW`` offers the complete set of service and action interfaces
+(see :ref:`franka_control `).
+
+.. note::
+
+ The ``FrankaCombinedHW`` class offers an additional action server in the control node namespace
+ to recover all robots. If a reflex or error occurs on any of the robots, the control loop of all
+ robots stops until they are recovered.
+
+.. important::
+
+ ``FrankaHW`` makes use of the ROS `joint_limits_interface `_
+ to `enforce position, velocity and effort safety limits
+ `_.
+ The utilized interfaces are listed below:
+
+ * joint_limits_interface::PositionJointSoftLimitsInterface
+ * joint_limits_interface::VelocityJointSoftLimitsInterface
+ * joint_limits_interface::EffortJointSoftLimitsInterface
+
+ Approaching the limits will result in the (unannounced) modification of the commands.
+
+.. _franka_control:
+
+franka_control
+--------------
+
+The ROS nodes ``franka_control_node`` and ``franka_combined_control_node`` are hardware nodes
+for ROS control that use according hardware classes from ``franka_hw``. They provide a variety
+of ROS services to expose the full ``libfranka`` API in the ROS ecosystem. The following services
+are provided:
+
+ * ``franka_msgs::SetJointImpedance`` specifies joint stiffness for the internal controller
+ (damping is automatically derived from the stiffness).
+ * ``franka_msgs::SetCartesianImpedance`` specifies Cartesian stiffness for the internal
+ controller (damping is automatically derived from the stiffness).
+ * ``franka_msgs::SetEEFrame`` specifies the transformation from _EE (end effector) to
+ _NE (nominal end effector) frame. The transformation from flange to end effector frame
+ is split into two transformations: _EE to _NE frame and _NE to
+ _link8 frame. The transformation from _NE to _link8 frame can only be
+ set through the administrator's interface.
+ * ``franka_msgs::SetKFrame`` specifies the transformation from _K to _EE frame.
+ * ``franka_msgs::SetForceTorqueCollisionBehavior`` sets thresholds for external Cartesian
+ wrenches to configure the collision reflex.
+ * ``franka_msgs::SetFullCollisionBehavior`` sets thresholds for external forces on Cartesian
+ and joint level to configure the collision reflex.
+ * ``franka_msgs::SetLoad`` sets an external load to compensate (e.g. of a grasped object).
+ * ``std_srvs::Trigger`` services allow to connect and disconnect your hardware node
+ (available from 0.8.0). When no active (commanding) controller is running, you can disconnect
+ the hardware node, freeing the respective robots for non-fci applications like e.g. Desk-based
+ operations. Once you want to resume fci operations you can call connect and start your
+ ros_control based controllers again.
+
+.. important::
+
+ The _EE frame denotes the part of the
+ configurable end effector frame which can be adjusted during run time through `franka_ros`. The
+ _K frame marks the center of the internal
+ Cartesian impedance. It also serves as a reference frame for external wrenches. *Neither the
+ _EE nor the _K are contained in the URDF as they can be changed at run time*.
+ By default, is set to "panda".
+
+ .. figure:: _static/frames.svg
+ :align: center
+ :figclass: align-center
+
+ Overview of the end-effector frames.
+
+
+To recover from errors and reflexes when the robot is in reflex mode, you can utilize the
+``franka_msgs::ErrorRecoveryAction``. This can be achieved through either an action client or by publishing on the
+action goal topic.
+
+.. code-block:: shell
+
+ rostopic pub -1 /franka_control/error_recovery/goal franka_msgs/ErrorRecoveryActionGoal "{}"
+
+
+After recovery, the ``franka_control_node`` restarts the controllers that were running. That is
+possible as the node does not die when robot reflexes are triggered or when errors have occurred.
+All of these functionalities are provided by the ``franka_control_node`` which can be launched
+with the following command:
+
+.. code-block:: shell
+
+ roslaunch franka_control franka_control.launch \
+ robot_ip:= # mandatory \
+ load_gripper:= # default: true \
+ robot:= # default: panda
+
+
+Besides loading the ``franka_control_node``, the launch file also starts a
+``franka_control::FrankaStateController`` for reading and publishing the robot states, including
+external wrenches, configurable transforms and the joint states required for visualization with
+rviz. For visualization purposes, a ``robot_state_publisher`` is started.
+
+This package also implements the ``franka_combined_control_node``, a hardware node for ``ros_control`` based
+on the ``franka_hw::FrankaCombinedHW`` class. The set of robots loaded are configured via the ROS parameter
+server. These parameters have to be in the hardware node's namespace (see `franka_combined_control_node.yaml
+`__
+as a reference) and look like this:
+
+.. code-block:: yaml
+
+ robot_hardware:
+ - panda_1
+ - panda_2
+ # (...)
+
+ panda_1:
+ type: franka_hw/FrankaCombinableHW
+ arm_id: panda_1
+ joint_names:
+ - panda_1_joint1
+ - panda_1_joint2
+ - panda_1_joint3
+ - panda_1_joint4
+ - panda_1_joint5
+ - panda_1_joint6
+ - panda_1_joint7
+ # Configure the threshold angle for printing joint limit warnings.
+ joint_limit_warning_threshold: 0.1 # [rad]
+ # Activate rate limiter? [true|false]
+ rate_limiting: true
+ # Cutoff frequency of the low-pass filter. Set to >= 1000 to deactivate.
+ cutoff_frequency: 1000
+ # Internal controller for motion generators [joint_impedance|cartesian_impedance]
+ internal_controller: joint_impedance
+ # Configure the initial defaults for the collision behavior reflexes.
+ collision_config:
+ lower_torque_thresholds_acceleration: [20.0, 20.0, 18.0, 18.0, 16.0, 14.0, 12.0] # [Nm]
+ upper_torque_thresholds_acceleration: [20.0, 20.0, 18.0, 18.0, 16.0, 14.0, 12.0] # [Nm]
+ lower_torque_thresholds_nominal: [20.0, 20.0, 18.0, 18.0, 16.0, 14.0, 12.0] # [Nm]
+ upper_torque_thresholds_nominal: [20.0, 20.0, 18.0, 18.0, 16.0, 14.0, 12.0] # [Nm]
+ lower_force_thresholds_acceleration: [20.0, 20.0, 20.0, 25.0, 25.0, 25.0] # [N, N, N, Nm, Nm, Nm]
+ upper_force_thresholds_acceleration: [20.0, 20.0, 20.0, 25.0, 25.0, 25.0] # [N, N, N, Nm, Nm, Nm]
+ lower_force_thresholds_nominal: [20.0, 20.0, 20.0, 25.0, 25.0, 25.0] # [N, N, N, Nm, Nm, Nm]
+ upper_force_thresholds_nominal: [20.0, 20.0, 20.0, 25.0, 25.0, 25.0] # [N, N, N, Nm, Nm, Nm]
+
+ panda_2:
+ type: franka_hw/FrankaCombinableHW
+ arm_id: panda_2
+ joint_names:
+ - panda_2_joint1
+ - panda_2_joint2
+ - panda_2_joint3
+ - panda_2_joint4
+ - panda_2_joint5
+ - panda_2_joint6
+ - panda_2_joint7
+ # Configure the threshold angle for printing joint limit warnings.
+ joint_limit_warning_threshold: 0.1 # [rad]
+ # Activate rate limiter? [true|false]
+ rate_limiting: true
+ # Cutoff frequency of the low-pass filter. Set to >= 1000 to deactivate.
+ cutoff_frequency: 1000
+ # Internal controller for motion generators [joint_impedance|cartesian_impedance]
+ internal_controller: joint_impedance
+ # Configure the initial defaults for the collision behavior reflexes.
+ collision_config:
+ lower_torque_thresholds_acceleration: [20.0, 20.0, 18.0, 18.0, 16.0, 14.0, 12.0] # [Nm]
+ upper_torque_thresholds_acceleration: [20.0, 20.0, 18.0, 18.0, 16.0, 14.0, 12.0] # [Nm]
+ lower_torque_thresholds_nominal: [20.0, 20.0, 18.0, 18.0, 16.0, 14.0, 12.0] # [Nm]
+ upper_torque_thresholds_nominal: [20.0, 20.0, 18.0, 18.0, 16.0, 14.0, 12.0] # [Nm]
+ lower_force_thresholds_acceleration: [20.0, 20.0, 20.0, 25.0, 25.0, 25.0] # [N, N, N, Nm, Nm, Nm]
+ upper_force_thresholds_acceleration: [20.0, 20.0, 20.0, 25.0, 25.0, 25.0] # [N, N, N, Nm, Nm, Nm]
+ lower_force_thresholds_nominal: [20.0, 20.0, 20.0, 25.0, 25.0, 25.0] # [N, N, N, Nm, Nm, Nm]
+ upper_force_thresholds_nominal: [20.0, 20.0, 20.0, 25.0, 25.0, 25.0] # [N, N, N, Nm, Nm, Nm]
+
+ # (+ more robots ...)
+
+.. note::
+
+ Be sure to choose unique and consistent ``arm_id`` parameters. The IDs must match the prefixes
+ in the joint names and should be according to the robot description loaded to the control
+ node's namespace.
+
+For more information on the parameter based loading of hardware classes, please refer to the
+official documentation of ``combined_robot_hw::CombinedRobotHW`` from
+``_.
+
+A second important parameter file
+(see franka_ros/franka_control/config/default_combined_controllers.yaml as a reference) configures
+a set of default controllers that can be started with the hardware node. The controllers have to match
+the launched hardware. The provided default parameterization (here for 2 robots) looks like:
+
+.. code-block:: yaml
+
+ panda_1_state_controller:
+ type: franka_control/FrankaStateController
+ arm_id: panda_1
+ joint_names:
+ - panda_1_joint1
+ - panda_1_joint2
+ - panda_1_joint3
+ - panda_1_joint4
+ - panda_1_joint5
+ - panda_1_joint6
+ - panda_1_joint7
+ publish_rate: 30 # [Hz]
+
+ panda_2_state_controller:
+ type: franka_control/FrankaStateController
+ arm_id: panda_2
+ joint_names:
+ - panda_2_joint1
+ - panda_2_joint2
+ - panda_2_joint3
+ - panda_2_joint4
+ - panda_2_joint5
+ - panda_2_joint6
+ - panda_2_joint7
+ publish_rate: 30 # [Hz]
+
+ # (+ more controllers ...)
+
+We provide a launch file to run the ``franka_combined_control_node`` with user specified configuration
+files for hardware and controllers which default to a configuration with 2 robots. Launch it with:
+
+.. code-block:: shell
+
+ roslaunch franka_control franka_combined_control.launch \
+ robot_ips:= # mandatory
+ robot:= \
+ args:= \ # if needed
+ robot_id:= \
+ hw_config_file:=\ # includes the robot ips!
+ controllers_file:=\
+ controllers_to_start:=\
+ joint_states_source_list:=
+
+This launch file can be parameterized to run an arbitrary number of robots.
+To do so just write your own configuration files in the style of
+franka_control/config/franka_combined_control_node.yaml and
+franka_ros/franka_control/config/default_combined_controllers.yaml.
+
+.. important::
+
+ Be sure to pass the correct IPs of your robots to `franka_combined_control.launch` as a map.
+ This looks like: `{/robot_ip: , /robot_ip: , ...}`
+
+
+
+.. _ros_visualization:
+
+franka_visualization
+--------------------
+This package contains publishers that connect to a robot and publish the robot and
+gripper joint states for visualization in RViz. To run this package launch:
+
+.. code-block:: shell
+
+ roslaunch franka_visualization franka_visualization.launch robot_ip:= \
+ load_gripper:= robot:=
+
+
+This is purely for visualization - no commands are sent to the robot. It can be useful to check the
+connection to the robot.
+
+.. important::
+
+ Only one instance of a ``franka::Robot`` can connect to the robot. This means, that for example
+ the ``franka_joint_state_publisher`` cannot run in parallel to the ``franka_control_node``.
+ This also implies that you cannot execute the visualization example alongside a separate
+ program running a controller.
+
+
+.. _example_controllers:
+
+franka_example_controllers
+--------------------------
+In this package a set of example controllers for controlling the robot via ROS are implemented.
+The controllers depict the variety of interfaces offered by the ``franka_hw::FrankaHW`` class and
+the according usage. Each example comes with a separate stand-alone launch file that starts the
+controller on the robot and visualizes it.
+
+To launch the joint impedance example, execute the following command:
+
+.. code-block:: shell
+
+ roslaunch franka_example_controllers joint_impedance_example_controller.launch \
+ robot_ip:= load_gripper:= robot:=
+
+Other single Panda examples are started in the same way.
+
+The ``dual_arm_cartesian_impedance_example_controller`` showcases the control of two Panda robots
+based on ``FrankaCombinedHW`` using one realtime controller for fulfilling Cartesian tasks with
+an impedance-based control approach. The example controller can be launched with
+
+.. code-block:: shell
+
+ roslaunch franka_example_controllers \
+ dual_arm_cartesian_impedance_example_controller.launch \
+ robot_id:= \
+ robot_ips:= \
+ rviz:= rqt:=
+
+The example assumes a robot configuration according to `dual_panda_example.urdf.xacro` where two
+Pandas are mounted at 1 meter distance on top of a box. Feel free to replace this robot description
+with one that matches your setup.
+The option `rviz` allows to choose whether a visualization should be launched. With `rqt` the user
+can choose to launch an rqt-gui which allows an online adaption of the rendered end-effector
+impedances at runtime via dynamic reconfigure.
+
+
+.. _franka_gazebo:
+
+franka_gazebo
+-------------
+This package allows you to simulate our robot in `Gazebo `_. This is possible
+because Gazebo is able to integrate into the ROS control framework with the
+`gazebo_ros `_ package.
+
+.. important:: This package is available from 0.8.0
+
+Pick & Place Example
+""""""""""""""""""""""
+Let's dive in and simulate transporting a stone from A to B. Run the following command to start Gazebo with a Panda
+and an example world.
+
+.. code-block:: shell
+
+ roslaunch franka_gazebo panda.launch x:=-0.5 \
+ world:=$(rospack find franka_gazebo)/world/stone.sdf \
+ controller:=cartesian_impedance_example_controller \
+ rviz:=true
+
+This will bring up the Gazebo GUI where you see the environment with the stone and RViz with which you can control
+the end-effector pose of the robot.
+
+
+.. figure:: _static/franka-gazebo-example.png
+ :align: center
+ :figclass: align-center
+
+ Gazebo GUI (left) and RViz (right) of the pick and place example
+
+To open the gripper, simply send a goal to the ``move`` action, similar to how the real ``franka_gripper``
+works. Let's move the gripper to a width of :math:`8\:cm` between the fingers with :math:`10\:\frac{cm}{s}`:
+
+.. code-block:: shell
+
+ rostopic pub --once /franka_gripper/move/goal franka_gripper/MoveActionGoal "goal: { width: 0.08, speed: 0.1 }"
+
+
+Since we launched our robot with the Cartesian Impedance controller from
+:ref:`franka_example_controllers`, we can move the end-effector around, just like in reality,
+with the interactive marker gizmo in RViz. Move the robot such that the white stone is between the fingers of the
+gripper ready to be picked up.
+
+.. note::
+
+ If the robot moves strangely with the elbow, this is because the default nullspace stiffness of the cartesian
+ impedance example controller is set to low. Launch `Dynamic Reconfigure `_
+ and adjust ``panda`` > ``cartesian_impedance_example_controller`` > ``nullspace_stiffness`` if necessary.
+
+To pick up the object, we use the ``grasp`` action this time, since we want to excerpt a force after
+the grasp to not drop the object. The stone is around :math:`3\:cm` wide and :math:`50\:g` heavy.
+Let's grasp it with :math:`5\:N`:
+
+.. code-block:: shell
+
+ rostopic pub --once /franka_gripper/grasp/goal \
+ franka_gripper/GraspActionGoal \
+ "goal: { width: 0.03, epsilon:{ inner: 0.005, outer: 0.005 }, speed: 0.1, force: 5.0}"
+.. note::
+
+ In top menu of Gazebo go to **View** > **Contacts** to visualize contact points and forces
+
+If the grasp succeeded, the fingers will now hold the stone in place. If not, probably the goal tolerances (inner
+and outer epsilon) were too small and the action failed. Now move the object gently over to the red dropoff area.
+
+.. figure:: _static/franka-gazebo-example-grasp.png
+ :align: center
+ :figclass: align-center
+
+ Transport the stone from blue to red
+
+After you placed it gently on the red pad, stop the grasp with the ``stop`` action from the gripper:
+
+.. code-block:: shell
+
+ rostopic pub --once /franka_gripper/stop/goal franka_gripper/StopActionGoal {}
+
+Note that the contact forces disappear now, since no force is applied anymore. Alternatively you can also use
+the ``move`` action.
+
+Customization
+""""""""""""""
+
+The launch file from ``franka_gazebo`` takes a lot of arguments with which you can customize the behavior
+of the simulation. For example to spawn two pandas in one simulation you can use the following:
+
+.. code-block:: xml
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+.. note::
+
+ To see which arguments are supported use: ``roslaunch franka_gazebo panda.launch --ros-args``
+
+FrankaHWSim
+"""""""""""
+
+By default Gazebo ROS can only simulate joints with "standard" hardware interfaces like `Joint State Interfaces`
+and `Joint Command Interfaces`. However our robot is quite different from this architecture! Next to
+these joint-specific interfaces it also supports **robot-specific** interfaces like the ``FrankaModelInterface`` (see
+:ref:`franka_hw `). Naturally gazebo does not understand these custom hardware interfaces by default.
+This is where the ``FrankaHWSim`` plugin comes in.
+
+To make your robot capable of supporting Franka interfaces, simply declare a custom ``robotSimType`` in your URDF:
+
+.. code-block:: xml
+
+
+
+ ${arm_id}
+ 0.001
+ franka_gazebo/FrankaHWSim
+
+ true
+
+
+
+When you spawn this robot with the `model spawner
+`_ this plugin
+will be loaded into the gazebo node. It will scan your URDF and try to find supported hardware interfaces. Up to now
+only some of the interfaces provided by :ref:`franka_hw ` are supported:
+
+
++---+-------------------------------------------------+----------------------------------------------+
+| | Interface | Function |
++===+=================================================+==============================================+
+| ✔ | ``hardware_interface::JointStateInterface`` | Reads joint states. |
++---+-------------------------------------------------+----------------------------------------------+
+| ✔ | ``hardware_interface::EffortJointInterface`` | Commands joint-level torques and reads |
+| | | joint states. |
++---+-------------------------------------------------+----------------------------------------------+
+| ✔ | ``hardware_interface::VelocityJointInterface`` | Commands joint velocities and reads joint |
+| | | states. |
++---+-------------------------------------------------+----------------------------------------------+
+| ✔ | ``hardware_interface::PositionJointInterface`` | Commands joint positions and reads joint |
+| | | states. |
++---+-------------------------------------------------+----------------------------------------------+
+| ✔ | ``franka_hw::FrankaStateInterface`` | Reads the full robot state. |
++---+-------------------------------------------------+----------------------------------------------+
+| ✔ | ``franka_hw::FrankaModelInterface`` | Reads the dynamic and kinematic model of the |
+| | | robot. |
++---+-------------------------------------------------+----------------------------------------------+
+| ✘ | ``franka_hw::FrankaPoseCartesianInterface`` | Commands Cartesian poses and reads the full |
+| | | robot state. |
++---+-------------------------------------------------+----------------------------------------------+
+| ✘ | ``franka_hw::FrankaVelocityCartesianInterface`` | Commands Cartesian velocities and reads the |
+| | | full robot state. |
++---+-------------------------------------------------+----------------------------------------------+
+
+.. important::
+
+ This implies that you can only simulate controllers, that claim these supported interfaces and none other!
+ For example the Cartesian Impedance Example Controller can be simulated, because it only requires the
+ ``EffortJoint``-, ``FrankaState``- and ``FrankaModelInterface``. However the Joint Impedance Example Controller
+ can't be simulated, because it requires the ``FrankaPoseCartesianInterface`` which is not supported yet.
+
+Next to the realtime hardware interfaces the ``FrankaHWSim`` plugin supports some of the non-realtime commands
+that :ref:`franka_control ` supports:
+
+
++---+-------------------------------------------+--------------------------------------------------------------+
+| | Service / Type | Explanation |
++===+===========================================+==============================================================+
+| ✘ | ``set_joint_impedance`` / | Gazebo does not simulate an internal impedance |
+| | `SetJointImpedance`_ | controller, but sets commanded torques directly |
++---+-------------------------------------------+--------------------------------------------------------------+
+| ✘ | ``set_cartesian_impedance`` / | Gazebo does not simulate an internal impedance |
+| | `SetCartesianImpedance`_ | controller, but sets commanded torques directly |
++---+-------------------------------------------+--------------------------------------------------------------+
+| ✔ | ``set_EE_frame`` / | Sets the :math:`{}^{\mathrm{NE}}\mathbf{T}_{\mathrm{EE}}` |
+| | `SetEEFrame`_ | i.e. the homogenous transformation from nominal end-effector |
+| | | to end-effector. You can also initialize this by setting the |
+| | | ROS parameter ``//NE_T_EE``. Normally you would set |
+| | | :math:`{}^{\mathrm{F}}\mathbf{T}_{\mathrm{NE}}` in Desk, but |
+| | | in ``franka_gazebo`` it's assumed as identity if no gripper |
+| | | was specified or defines a rotation around Z by :math:`45\:°`|
+| | | and an offset by :math:`10.34\:cm` (same as Desk for the |
+| | | hand). You can always overwrite this value by setting the ROS|
+| | | parameter ``//NE_T_EE`` manually. |
++---+-------------------------------------------+--------------------------------------------------------------+
+| ✔ | ``set_K_frame`` / | Sets the :math:`{}^{\mathrm{EE}}\mathbf{T}_{\mathrm{K}}` i.e.|
+| | `SetKFrame`_ | the homogenous transformation from end-effector to stiffness |
+| | | frame. |
++---+-------------------------------------------+--------------------------------------------------------------+
+| ✔ | ``set_force_torque_collision_behavior`` / | Sets thresholds above which external wrenches are treated as |
+| | `SetForceTorqueCollisionBehavior`_ | contacts and collisions. |
++---+-------------------------------------------+--------------------------------------------------------------+
+| ✘ | ``set_full_collision_behavior`` / | Not yet implemented |
+| | `SetFullCollisionBehavior`_ | |
++---+-------------------------------------------+--------------------------------------------------------------+
+| ✔ | ``set_load`` / | Sets an external load to compensate its gravity for, e.g. of |
+| | `SetLoad`_ | a grasped object. You can also initialize this by setting |
+| | | the ROS parameters ``//{m_load,I_load,F_x_load}`` |
+| | | for mass, inertia tensor and center of mass for the load, |
+| | | respectively. |
++---+-------------------------------------------+--------------------------------------------------------------+
+| ✔ | ``set_user_stop`` / | This is a special service only available in ``franka_gazebo``|
+| | `std_srvs::SetBool`_ | to simulate the user stop. Pressing the user stop (a.k.a |
+| | (since 0.9.1) | publishing a ``true`` via this service) will *disconnect* |
+| | | all command signals from ROS controllers to be fed to the |
+| | | joints. To connect them again call the ``error_recovery`` |
+| | | action. |
++---+-------------------------------------------+--------------------------------------------------------------+
+
+.. _SetJointImpedance: http://docs.ros.org/en/noetic/api/franka_msgs/html/srv/SetJointImpedance.html
+.. _SetCartesianImpedance: http://docs.ros.org/en/noetic/api/franka_msgs/html/srv/SetCartesianImpedance.html
+.. _SetEEFrame: http://docs.ros.org/en/noetic/api/franka_msgs/html/srv/SetEEFrame.html
+.. _SetKFrame: http://docs.ros.org/en/noetic/api/franka_msgs/html/srv/SetKFrame.html
+.. _SetForceTorqueCollisionBehavior:
+ http://docs.ros.org/en/noetic/api/franka_msgs/html/srv/SetForceTorqueCollisionBehavior.html
+.. _SetFullCollisionBehavior:
+ http://docs.ros.org/en/noetic/api/franka_msgs/html/srv/SetFullCollisionBehavior.html
+.. _SetLoad: http://docs.ros.org/en/noetic/api/franka_msgs/html/srv/SetLoad.html
+.. _std_srvs::SetBool: http://docs.ros.org/en/noetic/api/std_srvs/html/srv/SetBool.html
+
+FrankaGripperSim
+""""""""""""""""
+
+This plugin simulates the :ref:`franka_gripper_node ` in Gazebo. This is done as a ROS controller for
+the two finger joints with a position & force controller. It offers the same five actions like the real gripper node:
+
+* ``//franka_gripper/homing``
+* ``//franka_gripper/stop``
+* ``//franka_gripper/move``
+* ``//franka_gripper/grasp``
+* ``//franka_gripper/gripper_action``
+
+.. important::
+ The ``grasp`` action has a bug, that it will not succeed nor abort if the target width
+ lets the fingers **open**. This is because of missing the joint limits interface which
+ lets the finger oscillate at their limits. For now only use the ``grasp`` action to *close*
+ the fingers!
+
+
+It is assumed that the URDF contains two finger joints which can be force controlled, i.e. have a corresponding
+``EffortJointInterface`` transmission declared. This controller expects the following parameters in its namespace:
+
+* ``type`` (string, required): Should be ``franka_gazebo/FrankaGripperSim``
+* ``arm_id`` (string, required): The arm id of the panda, to infer the name of the finger joints
+* ``finger1/gains/p`` (double, required): The proportional gain for the position-force controller of the first finger
+* ``finger1/gains/i`` (double, default: 0): The integral gain for the position-force controller of the first finger
+* ``finger1/gains/d`` (double, default: 0): The differential gain for the position-force controller of the first finger
+* ``finger2/gains/p`` (double, required): The proportional gain for the position-force controller of the second finger
+* ``finger2/gains/i`` (double, default: 0): The integral gain for the position-force controller of the second finger
+* ``finger2/gains/d`` (double, default: 0): The differential gain for the position-force controller of the second finger
+* ``move/width_tolerance`` (double, default :math:`5\:mm`): The move action succeeds, when the finger width becomes
+ below this threshold
+* ``grasp/resting_threshold`` (double, default :math:`1\:\frac{mm}{s}`): Below which speed the target width should
+ be checked to abort or succeed the grasp action
+* ``grasp/consecutive_samples`` (double, default: 3): How many times the speed has to be consecutively below
+ ``resting_threshold`` before the grasping will be evaluated
+* ``gripper_action/width_tolerance`` (double, default :math:`5\:mm`): The gripper action succeeds, when the finger
+ width becomes below this threshold
+* ``gripper_action/speed`` (double, default :math:`10\:\frac{cm}{s}`): The speed to use during the gripper action
+
+
+
+
+JointStateInterface
+"""""""""""""""""""""
+
+To be able to access the joint state interface from a ROS controller you only have to declare the corresponding
+joint in any transmission tag in the URDF. Then a joint state interface will be automatically available. Usually
+you declare transmission tags for command interfaces like the :ref:`EffortJointInterface `.
+
+.. note::
+ For any joint named ``_jointN`` (with N as integer) FrankaHWSim will automatically compensate its gravity
+ to mimic the behavior of libfranka.
+
+.. _effort_joint_interface:
+
+EffortJointInterface
+""""""""""""""""""""""
+
+To be able to send effort commands from your controller to a joint, you simply declare a transmission tag for the
+joint in your URDF with the corresponding hardware interface type:
+
+.. code-block:: xml
+
+
+ transmission_interface/SimpleTransmission
+
+ hardware_interface/EffortJointInterface
+
+
+ ${transmission}
+
+
+
+
+
+ true
+
+
+
+.. note::
+
+ If you want to be able to read external forces or torques, which come e.g. from collisions, make sure to set the
+ ```` tag to ``true``.
+
+FrankaStateInterface
+""""""""""""""""""""""
+
+This is a **robot-specific** interface and thus a bit different from the normal hardware interfaces.
+To be able to access the franka state interface from your controller declare the following transmission tag with
+all seven joints in your URDF:
+
+.. code-block:: xml
+
+
+ franka_hw/FrankaStateInterface
+ franka_hw/FrankaStateInterface
+ franka_hw/FrankaStateInterface
+ franka_hw/FrankaStateInterface
+ franka_hw/FrankaStateInterface
+ franka_hw/FrankaStateInterface
+ franka_hw/FrankaStateInterface
+ franka_hw/FrankaStateInterface
+
+ franka_hw/FrankaStateInterface
+ franka_hw/FrankaStateInterface
+ franka_hw/FrankaStateInterface
+ franka_hw/FrankaStateInterface
+ franka_hw/FrankaStateInterface
+ franka_hw/FrankaStateInterface
+ franka_hw/FrankaStateInterface
+
+
+When your controller accesses the `robot state
+`_ via the ``FrankaStateInterface`` it can
+expect the following values to be simulated:
+
++---+----------------------------------+------------------------------------------------------------------------+
+| | Field | Comment |
++===+==================================+========================================================================+
+| ✔ | ``O_T_EE`` | |
++---+----------------------------------+------------------------------------------------------------------------+
+| ✘ | ``O_T_EE_d`` | Motion generation not yet supported, field will contain only zeros |
++---+----------------------------------+------------------------------------------------------------------------+
+| ✔ | ``F_T_EE`` | Can be configured via parameters ``F_T_NE``, ``NE_T_EE`` and/or |
+| | | service calls to ``set_EE_frame`` |
++---+----------------------------------+------------------------------------------------------------------------+
+| ✔ | ``NE_T_EE`` | Can be configured via parameter ``NE_T_EE`` and/or service calls |
+| | | to ``set_EE_frame`` |
++---+----------------------------------+------------------------------------------------------------------------+
+| ✔ | ``EE_T_K`` | Can be configured via parameter ``EE_T_K`` and/or service calls |
+| | | to ``set_K_frame`` |
++---+----------------------------------+------------------------------------------------------------------------+
+| ✔ | ``m_ee`` | Will be set from the mass in the inertial tag of URDF, if a hand can |
+| | | be found, otherwise zero. Can be overwritten by parameter ``m_ee`` |
++---+----------------------------------+------------------------------------------------------------------------+
+| ✔ | ``I_ee`` | Will be set from the inertia in the inertial tag of URDF, if a hand |
+| | | be found, otherwise zero. Can be overwritten by parameter ``I_ee`` |
++---+----------------------------------+------------------------------------------------------------------------+
+| ✔ | ``F_x_Cee`` | Will be set from the origin in the inertial tag of URDF, if a hand can |
+| | | be found, otherwise zero. Can be overwritten by parameter ``F_x_Cee`` |
++---+----------------------------------+------------------------------------------------------------------------+
+| ✔ | ``m_load`` | Can be configured via parameter ``m_load`` and/or service calls to |
+| | | ``set_load`` |
++---+----------------------------------+------------------------------------------------------------------------+
+| ✔ | ``I_load`` | Can be configured via parameter ``I_load`` and/or service calls to |
+| | | ``set_load`` |
++---+----------------------------------+------------------------------------------------------------------------+
+| ✔ | ``F_x_Cload`` | Can be configured via parameter ``F_x_Cload`` and/or service calls to |
+| | | ``set_load`` |
++---+----------------------------------+------------------------------------------------------------------------+
+| ✔ | ``m_total`` | |
++---+----------------------------------+------------------------------------------------------------------------+
+| ✔ | ``I_total`` | |
++---+----------------------------------+------------------------------------------------------------------------+
+| ✔ | ``F_x_Ctotal`` | |
++---+----------------------------------+------------------------------------------------------------------------+
+| ✘ | ``elbow`` | |
++---+----------------------------------+------------------------------------------------------------------------+
+| ✘ | ``elbow_d`` | |
++---+----------------------------------+------------------------------------------------------------------------+
+| ✘ | ``elbow_c`` | |
++---+----------------------------------+------------------------------------------------------------------------+
+| ✘ | ``delbow_d`` | |
++---+----------------------------------+------------------------------------------------------------------------+
+| ✘ | ``delbow_c`` | |
++---+----------------------------------+------------------------------------------------------------------------+
+| ✔ | ``tau_J`` | Comes directly from Gazebo |
++---+----------------------------------+------------------------------------------------------------------------+
+| ✔ | ``tau_J_d`` | The values send by your effort controller. Zero otherwise. |
++---+----------------------------------+------------------------------------------------------------------------+
+| ✔ | ``dtau_J`` | Numerical derivative of ``tau_J`` |
++---+----------------------------------+------------------------------------------------------------------------+
+| ✔ | ``q`` | Comes directly from Gazebo |
++---+----------------------------------+------------------------------------------------------------------------+
+| ✔ | ``q_d`` | The last commanded joint position when using the position interface. |
+| | | Same as ``q`` when using the velocity interface. However, |
+| | | the value will not be updated when using the effort interface. |
++---+----------------------------------+------------------------------------------------------------------------+
+| ✔ | ``dq`` | Comes directly from Gazebo |
++---+----------------------------------+------------------------------------------------------------------------+
+| ✔ | ``dq_d`` | The last commanded joint velocity when using the velocity interface. |
+| | | Same as ``dq`` when using the position interface. However, |
+| | | the value will be zero when using the effort interface. |
++---+----------------------------------+------------------------------------------------------------------------+
+| ✔ | ``ddq_d`` | Current acceleration when using the position or velocity interface. |
+| | | However, the value will be zero when using the effort interface. |
++---+----------------------------------+------------------------------------------------------------------------+
+| ✔ | ``joint_contact`` | :math:`\mid \hat{\tau}_{ext} \mid > \mathrm{thresh}_{lower}` where the |
+| | | threshold can be set by calling ``set_force_torque_collision_behavior``|
++---+----------------------------------+------------------------------------------------------------------------+
+| ✔ | ``joint_collision`` | :math:`\mid \hat{\tau}_{ext} \mid > \mathrm{thresh}_{upper}` where the |
+| | | threshold can be set by calling ``set_force_torque_collision_behavior``|
++---+----------------------------------+------------------------------------------------------------------------+
+| ✔ | ``cartesian_contact`` | :math:`\mid {}^K \hat{F}_{K,ext} \mid > \mathrm{thresh}_{lower}` where |
+| | | threshold can be set by calling ``set_force_torque_collision_behavior``|
++---+----------------------------------+------------------------------------------------------------------------+
+| ✔ | ``cartesian_collision`` | :math:`\mid {}^K \hat{F}_{K,ext} \mid > \mathrm{thresh}_{upper}` where |
+| | | threshold can be set by calling ``set_force_torque_collision_behavior``|
++---+----------------------------------+------------------------------------------------------------------------+
+| ✔ | ``tau_ext_hat_filtered`` | :math:`\hat{\tau}_{ext}` i.e. estimated external torques and forces at |
+| | | the end-effector, filtered with a exponential moving average filter |
+| | | (EMA). This filtering :math:`\alpha` can be configured via a ROS |
+| | | parameter. This field does not contain any gravity, i.e. |
+| | | :math:`\tau_{ext} = \tau_J - \tau_{J_d} - \tau_{gravity}` |
++---+----------------------------------+------------------------------------------------------------------------+
+| ✔ | ``O_F_ext_hat_K`` | :math:`{}^O\hat{F}_{K,ext} = J_O^{\top +} \cdot \hat{\tau}_{ext}` |
++---+----------------------------------+------------------------------------------------------------------------+
+| ✔ | ``K_F_ext_hat_K`` | :math:`{}^K\hat{F}_{K,ext} = J_K^{\top +} \cdot \hat{\tau}_{ext}` |
++---+----------------------------------+------------------------------------------------------------------------+
+| ✘ | ``O_dP_EE_d`` | |
++---+----------------------------------+------------------------------------------------------------------------+
+| ✔ | ``O_ddP_0`` | Will be the same as the ``gravity_vector`` ROS parameter. |
+| | | By default it is {0,0,-9.8} |
++---+----------------------------------+------------------------------------------------------------------------+
+| ✘ | ``O_T_EE_c`` | |
++---+----------------------------------+------------------------------------------------------------------------+
+| ✘ | ``O_dP_EE_c`` | |
++---+----------------------------------+------------------------------------------------------------------------+
+| ✘ | ``O_ddP_EE_c`` | |
++---+----------------------------------+------------------------------------------------------------------------+
+| ✔ | ``theta`` | Same as ``q``, since we don't simulate soft joints in Gazebo |
++---+----------------------------------+------------------------------------------------------------------------+
+| ✔ | ``dtheta`` | Same as ``dq``, since we don't simulate soft joints in Gazebo |
++---+----------------------------------+------------------------------------------------------------------------+
+| ✘ | ``current_errors`` | Will entirely be false, reflex system not yet implemented |
++---+----------------------------------+------------------------------------------------------------------------+
+| ✘ | ``last_motion_errors`` | Will entirely be false, reflex system not yet implemented |
++---+----------------------------------+------------------------------------------------------------------------+
+| ✔ | ``control_command_success_rate`` | Always 1.0 |
++---+----------------------------------+------------------------------------------------------------------------+
+| ✘ | ``robot_mode`` | Robot mode switches and reflex system not yet implemented |
++---+----------------------------------+------------------------------------------------------------------------+
+| ✔ | ``time`` | Current ROS time in simulation, comes from Gazebo |
++---+----------------------------------+------------------------------------------------------------------------+
+
+
+FrankaModelInterface
+""""""""""""""""""""""
+
+This is a **robot-specific** interface and thus a bit different from the normal hardware interfaces.
+To be able to access the franka model interface from your controller declare the following transmission tag with
+the root (e.g. ``panda_joint1``) and the tip (e.g. ``panda_joint8``) of your kinematic chain in your URDF:
+
+.. code-block:: xml
+
+
+ franka_hw/FrankaModelInterface
+
+ root
+ franka_hw/FrankaModelInterface
+
+
+ tip
+ franka_hw/FrankaModelInterface
+
+
+ franka_hw/FrankaModelInterface
+ franka_hw/FrankaModelInterface
+
+
+The model functions themselve are implemented with `KDL `_. This takes the kinematic
+structure and the inertial properties from the URDF to calculate model properties like the Jacobian or the mass matrix.
+
+Friction
+"""""""""
+
+For objects to have proper friction between each other (like fingers and objects) you need to tune
+some Gazebo parameters in your URDF. For the links ``panda_finger_joint1`` and ``panda_finger_joint2`` we recommend to
+set the following parameters:
+
+.. code-block:: xml
+
+
+
+ 10
+
+
+
+
+
+
+ 0
+ 0.003
+
+
+
+
+
+ 1.16
+ 1.16
+
+
+
+
+
+
+
+.. note::
+
+ Refer to `Gazebo Friction Documentation `_
+
+
+franka_msgs
+-----------
+This package contains message, service and action types that are primarily used the packages
+``franka_hw`` and ``franka_control`` to publish robot states or to expose the libfranka API
+in the ROS ecosystem. For more information about the services and actions offered in this
+package, please refer to :ref:`franka_control `.
+
+
+panda_moveit_config
+--------------------
+
+.. note::
+
+ This package was moved to the `ros_planning repos `_.
+
+For more details, documentation and tutorials, please have a look at the
+`MoveIt! tutorials website `_.
+
+
+.. _write_own_controller:
+
+Writing your own controller
+----------------------------
+All example controllers from the :ref:`example controllers package` are
+derived from the ``controller_interface::MultiInterfaceController`` class, which allows to claim
+up to four interfaces in one controller instance. The declaration of your class then looks like:
+
+.. code-block:: c++
+
+ class NameOfYourControllerClass : controller_interface::MultiInterfaceController <
+ my_mandatory_first_interface,
+ my_possible_second_interface,
+ my_possible_third_interface,
+ my_possible_fourth_interface> {
+ bool init (hardware_interface::RobotHW* hw, ros::NodeHandle& nh); // mandatory
+ void update (const ros::Time& time, const ros::Duration& period); // mandatory
+ void starting (const ros::Time& time) // optional
+ void stopping (const ros::Time& time); // optional
+ ...
+ }
+
+
+The available interfaces are described in Section :ref:`franka_hw `.
+
+.. important::
+
+ Note that the claimable combinations of commanding interfaces are restricted as it does not
+ make sense to e.g. command joint positions and Cartesian poses simultaneously. Read-only
+ interfaces like the *JointStateInterface*, the *FrankaStateInterface* or the
+ *FrankaModelInterface* can always be claimed and are not subject to restrictions.
+
+
+Possible claims to command interfaces are:
+
++-------------------------------------------------+----------------------------------------------+
+| ``franka_hw::FrankaHW`` | ``franka_combinable_hw::FrankaCombinableHW`` |
++=================================================+==============================================+
+| - all possible single interface claims | - ``EffortJointInterface`` |
+| - ``EffortJointInterface`` + | - ``EffortJointInterface`` + |
+| ``PositionJointInterface`` | ``FrankaCartesianPoseInterface`` |
+| - ``EffortJointInterface`` + | - ``EffortJointInterface`` + |
+| ``VelocityJointInterface`` | ``FrankaCartesianVelocityInterface`` |
+| - ``EffortJointInterface`` + | |
+| ``FrankaCartesianPoseInterface`` | |
+| - ``EffortJointInterface`` + | |
+| ``FrankaCartesianVelocityInterface`` | |
++-------------------------------------------------+----------------------------------------------+
+
+The idea behind offering the *EffortJointInterface* in combination with a motion generator
+interface is to expose the internal motion generators to the user. The calculated desired joint
+pose corresponding to a motion generator command is available in the robot state one time step
+later. One use case for this combination would be following a Cartesian trajectory using your own
+joint-level torque controller. In this case you would claim the combination *EffortJointInterface*
++ *FrankaCartesianPoseInterface*, stream your trajectory into the *FrankaCartesianPoseInterface*,
+and compute your joint-level torque commands based on the resulting desired joint pose (q_d) from
+the robot state. This allows to use the robot's built-in inverse kinematics instead of having to
+solve it on your own.
+
+To implement a fully functional controller you have to implement at least the inherited virtual
+functions ``init`` and ``update``. Initializing - e.g. start poses - should be done in the
+``starting`` function as ``starting`` is called when restarting the controller, while ``init`` is
+called only once when loading the controller. The ``stopping`` method should contain shutdown
+related functionality (if needed).
+
+.. important::
+
+ Always command a gentle slowdown before shutting down the controller. When using velocity
+ interfaces, do not simply command zero velocity in ``stopping``. Since it might be called
+ while the robot is still moving, it would be equivalent to commanding a jump in velocity
+ leading to very high resulting joint-level torques. In this case it would be better to keep the
+ same velocity and stop the controller than sending zeros and let the robot handle
+ the slowdown.
+
+Your controller class must be exported correctly with ``pluginlib`` which requires adding:
+
+.. code-block:: c++
+
+ #include
+ // Implementation ..
+ PLUGINLIB_EXPORT_CLASS(name_of_your_controller_package::NameOfYourControllerClass,
+ controller_interface::ControllerBase)
+
+
+at the end of the ``.cpp`` file. In addition you need to define a ``plugin.xml`` file with the
+following content:
+
+.. code-block:: xml
+
+
+
+
+ Some text to describe what your controller is doing
+
+
+
+
+
+which is exported by adding:
+
+.. code-block:: xml
+
+
+
+
+
+
+to your package.xml. Further, you need to load at least a controller name in combination with a
+controller type to the ROS parameter server. Additionally, you can include other parameters you
+need. An exemplary configuration.yaml file can look like:
+
+.. code-block:: yaml
+
+ your_custom_controller_name:
+ type: name_of_your_controller_package/NameOfYourControllerClass
+ additional_example_parameter: 0.0
+ # ..
+
+Now you can start your controller using the ``controller_spawner`` node from ROS control or via the
+service calls offered by the ``hardware_manager``. Just make sure that both the
+``controller_spawner`` and the ``franka_control_node`` run in the same namespace. For more details
+have a look at the controllers from the
+:ref:`franka_example_controllers package` or the
+`ROS control tutorials `_.
diff --git a/_sources/franka_ros2.rst.txt b/_sources/franka_ros2.rst.txt
new file mode 100644
index 0000000..48c2914
--- /dev/null
+++ b/_sources/franka_ros2.rst.txt
@@ -0,0 +1,674 @@
+franka_ros2
+===========
+.. note::
+
+ ``franka_ros2`` is not supported on Windows.
+
+The `franka_ros2 repo `_ contains a ROS 2 integration of
+:doc:`libfranka `.
+
+.. caution::
+ franka_ros2 is in rapid development. Anticipate breaking changes. Report bugs on
+ `GitHub `_.
+
+
+Prerequisites
+-------------
+
+* A `ROS 2 Humble installation `_
+ (ros-humble-desktop) or a VSCode IDE with DevContainer.
+* A :ref:`PREEMPT_RT kernel ` (optional, but strongly recommended).
+* For ``cartesian_pose``, ``joint_position`` and ``elbow_position`` command interfaces realtime-kernel is absolutely necessary.
+* A system-wide :ref:`libfranka installation `. Minimum supported version of libfranka is 0.13.0.
+ Here is a minimal example:
+
+.. code-block:: shell
+
+ sudo apt install -y libpoco-dev libeigen3-dev
+ git clone https://github.com/frankaemika/libfranka.git --recursive
+ cd libfranka
+ git switch 0.13.2
+ mkdir build && cd build
+ cmake -DCMAKE_BUILD_TYPE=Release -DBUILD_TESTS=OFF ..
+ cmake --build . -j$(nproc)
+ cpack -G DEB
+ sudo dpkg -i libfranka-*.deb
+
+Optional .bashrc Settings
+^^^^^^^^^^^^^^^^^^^^^^^^^
+
+* To get colorized warn and error messages you can put
+ ``export RCUTILS_COLORIZED_OUTPUT=1`` into your ``.bashrc``.
+
+* If your system language is not set to American English you should put
+ ``export LC_NUMERIC=en_US.UTF-8`` into your ``.bashrc`` to avoid issues in RViz.
+
+Setup
+------
+
+Install From Source
+^^^^^^^^^^^^^^^^^^^
+
+1. Install requirements::
+
+ sudo apt install -y \
+ ros-humble-hardware-interface \
+ ros-humble-generate-parameter-library \
+ ros-humble-ros2-control-test-assets \
+ ros-humble-controller-manager \
+ ros-humble-control-msgs \
+ ros-humble-xacro \
+ ros-humble-angles \
+ ros-humble-ros2-control \
+ ros-humble-realtime-tools \
+ ros-humble-control-toolbox \
+ ros-humble-moveit \
+ ros-humble-ros2-controllers \
+ ros-humble-joint-state-publisher \
+ ros-humble-joint-state-publisher-gui \
+ ros-humble-ament-cmake \
+ ros-humble-ament-cmake-clang-format \
+ python3-colcon-common-extensions
+
+2. Create a ROS 2 workspace::
+
+ mkdir -p ~/franka_ros2_ws/src
+
+3. Clone repo and build packages::
+
+ source /opt/ros/humble/setup.bash
+ cd ~/franka_ros2_ws
+ git clone https://github.com/frankaemika/franka_ros2.git src/franka_ros2
+ colcon build --cmake-args -DCMAKE_BUILD_TYPE=Release
+ source install/setup.sh
+
+Use VSCode DevContainer
+^^^^^^^^^^^^^^^^^^^^^^^
+FrankaROS2 package comes with .devcontainer folder which enables developer to use FrankaROS2 packages without manually installing ROS2 or libfranka.
+VSCode DevContainer working schematic is shown in the below image:
+
+ .. figure:: _static/vscode.png
+ :align: center
+ :figclass: align-center
+
+1. Follow the setup guide from VSCode `devcontainer_setup `_.
+
+2. Create a ROS 2 workspace::
+
+ mkdir franka_ros2_ws
+ cd franka_ros2_ws
+
+3. Clone repo::
+
+ git clone https://github.com/frankaemika/franka_ros2.git src/franka_ros2
+
+4. Move the .devcontainer folder to the franka_ros2_ws parent folder::
+
+ mv franka_ros2/.devcontainer .
+
+5. Open VSCode::
+
+ code .
+
+6. Open the current folder in DevContainer::
+
+ ctrl + shift + p
+
+ Write in the command prompt bar::
+
+ Dev Containers: Rebuild and Reopen in Container
+
+ and click this option in the search results
+
+7. Open up the terminal in VScode::
+
+ ctrl + `
+
+8. Source the environment::
+
+ source /ros_entrypoint.sh
+
+9. Install the Franka ROS 2 packages::
+
+ colcon build --cmake-args -DCMAKE_BUILD_TYPE=Release
+ source install/setup.sh
+
+MoveIt
+------
+
+To see if everything works, you can try to run the MoveIt example on the robot::
+
+ ros2 launch franka_moveit_config moveit.launch.py robot_ip:=
+
+Then activate the ``MotionPlanning`` display in RViz.
+
+If you do not have a robot you can still test your setup by running on a dummy hardware::
+
+ ros2 launch franka_moveit_config moveit.launch.py robot_ip:=dont-care use_fake_hardware:=true
+
+
+Wait until you can see the green ``You can start planning now!`` message from MoveIt inside the
+terminal. Then turn off the ``PlanningScene`` and turn it on again. After that turn on the ``MotionPlanning``.
+
+
+Example Controllers
+-------------------
+
+This repo comes with a few example controllers located in the ``franka_example_controllers`` package.
+
+The following launch files are executed with the gripper by default. If you
+do not have the gripper attached you can disable the gripper in the launch file with ``load_gripper:=false``.
+
+Move-to-start
+^^^^^^^^^^^^^
+
+This controller moves the robot to its home configuration.
+
+.. code-block:: shell
+
+ ros2 launch franka_bringup move_to_start_example_controller.launch.py robot_ip:=
+
+.. _gravity_example:
+
+Gravity Compensation
+^^^^^^^^^^^^^^^^^^^^
+
+This is the simplest controller that we have and is a good starting point to write your own.
+It sends zero as torque command to all joints, which means that the robot only compensates its own weight.
+
+.. code-block:: shell
+
+ ros2 launch franka_bringup gravity_compensation_example_controller.launch.py robot_ip:=
+
+Joint Impedance Example
+^^^^^^^^^^^^^^^^^^^^^^^
+
+The example moves joints 4 and 5 in a periodic movement that is very compliant. You can try to move the
+joints while it is running.
+
+.. code-block:: shell
+
+ ros2 launch franka_bringup joint_impedance_example_controller.launch.py robot_ip:=
+
+
+Model Example Controller
+^^^^^^^^^^^^^^^^^^^^^^^^
+This is a read-only controller which prints the coriolis force vector, gravity force vector, pose matrix of Joint4,
+Joint4 body jacobian and end-effector jacobian with respect to the base frame.
+
+.. code-block:: shell
+
+ ros2 launch franka_bringup model_example_controller.launch.py robot_ip:=
+
+Joint Position Example
+^^^^^^^^^^^^^^^^^^^^^^
+This example sends periodic position commands to the robot.
+
+.. important::
+ The position trajectory needs to start from the initial position of the robot.
+
+To read the start position of the robot, you can read claim the `initial_joint_position`.
+state interface values before starting to send any commands.
+
+.. code-block:: cpp
+
+ if (initialization_flag_) {
+ for (size_t i = 0; i < 7; ++i) {
+ initial_q_.at(i) = state_interface[i].get_value();
+ }
+ initialization_flag_ = false;
+ }
+
+
+.. code-block:: shell
+
+ ros2 launch franka_bringup joint_position_example_controller.launch.py robot_ip:=
+
+Joint Velocity Example
+^^^^^^^^^^^^^^^^^^^^^^
+This example sends periodic velocity commands to the 4th and 5th joint of the robot.
+
+.. code-block:: shell
+
+ ros2 launch franka_bringup joint_velocity_example_controller.launch.py robot_ip:=
+
+Cartesian Pose Example
+^^^^^^^^^^^^^^^^^^^^^^
+This example uses the CartesianPose interface to send periodic pose commands to the robot.
+
+.. code-block:: shell
+
+ ros2 launch franka_bringup cartesian_pose_example_controller.launch.py robot_ip:=
+
+Cartesian Orientation Example
+^^^^^^^^^^^^^^^^^^^^^^^^^^^^^
+This example uses CartesianOrientation interface to send periodic orientation commands around X axis of the end effector of the robot.
+
+.. code-block:: shell
+
+ ros2 launch franka_bringup cartesian_orientation_example_controller.launch.py robot_ip:=
+
+Cartesian Pose Elbow Example
+^^^^^^^^^^^^^^^^^^^^^^^^^^^^
+This example sends periodic elbow commands while keeping the end effector pose constant.
+
+.. code-block:: shell
+
+ ros2 launch franka_bringup cartesian_elbow_example_controller.launch.py robot_ip:=
+
+Cartesian Velocity Example
+^^^^^^^^^^^^^^^^^^^^^^^^^^
+This example uses the CartesianVelocity interface to send periodic velocity commands to the robot.
+
+.. code-block:: shell
+
+ ros2 launch franka_bringup cartesian_velocity_example_controller.launch.py robot_ip:=
+
+Cartesian Elbow Example
+^^^^^^^^^^^^^^^^^^^^^^^
+This example uses the CartesianElbow interface to send periodic elbow commands to the robot while keeping the end effector velocity constant.
+
+.. code-block:: shell
+
+ ros2 launch franka_bringup elbow_example_controller.launch.py robot_ip:=
+
+
+Package Descriptions
+--------------------
+
+This section contains more detailed descriptions of what each package does. In general the package structure tries to
+adhere to the structure that is proposed
+`here `_.
+
+
+franka_bringup
+^^^^^^^^^^^^^^
+
+This package contains the launch files for the examples as well as the basic ``franka.launch.py`` launch file, that
+can be used to start the robot without any controllers.
+
+When you start the robot with::
+
+ ros2 launch franka_bringup franka.launch.py robot_ip:= use_rviz:=true
+
+There is no controller running apart from the ``joint_state_broadcaster``. However, a connection with the robot is still
+established and the current robot pose is visualized in RViz. In this mode the robot can be guided when the user stop
+button is pressed. However, once a controller that uses the ``effort_command_interface`` is started, the robot will be
+using the torque interface from libfranka. For example it is possible to launch the
+``gravity_compensation_example_controller`` by running::
+
+ ros2 control load_controller --set-state active gravity_compensation_example_controller
+
+This is the equivalent of running the ``gravity_compensation_example_controller.launch.py`` launch file mentioned in
+:ref:`Gravity Compensation `.
+
+When the controller is stopped with::
+
+ ros2 control set_controller_state gravity_compensation_example_controller inactive
+
+the robot will stop the torque control and will only send its current state over the FCI.
+
+You can now choose to start the same controller again with::
+
+ ros2 control set_controller_state gravity_compensation_example_controller active
+
+or load and start a different one::
+
+ ros2 control load_controller --set-state active joint_impedance_example_controller
+
+
+.. _franka_description:
+
+franka_description
+^^^^^^^^^^^^^^^^^^
+
+This package contains the xacro files and meshes that are used to visualize the robot.
+Further, it contains a launch file that visualizes the robot model without access to a real robot::
+
+ ros2 launch franka_description visualize_franka.launch.py load_gripper:=
+
+
+.. _franka_example_controllers:
+
+franka_example_controllers
+^^^^^^^^^^^^^^^^^^^^^^^^^^
+
+This package contains a few controllers that can be seen as example of how to write controllers in ROS 2. Currently,
+a controller only has access to measured joint positions and joint velocities. Based on this information the controller
+can send torque commands. It is currently not possible to use other interfaces like the joint position interface.
+
+franka_gripper
+^^^^^^^^^^^^^^
+
+This package contains the ``franka_gripper_node`` for interfacing with the ``Franka Hand``.
+
+The ``franka_gripper_node`` provides the following actions:
+
+* ``homing`` - homes the gripper and updates the maximum width given the mounted fingers.
+* ``move`` - moves to a target width with the defined speed.
+* ``grasp`` - tries to grasp at the desired width with the desired force while closing with the given speed. The operation is successful if the
+ distance ``d`` between the gripper fingers is ``width - epsilon.inner < d < width + epsilon.outer``
+* ``gripper_action`` - a special grasping action for MoveIt.
+
+Also, there is a ``stop`` service that aborts gripper actions and stops grasping.
+
+
+Use the following launch file to start the gripper::
+
+ ros2 launch franka_gripper gripper.launch.py robot_ip:=
+
+
+In a different tab you can now perform the homing and send a grasp command.::
+
+
+ ros2 action send_goal /panda_gripper/homing franka_msgs/action/Homing {}
+ ros2 action send_goal -f /panda_gripper/grasp franka_msgs/action/Grasp "{width: 0.00, speed: 0.03, force: 50}"
+
+The inner and outer epsilon are 0.005 meter per default. You can also explicitly set the epsilon::
+
+ ros2 action send_goal -f /panda_gripper/grasp franka_msgs/action/Grasp "{width: 0.00, speed: 0.03, force: 50, epsilon: {inner: 0.01, outer: 0.01}}"
+
+To stop the grasping, you can use ``stop`` service.::
+
+ ros2 service call /panda_gripper/stop std_srvs/srv/Trigger {}
+
+.. _franka_hardware:
+
+franka_hardware
+^^^^^^^^^^^^^^^
+
+This package contains the ``franka_hardware`` plugin needed for `ros2_control `_.
+The plugin is loaded from the URDF of the robot and passed to the controller manager via the robot description.
+It provides for each joint:
+
+* a ``position state interface`` that contains the measured joint position.
+* a ``velocity state interface`` that contains the measured joint velocity.
+* an ``effort state interface`` that contains the measured link-side joint torques including gravity.
+* an ``initial_position state interface`` that contains the initial joint position of the robot.
+* an ``effort command interface`` that contains the desired joint torques without gravity.
+* a ``position command interface`` that contains the desired joint position.
+* a ``velocity command interface`` that contains the desired joint velocity.
+
+In addition
+
+* a ``franka_robot_state`` that contains the robot state information, `franka_robot_state `_.
+* a ``franka_robot_model_interface`` that contains the pointer to the model object.
+
+.. important::
+ ``franka_robot_state`` and ``franka_robot_model_interface`` state interfaces should not be used directly from hardware state interface.
+ Rather, they should be utilized by the :ref:`franka_semantic_components` interface.
+
+The IP of the robot is read over a parameter from the URDF.
+
+.. _franka_semantic_components:
+
+franka_semantic_components
+^^^^^^^^^^^^^^^^^^^^^^^^^^
+This package contains franka_robot_model, franka_robot_state and cartesian command classes.
+These classes are used to convert franka_robot_model object and franka_robot_state objects,
+which are stored in the hardware_state_interface as a double pointer.
+
+For further reference on how to use these classes:
+`Franka Robot State Broadcaster `_
+and
+`Franka Example Controllers(model_example_controller)
+`_
+
+- Cartesian Pose Interface:
+
+This interface is used to send Cartesian pose commands to the robot by using the loaned command interfaces.
+FrankaSemanticComponentInterface class is handling the loaned command interfaces and state interfaces.
+While starting the cartesian pose interface, the user needs to pass a boolean flag to the constructor
+to indicate whether the interface is for the elbow or not.
+
+.. code-block:: cpp
+
+ auto is_elbow_active = false;
+ CartesianPoseInterface cartesian_pose_interface(is_elbow_active);
+
+This interface allows users to read the current pose command interface values set by the franka hardware interface.
+
+.. code-block:: cpp
+
+ std::array pose;
+ pose = cartesian_pose_interface.getInitialPoseMatrix();
+
+One could also read quaternion and translation values in Eigen format.
+
+.. code-block:: cpp
+
+ Eigen::Quaterniond quaternion;
+ Eigen::Vector3d translation;
+ std::tie(quaternion, translation) = cartesian_pose_interface.getInitialOrientationAndTranslation();
+
+After setting up the cartesian interface, you need to ``assign_loaned_command_interfaces`` and ``assign_loaned_state_interfaces`` in your controller.
+This needs to be done in the on_activate() function of the controller. Examples can be found in the
+`assign loaned comamand interface example
+`_
+
+.. code-block:: cpp
+
+ cartesian_pose_interface.assign_loaned_command_interfaces(command_interfaces_);
+ cartesian_pose_interface.assign_loaned_state_interfaces(state_interfaces_);
+
+In the update function of the controller you can send pose commands to the robot.
+
+.. code-block:: cpp
+
+ std::array pose;
+ pose = {1, 0, 0, 0, 0, 1, 0, 0, 0, 0, 1, 0, 0.5, 0, 0.5, 1};
+ cartesian_pose_interface.setCommanded(pose);
+
+Or you can send quaternion, translation values in Eigen format.
+
+.. code-block:: cpp
+
+ Eigen::Quaterniond quaternion(1, 0, 0, 0);
+ Eigen::Vector3d translation(0.5, 0, 0.5);
+ cartesian_pose_interface.setCommand(quaternion, translation);
+
+- Cartesian Velocity Interface:
+
+This interface is used to send Cartesian velocity commands to the robot by using the loaned command interfaces.
+FrankaSemanticComponentInterface class is handling the loaned command interfaces and state interfaces.
+
+.. code-block:: cpp
+
+ auto is_elbow_active = false;
+ CartesianVelocityInterface cartesian_velocity_interface(is_elbow_active);
+
+To send the velocity command to the robot, you need to assign_loaned_command_interface in your custom controller.
+
+.. code-block:: cpp
+
+ cartesian_velocity_interface.assign_loaned_command_interface(command_interfaces_);
+
+In the update function of the controller you can send cartesian velocity command to the robot.
+
+.. code-block:: cpp
+
+ std::array cartesian_velocity;
+ cartesian_velocity = {0, 0, 0, 0, 0, 0.1};
+ cartesian_velocity_interface.setCommand(cartesian_velocity);
+
+.. _franka_robot_state_broadcaster:
+
+franka_robot_state_broadcaster
+^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^
+
+This package contains read-only franka_robot_state_broadcaster controller.
+It publishes franka_robot_state topic to the topic named `/franka_robot_state_broadcaster/robot_state`.
+This controller node is spawned by franka_launch.py in the franka_bringup.
+Therefore, all the examples that include the franka_launch.py publishes the robot_state topic.
+
+.. _franka_moveit_config:
+
+franka_moveit_config
+^^^^^^^^^^^^^^^^^^^^
+
+This package contains the configuration for MoveIt2. There is a new move group called
+``panda_manipulator`` that has its tip between the fingers of the gripper and has its Z-axis rotated by -45 degrees, so
+that the X-axis is now facing forward, making it easier to use. The ``panda_arm`` move group is still available
+for backward compatibility. New applications should use the new ``panda_manipulator`` move group instead.
+
+.. figure:: _static/move-groups.png
+ :align: center
+ :figclass: align-center
+
+ Visualization of the old and the new move group
+
+franka_msgs
+^^^^^^^^^^^
+
+This package contains the definitions for the different gripper actions and robot state message.
+
+
+joint_effort_trajectory_controller
+^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^
+
+This package contains a modified joint_trajectory_controller that can use the effort interface of the
+``franka_hardware::FrankaHardwareInterface``. It is based on this
+`Pull request `_.
+
+.. note::
+ This package will be soon deleted as the fix is available in
+ `ros2_controllers `_ master branch.
+ As soon as, it's backported to Humble, it will be deleted from franka_ros2 repository.
+
+Differences between franka_ros and franka_ros2
+----------------------------------------------
+
+This section gives an overview of the fundamental changes between ``franka_ros`` and ``franka_ros2``.
+
+franka_gripper
+^^^^^^^^^^^^^^
+
+* All topics and actions were previously prefixed with ``franka_gripper``. This prefix was renamed to ``panda_gripper``
+ to enable, in the future, a workflow where all prefixes are based on the ``arm_id``
+ to effortlessly enable multi arm setups.
+
+* The ``stop`` action is now a service action as it is not preemptable.
+
+* All actions (apart from the ``gripper_action``) have the current gripper width as feedback.
+
+franka_gazebo
+^^^^^^^^^^^^^
+
+Currently, we do not offer a Gazebo integration with ``franka_ros2``. However, we provide one
+with :doc:`franka_ros`.
+
+franka_visualization
+^^^^^^^^^^^^^^^^^^^^
+
+This package does not exist anymore. However, :ref:`franka_description` provides a launch file to visualize the robot
+model without a connection to a robot.
+
+franka_control
+^^^^^^^^^^^^^^
+
+This package does not exist anymore. The connection to the robot is provided by the hardware plugin in
+the :ref:`franka_hardware` package. The actions and services that it provided are currently
+not offered in ``franka_ros2``.
+
+
+Writing Controllers
+^^^^^^^^^^^^^^^^^^^
+
+Compared to ``franka_ros`` we currently offer a reduced set of controller interfaces:
+
+* Joint positions
+* Joint velocities
+* Measured torques
+* Franka robot state
+* Franka robot model
+
+.. important::
+ Franka robot state is published through :ref:`franka_robot_state_broadcaster`
+ package to the topic named `/franka_robot_state_broadcaster/robot_state`
+
+.. important::
+ Both Franka robot state and Franka robot model are advised to use through :ref:`franka_semantic_components` class.
+ They are stored in the state_interface as double pointers and casted back to their original objects inside the franka_semantic_component class.
+
+ Example of using franka_model can be found in the franka_example_controllers package:
+ `model_example_controller `_.
+
+
+You can base your own controller on one of the :ref:`franka_example_controllers`. To compute kinematic
+and dynamic quantities of the robot you can use the joint states and the URDF of the robot in libraries like
+`KDL `_ (of which there is also a ROS 2 package available).
+
+Non-realtime robot parameter setting
+------------------------------------
+
+Non-realtime robot parameter setting can be done via ROS 2 services. They are advertised after the robot hardware is initialized.
+
+Service names are given below::
+
+ * /service_server/set_cartesian_stiffness
+ * /service_server/set_force_torque_collision_behavior
+ * /service_server/set_full_collision_behavior
+ * /service_server/set_joint_stiffness
+ * /service_server/set_load
+ * /service_server/set_parameters
+ * /service_server/set_parameters_atomically
+ * /service_server/set_stiffness_frame
+ * /service_server/set_tcp_frame
+
+Service message descriptions are given below.
+
+ * ``franka_msgs::srv::SetJointStiffness`` specifies joint stiffness for the internal controller
+ (damping is automatically derived from the stiffness).
+ * ``franka_msgs::srv::SetCartesianStiffness`` specifies Cartesian stiffness for the internal
+ controller (damping is automatically derived from the stiffness).
+ * ``franka_msgs::srv::SetTCPFrame`` specifies the transformation from _EE (end effector) to
+ _NE (nominal end effector) frame. The transformation from flange to end effector frame
+ is split into two transformations: _EE to _NE frame and _NE to
+ _link8 frame. The transformation from _NE to _link8 frame can only be
+ set through the administrator's interface.
+ * ``franka_msgs::srv::SetStiffnessFrame`` specifies the transformation from _K to _EE frame.
+ * ``franka_msgs::srv::SetForceTorqueCollisionBehavior`` sets thresholds for external Cartesian
+ wrenches to configure the collision reflex.
+ * ``franka_msgs::srv::SetFullCollisionBehavior`` sets thresholds for external forces on Cartesian
+ and joint level to configure the collision reflex.
+ * ``franka_msgs::srv::SetLoad`` sets an external load to compensate (e.g. of a grasped object).
+
+Launch franka_bringup/franka.launch.py file to initialize robot hardware::
+
+ ros2 launch franka_bringup franka.launch.py robot_ip:=
+
+Here is a minimal example:
+
+.. code-block:: shell
+
+ ros2 service call /service_server/set_joint_stif
+ fness franka_msgs/srv/SetJointStiffness "{joint_stiffness: [1000.0, 1000.0, 10
+ 00.0, 1000.0, 1000.0, 1000.0, 1000.0]}"
+
+.. important::
+
+ Non-realtime parameter setting can only be done when the robot hardware is in `idle` mode.
+ If a controller is active and claims command interface this will put the robot in the `move` mode.
+ In `move` mode non-realtime param setting is not possible.
+
+.. important::
+
+ The _EE frame denotes the part of the
+ configurable end effector frame which can be adjusted during run time through `franka_ros`. The
+ _K frame marks the center of the internal
+ Cartesian impedance. It also serves as a reference frame for external wrenches. *Neither the
+ _EE nor the _K are contained in the URDF as they can be changed at run time*.
+ By default, is set to "panda".
+
+ .. figure:: _static/frames.svg
+ :align: center
+ :figclass: align-center
+
+ Overview of the end-effector frames.
+
+
+
+Known Issues
+------------
+
+* When using the ``fake_hardware`` with MoveIt, it takes some time until the default position is applied.
diff --git a/_sources/franka_ros2_changelog.rst.txt b/_sources/franka_ros2_changelog.rst.txt
new file mode 100644
index 0000000..afe9fd1
--- /dev/null
+++ b/_sources/franka_ros2_changelog.rst.txt
@@ -0,0 +1,6 @@
+franka_ros2 changelog
+=====================
+
+.. include:: ../franka_ros2/CHANGELOG.md
+ :parser: myst_parser.sphinx_
+ :start-line: 2
diff --git a/_sources/franka_ros_changelog.rst.txt b/_sources/franka_ros_changelog.rst.txt
new file mode 100644
index 0000000..7f23ecf
--- /dev/null
+++ b/_sources/franka_ros_changelog.rst.txt
@@ -0,0 +1,6 @@
+franka_ros changelog
+====================
+
+.. include:: ../franka_ros/CHANGELOG.md
+ :parser: myst_parser.sphinx_
+ :start-line: 2
diff --git a/_sources/getting_started.rst.txt b/_sources/getting_started.rst.txt
new file mode 100644
index 0000000..73f998b
--- /dev/null
+++ b/_sources/getting_started.rst.txt
@@ -0,0 +1,292 @@
+Getting started
+===============
+
+After setting up the required software for :doc:`Linux ` or
+:doc:`Windows `, it is time to connect to the robot and test the whole setup
+by using FCI to read the current robot state.
+
+Operating the robot
+-------------------
+
+Before going further though, here are a few safety considerations.
+Always check the following things before powering on the robot:
+
+1. Make sure that the Arm has been mounted on a stable base and cannot topple over, even
+ when performing fast motions or abrupt stops.
+
+.. caution::
+ Only tabletop mounting is supported, i.e. the Arm must be mounted perpendicular to the
+ ground! Other mountings will **void your warranty** and **might cause damage
+ to the robot**!
+
+2. Ensure that the cable connecting Arm and Control is firmly attached on both sides.
+3. Connect the external activation device to Arm's base and keep it next to you in order to be
+ able to stop the robot at any time.
+
+.. hint::
+ Activating the external activation device will disconnect the Arm from Control.
+ The joint motor controllers will then hold their current position.
+ **The external activation device is not an emergency stop!**
+
+This list is non-exhaustive! The manual delivered with your robot contains a chapter dedicated
+to safety. Please read it carefully and follow the instructions. The manual can also be found in
+our `Franka World Hub `_.
+
+.. important::
+ The workstation PC which commands your robot using the FCI must always be connected to the LAN
+ port of Control (shop floor network) and **not** to the LAN port of the Arm (robot network).
+
+Installing the FCI Feature
+--------------------------
+In order to be able to operate the robot through the Franka Control Interface, the corresponding
+Feature needs to be installed on your device. If it is already installed to the controller, it
+will be listed under Desk --> Settings --> System --> Installed Features.
+If FCI is not installed yet, you can synchronize it from your Franka World account to your
+controller. Therefore, you must have an available FCI license in your company/university account and have
+your controller registered in this account. The software (offline or online) synchronization
+process is described in more detail in the
+`Franka World user manual `_.
+
+
+.. _setting-up-the-network:
+
+Setting up the network
+----------------------
+
+Good network performance is crucial when controlling the robot using FCI.
+Therefore it is strongly recommended to use a direct connection between the
+workstation PC and Panda's Control. This section describes how to configure your
+network for this use case.
+
+.. figure:: _static/control.png
+ :align: center
+ :figclass: align-center
+
+ Use Control's LAN port when controlling the robot through FCI.
+ Do not connect to the port in Arm's base.
+
+The Control and your workstation must be configured to appear on the same
+network. Simplest way to achieve that is to use static IP addresses. Any two
+addresses on the same network would work, but the following values will be used
+for the purpose of this tutorial:
+
++---------+----------------+------------+
+| | Workstation PC | Control |
++=========+================+============+
+| Address | 172.16.0.1 | 172.16.0.2 |
++---------+----------------+------------+
+| Netmask | 24 | 24 |
++---------+----------------+------------+
+
+The Control's address (172.16.0.2) is called ```` in the following chapters.
+
+.. hint::
+ With this network configuration, Desk can be accessed via ``https://``, although
+ you will see a certificate warning in your browser.
+
+The configuration process consists of two steps:
+
+ * Configuring Control's network settings.
+ * Configuring your workstation's network settings.
+
+Control network configuration
+^^^^^^^^^^^^^^^^^^^^^^^^^^^^^
+
+For this step, the robot needs to be installed and tested. **Please read through
+the documents shipped with your robot and follow the setup instructions before
+continuing any further!**
+
+The Control's network can be configured in the administrator's interface. For
+the duration of this step you can connect to the robot through the port in the
+robot's base. For details, consult the `Connecting a user interface device`
+section in the manual delivered with your robot.
+
+.. figure:: _static/accessing-admin.png
+ :align: center
+ :figclass: align-center
+
+ Accessing the administrator's interface through Desk.
+
+To set up a static address, enter the following values in the `Network` section:
+
+.. figure:: _static/control-static-ip.png
+ :align: center
+ :figclass: align-center
+
+ Setting a static IP for the Control's LAN port (Shop Floor network).
+ DHCP Client option is deselected.
+
+Press `Apply`. After the settings are successfully applied, connect your
+workstation's LAN port to the robot's control unit.
+
+Linux workstation network configuration
+^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^
+
+This section describes how to set up a static IP address on Ubuntu 16.04
+using the GUI. Follow the official Ubuntu guide_ if you prefer to use the
+command line.
+
+.. _guide: https://help.ubuntu.com/lts/serverguide/network-configuration.html
+
+.. caution::
+ The following steps will modify your network settings. If in doubt,
+ contact your network's administrator.
+
+First, go to Network Connection widget. Select the wired connection you
+will be using and click edit.
+
+.. figure:: _static/edit-connections.png
+ :align: center
+ :figclass: align-center
+
+ Edit the connection in the Ethernet section.
+
+Next, click on the IPv4 settings tab, set the method to Manual, and enter the
+following values:
+
+.. figure:: _static/static-ip-ubuntu.png
+ :align: center
+ :figclass: align-center
+
+ Setting a static IP for the Workstation PC. Method is set to Manual.
+
+.. hint::
+ This step will disable DHCP, which means you will no longer obtain an address
+ when connecting to a DHCP server, like the one in Arm's base. When you no
+ longer use FCI, you can change the Method back to `Automatic (DHCP)`.
+
+Save the changes, and close the Network Connection window. Click on the
+connection name from the drop down menu. It should now be possible to connect to
+the robot from your workstation. To verify this, perform the
+:ref:`network-bandwidth-delay-test`. From now on, you can also access Desk
+through this address in your browser.
+
+Windows workstation network configuration
+^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^
+Setup a static IP address on the Windows workstation. Therefore, open **Control Panel** and go to
+**Network and Internet** > **Network and Sharing Center** > **Change adapter settings**.
+Right-click the network adapter and open **Properties**. Use the same example address and netmask
+as in the Linux workstation network configuration.
+
+.. _preparing_robot_in_desk:
+
+Preparing the robot for FCI usage in Desk
+-----------------------------------------
+
+In order to verify the connection, the robot's brakes need to be unlocked in Desk and the activation
+device needs to be relased so that the robot is ready for execution indicated by blue LED mode.
+
+Enabling the FCI mode
+^^^^^^^^^^^^^^^^^^^^^
+
+Using a robot with system version >= 4.2.0 requires to enable the FCI mode. To do that open Desk,
+expand the menu in the sidebar and press 'Activate FCI'.
+
+.. figure:: _static/activate_fci.png
+ :align: center
+ :figclass: align-center
+
+ Enabling the FCI mode in the Desk sidebar menu
+
+
+FCI mode in Panda
+"""""""""""""""""
+
+After enabling the FCI mode, a pop-up as shown below is appearing. This pop-up indicates that the FCI mode
+is currently active and that Desk interactions are not allowed while it is active. This pop-up needs to
+remain open while working with FCI. Further information about Single Point of Control (SPoC) can be found
+in the manual shipped with the robot which can also be found in our
+`Franka World Hub `_.
+
+.. figure:: _static/pop_up_fci.png
+ :align: center
+ :figclass: align-center
+
+ Pop-up when the FCI mode is enabled
+
+FCI mode in Franka Research 3
+"""""""""""""""""""""""""""""
+
+After enabling the FCI mode, the sidebar shows that FCI is on.
+
+.. figure:: _static/fci-on.png
+ :align: center
+ :figclass: align-center
+
+ Sidebar when the FCI mode is enabled
+
+Verifying the connection
+------------------------
+
+The previous sections described how to specify the IP address of the Control's
+LAN port. In the following sections that address is referred to as ````.
+
+In order to verify that everything is correctly set up, be sure :ref:`the robot is prepared for FCI
+usage in Desk ` and run the ``echo_robot_state``
+example from ``libfranka``. If you decided to install ``franka_ros`` and ``libfranka`` from the ROS
+repository, you can instead read the instructions for
+:ref:`visualizing the robot in ros ` .
+
+Change to the build directory of ``libfranka`` and execute the example:
+
+*Linux*:
+
+.. code-block:: shell
+
+ ./examples/echo_robot_state
+
+*Windows*:
+
+.. code-block:: shell
+
+ cd /path/to/libfranka/build/examples/
+ echo_robot_state.exe
+
+.. hint::
+ Before executing libfranka programms, make sure that the executables are able to find their runtime libraries.
+ On Windows, the easiest way is to copy the needed libraries into the same directory as the executable.
+
+The program will print the current state of the robot to the console and terminate after a few
+iterations. The fields are explained in the
+`libfranka API documentation `_.
+
+Example output:
+
+.. code-block:: json
+
+ {
+ "O_T_EE": [0.998578,0.0328747,-0.0417381,0,0.0335224,-0.999317,0.0149157,0,-0.04122,-0.016294,
+ -0.999017,0,0.305468,-0.00814133,0.483198,1],
+ "O_T_EE_d": [0.998582,0.0329548,-0.041575,0,0.0336027,-0.999313,0.0149824,0,-0.0410535,
+ -0.0163585,-0.999023,0,0.305444,-0.00810967,0.483251,1],
+ "F_T_EE": [0.7071,-0.7071,0,0,0.7071,0.7071,0,0,0,0,1,0,0,0,0.1034,1],
+ "EE_T_K": [1,0,0,0,0,1,0,0,0,0,1,0,0,0,0,1],
+ "m_ee": 0.73, "F_x_Cee": [-0.01,0,0.03], "I_ee": [0.001,0,0,0,0.0025,0,0,0,0.0017],
+ "m_load": 0, "F_x_Cload": [0,0,0], "I_load": [0,0,0,0,0,0,0,0,0],
+ "m_total": 0.73, "F_x_Ctotal": [-0.01,0,0.03], "I_total": [0.001,0,0,0,0.0025,0,0,0,0.0017],
+ "elbow": [-0.0207622,-1], "elbow_d": [-0.0206678,-1],
+ "tau_J": [-0.00359774,-5.08582,0.105732,21.8135,0.63253,2.18121,-0.0481953],
+ "tau_J_d": [0,0,0,0,0,0,0],
+ "dtau_J": [-54.0161,-18.9808,-64.6899,-64.2609,14.1561,28.5654,-11.1858],
+ "q": [0.0167305,-0.762614,-0.0207622,-2.34352,-0.0305686,1.53975,0.753872],
+ "dq": [0.00785939,0.00189343,0.00932415,0.0135431,-0.00220327,-0.00492024,0.00213604],
+ "q_d": [0.0167347,-0.762775,-0.0206678,-2.34352,-0.0305677,1.53975,0.753862],
+ "dq_d": [0,0,0,0,0,0,0],
+ "joint_contact": [0,0,0,0,0,0,0], "cartesian_contact": [0,0,0,0,0,0],
+ "joint_collision": [0,0,0,0,0,0,0], "cartesian_collision": [0,0,0,0,0,0],
+ "tau_ext_hat_filtered": [0.00187271,-0.700316,0.386035,0.0914781,-0.117258,-0.00667777,
+ -0.0252562],
+ "O_F_ext_hat_K": [-2.06065,0.45889,-0.150951,-0.482791,-1.39347,0.109695],
+ "K_F_ext_hat_K": [-2.03638,-0.529916,0.228266,-0.275938,0.434583,0.0317351],
+ "theta": [0.01673,-0.763341,-0.0207471,-2.34041,-0.0304783,1.54006,0.753865],
+ "dtheta": [0,0,0,0,0,0,0],
+ "current_errors": [], "last_motion_errors": [],
+ "control_command_success_rate": 0, "robot_mode": "Idle", "time": 3781435
+ }
+
+.. hint::
+
+ If an error occurs at this point, perform the
+ :ref:`ping test ` and ensure that the robot's fail-safe
+ safety locking system is opened. Further information are provided in the manual shipped with
+ the robot.
diff --git a/_sources/index.rst.txt b/_sources/index.rst.txt
new file mode 100644
index 0000000..06087c8
--- /dev/null
+++ b/_sources/index.rst.txt
@@ -0,0 +1,71 @@
+Franka Control Interface Documentation
+======================================
+
+.. note::
+
+ The software and its documentation support two different robots, the
+ `Franka Research 3 (FR3) `_ and an older Franka Emika Robot (FER or Panda).
+
+.. note::
+
+ There is an
+ `unofficial Chinese translation of this page `_ .
+ The Chinese version is not officially maintained by Franka Emika and might
+ thus contain errors and/or be outdated.
+
+
+.. todolist::
+
+The Franka Control Interface (FCI) can be accessed via several open source components which we
+provide on `GitHub `_. We welcome contributions and suggestions
+for improvements.
+
+These components are:
+
+ * ``libfranka``, a C++ library that provides low-level control of Franka Emika research robots.
+ Its source code is available at https://github.com/frankaemika/libfranka. API documentation is
+ available at https://frankaemika.github.io/libfranka.
+ * ``franka_ros``, our `ROS integration `_, including support for
+ ROS Control and MoveIt!. It also contains ``franka_description``, a collection of URDF models and
+ 3D meshes that can be useful outside of ROS.
+ The repository is available at https://github.com/frankaemika/franka_ros.
+ * ``franka_ros2`` is currently released only as beta software.
+ The repository is available at https://github.com/frankaemika/franka_ros2.
+ * ``Franka Matlab`` provides a Simulink and a Matlab API, together with helper functions and tools,
+ for rapid-prototyping in Real-Time on the Franka Robot"
+ Franka Matlab artifacts can be obtained at `Franka World `_.
+
+The source code of this documentation is also `available online
+`_.
+
+.. important::
+ Before you start using the FCI, please read through the documents shipped with the robot and
+ the :doc:`requirements` chapter.
+
+.. note::
+ Using a robot with system version 4.2.0 or higher requires to enable the FCI mode. To do that
+ open Desk -> expand the menu in the sidebar -> press `'Activate FCI'`. Further information
+ about Single Point of Control (SPoC) can be found in the manual shipped with the robot.
+
+.. toctree::
+ :maxdepth: 2
+ :caption: Contents:
+
+ overview
+ requirements
+ compatibility
+ installation_linux
+ installation_windows
+ getting_started
+ libfranka
+ libfranka_changelog
+ franka_ros
+ franka_ros_changelog
+ franka_ros2
+ franka_ros2_changelog
+ franka_matlab/index
+ franka_matlab/franka_matlab_changelog
+ control_parameters
+ fr3-certification-remarks
+ troubleshooting
+ faq
diff --git a/_sources/installation_linux.rst.txt b/_sources/installation_linux.rst.txt
new file mode 100644
index 0000000..1ddc859
--- /dev/null
+++ b/_sources/installation_linux.rst.txt
@@ -0,0 +1,330 @@
+Installation on Linux
+=====================
+
+This chapter describes how to install ``libfranka`` and ``franka_ros``, either
+as binary packages or by building from source, and how to install a real-time
+Linux kernel. ``franka_ros`` is only required if you want to control your robot
+using `ROS `_.
+
+.. note::
+
+ While ``libfranka`` and the ``franka_ros`` packages should work on different Linux distributions,
+ official support is currently only provided for:
+
+ * Ubuntu 18.04 LTS `Bionic Beaver` and ROS `Melodic Morenia` (requires at least ``libfranka`` 0.6.0)
+ * Ubuntu 20.04 LTS `Focal Fossa` and ROS `Noetic Ninjemys` (requires at least ``libfranka`` 0.8.0)
+
+ The following instructions are exemplary for Ubuntu 20.04 LTS system and ROS `Noetic Ninjemys`.
+ They only work in the supported environments.
+
+.. warning::
+ We do not offer support for Ubuntu 16.04 LTS `Xenial Xerus` and ROS `Kinetic Kame` anymore, as they have reached their
+ end-of-life.
+
+Installing from the ROS repositories
+------------------------------------
+
+.. hint::
+
+ These packages might not always be up-to-date, as they are only synced at certain intervals.
+ Read the changelog at https://frankaemika.github.io to find out which ``libfranka`` version is required for
+ a particular robot software version. If this doesn't match the ``ros-noetic-libfranka`` version from the
+ repositories, you need to :ref:`build from source `.
+
+Binary packages for ``libfranka`` and ``franka_ros`` are available from the ROS repositories.
+After `setting up ROS Noetic `__, execute::
+
+ sudo apt install ros-noetic-libfranka ros-noetic-franka-ros
+
+.. _installation-build-from-source:
+
+Building from source
+--------------------
+
+Before building from source, please uninstall existing installations of ``libfranka`` and
+``franka_ros`` to avoid conflicts::
+
+ sudo apt remove "*libfranka*"
+
+
+
+.. _build-libfranka:
+
+Building libfranka
+^^^^^^^^^^^^^^^^^^
+
+To build ``libfranka``, install the following dependencies from Ubuntu's package manager::
+
+ sudo apt install build-essential cmake git libpoco-dev libeigen3-dev
+
+Then, download the source code by cloning ``libfranka`` from `GitHub `__.
+
+For Panda you need to clone:
+
+.. code-block:: shell
+
+ git clone --recursive https://github.com/frankaemika/libfranka # only for panda
+ cd libfranka
+
+By default, this will check out the newest release of ``libfranka``. If you want to build a particular version of
+``libfranka`` instead, check out the corresponding Git tag::
+
+ git checkout
+ git submodule update
+
+
+
+The above instructions for cloning libfranka only work for Panda. For Franka Research 3 you have to clone:
+
+.. code-block::
+
+ git clone --recursive https://github.com/frankaemika/libfranka --branch 0.10.0 # only for FR3
+ cd libfranka
+
+In the source directory, create a build directory and run CMake:
+
+.. code-block:: shell
+
+ mkdir build
+ cd build
+ cmake -DCMAKE_BUILD_TYPE=Release -DBUILD_TESTS=OFF ..
+ cmake --build .
+
+Optionally (but recommended), a ``libfranka`` Debian package can be built using the following command in the same directory:
+
+.. code-block:: shell
+
+ cpack -G DEB
+
+This creates `libfranka--.deb`. This package can then be installed with:
+
+.. code-block:: shell
+
+ sudo dpkg -i libfranka*.deb
+
+Building the ROS packages
+^^^^^^^^^^^^^^^^^^^^^^^^^
+
+After `setting up ROS Noetic `__, create a Catkin
+workspace in a directory of your choice:
+
+.. code-block:: shell
+
+ cd /path/to/desired/folder
+ mkdir -p catkin_ws/src
+ cd catkin_ws
+ source /opt/ros/noetic/setup.sh
+ catkin_init_workspace src
+
+Then clone the ``franka_ros`` repository from `GitHub `__::
+
+ git clone --recursive https://github.com/frankaemika/franka_ros src/franka_ros
+
+By default, this will check out the newest release of ``franka_ros``. If you want to build a particular version of
+``franka_ros`` instead, check out the corresponding Git tag::
+
+ git checkout
+
+Install any missing dependencies and build the packages:
+
+.. code-block:: shell
+
+ rosdep install --from-paths src --ignore-src --rosdistro noetic -y --skip-keys libfranka
+ catkin_make -DCMAKE_BUILD_TYPE=Release -DFranka_DIR:PATH=/path/to/libfranka/build
+ source devel/setup.sh
+
+.. warning::
+ If you also installed ``ros-noetic-libfranka``, ``libfranka`` might be picked up from ``/opt/ros/noetic``
+ instead of from your custom ``libfranka`` build!
+
+.. _preempt:
+
+Setting up the 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. This section describes the procedure of patching a kernel to support
+``PREEMPT_RT`` and creating an installation package.
+
+.. note::
+
+ NVIDIA binary drivers are not supported on ``PREEMPT_RT`` kernels.
+
+First, install the necessary dependencies::
+
+ sudo apt-get install build-essential bc curl ca-certificates gnupg2 libssl-dev lsb-release libelf-dev bison flex dwarves zstd libncurses-dev
+
+Then, you have to decide which kernel version to use. To find the one you are
+using currently, use ``uname -r``. Real-time patches are only available for
+select kernel versions, see
+https://www.kernel.org/pub/linux/kernel/projects/rt/. We recommend choosing the
+version closest to the one you currently use. If you choose a
+different version, simply substitute the numbers. Having decided on a version,
+use ``curl`` to download the source files:
+
+.. note::
+ For Ubuntu 16.04 tested with the kernel version 4.14.12:
+
+ .. code::
+
+ curl -SLO https://www.kernel.org/pub/linux/kernel/v4.x/linux-4.14.12.tar.xz
+ curl -SLO https://www.kernel.org/pub/linux/kernel/v4.x/linux-4.14.12.tar.sign
+ curl -SLO https://www.kernel.org/pub/linux/kernel/projects/rt/4.14/older/patch-4.14.12-rt10.patch.xz
+ curl -SLO https://www.kernel.org/pub/linux/kernel/projects/rt/4.14/older/patch-4.14.12-rt10.patch.sign
+
+.. note::
+ For Ubuntu 18.04 tested with the kernel version 5.4.19:
+
+ .. code::
+
+ curl -SLO https://www.kernel.org/pub/linux/kernel/v5.x/linux-5.4.19.tar.xz
+ curl -SLO https://www.kernel.org/pub/linux/kernel/v5.x/linux-5.4.19.tar.sign
+ curl -SLO https://www.kernel.org/pub/linux/kernel/projects/rt/5.4/older/patch-5.4.19-rt10.patch.xz
+ curl -SLO https://www.kernel.org/pub/linux/kernel/projects/rt/5.4/older/patch-5.4.19-rt10.patch.sign
+
+.. note::
+ For Ubuntu 20.04 tested with the kernel version 5.9.1:
+
+ .. code::
+
+ curl -SLO https://www.kernel.org/pub/linux/kernel/v5.x/linux-5.9.1.tar.xz
+ curl -SLO https://www.kernel.org/pub/linux/kernel/v5.x/linux-5.9.1.tar.sign
+ curl -SLO https://www.kernel.org/pub/linux/kernel/projects/rt/5.9/patch-5.9.1-rt20.patch.xz
+ curl -SLO https://www.kernel.org/pub/linux/kernel/projects/rt/5.9/patch-5.9.1-rt20.patch.sign
+
+And decompress them with::
+
+ xz -d *.xz
+
+Verifying file integrity
+^^^^^^^^^^^^^^^^^^^^^^^^
+
+.. note::
+ This step is optional but recommended!
+
+The ``.sign`` files can be used to verify that the downloaded files were not
+corrupted or tampered with. The steps shown here are adapted from the
+`Linux Kernel Archive `_ , see the
+linked page for more details about the process.
+
+You can use ``gpg2`` to verify the ``.tar`` archives::
+
+ gpg2 --verify linux-*.tar.sign
+ gpg2 --verify patch-*.patch.sign
+
+If your output is similar to the following::
+
+ $ gpg2 --verify linux-*.tar.sign
+ gpg: assuming signed data in 'linux-4.14.12.tar'
+ gpg: Signature made Fr 05 Jan 2018 06:49:11 PST using RSA key ID 6092693E
+ gpg: Can't check signature: No public key
+
+You have to first download the public key of the person who signed the above
+file. As you can see from the above output, it has the ID ``6092693E``. You can
+obtain it from the key server::
+
+ gpg2 --keyserver hkp://keyserver.ubuntu.com:80 --recv-keys 6092693E
+
+Similarly for the patch::
+
+ gpg2 --keyserver hkp://keyserver.ubuntu.com:80 --recv-keys 2872E4CC
+
+Note that keys for other kernel version might have different IDs, you will have to
+adapt accordingly.
+
+Having downloaded the keys, you can now verify the sources. Here is an example of
+a correct output::
+
+ $ gpg2 --verify linux-*.tar.sign
+ gpg: assuming signed data in 'linux-4.14.12.tar'
+ gpg: Signature made Fr 05 Jan 2018 06:49:11 PST using RSA key ID 6092693E
+ gpg: Good signature from "Greg Kroah-Hartman " [unknown]
+ gpg: aka "Greg Kroah-Hartman " [unknown]
+ gpg: aka "Greg Kroah-Hartman (Linux kernel stable release signing key) " [unknown]
+ gpg: WARNING: This key is not certified with a trusted signature!
+ gpg: There is no indication that the signature belongs to the owner.
+ Primary key fingerprint: 647F 2865 4894 E3BD 4571 99BE 38DB BDC8 6092 693E
+
+See `Linux Kernel Archive `_
+for more information about the warning.
+
+
+
+Compiling the kernel
+^^^^^^^^^^^^^^^^^^^^
+
+Once you are sure the files were downloaded properly, you can extract the source
+code and apply the patch::
+
+ tar xf linux-*.tar
+ cd linux-*/
+ patch -p1 < ../patch-*.patch
+
+Next copy your currently booted kernel configuration as the default config for the new real time kernel::
+
+ cp -v /boot/config-$(uname -r) .config
+
+Now you can use this config as the default to configure the build::
+
+ make olddefconfig
+ make menuconfig
+
+The second command brings up a terminal interface in which you can configure the preemption model. Navigate with the
+arrow keys to *General Setup* > *Preemption Model* and select *Fully Preemptible Kernel (Real-Time)*.
+
+After that navigate to *Cryptographic API* > *Certificates for signature checking*
+(at the very bottom of the list) > *Provide system-wide ring of trusted keys* >
+*Additional X.509 keys for default system keyring*
+
+Remove the "debian/canonical-certs.pem" from the prompt and press Ok. Save this
+configuration to ``.config`` and exit the TUI.
+
+.. note::
+ If you prefer GUIs over TUIs use ``make xconfig`` instead of ``make menuconfig``
+
+Afterwards, you are ready to compile the kernel. As this is a lengthy process, set the
+multithreading option ``-j`` to the number of your CPU cores::
+
+ make -j$(nproc) deb-pkg
+
+Finally, you are ready to install the newly created package. The exact names
+depend on your environment, but you are looking for ``headers`` and ``images``
+packages without the ``dbg`` suffix. To install::
+
+ sudo dpkg -i ../linux-headers-*.deb ../linux-image-*.deb
+
+Verifying the new kernel
+^^^^^^^^^^^^^^^^^^^^^^^^
+
+Restart your system. The Grub boot menu should now allow you to choose your
+newly installed kernel. To see which one is currently being used, see the output
+of the ``uname -a`` command. It should contain the string ``PREEMPT RT`` and the
+version number you chose. Additionally, ``/sys/kernel/realtime`` should exist and
+contain the the number ``1``.
+
+.. note::
+ If you encounter errors that you fail to boot the new kernel see :ref:`troubleshooting_realtime_kernel`
+
+.. _installation-real-time:
+
+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::
+
+ sudo addgroup realtime
+ sudo usermod -a -G realtime $(whoami)
+
+Afterwards, add the following limits to the `realtime` group in
+``/etc/security/limits.conf``::
+
+ @realtime soft rtprio 99
+ @realtime soft priority 99
+ @realtime soft memlock 102400
+ @realtime hard rtprio 99
+ @realtime hard priority 99
+ @realtime hard memlock 102400
+
+The limits will be applied after you log out and in again.
diff --git a/_sources/installation_windows.rst.txt b/_sources/installation_windows.rst.txt
new file mode 100644
index 0000000..2fd3138
--- /dev/null
+++ b/_sources/installation_windows.rst.txt
@@ -0,0 +1,81 @@
+Installation on Windows
+=======================
+
+.. caution::
+ Windows support is very experimental and only intended for experienced users. Since Windows is not real-time capable,
+ communication errors are very likely. Further, these instructions are may not up-to-date and adjustments might be necessary.
+
+Starting from ``libfranka`` >= 0.6.0, **experimental** Windows support is provided.
+This chapter describes how to install ``libfranka`` on Windows.
+``franka_ros`` is not supported on Windows.
+
+.. note::
+
+ Windows support has only been tested with Windows 10 and Visual Studio 2017.
+
+
+Building from source
+--------------------
+
+To build ``libfranka``, install the following dependencies:
+
+ * `Eigen3 `__
+ * `Poco `__
+
+Both can be easily installed with `vcpkg `__ via cmd prompt:
+
+.. code-block:: shell
+
+ cd /path/to/vcpkg
+ vcpkg install eigen3
+ vcpkg install poco
+
+Download the source code by cloning ``libfranka`` from `GitHub `__:
+
+.. code-block:: shell
+
+ git clone --recursive https://github.com/frankaemika/libfranka
+
+By default, this will check out the newest release of ``libfranka``. If you want to build
+a particular version of ``libfranka`` instead, check out the corresponding Git tag::
+
+ git checkout
+ git submodule update
+
+.. important::
+
+ Only ``libfranka`` >= 0.6.0 has Windows support!
+
+To build ``libfranka`` with Visual Studio open it as a CMake Project.
+Choose **File** > **Open** > **CMake** > **C:\Path\To\libfranka\CMakeLists.txt**.
+
+Generate the ``CMakeSettings.json``, which contains the CMake project settings.
+Select **CMake** > **Cache** > **Generate** > **CMakeSettings.json**. The file will be placed in
+your main ``libfranka`` directory.
+
+The next step is to solve the build dependencies.
+Make sure the compiler is able to find
+the required dependencies. This can be done either by copying all needed .dll libries into
+the chosen build root determined by ``CMakeSettings.json`` or using cmakeCommandArgs.
+Open **CMake** > **Change CMake settings** > **libfranka** and add
+
+.. code-block:: json
+
+ {
+ "cmakeCommandArgs": "-DPoco_DIR=C:\Path\To\Poco-Install\lib\cmake\Poco -DEigen3_DIR=C:\Path\To\Eigen-Install\lib\cmake\eigen3"
+ }
+
+Choose **CMake** > **Build** to build ``libfranka`` into the build directory,
+determined in ``CMakeSettings.json``
+
+.. hint::
+
+ Alternatively you can build libfranka using the `Developer Command Prompt for VS`:
+
+ .. code-block:: shell
+
+ cd /path/to/libfranka
+ mkdir build
+ cd build
+ cmake -DPoco_DIR=/Path/To/Poco/CMake/Config -DEigen3_DIR=/Path/To/Eigen/CMake/Config -G Ninja ..
+ ninja
diff --git a/_sources/libfranka.rst.txt b/_sources/libfranka.rst.txt
new file mode 100644
index 0000000..8800f56
--- /dev/null
+++ b/_sources/libfranka.rst.txt
@@ -0,0 +1,622 @@
+libfranka
+=========
+
+Before continuing with this chapter, please install or compile libfranka for :doc:`Linux `
+or :doc:`Windows `.
+API documentation for the latest version of ``libfranka`` is available at
+https://frankaemika.github.io/libfranka.
+
+.. figure:: _static/libfranka-architecture.png
+ :align: center
+ :figclass: align-center
+
+ Schematic overview
+
+``libfranka`` is the C++ implementation of the client side of the FCI. It handles the network
+communication with Control and provides interfaces to easily:
+
+ * execute **non-realtime commands** to control the Hand and configure Arm parameters.
+ * execute **realtime commands** to run your own 1 kHz control loops.
+ * read the **robot state** to get sensor data at 1 kHz.
+ * access the **model library** to compute your desired kinematic and dynamic parameters.
+
+During operation you might also encounter several **errors** that we detail at the end of
+this section.
+
+Non-realtime commands
+---------------------
+
+Non-realtime commands are blocking, TCP/IP-based and always executed `outside` of any realtime
+control loop. They encompass all of the Hand commands and some configuration-related commands
+for the Arm.
+
+.. figure:: _static/fci-architecture-non-realtime.png
+ :align: center
+ :figclass: align-center
+
+ Non-realtime commands for both Arm and Hand.
+
+The most relevant ones for the Hand are
+
+ * ``homing`` which calibrates the maximum grasping width of the Hand.
+ * ``move``, ``grasp`` and ``stop``, to move or grasp with the Hand.
+ * ``readOnce``, which reads the Hand state.
+
+Concerning the Arm, some useful non-realtime commands are:
+
+ * ``setCollisionBehavior`` which sets the contact and collision detection thresholds.
+ * ``setCartesianImpedance`` and ``setJointImpedance`` which set the impedance parameters
+ for the internal Cartesian impedance and internal joint impedance controllers.
+ * ``setEE`` sets the transformation *NE_T_EE* from nominal end effector to end effector
+ frame. The transformation from flange to end effector frame *F_T_EE* is split into two
+ transformations: *F_T_NE* and *NE_T_EE*. The transformation from flange to nominal end
+ effector frame *F_T_NE* can only be set through the administrator's interface.
+ * ``setK`` sets the transformation *EE_T_K* from end effector frame to stiffness frame.
+ * ``setLoad`` sets the dynamic parameters of a payload.
+ * ``automaticErrorRecovery`` that clears any command or control exception that previously
+ happened in the robot.
+
+For a complete and fully specified list check the API documentation for the
+`Arm `__
+or the `Hand `__.
+
+All operations (non-realtime or realtime) on the Arm or the Hand are performed through the
+``franka::Robot`` and ``franka::Gripper`` objects respectively. A connection to the Arm/Hand
+will be established when the object is created:
+
+.. code-block:: c++
+
+ #include
+ #include
+
+ ...
+
+ franka::Gripper gripper("");
+ franka::Robot robot("");
+
+The address can be passed either as a hostname or an IP address. In case of any error, either due
+to networking or conflicting library version, an exception of type ``franka::Exception`` will
+be thrown. When using several robots at the same time, simply create several objects with
+appropriate IP addresses.
+
+To run a specific command, simply call the corresponding method, e.g.
+
+.. code-block:: c++
+
+ gripper.homing();
+ robot.automaticErrorRecovery();
+
+
+Realtime commands
+-----------------
+
+Realtime commands are UDP based and require a 1 kHz connection to Control.
+There are two types of realtime interfaces:
+
+ * **Motion generators**, which define a robot motion in joint or Cartesian space.
+ * **Controllers**, which define the torques to be sent to the robot joints.
+
+There are 4 different types of external motion generators and 3 different types of controllers
+(one external and 2 internal) as depicted in the following figure:
+
+.. figure:: _static/rt-interfaces.png
+ :align: center
+ :figclass: align-center
+
+ Realtime interfaces: motion generators and controllers.
+
+You can either use a single interface or combine two different types. Specifically, you can
+command:
+
+ * *only a motion generator* and therefore use one of the two internal controllers to follow
+ the commanded motion.
+ * *only an external controller* and ignore any motion generator signals, i.e. torque control only.
+ * *a motion generator and an external controller* to use the inverse kinematics of Control in
+ your external controller.
+
+All realtime loops (motion generator or controller) are defined by a callback function that
+receives the robot state and the duration of the last cycle (1 ms unless packet losses occur)
+and returns the specific type of the interface. The ``control`` method of the ``franka::Robot``
+class will then run the control loop by executing the callback function at a 1 kHz frequency,
+as shown in this example
+
+.. code-block:: c++
+
+ std::function
+ my_external_controller_callback;
+ // Define my_external_controller_callback
+ ...
+
+ std::function
+ my_external_motion_generator_callback;
+ // Define my_external_motion_generator_callback
+ ...
+
+ try {
+ franka::Robot robot("");
+ // only a motion generator
+ robot.control(my_external_motion_generator_callback);
+ // only an external controller
+ robot.control(my_external_controller_callback);
+ // a motion generator and an external controller
+ robot.control(my_external_motion_generator_callback, my_external_controller_callback);
+ } catch (franka::Exception const& e) {
+ std::cout << e.what() << std::endl;
+ return -1;
+ }
+ return 0;
+ }
+
+All control loops are finished once the ``motion_finished`` flag of a realtime command is set
+to ``true``. An excerpt of the ``generate_joint_velocity_motion`` example included
+in the `libfranka examples `__ is shown here
+
+.. code-block:: c++
+
+ robot.control(
+ [=, &time](const franka::RobotState&, franka::Duration period) -> franka::JointVelocities {
+ time += period.toSec();
+
+ double cycle = std::floor(std::pow(-1.0, (time - std::fmod(time, time_max)) / time_max));
+ double omega = cycle * omega_max / 2.0 * (1.0 - std::cos(2.0 * M_PI / time_max * time));
+
+ franka::JointVelocities velocities = {{0.0, 0.0, 0.0, omega, omega, omega, omega}};
+
+ if (time >= 2 * time_max) {
+ std::cout << std::endl << "Finished motion, shutting down example" << std::endl;
+ return franka::MotionFinished(velocities);
+ }
+ return velocities;
+ });
+
+In this case, the callback function is defined directly in the call of the
+``robot.control( ... )`` function. It uses the joint velocity motion generator interface,
+as it returns a ``franka::JointVelocities`` object. It commands joint velocities to the last four
+joints and move them by approx. +/-12 degrees. After ``2 * time_max`` seconds it will return a
+``motion_finished`` flag by setting it to true with the ``franka::MotionFinished`` method and
+the control loop will stop.
+
+Note that if you use only a motion generator, the default controller is the internal joint
+impedance controller. You can however use the internal Cartesian impedance controller by
+setting the optional argument of the control function, e.g.
+
+.. code-block:: c++
+
+ // Set joint impedance (optional)
+ robot.setJointImpedance({{3000, 3000, 3000, 3000, 3000, 3000, 3000}});
+ // Runs my_external_motion_generator_callback with the default joint impedance controller
+ robot.control(my_external_motion_generator_callback);
+ // Identical to the previous line (default franka::ControllerMode::kJointImpedance)
+ robot.control(my_external_motion_generator_callback, franka::ControllerMode::kJointImpedance);
+
+ // Set Cartesian impedance (optional)
+ robot.setCartesianImpedance({{2000, 2000, 2000, 100, 100, 100}});
+ // Runs my_external_motion_generator_callback with the Cartesian impedance controller
+ robot.control(my_external_motion_generator_callback, franka::ControllerMode::kCartesianImpedance);
+
+For writing a controller, the ``franka::Robot::control`` function is used as well. The following
+example shows a simple controller commanding zero torque for each joint. Gravity is
+compensated by the robot.
+
+.. code-block:: c++
+
+ robot.control([&](const franka::RobotState&, franka::Duration) -> franka::Torques {
+ return {{0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0}};
+ });
+
+You can find examples for all interfaces and combinations of control loops in the
+`libfranka examples `__. Prior to running
+the examples, verify that the robot has enough free space to move without colliding. Then, for
+instance for the ``generate_joint_velocity_motion`` example execute the following command from
+the ``libfranka`` build directory:
+
+.. code-block:: shell
+
+ ./examples/generate_joint_velocity_motion
+
+
+.. warning::
+
+ For writing your own motion generators or controllers it is crucial to deliver a smooth
+ signal to the robot. Nonsmooth signals can easily generate discontinuity errors or even
+ make the robot unstable. Check the :ref:`interface specifications
+ ` before starting.
+
+.. _signal-processing:
+
+Signal processing
+*******************
+To facilitate the control of the robot under non-ideal network connections, libfranka includes
+signal processing functions that will modify the user-commanded values to make them conform
+with the :ref:`limits of the interface`.
+There are two *optional* functions included in all realtime control loops:
+
+ * A first-order **low-pass filter** to smooth the user-commanded signal.
+ * A **rate limiter**, that saturates the time derivatives of the user-commanded values.
+
+* As of version ``0.5.0``, libfranka includes a **low-pass filter** for all realtime
+ interfaces **running by default** with a 100 Hz cutoff frequency.
+ The filter smooths commanded signals
+ to provide more stable robot motions but does not prevent the violation of the
+ :ref:`limits of the interface`.
+
+* As of version ``0.4.0``, **rate limiters** for all realtime interfaces are
+ **running by default**. `Rate limiters`, also called `safe controllers`, will limit the
+ rate of change of the signals sent by the user to prevent the violation of the
+ :ref:`limits of the interface`. For motion generators, it
+ will limit the acceleration and jerk, while, for an external controller, it will limit the
+ torque rate. Their main purpose is to increase the robustness of your control loop.
+ In case of packet losses, even when the signals that you send conform with the
+ interface limits, Control might detect a violation of velocity, acceleration or jerk limits.
+ Rate limiting will adapt your commands to make sure that this does not happen.
+ Check the :ref:`noncompliant errors section` for more details.
+
+ .. caution::
+
+ Rate limiting will ensure no limits are violated except for the joint limits after
+ inverse kinematics, whose violation produces the family of errors starting with
+ ``cartesian_motion_generator_joint_*``. Check the
+ :ref:`noncompliant errors section` for more details.
+
+ .. hint::
+
+ The limits used in the rate limiter are defined in ``franka/rate_limiting.h``
+ and are set to the interface limits. If this produces a jerky or unstable behavior
+ you can set the limits to lower values, activate the low-pass filter or reduce its cutoff
+ frequency.
+
+To control the signal processing functions, all ``robot.control()`` function calls
+have two additional optional parameters. The first one is a flag to activate or
+deactivate the rate limiter while the second one
+specifies the cutoff frequency of the first-order low-pass filter. If the cutoff frequency
+``>=1000.0`` the filter will be deactivated. For instance
+
+.. code-block:: c++
+
+ // Set Cartesian impedance (optional)
+ robot.setCartesianImpedance({{2000, 2000, 2000, 100, 100, 100}});
+ // Runs my_external_motion_generator_callback with the Cartesian impedance controller,
+ // rate limiters on and low-pass filter with 100 Hz cutoff
+ robot.control(my_external_motion_generator_callback, franka::ControllerMode::kCartesianImpedance);
+ // Identical to the previous line (default true, 100.0 Hz cutoff)
+ robot.control(my_external_motion_generator_callback, franka::ControllerMode::kCartesianImpedance, true, 100.0);
+ // Runs my_external_motion_generator_callback with the Cartesian impedance controller,
+ // rate limiters off and low-pass filter off
+ robot.control(my_external_motion_generator_callback, franka::ControllerMode::kCartesianImpedance, false, 1000.0);
+
+Or similarly for an external controller
+
+.. code-block:: c++
+
+ // With rate limiting and filter
+ robot.control(my_external_controller);
+ // Identical to the previous line (default true, 100.0 Hz cutoff)
+ robot.control(my_external_controller, true, 100.0);
+ // Without rate limiting but with low-pass filter (100.0 Hz)
+ robot.control(my_external_controller, false);
+ // Without rate limiting and without low-pass filter
+ robot.control(my_external_controller, false, 1000.0);
+
+.. danger::
+
+ The low-pass filter and the rate limiter are robustness features against packet losses
+ to be used **after** you have already designed a smooth motion generator or controller.
+ For the first tests of a new control loop we strongly recommend to deactivate these
+ features.
+ Filtering and limiting the rate of a nonsmooth signal can yield instabilities or
+ unexpected behavior. Too many packet losses can also generate unstable behavior.
+ Check your communication quality by monitoring the ``control_command_success_rate``
+ signal of the robot state.
+
+.. _control-side:
+
+Under the hood
+********************
+Until now we have covered details of the interface running on the client side, i.e your own
+workstation PC. The behavior of the full control loop including the Control side of the
+realtime interface is shown in the following figure
+
+.. figure:: _static/rt-loop.png
+ :align: center
+ :figclass: align-center
+
+ Realtime loop: from control commands to the robot desired joint torques.
+
+**Motion generators**: all motion generator commands sent by the user have the subscript `c`
+which stands for 'commanded'. When a motion generator is sent, the `Robot Kinematics completion`
+block will compute the forward/inverse kinematics of the user-commanded signal yielding the
+'desired' signals, subscript `d`. If an internal controller is used, it will generate the
+necessary torques :math:`\tau_{d}` to track the corresponding computed `d` signals (the internal
+joint impedance controller will follow the joint signals :math:`q_{d}, \dot{q}_{d}` and the
+internal Cartesian impedance controller the Cartesian ones
+:math:`{}^OT_{EE,d}, {}^O\dot{P}_{EE,d}`) and send them to the robot joints.
+All variables on the Control side of the figure, i.e. the last received `c` values
+(after the low pass filter and the extrapolation due to packet losses,
+read below for an explanation), the computed `d` values
+and their time derivatives are sent back to the user in the robot state. This way you can
+take advantage of the inverse kinematics in your own external controller and, at the same time,
+it will offer you `full transparency`: you will always know the exact values
+and derivatives that the robot received and tracked in the last sample.
+
+.. hint::
+
+ When you are using a *joint* motion generator, the `Robot kinematics completion` block will
+ not modify the commanded *joint* values and therefore :math:`q_d, \dot{q}_d, \ddot{q}_d` and
+ :math:`q_c, \dot{q}_c, \ddot{q}_c` are equivalent. Note that you will only find the
+ `d` signals in the robot state. If you use a *Cartesian* motion generator, the `Robot
+ kinematics completion` block might modify the user-commanded values to avoid singularities
+ and therefore the desired signals :math:`{}^OT_{EE,d}, {}^O\dot{P}_{EE,d}` and the commanded
+ signals :math:`{}^OT_{EE,c}, {}^O\dot{P}_{EE,c}, {}^O\ddot{P}_{EE,c}` might differ.
+ You will find both the `d` and the `c` signals in the robot state.
+
+**External controller**: if an external controller is sent, the desired joint torques commanded
+by the user :math:`\tau_{d}` are directly fed to the robot joints with the additional compensation
+of gravity and motor friction, resulting in the following equation:
+
+ :math:`\tau_{c} = \tau_{d} + \tau_{f} + \tau_{g}`
+
+Where:
+
+* :math:`\tau_{d}` is the desired torque given as input by the libfranka user,
+* :math:`\tau_{c}` is the torque effectively commanded to the joint,
+* :math:`\tau_{f}` is the torque to compensate the motor friction,
+* :math:`\tau_{g}` is the torque required for compensating the gravity of the whole kinematic chain.
+
+Note that, on the Control side, there are two things that could modify your signals:
+
+* `Packet losses`, which may occur if you:
+
+ * don't have a very good connection due to the performance of your PC + network card.
+ * your control loop is taking too long to compute (you have, depending on you network card and
+ PC configuration, approx. < 300 :math:`\mu s` for your own control loop).
+
+ In this case, Control assumes a constant acceleration model or a constant torque to extrapolate
+ your signals. If ``>=20`` packets are lost in a row the control loop is stopped with the
+ ``communication_constraints_violation`` exception.
+
+.. hint::
+
+ If you are not sure if your signals are being filtered or extrapolated, you can always check the
+ last commanded values that you sent and compare them with the values you receive on the robot
+ state in the next sample. You will also find these values after an exception occurs in the
+ ``franka::ControlException::log`` member of the exception.
+
+
+Robot state
+-----------------------
+The robot state delivers the robot sensor readings and estimated values at a 1 kHz rate.
+It provides:
+
+ * *Joint level signals*: motor and estimated joint angles and their derivatives,
+ joint torque and derivatives, estimated external torque, joint collision/contacts.
+ * *Cartesian level signals*: Cartesian pose, configured endeffector and load parameters,
+ external wrench acting on the endeffector, Cartesian collision
+ * *Interface signals*: the last commanded and desired values and their derivatives,
+ as explained in the previous subsection.
+
+For a complete list check the API of the ``franka::RobotState``
+`here `__.
+As shown in the the previous subsection, the robot state is always an input of all callback
+functions for control loops. However, if you wish to only read the robot state without controlling
+it, the functions ``read`` or ``readOnce`` can be used to gather it, e.g. for
+logging or visualization purposes.
+
+With a valid connection, *a single sample of the robot state* can be read using the ``readOnce``
+function:
+
+.. code-block:: c++
+
+ franka::RobotState state = robot.readOnce();
+
+The next example shows how to continuously read the robot state using the ``read`` function and a
+callback. Returning ``false`` in the callback stops the loop. In the following, an excerpt of the
+``echo_robot_state`` example is shown:
+
+.. code-block:: c++
+
+ size_t count = 0;
+ robot.read([&count](const franka::RobotState& robot_state) {
+ // Printing to std::cout adds a delay. This is acceptable for a read loop such as this,
+ // but should not be done in a control loop.
+ std::cout << robot_state << std::endl;
+ return count++ < 100;
+ });
+
+
+Model library
+--------------------
+The robot model library provides
+
+ - The forward kinematics of all robot joints.
+ - The body and zero jacobian matrices of all robot joints.
+ - Dynamic parameters: inertia matrix, Coriolis and centrifugal vector and gravity vector.
+
+Note that after you load the model library, you can compute kinematic and dynamic parameters for
+an arbitrary robot state, not just the current one. You can also use the model library in a non
+realtime fashion, e.g. in an optimization loop. The libfranka examples include exemplary code
+`printing joint poses
+`_
+or `computing jacobians and dynamic parameters
+`_.
+
+.. _control-errors:
+
+Errors
+-------
+
+Using the FCI you will encounter several errors that happen either due to noncompliant
+commands sent by the user, due to communication problems or due to the robot behavior.
+The most relevant ones are detailed in the following subsections.
+For a complete list please check the `API documentation
+`_.
+
+.. hint::
+
+ Note that, after an error occurs, you can automatically clear it and continue running your
+ program with the ``franka::Robot::automaticErrorRecovery()`` command without user intervention.
+ Check the exception string before continuing to make sure that the error is not a critical
+ one.
+
+ Some errors can also be cleared manually by toggling the external activation device or by
+ using the error recovery button in Desk.
+
+.. _noncompliant-errors:
+
+Errors due to noncompliant commanded values
+********************************************
+If the :ref:`commanded values` sent by the user
+do not comply with the :ref:`interface requirements`,
+one of the following errors will occur:
+
+* Errors due to **wrong initial values of a motion generator**:
+
+ - ``joint_motion_generator_start_pose_invalid``
+ - ``cartesian_position_motion_generator_start_pose_invalid``
+ - ``cartesian_motion_generator_start_elbow_invalid``
+ - ``cartesian_motion_generator_elbow_sign_inconsistent``
+
+ These errors indicate a discrepancy between the current robot values and the initial values sent
+ by the user. To fix these errors, make sure that your control loop starts with the last commanded
+ value observed in the robot state. For instance, for the joint position interface
+
+ .. code-block:: c++
+
+ double time{0.0};
+ robot.control(
+ [=, &time](const franka::RobotState& robot_state, franka::Duration period) -> franka::JointPositions {
+ time += period.toSec();
+ if (time == 0) {
+ // Send the last commanded q_c as the initial value
+ return franka::JointPositions(robot_state.q_c);
+ } else {
+ // The rest of your control loop
+ ...
+ }
+ });
+
+
+* Errors due to a **position limit** violation using a joint position/velocity motion generator,
+ which will produce a ``joint_motion_generator_position_limits_violation``. Solving this error
+ should be simple: make sure that the values that you send are in the
+ :ref:`limits`. Cartesian
+ interfaces also have limits on the joint signals that result after the inverse kinematics: the
+ ``cartesian_motion_generator_joint_position_limits_violation`` will be triggered if the inverse
+ kinematics solver of Control yields a joint configuration out of the limits.
+
+* Errors due to **velocity** limits violation and **discontinuity errors**, which refer to
+ **acceleration** and/or **jerk** limits violation. If you use a joint motion generator the
+ possible errors are
+
+ - ``joint_motion_generator_velocity_limits_violation``
+ - ``joint_motion_generator_velocity_discontinuity`` (acceleration limit violation)
+ - ``joint_motion_generator_acceleration_discontinuity`` (jerk limit violation)
+
+ If you use a Cartesian one, the possible errors are
+
+ - Cartesian limits:
+
+ - ``cartesian_motion_generator_velocity_limits_violation``
+ - ``cartesian_motion_generator_velocity_discontinuity`` (acceleration limit violation)
+ - ``cartesian_motion_generator_acceleration_discontinuity`` (jerk limit violation)
+
+ - Joint limits after the inverse kinematics
+
+ - ``cartesian_motion_generator_joint_velocity_limits_violation``
+ - ``cartesian_motion_generator_joint_velocity_discontinuity``
+ (acceleration limit violation)
+ - ``cartesian_motion_generator_joint_acceleration_discontinuity`` (jerk limit violation)
+
+ To mitigate velocity violations or discontinuity errors, make sure that the signals that
+ you command do not violate the :ref:`limits`. For every
+ motion generator, Control differentiates the signals sent by the user with backwards Euler.
+ For instance, if, using a joint position motion generator, at time :math:`k` the user sends
+ the command :math:`q_{c,k}`, the resulting velocity, acceleration and jerk will be
+
+ - Velocity :math:`\dot{q}_{c,k} = \frac{q_{c,k} - q_{c,k-1}}{\Delta t}`
+ - Acceleration :math:`\ddot{q}_{c,k} = \frac{\dot{q}_{c,k} - \dot{q}_{c,k-1}}{\Delta t}`
+ - Jerk :math:`\dddot{q}_{c,k} = \frac{\ddot{q}_{c,k} - \ddot{q}_{c,k-1}}{\Delta t}` ,
+
+ where :math:`\Delta t = 0.001`. Note that :math:`q_{c,k-1}, \dot{q}_{c,k-1}` and
+ :math:`\ddot{q}_{c,k-1}` are always sent back
+ to the user in the robot state as :math:`q_{d}, \dot{q}_{d}` and
+ :math:`\ddot{q}_{d}` so you will be able to
+ compute the resulting derivatives in advance, even in case of packet losses. Check the
+ :ref:`section about the details of the Control side of the interface`
+ for more details.
+
+ Finally, for the torque interface a **torque rate** limit violation triggers the error
+
+ - ``controller_torque_discontinuity``
+
+ Control also computes the torque rate with backwards Euler, i.e.
+ :math:`\dot{\tau}_{d,k} = \frac{\tau_{d,k} - \tau_{d,k-1}}{\Delta t}`. The previous desired
+ torque commanded by the user is also sent back in the robot state as :math:`\tau_d`
+ so you will also be able to compute the resulting torque rate in advance,
+ even in case of packet losses.
+
+.. hint::
+
+ The rate limiters included in ``libfranka`` since version ``0.4.0`` modify the signals
+ sent by the user to make them conform with all these limits except for the joint limits
+ after the inverse kinematics. You can check the ``include/franka/rate_limiting.h`` and
+ ``src/rate_limiting.cpp`` for exemplary code on how to compute resulting velocity,
+ acceleration and jerk for all interfaces. We emphasize again that using rate limiting on a
+ discontinuous signal can easily yield to unstable behavior, so please make sure that
+ your signal is smooth enough before enabling this *robustness* feature.
+
+
+Errors due to communication problems
+************************************
+If during a realtime loop Control does not receive any packets during 20 cycles, i.e. 20 ms, you
+will receive a ``communication_constraints_violation`` error.
+Note that if your connection has intermittent packet drops, it might not stop, but it could
+trigger `discontinuity` errors even when your source signals conform with the interface
+specification.
+In that case, check our :ref:`troubleshooting section `
+and consider enabling the :ref:`signal processing functions `
+to increase the robustness of your control loop.
+
+
+Behavioral errors
+******************
+.. warning::
+
+ These monitoring features are by no means conform with any safety norm and do not
+ guarantee any safety to the user. They only aim for helping researchers during the
+ development and testing of their control algorithms.
+
+* **Reflex errors**. If the estimated external torques :math:`\hat{\tau}_{ext}` or forces
+ :math:`{}^O\hat{F}_{ext}` surpass the configured thresholds, a ``cartesian_reflex`` or
+ ``joint_reflex`` error will be triggered respectively. You can configure the thresholds
+ with the ``franka::Robot::setCollisionBehavior`` non realtime command.
+
+ .. hint::
+
+ If you wish the robot to have contacts with the environment you must set the
+ collision thresholds to higher values. Otherwise, once you grasp an object or push
+ against a surface, a reflex will be triggered. Also, very fast or abrupt motions
+ without contacts could
+ trigger a reflex if thresholds are low; the external torques and forces are
+ only *estimated* values that could be innacurate depending on the robot
+ configuration, especially during high acceleration phases. You can monitor
+ their values observing :math:`\hat{\tau}_{ext}` and :math:`{}^O\hat{F}_{ext}`
+ in the robot state.
+
+* **Self-collision avoidance**. If the robot reaches a configuration which is close to a
+ self-collision, it will trigger a ``self_collision_avoidance_violation`` error.
+
+ .. warning::
+ This error does not guarantee that the robot will prevent a self collision at any
+ configuration and speed. If, using the torque interface, you drive the robot at
+ full speed against itself the robot might self-collide.
+
+
+* If the **torque sensor limit** is reached, a ``tau_j_range_violation``
+ will be triggered. This does not guarantee that the sensor will not be damaged after any
+ high-torque interactions or motions but aims for preventing some of it.
+
+* If the **maximum allowed power** is reached, the ``power_limit_violation`` will be triggered.
+ It will prevent the robot from moving and continuing the control loop.
+
+* If you reach the joint or the Cartesian limits you will get
+ a ``joint_velocity_violation`` or a ``cartesian_velocity_violation`` error respectively.
diff --git a/_sources/libfranka_changelog.rst.txt b/_sources/libfranka_changelog.rst.txt
new file mode 100644
index 0000000..3554ae5
--- /dev/null
+++ b/_sources/libfranka_changelog.rst.txt
@@ -0,0 +1,6 @@
+libfranka changelog
+===================
+
+.. include:: ../libfranka/CHANGELOG.md
+ :parser: myst_parser.sphinx_
+ :start-line: 2
diff --git a/_sources/overview.rst.txt b/_sources/overview.rst.txt
new file mode 100644
index 0000000..d67e7d5
--- /dev/null
+++ b/_sources/overview.rst.txt
@@ -0,0 +1,48 @@
+Overview
+========
+
+.. figure:: _static/fci-architecture.png
+ :align: center
+ :figclass: align-center
+
+
+ Schematic overview.
+
+The Franka Control Interface (FCI) allows a fast and direct low-level bidirectional connection
+to the Arm and Hand. It provides the current status of the robot and enables its direct control
+with an external workstation PC connected via Ethernet.
+By using ``libfranka``, our open source C++ interface, you can send real-time control values
+at 1 kHz with 5 different interfaces:
+
+ * Gravity & friction compensated joint level torque commands.
+ * Joint position or velocity commands.
+ * Cartesian pose or velocity commands.
+
+At the same time, you get access to 1 kHz measurements of:
+
+ * Measured joint data, such as the position, velocity and link side torque sensor signals.
+ * Estimation of externally applied torques and forces.
+ * Various collision and contact information.
+
+You also get access to the robot model library which provides:
+
+ * Forward kinematics of all robot joints.
+ * Jacobian matrix of all robot joints.
+ * Dynamics: inertia matrix, Coriolis and centrifugal vector and gravity vector.
+
+In addition, ``franka_ros`` connects Franka Emika research robots with the entire ROS ecosystem.
+It integrates ``libfranka`` into `ROS Control `_.
+Additionally, it includes `URDF `_ models and detailed 3D meshes of our
+robots and end effectors, which allows visualization (e.g. RViz) and kinematic simulations.
+`MoveIt! `_ integration makes it easy to move the robot and control
+the gripper, and the provided examples show you how to control your robot using ROS.
+
+.. important::
+
+ Data is sent over the network with a frequency of 1 kHz. Therefore, a good network connection
+ is required!
+
+.. important::
+
+ While the FCI is active you have full, exclusive control of the Arm and Hand. This means that
+ you `cannot` use Desk or Apps at the same time as the FCI.
diff --git a/_sources/requirements.rst.txt b/_sources/requirements.rst.txt
new file mode 100644
index 0000000..0bd7f1f
--- /dev/null
+++ b/_sources/requirements.rst.txt
@@ -0,0 +1,66 @@
+Minimum system and network requirements
+=======================================
+
+This page only specifies the requirements for running the Franka Control Interface (FCI).
+Additional requirements are specified in the documents that you have received with your robot.
+
+Workstation PC
+--------------
+
++---------------------------------------------------------+
+| Minimum System Requirements |
++===================+=====================================+
+| Operating System | Linux with PREEMPT_RT patched kernel|
+| | **OR** Windows 10 (experimental) |
++-------------------+-------------------------------------+
+| Network card | 100BASE-TX |
++-------------------+-------------------------------------+
+
+Since the robot sends data at 1 kHz frequency, it is important that the workstation PC is configured
+to minimize latencies. For example, we recommend to
+:ref:`disable CPU frequency scaling