Merge pull request #44 in SRR/franka_ros2 from feat/joint-velocity-in…

…terface to humble * commit '221229e1bb19ae881032456ce979c150c13cefd8': bump up version update velocity controller example collision thresholds test: write velocity command interface tests feat/joint-velocity-interface
frankaemika · Oct 16, 2023 · 9c71f46 · 9c71f46
2 parents 6f89539 + 221229e
commit 9c71f46
Show file tree

Hide file tree

Showing 22 changed files with 493 additions and 88 deletions.
diff --git a/CHANGELOG.md b/CHANGELOG.md
@@ -1,12 +1,19 @@
 # Changelog
 
+## 0.1.5 - 2023-10-13
+
+Requires libfranka >= 0.12.1, required ROS 2 Humble
+
+* franka\_hardware: support joint velocity command interface
+* franka\_example\_controllers: implement joint velocity example controller
+* franka\_description: add velocity command interface to the control tag
+
 ## 0.1.4 - 2023-09-26
 
 Requires libfranka >= 0.12.1, required ROS 2 Humble
 
 * franka\_hardware: adapt to libfranka active control v0.12.1
 
-
 ## 0.1.3 - 2023-08-24
 
 Requires libfranka >= 0.11.0, required ROS 2 Humble

diff --git a/franka_bringup/config/controllers.yaml b/franka_bringup/config/controllers.yaml
@@ -8,6 +8,9 @@ controller_manager:
     joint_impedance_example_controller:
       type: franka_example_controllers/JointImpedanceExampleController
 
+    joint_velocity_example_controller:
+      type: franka_example_controllers/JointVelocityExampleController
+
     move_to_start_example_controller:
       type: franka_example_controllers/MoveToStartExampleController
 

diff --git a/franka_bringup/launch/joint_velocity_example_controller.launch.py b/franka_bringup/launch/joint_velocity_example_controller.launch.py
@@ -0,0 +1,77 @@
+#  Copyright (c) 2021 Franka Emika GmbH
+#
+#  Licensed under the Apache License, Version 2.0 (the "License");
+#  you may not use this file except in compliance with the License.
+#  You may obtain a copy of the License at
+#
+#      http://www.apache.org/licenses/LICENSE-2.0
+#
+#  Unless required by applicable law or agreed to in writing, software
+#  distributed under the License is distributed on an "AS IS" BASIS,
+#  WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+#  See the License for the specific language governing permissions and
+#  limitations under the License.
+
+
+from launch import LaunchDescription
+from launch.actions import DeclareLaunchArgument, IncludeLaunchDescription
+from launch.launch_description_sources import PythonLaunchDescriptionSource
+from launch.substitutions import LaunchConfiguration, PathJoinSubstitution
+from launch_ros.actions import Node
+from launch_ros.substitutions import FindPackageShare
+
+
+def generate_launch_description():
+    robot_ip_parameter_name = 'robot_ip'
+    load_gripper_parameter_name = 'load_gripper'
+    use_fake_hardware_parameter_name = 'use_fake_hardware'
+    fake_sensor_commands_parameter_name = 'fake_sensor_commands'
+    use_rviz_parameter_name = 'use_rviz'
+
+    robot_ip = LaunchConfiguration(robot_ip_parameter_name)
+    load_gripper = LaunchConfiguration(load_gripper_parameter_name)
+    use_fake_hardware = LaunchConfiguration(use_fake_hardware_parameter_name)
+    fake_sensor_commands = LaunchConfiguration(fake_sensor_commands_parameter_name)
+    use_rviz = LaunchConfiguration(use_rviz_parameter_name)
+
+    return LaunchDescription([
+        DeclareLaunchArgument(
+            robot_ip_parameter_name,
+            description='Hostname or IP address of the robot.'),
+        DeclareLaunchArgument(
+            use_rviz_parameter_name,
+            default_value='false',
+            description='Visualize the robot in Rviz'),
+        DeclareLaunchArgument(
+            use_fake_hardware_parameter_name,
+            default_value='false',
+            description='Use fake hardware'),
+        DeclareLaunchArgument(
+            fake_sensor_commands_parameter_name,
+            default_value='false',
+            description="Fake sensor commands. Only valid when '{}' is true".format(
+                use_fake_hardware_parameter_name)),
+        DeclareLaunchArgument(
+            load_gripper_parameter_name,
+            default_value='true',
+            description='Use Franka Gripper as an end-effector, otherwise, the robot is loaded '
+                        'without an end-effector.'),
+
+        IncludeLaunchDescription(
+            PythonLaunchDescriptionSource([PathJoinSubstitution(
+                [FindPackageShare('franka_bringup'), 'launch', 'franka.launch.py'])]),
+            launch_arguments={robot_ip_parameter_name: robot_ip,
+                              load_gripper_parameter_name: load_gripper,
+                              use_fake_hardware_parameter_name: use_fake_hardware,
+                              fake_sensor_commands_parameter_name: fake_sensor_commands,
+                              use_rviz_parameter_name: use_rviz
+                              }.items(),
+        ),
+
+        Node(
+            package='controller_manager',
+            executable='spawner',
+            arguments=['joint_velocity_example_controller'],
+            output='screen',
+        ),
+    ])
diff --git a/franka_bringup/package.xml b/franka_bringup/package.xml
@@ -2,7 +2,7 @@
 <?xml-model href="http://download.ros.org/schema/package_format3.xsd" schematypens="http://www.w3.org/2001/XMLSchema"?>
 <package format="3">
   <name>franka_bringup</name>
