Merge pull request frankaemika#46 in SRR/franka_ros2 from feat/joint-…

…position-interface to humble

* commit '9868cde2d966e8d3dbd86729382ec12d93de1a96':
  bump version 0.1.7
  chore: update joint impedance example controller time
  chore: fix linting errors
  chore: devcontainer packages update
  hotfix: automatic acknowledge for reflex errors
  hotfix: current state during the control assigned correctly
  feat: joint position command interface

.devcontainer/Dockerfile

@@ -38,7 +38,10 @@ RUN apt-get update && \
     ros-humble-ament-cmake \
     ros-humble-ament-cmake-clang-format \
     ros-humble-moveit \
+    ros-humble-ros2-control \
+    ros-humble-ros2-controllers \
     ros-humble-joint-state-broadcaster \
+    ros-humble-ament-cmake-clang-tidy \
     libignition-gazebo6-dev \
     ros-humble-joint-trajectory-controller \
     libpoco-dev \

CHANGELOG.md

@@ -1,5 +1,14 @@
 # Changelog
 
+## 0.1.7 - 2023-11-10
+
+Requires libfranka >= 0.12.1, required ROS 2 Humble
+
+* franka\_hardware: joint position command inteface supported
+* franka\_hardware: controller initializer automatically acknowledges error, if arm is in reflex mode
+* franka\_example\_controllers: joint position example controller provided
+* franka\_example\_controllers: fix second start bug with the example controllers
+
 ## 0.1.6 - 2023-11-03
 
 Requires libfranka >= 0.12.1, required ROS 2 Humble

franka_bringup/config/controllers.yaml

@@ -11,6 +11,9 @@ controller_manager:
     joint_velocity_example_controller:
       type: franka_example_controllers/JointVelocityExampleController
 
+    joint_position_example_controller:
+      type: franka_example_controllers/JointPositionExampleController
+
     cartesian_velocity_example_controller:
       type: franka_example_controllers/CartesianVelocityExampleController
 

franka_bringup/launch/joint_position_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_position_example_controller'],
+            output='screen',
+        ),
+    ])

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.6</version>
+  <version>0.1.7</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>

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.6</version>
+  <version>0.1.7</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>

franka_description/robots/panda_arm.ros2_control.xacro

@@ -20,6 +20,7 @@
           <param name="initial_position">${initial_position}</param>
           <command_interface name="effort"/>
           <command_interface name="velocity"/>
+          <command_interface name="position"/>
           <state_interface name="position"/>
           <state_interface name="velocity"/>
           <state_interface name="effort"/>

franka_example_controllers/CMakeLists.txt

@@ -31,6 +31,7 @@ add_library(
         src/gravity_compensation_example_controller.cpp
         src/joint_impedance_example_controller.cpp
         src/joint_velocity_example_controller.cpp
+        src/joint_position_example_controller.cpp
         src/cartesian_velocity_example_controller.cpp
         src/elbow_example_controller.cpp
         src/model_example_controller.cpp

franka_example_controllers/franka_example_controllers.xml

@@ -17,6 +17,12 @@
             The joint velocity example controller moves joint 4 and 5 in a periodic movement.
         </description>
     </class>
+    <class name="franka_example_controllers/JointPositionExampleController"
+           type="franka_example_controllers::JointPositionExampleController" base_class_type="controller_interface::ControllerInterface">
+        <description>
+            The joint position example controller moves in a periodic movement.
+        </description>
+    </class>
     <class name="franka_example_controllers/CartesianVelocityExampleController"
            type="franka_example_controllers::CartesianVelocityExampleController" base_class_type="controller_interface::ControllerInterface">
         <description>

franka_example_controllers/include/franka_example_controllers/elbow_example_controller.hpp

@@ -47,7 +47,7 @@ class ElbowExampleController : public controller_interface::ControllerInterface
 
   const bool k_elbow_activated_{true};
   std::vector<double> initial_cartesian_velocity_and_elbow;
-  bool first_pass_{true};
+  bool initialization_flag_{true};
   std::array<double, 2> initial_elbow_configuration_{0.0, 0.0};
   double elapsed_time_{0.0};
   const double traj_frequency_{0.001};

franka_example_controllers/include/franka_example_controllers/joint_impedance_example_controller.hpp

@@ -49,7 +49,7 @@ class JointImpedanceExampleController : public controller_interface::ControllerI
   Vector7d dq_filtered_;
   Vector7d k_gains_;
   Vector7d d_gains_;
-  rclcpp::Time start_time_;
+  double elapsed_time_{0.0};
   void updateJointStates();
 };
 

franka_example_controllers/include/franka_example_controllers/joint_position_example_controller.hpp

