From 31f54ed106e580f28d47aa3cd8e0c0d27a33fc98 Mon Sep 17 00:00:00 2001 From: Baris Yazici Date: Thu, 23 Nov 2023 13:31:57 +0100 Subject: [PATCH 1/4] docs: franka_ros2 --- source/franka_ros2.rst | 162 ++++++++++++++++++++++++++++++++++++++++- 1 file changed, 158 insertions(+), 4 deletions(-) diff --git a/source/franka_ros2.rst b/source/franka_ros2.rst index ddb2e74..a92e712 100644 --- a/source/franka_ros2.rst +++ b/source/franka_ros2.rst @@ -18,7 +18,8 @@ Prerequisites * A `ROS 2 Humble installation `_ (ros-humble-desktop) or a VSCode IDE with DevContainer. * A :ref:`PREEMPT_RT kernel ` (optional, but strongly recommended). -* A system-wide :ref:`libfranka installation `. Minimum supported version of libfranka is 0.11.0. +* For cartesian_pose, joint_position and elbow position controllers 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 @@ -26,7 +27,7 @@ Prerequisites sudo apt install -y libpoco-dev libeigen3-dev git clone https://github.com/frankaemika/libfranka.git --recursive cd libfranka - git switch 0.11.0 + git switch 0.13.0 mkdir build && cd build cmake -DCMAKE_BUILD_TYPE=Release -DBUILD_TESTS=OFF .. cmake --build . -j$(nproc) @@ -115,7 +116,7 @@ VSCode DevContainer working schematic is shown in the below image: Write in the command prompt bar:: - open folder in container + Dev Containers: Rebuild and Reopen in Container and click this option in the search results @@ -199,6 +200,74 @@ Joint4 body jacobian and end-effector jacobian with respect to the base frame. 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 based from the current position of the robot. + +to read the the start position of the robot, you can read the command interface values before starting sending any commands. + +.. code-block:: shell + + if (initialization_flag_) { + for (size_t i = 0; i < 7; ++i) { + initial_q_.at(i) = command_interfaces_[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 + + +Cartesian Pose Example +^^^^^^^^^^^^^^^^^^^^^^ +This example uses 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 command 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 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 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 -------------------- @@ -312,6 +381,8 @@ It provides for each joint: * 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 ``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 @@ -332,7 +403,7 @@ The IP of the robot is read over a parameter from the URDF. franka_semantic_components ^^^^^^^^^^^^^^^^^^^^^^^^^^ -This package contains franka_robot_model and franka_robot_state classes. +This package contains franka_robot_model and 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. @@ -342,6 +413,89 @@ 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, user needs to pass a boolean flag to the constructor to indicate whether the interface is for the elbow or not. + +.. code-block:: shell + + CartesianPoseInterface cartesian_pose_interface(false); + +This interface allows users to read the current pose command interface values set by the franka hardware interface. + +.. code-block:: shell + + std::array pose; + cartesian_pose_interface.getCommandedPoseConfiguration(pose); + +One could also read quaternion and translation values in Eigen format. + +.. code-block:: shell + + Eigen::Quaterniond quaternion; + Eigen::Vector3d translation; + cartesian_pose_interface.getCommandedPoseConfiguration(quaternion, translation); + +After setting up the cartesian interface, you need to assign_loaned_command_interface in your custom controller. +This needs to be done in the on_activate() function of the controller. Examples can be found in the +create a link to the line `assign loaned comamand interface example +`_ + +.. code-block:: shell + + cartesian_pose_interface.assign_loaned_command_interface(command_interfaces_); + +In the update function of the controller you can send pose command to the robot. + +.. code-block:: shell + + std::array pose; + // column major homogenous transformation matrix + pose = {1, 0, 0, 0, 0, 1, 0, 0, 0, 0, 1, 0, 0.5, 0, 0.5, 1}; + cartesian_pose_interface.setCommandedPoseConfiguration(pose); + +Or you can send quaternion, translation values in Eigen format. + +.. code-block:: shell + + Eigen::Quaterniond quaternion; + Eigen::Vector3d translation; + quaternion = Eigen::Quaterniond(1, 0, 0, 0); + translation = Eigen::Vector3d(0.5, 0, 0.5); + cartesian_pose_interface.setCommandedPoseConfiguration(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:: shell + + CartesianVelocityInterface cartesian_velocity_interface(k_elbow_active); + +This interface allows users to read the current velocity command interface values set by the franka hardware interface. + +.. code-block:: shell + + std::array velocity; + cartesian_velocity_interface.getCommandedVelocityConfiguration(velocity); + +To send the velocity command to the robot, you need to assign_loaned_command_interface in your custom controller. + +.. code-block:: shell + + cartesian_velocity_interface.assign_loaned_command_interface(command_interfaces_); + +In the update function of the controller you can send velocity command to the robot. + +.. code-block:: shell + + std::array velocity; + velocity = {0, 0, 0, 0, 0, 0.1}; + cartesian_velocity_interface.setCommandedVelocityConfiguration(velocity); + .. _franka_robot_state_broadcaster: franka_robot_state_broadcaster From ed5b80521eca622044f4044bf5b80f17b1d59189 Mon Sep 17 00:00:00 2001 From: Baris Yazici Date: Thu, 23 Nov 2023 13:45:31 +0100 Subject: [PATCH 2/4] update submodules --- franka_ros2 | 2 +- libfranka | 2 +- 2 files changed, 2 insertions(+), 2 deletions(-) diff --git a/franka_ros2 b/franka_ros2 index 2483790..1768569 160000 --- a/franka_ros2 +++ b/franka_ros2 @@ -1 +1 @@ -Subproject commit 2483790c740c02a51cca154d6b22e55392b611fd +Subproject commit 1768569ca011cdcdf0a56be6e882d0a8800e594d diff --git a/libfranka b/libfranka index 4f9e3cc..457021b 160000 --- a/libfranka +++ b/libfranka @@ -1 +1 @@ -Subproject commit 4f9e3cc666e42d267f1ab566869c4f4c552e5b57 +Subproject commit 457021b7f05493046bc43f159f3e4e703c5e88b3 From f6a42f22024252ed342f2af8881f697b43dac15e Mon Sep 17 00:00:00 2001 From: Baris Yazici Date: Thu, 23 Nov 2023 18:25:06 +0100 Subject: [PATCH 3/4] update joint velocity docu --- source/franka_ros2.rst | 115 ++++++++++++++++++++--------------------- 1 file changed, 55 insertions(+), 60 deletions(-) diff --git a/source/franka_ros2.rst b/source/franka_ros2.rst index a92e712..5d2d881 100644 --- a/source/franka_ros2.rst +++ b/source/franka_ros2.rst @@ -18,7 +18,7 @@ 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 controllers realtime-kernel is absolutely necessary. +* 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: @@ -102,7 +102,7 @@ VSCode DevContainer working schematic is shown in the below image: git clone https://github.com/frankaemika/franka_ros2.git src/franka_ros2 -4. Move the .devcontainer folder to the frnaka_ros2_ws parent folder:: +4. Move the .devcontainer folder to the franka_ros2_ws parent folder:: mv franka_ros2/.devcontainer . @@ -205,15 +205,16 @@ Joint Position Example This example sends periodic position commands to the robot. .. important:: - The position trajectory needs to based from the current position of the robot. + The position trajectory needs to start from the initial position of the robot. -to read the the start position of the robot, you can read the command interface values before starting sending any commands. +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:: shell +.. code-block:: cpp if (initialization_flag_) { for (size_t i = 0; i < 7; ++i) { - initial_q_.at(i) = command_interfaces_[i].get_value(); + initial_q_.at(i) = state_interface[i].get_value(); } initialization_flag_ = false; } @@ -225,12 +226,15 @@ to read the the start position of the robot, you can read the command interface Joint Velocity Example ^^^^^^^^^^^^^^^^^^^^^^ -This example sends +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 CartesianPose interface to send periodic pose commands to the robot. +This example uses the CartesianPose interface to send periodic pose commands to the robot. .. code-block:: shell @@ -246,7 +250,7 @@ This example uses CartesianOrientation interface to send periodic orientation co Cartesian Pose Elbow Example ^^^^^^^^^^^^^^^^^^^^^^^^^^^^ -This example sends periodic elbow command while keeping the end effector pose constant. +This example sends periodic elbow commands while keeping the end effector pose constant. .. code-block:: shell @@ -254,7 +258,7 @@ This example sends periodic elbow command while keeping the end effector pose co Cartesian Velocity Example ^^^^^^^^^^^^^^^^^^^^^^^^^^ -This example uses CartesianVelocity interface to send periodic velocity commands to the robot. +This example uses the CartesianVelocity interface to send periodic velocity commands to the robot. .. code-block:: shell @@ -262,7 +266,7 @@ This example uses CartesianVelocity interface to send periodic velocity commands Cartesian Elbow Example ^^^^^^^^^^^^^^^^^^^^^^^ -This example uses CartesianElbow interface to send periodic elbow commands to the robot while keeping the end effector velocity constant. +This example uses the CartesianElbow interface to send periodic elbow commands to the robot while keeping the end effector velocity constant. .. code-block:: shell @@ -374,12 +378,13 @@ 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 manger via the robot description. +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. @@ -395,15 +400,11 @@ In addition 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, and cartesian command classes. +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. @@ -415,86 +416,80 @@ and - Cartesian Pose Interface: -This interface is used to send Cartesian pose commands to the robot by using the loaned command interfaces. +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, user needs to pass a boolean flag to the constructor to indicate whether the interface is for the elbow or not. +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:: shell - - CartesianPoseInterface cartesian_pose_interface(false); +.. 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:: shell - +.. code-block:: cpp + std::array pose; - cartesian_pose_interface.getCommandedPoseConfiguration(pose); + pose = cartesian_pose_interface.getInitialPoseMatrix(); One could also read quaternion and translation values in Eigen format. -.. code-block:: shell - +.. code-block:: cpp + Eigen::Quaterniond quaternion; Eigen::Vector3d translation; - cartesian_pose_interface.getCommandedPoseConfiguration(quaternion, translation); + std::tie(quaternion, translation) = cartesian_pose_interface.getInitialOrientationAndTranslation(); -After setting up the cartesian interface, you need to assign_loaned_command_interface in your custom controller. -This needs to be done in the on_activate() function of the controller. Examples can be found in the -create a link to the line `assign loaned comamand interface example -`_ +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:: shell - - cartesian_pose_interface.assign_loaned_command_interface(command_interfaces_); +.. code-block:: cpp -In the update function of the controller you can send pose command to the robot. + cartesian_pose_interface.assign_loaned_command_interfaces(command_interfaces_); + cartesian_pose_interface.assign_loaned_state_interfaces(state_interfaces_); -.. code-block:: shell +In the update function of the controller you can send pose commands to the robot. + +.. code-block:: cpp std::array pose; - // column major homogenous transformation matrix pose = {1, 0, 0, 0, 0, 1, 0, 0, 0, 0, 1, 0, 0.5, 0, 0.5, 1}; - cartesian_pose_interface.setCommandedPoseConfiguration(pose); + cartesian_pose_interface.setCommanded(pose); Or you can send quaternion, translation values in Eigen format. -.. code-block:: shell +.. code-block:: cpp - Eigen::Quaterniond quaternion; - Eigen::Vector3d translation; - quaternion = Eigen::Quaterniond(1, 0, 0, 0); - translation = Eigen::Vector3d(0.5, 0, 0.5); - cartesian_pose_interface.setCommandedPoseConfiguration(quaternion, translation); + 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:: shell - - CartesianVelocityInterface cartesian_velocity_interface(k_elbow_active); - -This interface allows users to read the current velocity command interface values set by the franka hardware interface. - -.. code-block:: shell +.. code-block:: cpp - std::array velocity; - cartesian_velocity_interface.getCommandedVelocityConfiguration(velocity); + 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:: shell +.. code-block:: cpp cartesian_velocity_interface.assign_loaned_command_interface(command_interfaces_); -In the update function of the controller you can send velocity command to the robot. +In the update function of the controller you can send cartesian velocity command to the robot. -.. code-block:: shell +.. code-block:: cpp - std::array velocity; - velocity = {0, 0, 0, 0, 0, 0.1}; - cartesian_velocity_interface.setCommandedVelocityConfiguration(velocity); + std::array cartesian_velocity; + cartesian_velocity = {0, 0, 0, 0, 0, 0.1}; + cartesian_velocity_interface.setCommand(cartesian_velocity); .. _franka_robot_state_broadcaster: From dbd2607ecbd04a25c22f5bce36d09687cc422406 Mon Sep 17 00:00:00 2001 From: Baris Yazici Date: Tue, 5 Dec 2023 10:18:13 +0100 Subject: [PATCH 4/4] update submodules --- franka_ros2 | 2 +- libfranka | 2 +- source/franka_ros2.rst | 2 +- 3 files changed, 3 insertions(+), 3 deletions(-) diff --git a/franka_ros2 b/franka_ros2 index 1768569..4cea20f 160000 --- a/franka_ros2 +++ b/franka_ros2 @@ -1 +1 @@ -Subproject commit 1768569ca011cdcdf0a56be6e882d0a8800e594d +Subproject commit 4cea20feee12b2403bc6a7d2eaee98801e048f8f diff --git a/libfranka b/libfranka index 457021b..3fe9b08 160000 --- a/libfranka +++ b/libfranka @@ -1 +1 @@ -Subproject commit 457021b7f05493046bc43f159f3e4e703c5e88b3 +Subproject commit 3fe9b0853f4be58258814fb69921e5c59e81a9c5 diff --git a/source/franka_ros2.rst b/source/franka_ros2.rst index 5d2d881..48c2914 100644 --- a/source/franka_ros2.rst +++ b/source/franka_ros2.rst @@ -27,7 +27,7 @@ Prerequisites sudo apt install -y libpoco-dev libeigen3-dev git clone https://github.com/frankaemika/libfranka.git --recursive cd libfranka - git switch 0.13.0 + git switch 0.13.2 mkdir build && cd build cmake -DCMAKE_BUILD_TYPE=Release -DBUILD_TESTS=OFF .. cmake --build . -j$(nproc)