-  <version>0.1.4</version>
+  <version>0.1.5</version>
   <description>Package with launch files and run-time configurations for using Franka Emika research robots with ros2_control</description>
   <maintainer email="support@franka.de">Franka Emika GmbH</maintainer>
   <license>Apache 2.0</license>

diff --git a/franka_description/package.xml b/franka_description/package.xml
@@ -2,7 +2,7 @@
 <?xml-model href="http://download.ros.org/schema/package_format3.xsd" schematypens="http://www.w3.org/2001/XMLSchema"?>
 <package format="3">
   <name>franka_description</name>
-  <version>0.1.4</version>
+  <version>0.1.5</version>
   <description>franka_description contains URDF files and meshes of Franka Emika robots</description>
   <maintainer email="support@franka.de">Franka Emika GmbH</maintainer>
   <license>Apache 2.0</license>

diff --git a/franka_description/robots/panda_arm.ros2_control.xacro b/franka_description/robots/panda_arm.ros2_control.xacro
@@ -19,6 +19,7 @@
         <joint name="${joint_name}">
           <param name="initial_position">${initial_position}</param>
           <command_interface name="effort"/>
+          <command_interface name="velocity"/>
           <state_interface name="position"/>
           <state_interface name="velocity"/>
           <state_interface name="effort"/>

diff --git a/franka_example_controllers/CMakeLists.txt b/franka_example_controllers/CMakeLists.txt
@@ -22,14 +22,15 @@ find_package(rclcpp_lifecycle REQUIRED)
 find_package(hardware_interface REQUIRED)
 find_package(franka_msgs REQUIRED)
 find_package(Eigen3 REQUIRED)
-find_package(franka_semantic_components)
+find_package(franka_semantic_components REQUIRED)
 find_package(generate_parameter_library)
 
 add_library(
         ${PROJECT_NAME}
         SHARED
         src/gravity_compensation_example_controller.cpp
         src/joint_impedance_example_controller.cpp
+        src/joint_velocity_example_controller.cpp
         src/model_example_controller.cpp
         src/move_to_start_example_controller.cpp
         src/motion_generator.cpp)

diff --git a/franka_example_controllers/franka_example_controllers.xml b/franka_example_controllers/franka_example_controllers.xml
@@ -11,6 +11,12 @@
             The joint impedance example controller moves joint 4 and 5 in a very compliant periodic movement.
         </description>
     </class>
+    <class name="franka_example_controllers/JointVelocityExampleController"
+           type="franka_example_controllers::JointVelocityExampleController" base_class_type="controller_interface::ControllerInterface">
+        <description>
+            The joint velocity example controller moves joint 4 and 5 in a periodic movement.
+        </description>
+    </class>
     <class name="franka_example_controllers/MoveToStartExampleController"
            type="franka_example_controllers::MoveToStartExampleController" base_class_type="controller_interface::ControllerInterface">
         <description>

