Merge pull request #42 from kroshu/feature/rox_driver_joint_impedance

Feature/rox driver joint impedance

.github/workflows/industrial_ci.yml

@@ -44,6 +44,7 @@ jobs:
           ROS_DISTRO: humble
     env:
       CCACHE_DIR: /github/home/.ccache # Directory for ccache (and how we enable ccache in industrial_ci)
+      BEFORE_INIT: 'apt update && apt-get install -y libgrpc++-dev && apt-get install -y libnanopb-dev'
       EVENT_NAME: ${{ github.event_name }}
       BRANCH: ${{ github.event.ref }}
       PR_BRANCH: ${{ github.event.pull_request.head.ref }}

kuka_controllers/CMakeLists.txt

@@ -0,0 +1,54 @@
+cmake_minimum_required(VERSION 3.5)
+project(kuka_controllers)
+
+if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang")
+  add_compile_options(-Wall -Wextra)
+endif()
+
+find_package(ament_cmake REQUIRED)
+find_package(controller_interface REQUIRED)
+find_package(pluginlib REQUIRED)
+find_package(rclcpp REQUIRED)
+find_package(rclcpp_lifecycle REQUIRED)
+find_package(kuka_sunrise_interfaces REQUIRED)
+
+include_directories(include)
+
+add_library(${PROJECT_NAME} SHARED
+  src/joint_impedance_controller.cpp)
+
+target_include_directories(${PROJECT_NAME} PRIVATE
+  include
+)
+
+ament_target_dependencies(${PROJECT_NAME} rclcpp controller_interface std_msgs kuka_sunrise_interfaces
+)
+
+pluginlib_export_plugin_description_file(controller_interface controller_plugins.xml)
+
+install(TARGETS ${PROJECT_NAME}
+  ARCHIVE DESTINATION lib
+  LIBRARY DESTINATION lib
+  RUNTIME DESTINATION bin
+)
+
+install(DIRECTORY include/
+  DESTINATION include
+)
+
+install(FILES controller_plugins.xml
+  DESTINATION share/${PROJECT_NAME}
+)
+
+ament_export_include_directories(include)
+ament_export_libraries(${PROJECT_NAME})
+
+ament_export_include_directories(
+  include
+)
+
+ament_export_libraries(
+  ${PROJECT_NAME}
+)
+
+ament_package()

kuka_controllers/controller_plugins.xml

@@ -0,0 +1,12 @@
+<library path="kuka_controllers">
+  <class name="kuka_controllers/JointImpedanceController" type="kuka_controllers::JointImpedanceController" base_class_type="controller_interface::ControllerInterface">
+    <description>
+      This controller sets the joint impedance parameters in real time
+    </description>
+  </class>
+  <class name="kuka_controllers/RobotStateBroadcaster" type="kuka_controllers::RobotStateBroadcaster" base_class_type="controller_interface::ControllerInterface">
+    <description>
+      This broadcaster publishes the state changes of the FRI
+    </description>
+  </class>
+</library>

kuka_controllers/include/kuka_controllers/joint_impedance_controller.hpp

@@ -0,0 +1,61 @@
+// Copyright 2022 Áron Svastits
+//
+// 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.
+
+
+#ifndef KUKA_CONTROLLERS__JOINT_IMPEDANCE_CONTROLLER_HPP_
+#define KUKA_CONTROLLERS__JOINT_IMPEDANCE_CONTROLLER_HPP_
+
+#include <memory>
+#include <string>
+#include <vector>
+
+#include "controller_interface/controller_interface.hpp"
+#include "rclcpp_lifecycle/node_interfaces/lifecycle_node_interface.hpp"
+#include "rclcpp/time.hpp"
+#include "rclcpp/duration.hpp"
+#include "std_msgs/msg/float64_multi_array.hpp"
+
+#include "pluginlib/class_list_macros.hpp"
+
+namespace kuka_controllers
+{
+class JointImpedanceController : public controller_interface::ControllerInterface
+{
+public:
+  controller_interface::InterfaceConfiguration command_interface_configuration() const override;
+
+  controller_interface::InterfaceConfiguration state_interface_configuration() const override;
+
+  controller_interface::return_type update(
+    const rclcpp::Time & time,
+    const rclcpp::Duration & period) override;
+
+  controller_interface::CallbackReturn on_configure(const rclcpp_lifecycle::State & previous_state)
+  override;
+
+  controller_interface::CallbackReturn on_activate(const rclcpp_lifecycle::State & previous_state)
+  override;
+
+  controller_interface::CallbackReturn on_deactivate(const rclcpp_lifecycle::State & previous_state)
+  override;
+
+  controller_interface::CallbackReturn on_init() override;
+
+private:
+  rclcpp::Subscription<std_msgs::msg::Float64MultiArray>::SharedPtr joint_impedance_subscriber_;
+  std::vector<double> stiffness_;
+  std::vector<double> damping_;
+};
+}  // namespace kuka_controllers
+#endif  // KUKA_CONTROLLERS__JOINT_IMPEDANCE_CONTROLLER_HPP_

kuka_controllers/package.xml

