diff --git a/source/_static/vscode.png b/source/_static/vscode.png new file mode 100644 index 0000000..5eb8d58 Binary files /dev/null and b/source/_static/vscode.png differ diff --git a/source/compatibility.rst b/source/compatibility.rst index 327bab3..33b4f43 100644 --- a/source/compatibility.rst +++ b/source/compatibility.rst @@ -35,10 +35,10 @@ compatible with the libfranka version 0.4.0 and 0.5.0. | >= 1.1.0 (FER) | 0.2.0 | 2 / 1 | | | +----------------------+-------------------+-----------------+--------------------+-----------------+ -`Robot version -`_ -and `Gripper version -`_ +`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`. diff --git a/source/franka_ros2.rst b/source/franka_ros2.rst index 3570837..ddb2e74 100644 --- a/source/franka_ros2.rst +++ b/source/franka_ros2.rst @@ -8,7 +8,7 @@ The `franka_ros2 repo `_ contains a :doc:`libfranka `. .. caution:: - franka_ros2 is currently beta software, so be careful while using it and report bugs on + franka_ros2 is in rapid development. Anticipate breaking changes. Report bugs on `GitHub `_. @@ -16,15 +16,17 @@ Prerequisites ------------- * A `ROS 2 Humble installation `_ - (ros-humble-desktop) + (ros-humble-desktop) or a VSCode IDE with DevContainer. * A :ref:`PREEMPT_RT kernel ` (optional, but strongly recommended). -* A system-wide :ref:`libfranka installation `. Here is a minimal example: +* A system-wide :ref:`libfranka installation `. Minimum supported version of libfranka is 0.11.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.11.0 mkdir build && cd build cmake -DCMAKE_BUILD_TYPE=Release -DBUILD_TESTS=OFF .. cmake --build . -j$(nproc) @@ -41,11 +43,18 @@ Optional .bashrc Settings ``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 \ @@ -56,6 +65,7 @@ Setup 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 @@ -71,6 +81,56 @@ Setup 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 frnaka_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:: + + open folder 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 ------ @@ -112,15 +172,13 @@ This controller moves the robot to its home configuration. 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. +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 ^^^^^^^^^^^^^^^^^^^^^^^ @@ -131,6 +189,17 @@ joints while it is running. 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:= + + Package Descriptions -------------------- @@ -155,30 +224,25 @@ button is pressed. However, once a controller that uses the ``effort_command_int 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 start gravity_compensation_example_controller + 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 stop + 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 start + ros2 control set_controller_state gravity_compensation_example_controller active or load and start a different one:: - ros2 control load_controller --set-state start joint_impedance_example_controller + ros2 control load_controller --set-state active joint_impedance_example_controller -.. note:: - - When the robot stops due to an error, the ``ros2_control_node`` dies. This will shutdown all - other nodes as well. To recover from this, you have to toggle the user stop button before you can restart the - launch file. .. _franka_description: @@ -200,10 +264,6 @@ This package contains a few controllers that can be seen as example of how to wr 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. -.. important:: - In contrast to franka_ros, it is currently not possible to directly access properties like the mass matrix, - coriolis torques or jacobians. - franka_gripper ^^^^^^^^^^^^^^ @@ -225,9 +285,8 @@ 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. +In a different tab you can now perform the homing and send a grasp command.:: -.. code-block:: shell 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}" @@ -236,9 +295,7 @@ The inner and outer epsilon are 0.005 meter per default. You can also explicitly 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. - -.. code-block:: shell +To stop the grasping, you can use ``stop`` service.:: ros2 service call /panda_gripper/stop std_srvs/srv/Trigger {} @@ -247,7 +304,7 @@ To stop the grasping, you can use ``stop`` service. franka_hardware ^^^^^^^^^^^^^^^ -This package contains the ``franka_hardware`` plugin needed for `ros2_control `_. +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 manger via the robot description. It provides for each joint: @@ -256,8 +313,47 @@ It provides for each joint: * an ``effort state interface`` that contains the measured link-side joint torques including gravity. * an ``effort command interface`` that contains the desired joint torques without gravity. +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. +.. hint:: + Joint Position and Velocity controllers will be soon available + +.. _franka_semantic_components: + +franka_semantic_components +^^^^^^^^^^^^^^^^^^^^^^^^^^ + +This package contains franka_robot_model and franka_robot_state 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) +`_ + +.. _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 ^^^^^^^^^^^^^^^^^^^^ @@ -275,20 +371,20 @@ for backward compatibility. New applications should use the new ``panda_manipula franka_msgs ^^^^^^^^^^^ -This package contains the definitions for the different gripper actions. +This package contains the definitions for the different gripper actions and robot state message. -.. important:: - In contrast to franka_ros, there is no longer a FrankaState message, as there is currently no way to communicate it - from the hardware class. 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 `_ and backported to Humble. It offers -a ``FollowJointTrajectory`` action that is needed for MoveIt. +`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 ---------------------------------------------- @@ -334,14 +430,94 @@ Compared to ``franka_ros`` we currently offer a reduced set of controller interf * 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 `_. -The reason is that the hardware interface currently only supports double data types, -making it impossible to expose e.g. a ``franka::RobotState``. 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 ------------