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 @@ -