From acd0b877a6b87092f6e78647c81b4a64d3fa9cdc Mon Sep 17 00:00:00 2001 From: Baris Yazici Date: Mon, 18 Dec 2023 16:32:17 +0100 Subject: [PATCH 1/3] feat: joint impedance controller with moveit ik service --- Dockerfile | 1 + franka_bringup/config/controllers.yaml | 23 ++ ...dance_with_ik_example_controller.launch.py | 97 ++++++ franka_example_controllers/CMakeLists.txt | 4 + .../franka_example_controllers.xml | 6 + ...t_impedance_with_ik_example_controller.hpp | 91 ++++++ ...t_impedance_with_ik_example_controller.cpp | 277 ++++++++++++++++++ .../src/franka_hardware_interface.cpp | 7 +- franka_moveit_config/config/kinematics.yaml | 14 + .../launch/move_group.launch.py | 113 +++++++ franka_moveit_config/srdf/panda_arm.xacro | 1 + .../franka_robot_state.hpp | 2 +- .../src/franka_robot_state.cpp | 2 +- 13 files changed, 632 insertions(+), 6 deletions(-) create mode 100644 franka_bringup/launch/joint_impedance_with_ik_example_controller.launch.py create mode 100644 franka_example_controllers/include/franka_example_controllers/joint_impedance_with_ik_example_controller.hpp create mode 100644 franka_example_controllers/src/joint_impedance_with_ik_example_controller.cpp create mode 100644 franka_moveit_config/launch/move_group.launch.py diff --git a/Dockerfile b/Dockerfile index eaa69cc7..5b173a8d 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 \ 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..b06f353f --- /dev/null +++ b/franka_bringup/launch/joint_impedance_with_ik_example_controller.launch.py @@ -0,0 +1,97 @@ +# 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_example_controllers/CMakeLists.txt b/franka_example_controllers/CMakeLists.txt index a40d8a7e..82804eec 100644 --- a/franka_example_controllers/CMakeLists.txt +++ b/franka_example_controllers/CMakeLists.txt @@ -24,6 +24,7 @@ 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) add_library( ${PROJECT_NAME} @@ -34,6 +35,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 +56,7 @@ ament_target_dependencies( rclcpp rclcpp_lifecycle franka_semantic_components + moveit_core ) generate_parameter_library(franka_example_controllers_parameters src/model_example_controller_parameters.yaml) @@ -141,5 +144,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..70d293a1 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(bio-ik) and commands torques based on joint impedance control law. + 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..50d3e0d5 --- /dev/null +++ b/franka_example_controllers/include/franka_example_controllers/joint_impedance_with_ik_example_controller.hpp @@ -0,0 +1,91 @@ +// 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 moveit inverse kinematics + */ +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(); + Eigen::Vector3d compute_new_position(); + std::shared_ptr create_ik_service_request( + Eigen::Vector3d new_position, + Eigen::Quaterniond new_orientation, + std::vector joint_positions_desired, + std::vector joint_positions_current, + std::vector joint_efforts_current); + + Vector7d compute_torque_command(Vector7d joint_positions_desired, + Vector7d joint_positions_current, + Vector7d joint_velocities_current); + + 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/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..7dba030b --- /dev/null +++ b/franka_example_controllers/src/joint_impedance_with_ik_example_controller.cpp @@ -0,0 +1,277 @@ +// 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 <= 7; ++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 <= 7; ++i) { + config.names.push_back(arm_id_ + "_joint" + std::to_string(i) + "/position"); + } + for (int i = 1; i <= 7; ++i) { + config.names.push_back(arm_id_ + "_joint" + std::to_string(i) + "/velocity"); + } + for (int i = 1; i <= 7; ++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 < 7; ++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( + Eigen::Vector3d position, + Eigen::Quaterniond orientation, + std::vector joint_positions_current, + std::vector joint_velocities_current, + std::vector joint_efforts_current) { + auto service_request = std::make_shared(); + service_request->ik_request.group_name = arm_id_ + "_arm_ik"; + 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"; + service_request->ik_request.timeout = rclcpp::Duration(0.001, 0); + return service_request; +} + +Vector7d JointImpedanceWithIKExampleController::compute_torque_command( + Vector7d joint_positions_desired, + Vector7d joint_positions_current, + 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) { + 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(7); + joint_positions_current.reserve(7); + joint_velocities_current.reserve(7); + joint_efforts_current.reserve(7); + + 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_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..7a01498c 100644 --- a/franka_moveit_config/config/kinematics.yaml +++ b/franka_moveit_config/config/kinematics.yaml @@ -2,3 +2,17 @@ panda_arm: kinematics_solver: kdl_kinematics_plugin/KDLKinematicsPlugin kinematics_solver_search_resolution: 0.005 kinematics_solver_timeout: 0.05 + +panda_arm_ik: + kinematics_solver: pick_ik/PickIkPlugin + kinematics_solver_timeout: 0.05 + kinematics_solver_attempts: 3 + mode: global + stop_optimization_on_valid_solution: true + position_scale: 1.0 + rotation_scale: 0.5 + position_threshold: 0.001 + orientation_threshold: 0.01 + cost_threshold: 0.001 + minimal_displacement_weight: 0.0001 + gd_step_size: 0.0001 \ No newline at end of file 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..58a43ee1 --- /dev/null +++ b/franka_moveit_config/launch/move_group.launch.py @@ -0,0 +1,113 @@ +# 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. + +# 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/srdf/panda_arm.xacro b/franka_moveit_config/srdf/panda_arm.xacro index ca95cb1b..03772799 100644 --- a/franka_moveit_config/srdf/panda_arm.xacro +++ b/franka_moveit_config/srdf/panda_arm.xacro @@ -4,6 +4,7 @@ + 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 From 76fb9dff625ce329232bc5560f91e248336ace7a Mon Sep 17 00:00:00 2001 From: Baris Yazici Date: Wed, 20 Dec 2023 14:16:30 +0100 Subject: [PATCH 2/3] chore: switch to orocos-lma-ik jenkins remove xmllint --- .clang-tidy | 7 +- .devcontainer/Dockerfile | 2 +- Dockerfile | 2 +- NOTICE | 2 +- ...dance_with_ik_example_controller.launch.py | 1 - franka_example_controllers/CMakeLists.txt | 2 + .../franka_example_controllers.xml | 2 +- .../default_robot_behavior_utils.hpp | 24 ++++-- ...t_impedance_with_ik_example_controller.hpp | 57 +++++++++++---- ...t_impedance_with_ik_example_controller.cpp | 73 ++++++++++--------- franka_moveit_config/config/kinematics.yaml | 18 +---- .../launch/move_group.launch.py | 48 ++++++------ franka_moveit_config/launch/moveit.launch.py | 2 +- franka_moveit_config/srdf/panda_arm.xacro | 1 - 14 files changed, 130 insertions(+), 111 deletions(-) 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/Dockerfile b/Dockerfile index 5b173a8d..dfcd803d 100644 --- a/Dockerfile +++ b/Dockerfile @@ -60,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/launch/joint_impedance_with_ik_example_controller.launch.py b/franka_bringup/launch/joint_impedance_with_ik_example_controller.launch.py index b06f353f..869e3609 100644 --- a/franka_bringup/launch/joint_impedance_with_ik_example_controller.launch.py +++ b/franka_bringup/launch/joint_impedance_with_ik_example_controller.launch.py @@ -94,4 +94,3 @@ def generate_launch_description(): output='screen', ), ]) - diff --git a/franka_example_controllers/CMakeLists.txt b/franka_example_controllers/CMakeLists.txt index 82804eec..e1d9e615 100644 --- a/franka_example_controllers/CMakeLists.txt +++ b/franka_example_controllers/CMakeLists.txt @@ -25,6 +25,7 @@ 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} @@ -57,6 +58,7 @@ ament_target_dependencies( rclcpp_lifecycle franka_semantic_components moveit_core + moveit_msgs ) generate_parameter_library(franka_example_controllers_parameters src/model_example_controller_parameters.yaml) diff --git a/franka_example_controllers/franka_example_controllers.xml b/franka_example_controllers/franka_example_controllers.xml index 70d293a1..c204c87b 100644 --- a/franka_example_controllers/franka_example_controllers.xml +++ b/franka_example_controllers/franka_example_controllers.xml @@ -38,7 +38,7 @@ - The joint impedance using inverse kinematics from moveit(bio-ik) and commands torques based on joint impedance control law. + The joint impedance using inverse kinematics from moveit-OrocosKDL(LMA-ik) and commands torques based on joint impedance control law. (); - 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 index 50d3e0d5..3871dd8e 100644 --- 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 @@ -28,7 +28,10 @@ using CallbackReturn = rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface namespace franka_example_controllers { /** - * joint impedance example controller get desired pose and use moveit inverse kinematics + * 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: @@ -46,18 +49,42 @@ class JointImpedanceWithIKExampleController : public controller_interface::Contr private: void update_joint_states(); - Eigen::Vector3d compute_new_position(); - std::shared_ptr create_ik_service_request( - Eigen::Vector3d new_position, - Eigen::Quaterniond new_orientation, - std::vector joint_positions_desired, - std::vector joint_positions_current, - std::vector joint_efforts_current); - Vector7d compute_torque_command(Vector7d joint_positions_desired, - Vector7d joint_positions_current, - Vector7d joint_velocities_current); + /** + * @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_; @@ -83,9 +110,9 @@ class JointImpedanceWithIKExampleController : public controller_interface::Contr 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}; + 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/src/joint_impedance_with_ik_example_controller.cpp b/franka_example_controllers/src/joint_impedance_with_ik_example_controller.cpp index 7dba030b..1acbb44d 100644 --- a/franka_example_controllers/src/joint_impedance_with_ik_example_controller.cpp +++ b/franka_example_controllers/src/joint_impedance_with_ik_example_controller.cpp @@ -31,7 +31,7 @@ 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 <= 7; ++i) { + for (int i = 1; i <= num_joints_; ++i) { config.names.push_back(arm_id_ + "_joint" + std::to_string(i) + "/effort"); } return config; @@ -42,13 +42,13 @@ 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 <= 7; ++i) { + 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 <= 7; ++i) { + 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 <= 7; ++i) { + 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()) { @@ -59,14 +59,14 @@ JointImpedanceWithIKExampleController::state_interface_configuration() const { } void JointImpedanceWithIKExampleController::update_joint_states() { - for (auto i = 0; i < 7; ++i) { + 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(); + joint_positions_current_[i] = position_interface.get_value(); + joint_velocities_current_[i] = velocity_interface.get_value(); + joint_efforts_current_[i] = effort_interface.get_value(); } } @@ -88,13 +88,14 @@ Eigen::Vector3d JointImpedanceWithIKExampleController::compute_new_position() { std::shared_ptr JointImpedanceWithIKExampleController::create_ik_service_request( - Eigen::Vector3d position, - Eigen::Quaterniond orientation, - std::vector joint_positions_current, - std::vector joint_velocities_current, - std::vector joint_efforts_current) { + 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_ik"; + + 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(); @@ -112,14 +113,13 @@ JointImpedanceWithIKExampleController::create_ik_service_request( // If Franka Hand is not connected, the following line should be commented out. service_request->ik_request.ik_link_name = arm_id_ + "_hand_tcp"; - service_request->ik_request.timeout = rclcpp::Duration(0.001, 0); return service_request; } Vector7d JointImpedanceWithIKExampleController::compute_torque_command( - Vector7d joint_positions_desired, - Vector7d joint_positions_current, - Vector7d joint_velocities_current) { + 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; @@ -144,25 +144,26 @@ controller_interface::return_type JointImpedanceWithIKExampleController::update( 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); + 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) { - 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 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()); + 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); @@ -253,10 +254,10 @@ CallbackReturn JointImpedanceWithIKExampleController::on_activate( initialization_flag_ = true; elapsed_time_ = 0.0; dq_filtered_.setZero(); - joint_positions_desired.reserve(7); - joint_positions_current.reserve(7); - joint_velocities_current.reserve(7); - joint_efforts_current.reserve(7); + 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_); diff --git a/franka_moveit_config/config/kinematics.yaml b/franka_moveit_config/config/kinematics.yaml index 7a01498c..d3f2a44f 100644 --- a/franka_moveit_config/config/kinematics.yaml +++ b/franka_moveit_config/config/kinematics.yaml @@ -1,18 +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 - -panda_arm_ik: - kinematics_solver: pick_ik/PickIkPlugin - kinematics_solver_timeout: 0.05 - kinematics_solver_attempts: 3 - mode: global - stop_optimization_on_valid_solution: true - position_scale: 1.0 - rotation_scale: 0.5 - position_threshold: 0.001 - orientation_threshold: 0.01 - cost_threshold: 0.001 - minimal_displacement_weight: 0.0001 - gd_step_size: 0.0001 \ No newline at end of file + 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 index 58a43ee1..1445bfc9 100644 --- a/franka_moveit_config/launch/move_group.launch.py +++ b/franka_moveit_config/launch/move_group.launch.py @@ -1,4 +1,4 @@ -# Copyright (c) 2023 Franka Emika GmbH +# 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. @@ -36,7 +36,7 @@ def load_yaml(package_name, file_path): absolute_file_path = os.path.join(package_path, file_path) try: - with open(absolute_file_path, "r") as file: + with open(absolute_file_path, 'r') as file: return yaml.safe_load(file) except ( EnvironmentError @@ -45,9 +45,9 @@ def load_yaml(package_name, file_path): 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_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) @@ -55,49 +55,49 @@ def generate_launch_description(): # Command-line arguments db_arg = DeclareLaunchArgument( - "db", default_value="False", description="Database flag" + '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", + get_package_share_directory('franka_description'), + 'robots', + 'panda_arm.urdf.xacro', ) robot_description_config = Command( [ - FindExecutable(name="xacro"), - " ", + FindExecutable(name='xacro'), + ' ', franka_xacro_file, - " hand:=true", - " robot_ip:=", + ' hand:=true', + ' robot_ip:=', robot_ip, - " use_fake_hardware:=", + ' use_fake_hardware:=', use_fake_hardware, - " fake_sensor_commands:=", + ' fake_sensor_commands:=', fake_sensor_commands, ] ) - robot_description = {"robot_description": robot_description_config} + 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", + 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"] + [FindExecutable(name='xacro'), ' ', franka_semantic_xacro_file, ' hand:=true'] ) robot_description_semantic = { - "robot_description_semantic": robot_description_semantic_config + 'robot_description_semantic': robot_description_semantic_config } - kinematics_yaml = load_yaml("franka_moveit_config", "config/kinematics.yaml") + 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", + package='moveit_ros_move_group', + executable='move_group', parameters=[ robot_description, robot_description_semantic, 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/srdf/panda_arm.xacro b/franka_moveit_config/srdf/panda_arm.xacro index 03772799..ca95cb1b 100644 --- a/franka_moveit_config/srdf/panda_arm.xacro +++ b/franka_moveit_config/srdf/panda_arm.xacro @@ -4,7 +4,6 @@ - From 30347acfe6122b8dd6ffd55e0a316cde2e0e73a8 Mon Sep 17 00:00:00 2001 From: Baris Yazici Date: Wed, 20 Dec 2023 15:37:32 +0100 Subject: [PATCH 3/3] bump version 0.1.11 --- CHANGELOG.md | 7 +++++++ franka_bringup/package.xml | 2 +- franka_description/package.xml | 2 +- franka_example_controllers/package.xml | 2 +- franka_gripper/package.xml | 2 +- franka_hardware/package.xml | 2 +- franka_moveit_config/package.xml | 2 +- franka_msgs/package.xml | 2 +- franka_robot_state_broadcaster/package.xml | 2 +- integration_launch_testing/package.xml | 2 +- 10 files changed, 16 insertions(+), 9 deletions(-) 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/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/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_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_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/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