chore: switch to orocos-lma-ik

jenkins remove xmllint

.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, ...

.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 \

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 \

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.

franka_bringup/launch/joint_impedance_with_ik_example_controller.launch.py

@@ -94,4 +94,3 @@ def generate_launch_description():
             output='screen',
         ),
     ])
-

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)

franka_example_controllers/franka_example_controllers.xml

@@ -38,7 +38,7 @@
         <class name="franka_example_controllers/JointImpedanceWithIKExampleController"
            type="franka_example_controllers::JointImpedanceWithIKExampleController" base_class_type="controller_interface::ControllerInterface">
         <description>
-            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.
         </description>
     </class>
     <class name="franka_example_controllers/CartesianElbowExampleController"

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<franka_msgs::srv::SetFullCollisionBehavior::Request>();
 
-  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;
 }

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<moveit_msgs::srv::GetPositionIK::Request> create_ik_service_request(
-      Eigen::Vector3d new_position,
-      Eigen::Quaterniond new_orientation,
-      std::vector<double> joint_positions_desired,
-      std::vector<double> joint_positions_current,
-      std::vector<double> 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<moveit_msgs::srv::GetPositionIK::Request> request service message
+   */
+  std::shared_ptr<moveit_msgs::srv::GetPositionIK::Request> create_ik_service_request(
+      const Eigen::Vector3d& new_position,
+      const Eigen::Quaterniond& new_orientation,
+      const std::vector<double>& joint_positions_desired,
+      const std::vector<double>& joint_positions_current,
+      const std::vector<double>& 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_semantic_components::FrankaCartesianPoseInterface> franka_cartesian_pose_;
@@ -83,9 +110,9 @@ class JointImpedanceWithIKExampleController : public controller_interface::Contr
   Vector7d d_gains_;
   int num_joints_{7};
 
-  std::vector<double> joint_positions_desired;
-  std::vector<double> joint_positions_current{0, 0, 0, 0, 0, 0, 0};
-  std::vector<double> joint_velocities_current{0, 0, 0, 0, 0, 0, 0};
-  std::vector<double> joint_efforts_current{0, 0, 0, 0, 0, 0, 0};
+  std::vector<double> joint_positions_desired_;
+  std::vector<double> joint_positions_current_{0, 0, 0, 0, 0, 0, 0};
+  std::vector<double> joint_velocities_current_{0, 0, 0, 0, 0, 0, 0};
+  std::vector<double> joint_efforts_current_{0, 0, 0, 0, 0, 0, 0};
 };
 }  // namespace franka_example_controllers

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<moveit_msgs::srv::GetPositionIK::Request>
 JointImpedanceWithIKExampleController::create_ik_service_request(
-    Eigen::Vector3d position,
-    Eigen::Quaterniond orientation,
-    std::vector<double> joint_positions_current,
-    std::vector<double> joint_velocities_current,
-    std::vector<double> joint_efforts_current) {
+    const Eigen::Vector3d& position,
+    const Eigen::Quaterniond& orientation,
+    const std::vector<double>& joint_positions_current,
+    const std::vector<double>& joint_velocities_current,
+    const std::vector<double>& joint_efforts_current) {
   auto service_request = std::make_shared<moveit_msgs::srv::GetPositionIK::Request>();
-  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<double, 7> 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<moveit_msgs::srv::GetPositionIK>::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_);

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
+  kinematics_solver_timeout: 0.005