diff --git a/...mple_controllers/include/franka_example_controllers/joint_velocity_example_controller.hpp b/...mple_controllers/include/franka_example_controllers/joint_velocity_example_controller.hpp
@@ -0,0 +1,47 @@
+// Copyright (c) 2021 Franka Emika GmbH
+//
+// Licensed under the Apache License, Version 2.0 (the "License");
+// you may not use this file except in compliance with the License.
+// You may obtain a copy of the License at
+//
+//     http://www.apache.org/licenses/LICENSE-2.0
+//
+// Unless required by applicable law or agreed to in writing, software
+// distributed under the License is distributed on an "AS IS" BASIS,
+// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+// See the License for the specific language governing permissions and
+// limitations under the License.
+
+#pragma once
+
+#include <string>
+
+#include <controller_interface/controller_interface.hpp>
+#include <rclcpp/rclcpp.hpp>
+
+using CallbackReturn = rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn;
+
+namespace franka_example_controllers {
+
+/**
+ * The joint velocity example controller
+ */
+class JointVelocityExampleController : public controller_interface::ControllerInterface {
+ public:
+  [[nodiscard]] controller_interface::InterfaceConfiguration command_interface_configuration()
+      const override;
+  [[nodiscard]] controller_interface::InterfaceConfiguration state_interface_configuration()
+      const override;
+  controller_interface::return_type update(const rclcpp::Time& time,
+                                           const rclcpp::Duration& period) override;
+  CallbackReturn on_init() override;
+  CallbackReturn on_configure(const rclcpp_lifecycle::State& previous_state) override;
+  CallbackReturn on_activate(const rclcpp_lifecycle::State& previous_state) override;
+
+ private:
+  std::string arm_id_;
+  const int num_joints = 7;
+  rclcpp::Duration elapsed_time_ = rclcpp::Duration(0, 0);
+};
+
+}  // namespace franka_example_controllers
diff --git a/franka_example_controllers/package.xml b/franka_example_controllers/package.xml
@@ -2,7 +2,7 @@
 <?xml-model href="http://download.ros.org/schema/package_format3.xsd" schematypens="http://www.w3.org/2001/XMLSchema"?>
 <package format="3">
   <name>franka_example_controllers</name>
-  <version>0.1.4</version>
+  <version>0.1.5</version>
   <description>franka_example_controllers provides example code for controlling Franka Emika research robots with ros2_control</description>
   <maintainer email="support@franka.de">Franka Emika GmbH</maintainer>
   <license>Apache 2.0</license>

