diff --git a/.clang-tidy b/.clang-tidy index 95e909d0..902c7349 100644 --- a/.clang-tidy +++ b/.clang-tidy @@ -1,5 +1,4 @@ -Checks: 'cppcoreguidelines-*, - -cppcoreguidelines-pro-type-vararg, +Checks: '-cppcoreguidelines-pro-type-vararg, -cppcoreguidelines-pro-bounds-array-to-pointer-decay, clang-diagnostic-*, clang-analyzer-*, @@ -15,9 +14,7 @@ Checks: 'cppcoreguidelines-*, -clang-diagnostic-reinterpret-base-class, -clang-diagnostic-return-type, -clang-diagnostic-switch, - -bugprone-lambda-function-name, - -cppcoreguidelines-avoid-magic-numbers, - -readability-magic-numbers' + -bugprone-lambda-function-name' HeaderFilterRegex: '^franka_.*' CheckOptions: # Classes, structs, ... diff --git a/.devcontainer/Dockerfile b/.devcontainer/Dockerfile index 6c339d31..42ac24e6 100644 --- a/.devcontainer/Dockerfile +++ b/.devcontainer/Dockerfile @@ -52,7 +52,7 @@ RUN apt-get update && \ RUN mkdir ~/source_code RUN cd ~/source_code && git clone https://github.com/frankaemika/libfranka.git \ && cd libfranka \ - && git switch fr3-develop \ + && git switch 0.13.2 \ && git submodule init \ && git submodule update \ && mkdir build && cd build \ diff --git a/CHANGELOG.md b/CHANGELOG.md index bba65e53..42558fab 100644 --- a/CHANGELOG.md +++ b/CHANGELOG.md @@ -1,5 +1,12 @@ # Changelog +## 0.1.11 - 2023-12-20 + +Requires libfranka >= 0.13.2, requires ROS 2 Humble + +* franka\_example\_controllers: Add a joint impedance example using OrocosKDL(LMA-ik) through MoveIt service. +* franka\_hardware: Register initial joint positions and cartesian pose state interface without having running command interfaces. + ## 0.1.10 - 2023-12-04 Requires libfranka >= 0.13.0, required ROS 2 Humble diff --git a/Dockerfile b/Dockerfile index eaa69cc7..dfcd803d 100644 --- a/Dockerfile +++ b/Dockerfile @@ -40,6 +40,7 @@ RUN apt-get update -y && apt-get install -y --allow-unauthenticated \ ros-humble-controller-interface \ ros-humble-ros2-control-test-assets \ ros-humble-controller-manager \ + ros-humble-moveit \ && rm -rf /var/lib/apt/lists/* RUN python3 -m pip install -U \ @@ -59,7 +60,7 @@ RUN python3 -m pip install -U \ RUN mkdir ~/source_code RUN cd ~/source_code && git clone https://github.com/frankaemika/libfranka.git \ && cd libfranka \ - && git checkout 0.13.0 \ + && git checkout 0.13.2 \ && git submodule init \ && git submodule update \ && mkdir build && cd build \ diff --git a/NOTICE b/NOTICE index c179c759..664ea276 100644 --- a/NOTICE +++ b/NOTICE @@ -1,6 +1,6 @@ franka_ros2 -Copyright 2021 Franka Emika GmbH +Copyright 2023 Franka Robotics GmbH Licensed under the Apache License, Version 2.0 (the "License"); you may not use this file except in compliance with the License. diff --git a/franka_bringup/config/controllers.yaml b/franka_bringup/config/controllers.yaml index bbfc1148..4ca3d451 100644 --- a/franka_bringup/config/controllers.yaml +++ b/franka_bringup/config/controllers.yaml @@ -23,6 +23,9 @@ controller_manager: cartesian_pose_elbow_example_controller: type: franka_example_controllers/CartesianElbowExampleController + joint_impedance_with_ik_example_controller: + type: franka_example_controllers/JointImpedanceWithIKExampleController + cartesian_orientation_example_controller: type: franka_example_controllers/CartesianOrientationExampleController @@ -44,6 +47,26 @@ controller_manager: franka_robot_state_broadcaster: type: franka_robot_state_broadcaster/FrankaRobotStateBroadcaster +joint_impedance_with_ik_example_controller: + ros__parameters: + arm_id: panda + k_gains: + - 600.0 + - 600.0 + - 600.0 + - 600.0 + - 250.0 + - 150.0 + - 50.0 + d_gains: + - 30.0 + - 30.0 + - 30.0 + - 30.0 + - 10.0 + - 10.0 + - 5. + franka_robot_state_broadcaster: ros__parameters: arm_id: panda diff --git a/franka_bringup/launch/joint_impedance_with_ik_example_controller.launch.py b/franka_bringup/launch/joint_impedance_with_ik_example_controller.launch.py new file mode 100644 index 00000000..869e3609 --- /dev/null +++ b/franka_bringup/launch/joint_impedance_with_ik_example_controller.launch.py @@ -0,0 +1,96 @@ +# Copyright (c) 2023 Franka Robotics 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(), + ), + IncludeLaunchDescription( + PythonLaunchDescriptionSource( + [ + PathJoinSubstitution( + [ + FindPackageShare('franka_moveit_config'), + 'launch', + 'move_group.launch.py', + ] + ) + ] + ), + launch_arguments={ + robot_ip: robot_ip, + load_gripper_parameter_name: load_gripper, + use_fake_hardware_parameter_name: 'true', + fake_sensor_commands_parameter_name: fake_sensor_commands, + use_rviz_parameter_name: use_rviz, + }.items(), + ), + Node( + package='controller_manager', + executable='spawner', + arguments=['joint_impedance_with_ik_example_controller'], + output='screen', + ), + ]) diff --git a/franka_bringup/package.xml b/franka_bringup/package.xml index cbb21670..6538bde0 100644 --- a/franka_bringup/package.xml +++ b/franka_bringup/package.xml @@ -2,7 +2,7 @@ franka_bringup - 0.1.9 + 0.1.11 Package with launch files and run-time configurations for using Franka Emika research robots with ros2_control Franka Emika GmbH Apache 2.0 diff --git a/franka_description/package.xml b/franka_description/package.xml index 7c55d9f5..32de8da1 100644 --- a/franka_description/package.xml +++ b/franka_description/package.xml @@ -2,7 +2,7 @@ franka_description - 0.1.9 + 0.1.11 franka_description contains URDF files and meshes of Franka Emika robots Franka Emika GmbH Apache 2.0 diff --git a/franka_example_controllers/CMakeLists.txt b/franka_example_controllers/CMakeLists.txt index a40d8a7e..e1d9e615 100644 --- a/franka_example_controllers/CMakeLists.txt +++ b/franka_example_controllers/CMakeLists.txt @@ -24,6 +24,8 @@ find_package(franka_msgs REQUIRED) find_package(Eigen3 REQUIRED) find_package(franka_semantic_components REQUIRED) find_package(generate_parameter_library) +find_package(moveit_core REQUIRED) +find_package(moveit_msgs REQUIRED) add_library( ${PROJECT_NAME} @@ -34,6 +36,7 @@ add_library( src/joint_position_example_controller.cpp src/cartesian_velocity_example_controller.cpp src/cartesian_pose_example_controller.cpp + src/joint_impedance_with_ik_example_controller.cpp src/cartesian_elbow_example_controller.cpp src/cartesian_orientation_example_controller.cpp src/elbow_example_controller.cpp @@ -54,6 +57,8 @@ ament_target_dependencies( rclcpp rclcpp_lifecycle franka_semantic_components + moveit_core + moveit_msgs ) generate_parameter_library(franka_example_controllers_parameters src/model_example_controller_parameters.yaml) @@ -141,5 +146,6 @@ ament_export_dependencies( rclcpp rclcpp_lifecycle hardware_interface + moveit_core ) ament_package() diff --git a/franka_example_controllers/franka_example_controllers.xml b/franka_example_controllers/franka_example_controllers.xml index 942a7cfe..c204c87b 100644 --- a/franka_example_controllers/franka_example_controllers.xml +++ b/franka_example_controllers/franka_example_controllers.xml @@ -34,6 +34,12 @@ The cartesian pose example controller commands to translation on x and z components. + + + + The joint impedance using inverse kinematics from moveit-OrocosKDL(LMA-ik) and commands torques based on joint impedance control law. + diff --git a/franka_example_controllers/include/franka_example_controllers/default_robot_behavior_utils.hpp b/franka_example_controllers/include/franka_example_controllers/default_robot_behavior_utils.hpp index 20c5c54d..ac645a69 100644 --- a/franka_example_controllers/include/franka_example_controllers/default_robot_behavior_utils.hpp +++ b/franka_example_controllers/include/franka_example_controllers/default_robot_behavior_utils.hpp @@ -22,14 +22,22 @@ inline franka_msgs::srv::SetFullCollisionBehavior::Request::SharedPtr getDefaultCollisionBehaviorRequest() { auto request = std::make_shared(); - request->lower_torque_thresholds_nominal = {25.0, 25.0, 22.0, 20.0, 19.0, 17.0, 14.0}; - request->upper_torque_thresholds_nominal = {35.0, 35.0, 32.0, 30.0, 29.0, 27.0, 24.0}; - request->lower_torque_thresholds_acceleration = {25.0, 25.0, 22.0, 20.0, 19.0, 17.0, 14.0}; - request->upper_torque_thresholds_acceleration = {35.0, 35.0, 32.0, 30.0, 29.0, 27.0, 24.0}; - request->lower_force_thresholds_nominal = {30.0, 30.0, 30.0, 25.0, 25.0, 25.0}; - request->upper_force_thresholds_nominal = {40.0, 40.0, 40.0, 35.0, 35.0, 35.0}; - request->lower_force_thresholds_acceleration = {30.0, 30.0, 30.0, 25.0, 25.0, 25.0}; - request->upper_force_thresholds_acceleration = {40.0, 40.0, 40.0, 35.0, 35.0, 35.0}; + request->lower_torque_thresholds_nominal = { + 25.0, 25.0, 22.0, 20.0, 19.0, 17.0, 14.0}; // NOLINT(cppcoreguidelines-avoid-magic-numbers) + request->upper_torque_thresholds_nominal = { + 35.0, 35.0, 32.0, 30.0, 29.0, 27.0, 24.0}; // NOLINT(cppcoreguidelines-avoid-magic-numbers) + request->lower_torque_thresholds_acceleration = { + 25.0, 25.0, 22.0, 20.0, 19.0, 17.0, 14.0}; // NOLINT(cppcoreguidelines-avoid-magic-numbers) + request->upper_torque_thresholds_acceleration = { + 35.0, 35.0, 32.0, 30.0, 29.0, 27.0, 24.0}; // NOLINT(cppcoreguidelines-avoid-magic-numbers) + request->lower_force_thresholds_nominal = { + 30.0, 30.0, 30.0, 25.0, 25.0, 25.0}; // NOLINT(cppcoreguidelines-avoid-magic-numbers) + request->upper_force_thresholds_nominal = { + 40.0, 40.0, 40.0, 35.0, 35.0, 35.0}; // NOLINT(cppcoreguidelines-avoid-magic-numbers) + request->lower_force_thresholds_acceleration = { + 30.0, 30.0, 30.0, 25.0, 25.0, 25.0}; // NOLINT(cppcoreguidelines-avoid-magic-numbers) + request->upper_force_thresholds_acceleration = { + 40.0, 40.0, 40.0, 35.0, 35.0, 35.0}; // NOLINT(cppcoreguidelines-avoid-magic-numbers) return request; } diff --git a/franka_example_controllers/include/franka_example_controllers/joint_impedance_with_ik_example_controller.hpp b/franka_example_controllers/include/franka_example_controllers/joint_impedance_with_ik_example_controller.hpp new file mode 100644 index 00000000..3871dd8e --- /dev/null +++ b/franka_example_controllers/include/franka_example_controllers/joint_impedance_with_ik_example_controller.hpp @@ -0,0 +1,118 @@ +// Copyright (c) 2023 Franka Robotics 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 +#include + +#include +#include +#include +#include "franka_semantic_components/franka_cartesian_pose_interface.hpp" +#include "franka_semantic_components/franka_robot_model.hpp" + +using CallbackReturn = rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn; + +namespace franka_example_controllers { + +/** + * joint impedance example controller get desired pose and use inverse kinematics LMA + * (Levenberg-Marquardt) from Orocos KDL. IK returns the desired joint positions from the desired + * pose. Desired joint positions are fed to the impedance control law together with the current + * joint velocities to calculate the desired joint torques. + */ +class JointImpedanceWithIKExampleController : public controller_interface::ControllerInterface { + public: + using Vector7d = Eigen::Matrix; + [[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; + CallbackReturn on_deactivate(const rclcpp_lifecycle::State& previous_state) override; + + private: + void update_joint_states(); + + /** + * @brief Calculates the new pose based on the initial pose. + * + * @return Eigen::Vector3d calculated sinosuidal period for the x,z position of the pose. + */ + Eigen::Vector3d compute_new_position(); + + /** + * @brief creates the ik service request for ik service from moveit. Assigns the move-group, + * desired pose of the desired link. + * + * @return std::shared_ptr request service message + */ + std::shared_ptr create_ik_service_request( + const Eigen::Vector3d& new_position, + const Eigen::Quaterniond& new_orientation, + const std::vector& joint_positions_desired, + const std::vector& joint_positions_current, + const std::vector& joint_efforts_current); + + /** + * @brief computes the torque commands based on impedance control law with compensated coriolis + * terms + * + * @return Eigen::Vector7d torque for each joint of the robot + */ + Vector7d compute_torque_command(const Vector7d& joint_positions_desired, + const Vector7d& joint_positions_current, + const Vector7d& joint_velocities_current); + + /** + * @brief assigns the Kp, Kd and arm_id parameters + * + * @return true when parameters are present, false when parameters are not available + */ + bool assign_parameters(); + + std::unique_ptr franka_cartesian_pose_; + + Eigen::Quaterniond orientation_; + Eigen::Vector3d position_; + rclcpp::Client::SharedPtr compute_ik_client_; + + double trajectory_period_{0.001}; + const bool k_elbow_activated_{false}; + bool initialization_flag_{true}; + + std::string arm_id_; + + double elapsed_time_{0.0}; + std::unique_ptr franka_robot_model_; + + const std::string k_robot_state_interface_name{"robot_state"}; + const std::string k_robot_model_interface_name{"robot_model"}; + + Vector7d dq_filtered_; + Vector7d k_gains_; + Vector7d d_gains_; + int num_joints_{7}; + + std::vector joint_positions_desired_; + std::vector joint_positions_current_{0, 0, 0, 0, 0, 0, 0}; + std::vector joint_velocities_current_{0, 0, 0, 0, 0, 0, 0}; + std::vector joint_efforts_current_{0, 0, 0, 0, 0, 0, 0}; +}; +} // namespace franka_example_controllers \ No newline at end of file diff --git a/franka_example_controllers/package.xml b/franka_example_controllers/package.xml index 34b91490..3039ff7f 100644 --- a/franka_example_controllers/package.xml +++ b/franka_example_controllers/package.xml @@ -2,7 +2,7 @@ franka_example_controllers - 0.1.9 + 0.1.11 franka_example_controllers provides example code for controlling Franka Emika research robots with ros2_control Franka Emika GmbH Apache 2.0 diff --git a/franka_example_controllers/src/joint_impedance_with_ik_example_controller.cpp b/franka_example_controllers/src/joint_impedance_with_ik_example_controller.cpp new file mode 100644 index 00000000..1acbb44d --- /dev/null +++ b/franka_example_controllers/src/joint_impedance_with_ik_example_controller.cpp @@ -0,0 +1,278 @@ +// Copyright (c) 2023 Franka Robotics 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 +#include + +#include +#include +#include +#include + +#include + +using namespace std::chrono_literals; +using Vector7d = Eigen::Matrix; + +namespace franka_example_controllers { + +controller_interface::InterfaceConfiguration +JointImpedanceWithIKExampleController::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) + "/effort"); + } + return config; +} + +controller_interface::InterfaceConfiguration +JointImpedanceWithIKExampleController::state_interface_configuration() const { + controller_interface::InterfaceConfiguration config; + config.type = controller_interface::interface_configuration_type::INDIVIDUAL; + config.names = franka_cartesian_pose_->get_state_interface_names(); + for (int i = 1; i <= num_joints_; ++i) { + config.names.push_back(arm_id_ + "_joint" + std::to_string(i) + "/position"); + } + for (int i = 1; i <= num_joints_; ++i) { + config.names.push_back(arm_id_ + "_joint" + std::to_string(i) + "/velocity"); + } + for (int i = 1; i <= num_joints_; ++i) { + config.names.push_back(arm_id_ + "_joint" + std::to_string(i) + "/effort"); + } + for (const auto& franka_robot_model_name : franka_robot_model_->get_state_interface_names()) { + config.names.push_back(franka_robot_model_name); + } + + return config; +} + +void JointImpedanceWithIKExampleController::update_joint_states() { + for (auto i = 0; i < num_joints_; ++i) { + // TODO(yazi_ba) Can we get the state from its name? + const auto& position_interface = state_interfaces_.at(16 + i); + const auto& velocity_interface = state_interfaces_.at(23 + i); + const auto& effort_interface = state_interfaces_.at(30 + i); + joint_positions_current_[i] = position_interface.get_value(); + joint_velocities_current_[i] = velocity_interface.get_value(); + joint_efforts_current_[i] = effort_interface.get_value(); + } +} + +Eigen::Vector3d JointImpedanceWithIKExampleController::compute_new_position() { + elapsed_time_ = elapsed_time_ + trajectory_period_; + double radius = 0.1; + + double angle = M_PI / 4 * (1 - std::cos(M_PI / 5.0 * elapsed_time_)); + + double delta_x = radius * std::sin(angle); + double delta_z = radius * (std::cos(angle) - 1); + + Eigen::Vector3d new_position = position_; + new_position.x() -= delta_x; + new_position.z() -= delta_z; + + return new_position; +} + +std::shared_ptr +JointImpedanceWithIKExampleController::create_ik_service_request( + const Eigen::Vector3d& position, + const Eigen::Quaterniond& orientation, + const std::vector& joint_positions_current, + const std::vector& joint_velocities_current, + const std::vector& joint_efforts_current) { + auto service_request = std::make_shared(); + + service_request->ik_request.group_name = arm_id_ + "_arm"; + service_request->ik_request.pose_stamped.header.frame_id = arm_id_ + "_link0"; + service_request->ik_request.pose_stamped.pose.position.x = position.x(); + service_request->ik_request.pose_stamped.pose.position.y = position.y(); + service_request->ik_request.pose_stamped.pose.position.z = position.z(); + service_request->ik_request.pose_stamped.pose.orientation.x = orientation.x(); + service_request->ik_request.pose_stamped.pose.orientation.y = orientation.y(); + service_request->ik_request.pose_stamped.pose.orientation.z = orientation.z(); + service_request->ik_request.pose_stamped.pose.orientation.w = orientation.w(); + service_request->ik_request.robot_state.joint_state.name = { + arm_id_ + "_joint1", arm_id_ + "_joint2", arm_id_ + "_joint3", arm_id_ + "_joint4", + arm_id_ + "_joint5", arm_id_ + "_joint6", arm_id_ + "_joint7"}; + service_request->ik_request.robot_state.joint_state.position = joint_positions_current; + service_request->ik_request.robot_state.joint_state.velocity = joint_velocities_current; + service_request->ik_request.robot_state.joint_state.effort = joint_efforts_current; + + // If Franka Hand is not connected, the following line should be commented out. + service_request->ik_request.ik_link_name = arm_id_ + "_hand_tcp"; + return service_request; +} + +Vector7d JointImpedanceWithIKExampleController::compute_torque_command( + const Vector7d& joint_positions_desired, + const Vector7d& joint_positions_current, + const Vector7d& joint_velocities_current) { + std::array coriolis_array = franka_robot_model_->getCoriolisForceVector(); + Vector7d coriolis(coriolis_array.data()); + const double kAlpha = 0.99; + dq_filtered_ = (1 - kAlpha) * dq_filtered_ + kAlpha * joint_velocities_current; + Vector7d q_error = joint_positions_desired - joint_positions_current; + Vector7d tau_d_calculated = + k_gains_.cwiseProduct(q_error) - d_gains_.cwiseProduct(dq_filtered_) + coriolis; + + return tau_d_calculated; +} + +controller_interface::return_type JointImpedanceWithIKExampleController::update( + const rclcpp::Time& /*time*/, + const rclcpp::Duration& /*period*/) { + if (initialization_flag_) { + std::tie(orientation_, position_) = + franka_cartesian_pose_->getInitialOrientationAndTranslation(); + initialization_flag_ = false; + } + update_joint_states(); + + Eigen::Vector3d new_position = compute_new_position(); + + auto service_request = + create_ik_service_request(new_position, orientation_, joint_positions_current_, + joint_velocities_current_, joint_efforts_current_); + + using ServiceResponseFuture = rclcpp::Client::SharedFuture; + auto response_received_callback = + [&](ServiceResponseFuture future) { // NOLINT(performance-unnecessary-value-param) + const auto& response = future.get(); + + if (response->error_code.val == response->error_code.SUCCESS) { + joint_positions_desired_ = response->solution.joint_state.position; + } else { + RCLCPP_INFO(get_node()->get_logger(), "Inverse kinematics solution failed."); + } + }; + auto result_future_ = + compute_ik_client_->async_send_request(service_request, response_received_callback); + + Vector7d joint_positions_desired_eigen(joint_positions_desired_.data()); + Vector7d joint_positions_current_eigen(joint_positions_current_.data()); + Vector7d joint_velocities_current_eigen(joint_velocities_current_.data()); + + auto tau_d_calculated = compute_torque_command( + joint_positions_desired_eigen, joint_positions_current_eigen, joint_velocities_current_eigen); + + for (int i = 0; i < num_joints_; i++) { + command_interfaces_[i].set_value(tau_d_calculated(i)); + } + + return controller_interface::return_type::OK; +} + +CallbackReturn JointImpedanceWithIKExampleController::on_init() { + franka_cartesian_pose_ = + std::make_unique( + franka_semantic_components::FrankaCartesianPoseInterface(k_elbow_activated_)); + + return CallbackReturn::SUCCESS; +} + +bool JointImpedanceWithIKExampleController::assign_parameters() { + arm_id_ = get_node()->get_parameter("arm_id").as_string(); + auto k_gains = get_node()->get_parameter("k_gains").as_double_array(); + auto d_gains = get_node()->get_parameter("d_gains").as_double_array(); + if (k_gains.empty()) { + RCLCPP_FATAL(get_node()->get_logger(), "k_gains parameter not set"); + return false; + } + if (k_gains.size() != static_cast(num_joints_)) { + RCLCPP_FATAL(get_node()->get_logger(), "k_gains should be of size %d but is of size %ld", + num_joints_, k_gains.size()); + return false; + } + if (d_gains.empty()) { + RCLCPP_FATAL(get_node()->get_logger(), "d_gains parameter not set"); + return false; + } + if (d_gains.size() != static_cast(num_joints_)) { + RCLCPP_FATAL(get_node()->get_logger(), "d_gains should be of size %d but is of size %ld", + num_joints_, d_gains.size()); + return false; + } + for (int i = 0; i < num_joints_; ++i) { + d_gains_(i) = d_gains.at(i); + k_gains_(i) = k_gains.at(i); + } + return true; +} + +CallbackReturn JointImpedanceWithIKExampleController::on_configure( + const rclcpp_lifecycle::State& /*previous_state*/) { + if (!assign_parameters()) { + return CallbackReturn::FAILURE; + } + + franka_robot_model_ = std::make_unique( + franka_semantic_components::FrankaRobotModel(arm_id_ + "/" + k_robot_model_interface_name, + arm_id_ + "/" + k_robot_state_interface_name)); + + auto collision_client = get_node()->create_client( + "service_server/set_full_collision_behavior"); + compute_ik_client_ = get_node()->create_client("compute_ik"); + + while (!compute_ik_client_->wait_for_service(1ms) && !collision_client->wait_for_service(1ms)) { + if (!rclcpp::ok()) { + RCLCPP_ERROR(get_node()->get_logger(), "Interrupted while waiting for the service. Exiting."); + return CallbackReturn::ERROR; + } + RCLCPP_INFO(get_node()->get_logger(), "service not available, waiting again..."); + } + + auto request = DefaultRobotBehavior::getDefaultCollisionBehaviorRequest(); + auto future_result = collision_client->async_send_request(request); + + auto success = future_result.get(); + + if (!success->success) { + RCLCPP_FATAL(get_node()->get_logger(), "Failed to set default collision behavior."); + return CallbackReturn::ERROR; + } else { + RCLCPP_INFO(get_node()->get_logger(), "Default collision behavior set."); + } + + return CallbackReturn::SUCCESS; +} + +CallbackReturn JointImpedanceWithIKExampleController::on_activate( + const rclcpp_lifecycle::State& /*previous_state*/) { + initialization_flag_ = true; + elapsed_time_ = 0.0; + dq_filtered_.setZero(); + joint_positions_desired_.reserve(num_joints_); + joint_positions_current_.reserve(num_joints_); + joint_velocities_current_.reserve(num_joints_); + joint_efforts_current_.reserve(num_joints_); + + franka_cartesian_pose_->assign_loaned_state_interfaces(state_interfaces_); + franka_robot_model_->assign_loaned_state_interfaces(state_interfaces_); + + return CallbackReturn::SUCCESS; +} + +controller_interface::CallbackReturn JointImpedanceWithIKExampleController::on_deactivate( + const rclcpp_lifecycle::State& /*previous_state*/) { + franka_cartesian_pose_->release_interfaces(); + return CallbackReturn::SUCCESS; +} + +} // namespace franka_example_controllers +#include "pluginlib/class_list_macros.hpp" +// NOLINTNEXTLINE +PLUGINLIB_EXPORT_CLASS(franka_example_controllers::JointImpedanceWithIKExampleController, + controller_interface::ControllerInterface) \ No newline at end of file diff --git a/franka_gripper/package.xml b/franka_gripper/package.xml index b1b544d9..ff310142 100644 --- a/franka_gripper/package.xml +++ b/franka_gripper/package.xml @@ -2,7 +2,7 @@ franka_gripper - 0.1.9 + 0.1.11 This package implements the franka gripper of type Franka Hand for the use in ROS2 Franka Emika GmbH Apache 2.0 diff --git a/franka_hardware/package.xml b/franka_hardware/package.xml index fb504aa6..d79b7cb1 100644 --- a/franka_hardware/package.xml +++ b/franka_hardware/package.xml @@ -2,7 +2,7 @@ franka_hardware - 0.1.9 + 0.1.11 franka_hardware provides hardware interfaces for using Franka Emika research robots with ros2_control Franka Emika GmbH Apache 2.0 diff --git a/franka_hardware/src/franka_hardware_interface.cpp b/franka_hardware/src/franka_hardware_interface.cpp index 9542b7f0..e4900803 100644 --- a/franka_hardware/src/franka_hardware_interface.cpp +++ b/franka_hardware/src/franka_hardware_interface.cpp @@ -156,10 +156,9 @@ void FrankaHardwareInterface::initializePositionCommands(const franka::RobotStat hw_position_commands_, robot_state.q_d); initializeCommand(first_cartesian_pose_update_, pose_cartesian_interface_running_, hw_cartesian_pose_, robot_state.O_T_EE_d); - initializeCommand(initial_robot_state_update_, pose_cartesian_interface_running_, - initial_robot_pose_, robot_state.O_T_EE_d); - initializeCommand(initial_joint_position_update_, position_joint_interface_running_, - initial_joint_positions_, robot_state.q_d); + initializeCommand(initial_robot_state_update_, true, initial_robot_pose_, robot_state.O_T_EE_d); + initializeCommand(initial_joint_position_update_, true, initial_joint_positions_, + robot_state.q_d); } hardware_interface::return_type FrankaHardwareInterface::read(const rclcpp::Time& /*time*/, diff --git a/franka_moveit_config/config/kinematics.yaml b/franka_moveit_config/config/kinematics.yaml index e721385b..d3f2a44f 100644 --- a/franka_moveit_config/config/kinematics.yaml +++ b/franka_moveit_config/config/kinematics.yaml @@ -1,4 +1,4 @@ panda_arm: - kinematics_solver: kdl_kinematics_plugin/KDLKinematicsPlugin + kinematics_solver: lma_kinematics_plugin/LMAKinematicsPlugin kinematics_solver_search_resolution: 0.005 - kinematics_solver_timeout: 0.05 + kinematics_solver_timeout: 0.005 diff --git a/franka_moveit_config/launch/move_group.launch.py b/franka_moveit_config/launch/move_group.launch.py new file mode 100644 index 00000000..1445bfc9 --- /dev/null +++ b/franka_moveit_config/launch/move_group.launch.py @@ -0,0 +1,113 @@ +# Copyright (c) 2023 Franka Robotics 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. + +# This file is an adapted version of +# https://github.com/ros-planning/moveit_resources/blob/ca3f7930c630581b5504f3b22c40b4f82ee6369d/panda_moveit_config/launch/demo.launch.py + +import os + +from ament_index_python.packages import get_package_share_directory +from launch import LaunchDescription +from launch.actions import ( + DeclareLaunchArgument, +) +from launch.substitutions import ( + Command, + FindExecutable, + LaunchConfiguration, +) +from launch_ros.actions import Node +import yaml + + +def load_yaml(package_name, file_path): + package_path = get_package_share_directory(package_name) + absolute_file_path = os.path.join(package_path, file_path) + + try: + with open(absolute_file_path, 'r') as file: + return yaml.safe_load(file) + except ( + EnvironmentError + ): # parent of IOError, OSError *and* WindowsError where available + return None + + +def generate_launch_description(): + robot_ip_parameter_name = 'robot_ip' + use_fake_hardware_parameter_name = 'use_fake_hardware' + fake_sensor_commands_parameter_name = 'fake_sensor_commands' + + robot_ip = LaunchConfiguration(robot_ip_parameter_name) + use_fake_hardware = LaunchConfiguration(use_fake_hardware_parameter_name) + fake_sensor_commands = LaunchConfiguration(fake_sensor_commands_parameter_name) + + # Command-line arguments + db_arg = DeclareLaunchArgument( + 'db', default_value='False', description='Database flag' + ) + + # planning_context + franka_xacro_file = os.path.join( + get_package_share_directory('franka_description'), + 'robots', + 'panda_arm.urdf.xacro', + ) + robot_description_config = Command( + [ + FindExecutable(name='xacro'), + ' ', + franka_xacro_file, + ' hand:=true', + ' robot_ip:=', + robot_ip, + ' use_fake_hardware:=', + use_fake_hardware, + ' fake_sensor_commands:=', + fake_sensor_commands, + ] + ) + + robot_description = {'robot_description': robot_description_config} + + franka_semantic_xacro_file = os.path.join( + get_package_share_directory('franka_moveit_config'), + 'srdf', + 'panda_arm.srdf.xacro', + ) + robot_description_semantic_config = Command( + [FindExecutable(name='xacro'), ' ', franka_semantic_xacro_file, ' hand:=true'] + ) + robot_description_semantic = { + 'robot_description_semantic': robot_description_semantic_config + } + + kinematics_yaml = load_yaml('franka_moveit_config', 'config/kinematics.yaml') + # Start the actual move_group node/action server + run_move_group_node = Node( + package='moveit_ros_move_group', + executable='move_group', + parameters=[ + robot_description, + robot_description_semantic, + kinematics_yaml, + ], + ) + + return LaunchDescription( + [ + db_arg, + run_move_group_node, + ] + ) diff --git a/franka_moveit_config/launch/moveit.launch.py b/franka_moveit_config/launch/moveit.launch.py index 0f527b82..3d52eacb 100644 --- a/franka_moveit_config/launch/moveit.launch.py +++ b/franka_moveit_config/launch/moveit.launch.py @@ -1,4 +1,4 @@ -# Copyright (c) 2021 Franka Emika GmbH +# Copyright (c) 2021 Franka Robotics GmbH # # Licensed under the Apache License, Version 2.0 (the "License"); # you may not use this file except in compliance with the License. diff --git a/franka_moveit_config/package.xml b/franka_moveit_config/package.xml index a5067ae1..dcf6aae3 100644 --- a/franka_moveit_config/package.xml +++ b/franka_moveit_config/package.xml @@ -2,7 +2,7 @@ franka_moveit_config - 0.1.9 + 0.1.11 Contains Moveit2 configuration files for Franka Emika research robots Franka Emika GmbH Apache 2.0 diff --git a/franka_msgs/package.xml b/franka_msgs/package.xml index 0106479b..66117a16 100644 --- a/franka_msgs/package.xml +++ b/franka_msgs/package.xml @@ -2,7 +2,7 @@ franka_msgs - 0.1.9 + 0.1.11 franka_msgs provides messages and actions specific to Franka Emika research robots Franka Emika GmbH Apache 2.0 diff --git a/franka_robot_state_broadcaster/package.xml b/franka_robot_state_broadcaster/package.xml index 0997c647..6c0a3c8d 100644 --- a/franka_robot_state_broadcaster/package.xml +++ b/franka_robot_state_broadcaster/package.xml @@ -1,7 +1,7 @@ franka_robot_state_broadcaster - 0.1.9 + 0.1.11 Broadcaster to publish robot states Franka Emika GmbH Apache 2.0 diff --git a/franka_semantic_components/include/franka_semantic_components/franka_robot_state.hpp b/franka_semantic_components/include/franka_semantic_components/franka_robot_state.hpp index 2010cb28..645d1f65 100644 --- a/franka_semantic_components/include/franka_semantic_components/franka_robot_state.hpp +++ b/franka_semantic_components/include/franka_semantic_components/franka_robot_state.hpp @@ -49,4 +49,4 @@ class FrankaRobotState const std::string state_interface_name_{"robot_state"}; }; -} // namespace franka_semantic_components +} // namespace franka_semantic_components \ No newline at end of file diff --git a/franka_semantic_components/src/franka_robot_state.cpp b/franka_semantic_components/src/franka_robot_state.cpp index 824539a0..2f6f69c8 100644 --- a/franka_semantic_components/src/franka_robot_state.cpp +++ b/franka_semantic_components/src/franka_robot_state.cpp @@ -212,4 +212,4 @@ auto FrankaRobotState::get_values_as_message(franka_msgs::msg::FrankaRobotState& return true; } -} // namespace franka_semantic_components +} // namespace franka_semantic_components \ No newline at end of file diff --git a/integration_launch_testing/package.xml b/integration_launch_testing/package.xml index 663e7bdf..61398f79 100644 --- a/integration_launch_testing/package.xml +++ b/integration_launch_testing/package.xml @@ -2,7 +2,7 @@ integration_launch_testing - 0.1.9 + 0.1.11 Functional integration tests for franka controllers Franka Emika GmbH Apache 2.0