@@ -0,0 +1,55 @@
+// Copyright (c) 2023 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 <Eigen/Eigen>
+#include <controller_interface/controller_interface.hpp>
+#include <rclcpp/rclcpp.hpp>
+#include "franka_semantic_components/franka_robot_state.hpp"
+
+using CallbackReturn = rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn;
+
+namespace franka_example_controllers {
+
+/**
+ * The joint position example controller moves in a periodic movement.
+ */
+class JointPositionExampleController : 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;
+  std::array<double, 7> initial_q_{0, 0, 0, 0, 0, 0, 0};
+  const double trajectory_period{0.001};
+  double elapsed_time_ = 0.0;
+  std::string arm_id{"panda"};
+
+  bool initialization_flag_{true};
+  rclcpp::Time start_time_;
+};
+
+}  // namespace franka_example_controllers

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.6</version>
+  <version>0.1.7</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>

franka_example_controllers/src/cartesian_velocity_example_controller.cpp

@@ -96,7 +96,7 @@ CallbackReturn CartesianVelocityExampleController::on_configure(
 CallbackReturn CartesianVelocityExampleController::on_activate(
     const rclcpp_lifecycle::State& /*previous_state*/) {
   franka_cartesian_velocity_->assign_loaned_command_interfaces(command_interfaces_);
-
+  elapsed_time_ = rclcpp::Duration(0, 0);
   return CallbackReturn::SUCCESS;
 }
 

franka_example_controllers/src/elbow_example_controller.cpp

@@ -42,7 +42,7 @@ controller_interface::InterfaceConfiguration ElbowExampleController::state_inter
 controller_interface::return_type ElbowExampleController::update(
     const rclcpp::Time& /*time*/,
     const rclcpp::Duration& /*period*/) {
-  if (first_pass_) {
+  if (initialization_flag_) {
     // Get initial elbow configuration values
     if (!franka_cartesian_velocity_->getCommandedElbowConfiguration(initial_elbow_configuration_)) {
       RCLCPP_FATAL(get_node()->get_logger(),
@@ -51,11 +51,11 @@ controller_interface::return_type ElbowExampleController::update(
       return controller_interface::return_type::ERROR;
     }
 
-    first_pass_ = false;
+    initialization_flag_ = false;
   }
   elapsed_time_ = elapsed_time_ + traj_frequency_;
 
-  double angle = M_PI / 10.0 * (1.0 - std::cos(M_PI / 5.0 * elapsed_time_));
+  double angle = M_PI / 15.0 * (1.0 - std::cos(M_PI / 5.0 * elapsed_time_));
 
   std::array<double, 6> cartesian_velocity_command = {{0.0, 0.0, 0.0, 0.0, 0.0, 0.0}};
   std::array<double, 2> elbow_command = {
@@ -103,7 +103,8 @@ CallbackReturn ElbowExampleController::on_configure(
 CallbackReturn ElbowExampleController::on_activate(
     const rclcpp_lifecycle::State& /*previous_state*/) {
   franka_cartesian_velocity_->assign_loaned_command_interfaces(command_interfaces_);
-
+  initialization_flag_ = true;
+  elapsed_time_ = 0.0;
   return CallbackReturn::SUCCESS;
 }
 

franka_example_controllers/src/joint_impedance_example_controller.cpp

@@ -47,11 +47,12 @@ JointImpedanceExampleController::state_interface_configuration() const {
 
 controller_interface::return_type JointImpedanceExampleController::update(
     const rclcpp::Time& /*time*/,
-    const rclcpp::Duration& /*period*/) {
+    const rclcpp::Duration& period) {
   updateJointStates();
   Vector7d q_goal = initial_q_;
-  auto time = this->get_node()->now() - start_time_;
-  double delta_angle = M_PI / 8.0 * (1 - std::cos(M_PI / 2.5 * time.seconds()));
+  elapsed_time_ = elapsed_time_ + period.seconds();
+
+  double delta_angle = M_PI / 8.0 * (1 - std::cos(M_PI / 2.5 * elapsed_time_));
   q_goal(3) += delta_angle;
   q_goal(4) += delta_angle;
 
@@ -111,8 +112,10 @@ CallbackReturn JointImpedanceExampleController::on_configure(
 CallbackReturn JointImpedanceExampleController::on_activate(
     const rclcpp_lifecycle::State& /*previous_state*/) {
   updateJointStates();
+  dq_filtered_.setZero();
   initial_q_ = q_;
-  start_time_ = this->get_node()->now();
+  elapsed_time_ = 0.0;
+
   return CallbackReturn::SUCCESS;
 }