diff --git a/franka_example_controllers/src/joint_velocity_example_controller.cpp b/franka_example_controllers/src/joint_velocity_example_controller.cpp
@@ -0,0 +1,120 @@
+// Copyright (c) 2021 Franka Emika GmbH
+//
+// Licensed under the Apache License, Version 2.0 (the "License");
+// you may not use this file except in compliance with the License.
+// You may obtain a copy of the License at
+//
+//     http://www.apache.org/licenses/LICENSE-2.0
+//
+// Unless required by applicable law or agreed to in writing, software
+// distributed under the License is distributed on an "AS IS" BASIS,
+// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+// See the License for the specific language governing permissions and
+// limitations under the License.
+
+#include <franka_example_controllers/joint_velocity_example_controller.hpp>
+#include <franka_msgs/srv/set_full_collision_behavior.hpp>
+
+#include <cassert>
+#include <cmath>
+#include <exception>
+#include <string>
+
+#include <Eigen/Eigen>
+
+using namespace std::chrono_literals;
+
+namespace franka_example_controllers {
+
+controller_interface::InterfaceConfiguration
+JointVelocityExampleController::command_interface_configuration() const {
+  controller_interface::InterfaceConfiguration config;
+  config.type = controller_interface::interface_configuration_type::INDIVIDUAL;
+
+  for (int i = 1; i <= num_joints; ++i) {
+    config.names.push_back(arm_id_ + "_joint" + std::to_string(i) + "/velocity");
+  }
+  return config;
+}
+
+controller_interface::InterfaceConfiguration
+JointVelocityExampleController::state_interface_configuration() const {
+  controller_interface::InterfaceConfiguration config;
+  config.type = controller_interface::interface_configuration_type::INDIVIDUAL;
+  for (int i = 1; i <= num_joints; ++i) {
+    config.names.push_back(arm_id_ + "_joint" + std::to_string(i) + "/position");
+    config.names.push_back(arm_id_ + "_joint" + std::to_string(i) + "/velocity");
+  }
+  return config;
+}
+
+controller_interface::return_type JointVelocityExampleController::update(
+    const rclcpp::Time& /*time*/,
+    const rclcpp::Duration& period) {
+  elapsed_time_ = elapsed_time_ + period;
+  rclcpp::Duration time_max(8.0, 0.0);
+  double omega_max = 0.1;
+  double cycle = std::floor(std::pow(
+      -1.0, (elapsed_time_.seconds() - std::fmod(elapsed_time_.seconds(), time_max.seconds())) /
+                time_max.seconds()));
+  double omega = cycle * omega_max / 2.0 *
+                 (1.0 - std::cos(2.0 * M_PI / time_max.seconds() * elapsed_time_.seconds()));
+
+  for (int i = 0; i < num_joints; i++) {
+    if (i == 3 || i == 4) {
+      command_interfaces_[i].set_value(omega);
+    } else {
+      command_interfaces_[i].set_value(0.0);
+    }
+  }
+  return controller_interface::return_type::OK;
+}
+
+CallbackReturn JointVelocityExampleController::on_init() {
+  try {
+    auto_declare<std::string>("arm_id", "panda");
+
+  } catch (const std::exception& e) {
+    fprintf(stderr, "Exception thrown during init stage with message: %s \n", e.what());
+    return CallbackReturn::ERROR;
+  }
+  return CallbackReturn::SUCCESS;
+}
+
+CallbackReturn JointVelocityExampleController::on_configure(
+    const rclcpp_lifecycle::State& /*previous_state*/) {
+  arm_id_ = get_node()->get_parameter("arm_id").as_string();
+
+  auto client = get_node()->create_client<franka_msgs::srv::SetFullCollisionBehavior>(
+      "service_server/set_full_collision_behavior");
+  auto request = std::make_shared<franka_msgs::srv::SetFullCollisionBehavior::Request>();
+
+  request->lower_torque_thresholds_acceleration = {20.0, 20.0, 18.0, 18.0, 16.0, 14.0, 12.0};
+  request->upper_torque_thresholds_acceleration = {20.0, 20.0, 18.0, 18.0, 16.0, 14.0, 12.0};
+  request->lower_torque_thresholds_nominal = {20.0, 20.0, 18.0, 18.0, 16.0, 14.0, 12.0};
+  request->upper_torque_thresholds_nominal = {20.0, 20.0, 18.0, 18.0, 16.0, 14.0, 12.0};
+  request->lower_force_thresholds_acceleration = {20.0, 20.0, 20.0, 25.0, 25.0, 25.0};
+  request->upper_force_thresholds_acceleration = {20.0, 20.0, 20.0, 25.0, 25.0, 25.0};
+  request->lower_force_thresholds_nominal = {20.0, 20.0, 20.0, 25.0, 25.0, 25.0};
+  request->upper_force_thresholds_nominal = {20.0, 20.0, 20.0, 25.0, 25.0, 25.0};
+
+  if (!client->wait_for_service(20s)) {
+    RCLCPP_FATAL(get_node()->get_logger(), "service server can't be found.");
+    return CallbackReturn::FAILURE;
+  }
+
+  client->async_send_request(request);
+
+  return CallbackReturn::SUCCESS;
+}
+
+CallbackReturn JointVelocityExampleController::on_activate(
+    const rclcpp_lifecycle::State& /*previous_state*/) {
+  return CallbackReturn::SUCCESS;
+}
+
+}  // namespace franka_example_controllers
+#include "pluginlib/class_list_macros.hpp"
+// NOLINTNEXTLINE
+PLUGINLIB_EXPORT_CLASS(franka_example_controllers::JointVelocityExampleController,
+                       controller_interface::ControllerInterface)
diff --git a/franka_gripper/package.xml b/franka_gripper/package.xml
@@ -2,7 +2,7 @@
 <?xml-model href="http://download.ros.org/schema/package_format3.xsd" schematypens="http://www.w3.org/2001/XMLSchema"?>
 <package format="3">
   <name>franka_gripper</name>
-  <version>0.1.4</version>
+  <version>0.1.5</version>
   <description>This package implements the franka gripper of type Franka Hand for the use in ROS2</description>
   <maintainer email="support@franka.de">Franka Emika GmbH</maintainer>
   <license>Apache 2.0</license>

diff --git a/franka_hardware/include/franka_hardware/franka_hardware_interface.hpp b/franka_hardware/include/franka_hardware/franka_hardware_interface.hpp
@@ -66,13 +66,17 @@ class FrankaHardwareInterface : public hardware_interface::SystemInterface {
   std::array<double, kNumberOfJoints> hw_positions_{0, 0, 0, 0, 0, 0, 0};
   std::array<double, kNumberOfJoints> hw_velocities_{0, 0, 0, 0, 0, 0, 0};
   std::array<double, kNumberOfJoints> hw_efforts_{0, 0, 0, 0, 0, 0, 0};
+
   franka::RobotState hw_franka_robot_state_;
   franka::RobotState* hw_franka_robot_state_addr_ = &hw_franka_robot_state_;
   Model* hw_franka_model_ptr_ = nullptr;
 
   bool effort_interface_claimed_ = false;
   bool effort_interface_running_ = false;
 
+  bool velocity_joint_interface_claimed_ = false;
+  bool velocity_joint_interface_running_ = false;
+
   static rclcpp::Logger getLogger();
 
   const std::string k_robot_name{"panda"};