@@ -0,0 +1,23 @@
+<?xml version="1.0"?>
+<?xml-model href="http://download.ros.org/schema/package_format3.xsd" schematypens="http://www.w3.org/2001/XMLSchema"?>
+<package format="3">
+  <name>kuka_controllers</name>
+  <version>0.0.0</version>
+  <description>Controller for managing the timing of the FRI hardware interface</description>
+
+  <maintainer email="svastits.aron@gmail.com">Aron Svastits</maintainer>
+
+  <license>Apache 2.0</license>
+
+  <buildtool_depend>ament_cmake</buildtool_depend>
+
+  <depend>controller_interface</depend>
+  <depend>pluginlib</depend>
+  <depend>rclcpp_lifecycle</depend>
+  <depend>std_msgs</depend>
+  <depend>kuka_sunrise_interfaces</depend>
+
+  <export>
+    <build_type>ament_cmake</build_type>
+  </export>
+</package>

kuka_controllers/src/joint_impedance_controller.cpp

@@ -0,0 +1,99 @@
+// Copyright 2022 Áron Svastits
+//
+// 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 "kuka_controllers/joint_impedance_controller.hpp"
+
+namespace kuka_controllers
+{
+controller_interface::CallbackReturn JointImpedanceController::on_init()
+{
+  stiffness_.resize(6, 10);
+  damping_.resize(6, 0.7);
+  return controller_interface::CallbackReturn::SUCCESS;
+}
+
+controller_interface::InterfaceConfiguration JointImpedanceController::
+command_interface_configuration()
+const
+{
+  controller_interface::InterfaceConfiguration config;
+  config.type = controller_interface::interface_configuration_type::INDIVIDUAL;
+  // config.names.emplace_back("joint_impedance");
+  config.names.emplace_back("joint_a1/stiffness");
+  config.names.emplace_back("joint_a2/stiffness");
+  config.names.emplace_back("joint_a3/stiffness");
+  config.names.emplace_back("joint_a4/stiffness");
+  config.names.emplace_back("joint_a5/stiffness");
+  config.names.emplace_back("joint_a6/stiffness");
+  config.names.emplace_back("joint_a1/damping");
+  config.names.emplace_back("joint_a2/damping");
+  config.names.emplace_back("joint_a3/damping");
+  config.names.emplace_back("joint_a4/damping");
+  config.names.emplace_back("joint_a5/damping");
+  config.names.emplace_back("joint_a6/damping");
+  return config;
+}
+
+controller_interface::InterfaceConfiguration JointImpedanceController::state_interface_configuration()
+const
+{
+  return controller_interface::InterfaceConfiguration{controller_interface::
+    interface_configuration_type::NONE};
+}
+
+controller_interface::CallbackReturn
+JointImpedanceController::on_configure(const rclcpp_lifecycle::State &)
+{
+  joint_impedance_subscriber_ = get_node()->create_subscription<std_msgs::msg::Float64MultiArray>(
+    "joint_impedance", rclcpp::SystemDefaultsQoS(),
+    [this](const std_msgs::msg::Float64MultiArray::SharedPtr msg) {
+      for (auto i = 0; i < 6; ++i) {
+        stiffness_[i] = msg->data[i];
+        damping_[i] = msg->data[i + 6];
+      }
+    });
+  return controller_interface::CallbackReturn::SUCCESS;
+}
+
+controller_interface::CallbackReturn
+JointImpedanceController::on_activate(const rclcpp_lifecycle::State &)
+{
+  return controller_interface::CallbackReturn::SUCCESS;
+}
+
+controller_interface::CallbackReturn
+JointImpedanceController::on_deactivate(const rclcpp_lifecycle::State &)
+{
+  return controller_interface::CallbackReturn::SUCCESS;
+}
+
+controller_interface::return_type JointImpedanceController::update(
+  const rclcpp::Time &,
+  const rclcpp::Duration &)
+{
+
+  for (auto index = 0; index < 6; ++index) {
+    command_interfaces_[index].set_value(stiffness_[index]);
+  }
+  for (auto index = 6; index < 12; ++index) {
+    command_interfaces_[index].set_value(damping_[index-6]);
+  }
+  return controller_interface::return_type::OK;
+}
+
+}  // namespace kuka_controllers
+
+PLUGINLIB_EXPORT_CLASS(
+  kuka_controllers::JointImpedanceController,
+  controller_interface::ControllerInterface)

kuka_driver_examples/eci_demo/CMakeLists.txt

@@ -27,10 +27,22 @@ ament_target_dependencies(moveit_circular
   moveit_visual_tools
 )
 
+add_executable(moveit_basic_plan src/MoveitBasicPlan.cpp)
+ament_target_dependencies(moveit_basic_plan
+  moveit_ros_planning_interface
+  rclcpp
+  rviz_visual_tools
+  moveit_visual_tools
+)
+
 install(TARGETS moveit_circular
   EXPORT export_${PROJECT_NAME}
   DESTINATION lib/${PROJECT_NAME}
 )
+install(TARGETS moveit_basic_plan
+  EXPORT export_${PROJECT_NAME}
+  DESTINATION lib/${PROJECT_NAME}
+)
 install(DIRECTORY launch
   DESTINATION share/${PROJECT_NAME}
 )