Port cartesian_impedance_controller to franka_ros2 example controllers #51

sp-sophia-labs · 2024-03-19T10:20:36Z

Description:

This PR offers a high level port of Cartesian impedance example controller provided in Franka ROS1 and requests feedback.

ros humble run test

The port was basically based on :

ROS2 Franka example controllers for global architecture of controller and getting robot information (Robot State and Robot Model).
ROS1 Cartesian example controller example for the theoretical part (basically real-time update() method), all computation is pretty much ported with no changes.

Architecture:

Controller class is implemented based on ROS2 Control Controller Interface Class :

Member variables initialized in the constructor header.
init() method calls ControllerInterface::init() to initialize the lifecycle of the controller. Following this, a declaration of
all parameters defining their default values are.
Get Robot State and Robot Model Interfaces in State_interface_configuration() :

`

const std::string k_robot_state_interface_name{"robot_state"};
const std::string k_robot_model_interface_name{"robot_model"};

for (const auto& franka_robot_model_name : franka_robot_model_->get_state_interface_names()) {
      state_interfaces_config.names.push_back(franka_robot_model_name);
 }

for (const auto& franka_robot_state_name : franka_robot_state_->get_state_interface_names()) {
      state_interfaces_config.names.push_back(franka_robot_state_name);
 }

`

Get Effort Command interface in Command_interface_configuration() :

`

for (int i = 1; i <= num_joints; i++) 
{
      command_interfaces_config.names.push_back(arm_id_ + "_joint" + std::to_string(i) + "/effort");     
 }

`

Real-time update() method is pretty much ported from ROS1 version (requires release build : DCMAKE_BUILD_TYPE=Release)
lifecycle management methods :

on_configure() and on_deactivate() : Nothing special here, first to configures controller and second is called when controller is deactivated (stopped).
on_activate() - called when controller is activated (started), I think this method is the equivalent functionality to the starting() method in ROS 1, since it is called when a controller is activated, and this is a suitable place to perform actions
that need to occur just before the controller starts updating its state.

Notes :

For Equilibrium pose, I first set it as initial state, then published through a separated node
Compliance Parameters are set and updated through the code, Not dynamically configured yet.
Controller Have been tested on Franka Emika research3 FR3.

Questions :

I need feedback/confirmation about those points if possible :

Getting robot state
Getting Initial robot state and Setting parameters on Activate() method

Thanks in advance !

sp-sophia-labs · 2024-03-27T10:22:36Z

Friendly ping @BarisYazici

BarisYazici · 2024-06-19T11:54:01Z

Sorry for the delay! I will try to review in short the upcoming days

franka_example_controllers/src/cartesian_impedance_example_controller.cpp

BarisYazici · 2024-07-01T12:18:29Z

Can you also create the bringup launch file that starts this controller?

BarisYazici · 2024-07-01T12:19:17Z

franka_example_controllers/src/cartesian_impedance_example_controller.cpp

@@ -0,0 +1,243 @@
+#include "franka_example_controllers/cartesian_impedance_example_controller.hpp"
+#include "franka_example_controllers/pseudo_inverse.hpp"


Header file is called pseudo_inversion.hpp

BarisYazici · 2024-07-01T12:20:25Z

franka_example_controllers/src/cartesian_impedance_example_controller.cpp

+    // Equilibrium pose subscription
+    sub_equilibrium_pose_ = get_node()->create_subscription<geometry_msgs::msg::PoseStamped>(
+      "equilibrium_pose", 20, 
+      std::bind(&FSMImpedanceController::equilibriumPoseCallback, this, std::placeholders::_1));


where is this &FSMImpedanceController coming from? Could you also provide the code for that?

This was related to local class on our project, now equilibriumPoseCallback is memeber of &CartesianImpedanceExampleController

… header

sp-sophia-labs · 2024-07-03T15:37:49Z

@BarisYazici Thank you for your feedback! I have made the requested changes:

Updated equilibriumPoseCallback call.
Cleaned up leftover and header call.
Added launch for test.

Please let me know if there are any further adjustments needed or if there's anything else you'd like me to address

...le_controllers/include/franka_example_controllers/cartesian_impedance_example_controller.hpp

franka_example_controllers/src/cartesian_impedance_example_controller.cpp

franka_example_controllers/include/franka_example_controllers/pseudo_inversion.hpp

sp-sophia-labs · 2024-08-05T09:08:31Z

@AndreasKuhner Thanks for your feedback! I have made the requested changes.

Please let me know if there are any further adjustments needed or if there's anything else you'd like me to address

franka_example_controllers/src/cartesian_impedance_example_controller.cpp

AndreasKuhner · 2024-08-09T11:54:47Z

Looks good from my side. I guess we would only need to run it on the robot for testing it :)

BarisYazici · 2024-08-12T13:10:27Z

Please add the src/cartesian_impedance_example_controller.cpp to the CMakeLists.txt under franka_example_controllers

franka_ros2/franka_example_controllers/CMakeLists.txt

Lines 29 to 44 in d3d0fb4

    
           add_library( 
        
                   ${PROJECT_NAME} 
        
                   SHARED 
        
                   src/gravity_compensation_example_controller.cpp 
        
                   src/joint_impedance_example_controller.cpp 
        
                   src/joint_velocity_example_controller.cpp 
        
                   src/joint_position_example_controller.cpp 
        
                   src/cartesian_velocity_example_controller.cpp 
        
                   src/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 
        
                   src/model_example_controller.cpp 
        
                   src/move_to_start_example_controller.cpp 
        
                   src/motion_generator.cpp)

BarisYazici · 2024-08-12T13:11:35Z

Add the arm_id parameter similar to the other launch file examples and pass parameter this to the franka.launch.py

BarisYazici · 2024-08-12T13:34:05Z

franka_example_controllers/src/cartesian_impedance_example_controller.cpp

+
+  // pseudoinverse for nullspace handling
+  Eigen::MatrixXd jacobian_transpose_pinv;
+  pseudoInverse(jacobian.transpose(), jacobian_transpose_pinv);


based on the definition on PseudoInverse from you:

inline Eigen::MatrixXd pseudoInverse(const Eigen::MatrixXd& M_, bool damped = true) { double lambda_ = damped ? 0.2 : 0.0; Eigen::JacobiSVD<Eigen::MatrixXd> svd(M_, Eigen::ComputeFullU | Eigen::ComputeFullV); Eigen::JacobiSVD<Eigen::MatrixXd>::SingularValuesType sing_vals_ = svd.singularValues(); Eigen::MatrixXd S_ = M_; // copying the dimensions of M_, its content is not needed. S_.setZero(); for (int i = 0; i < sing_vals_.size(); i++) S_(i, i) = (sing_vals_(i)) / (sing_vals_(i) * sing_vals_(i) + lambda_ * lambda_); return svd.matrixV() * S_.transpose() * svd.matrixU().transpose(); }

second parameter should be bool damped. And the returned value should be the inverted matrix.

sp-sophia-labs · 2024-08-12T14:43:47Z

Thanks for the review @BarisYazici. Updated

AndreasKuhner · 2024-08-14T06:43:02Z

I think Baris tried to run it on the robot and it didn't work - did you do tests on the robot?

BarisYazici · 2024-08-14T10:12:01Z

I think Baris tried to run it on the robot and it didn't work - did you do tests on the robot?

Yes after fixing all the issues in the PR, it still didn't run on the robot. Did you do any tests on the robot?

sp-sophia-labs · 2024-08-14T11:21:49Z

Yes, tested on FR3 and i could access the robot interface, send commands and the robot successfully converged to equilibrium pose (set it manually). I'll provide terminal outputs once i get access to the robot.
@BarisYazici can you provide which part doesn't work, so i can help

sp-sophia-labs · 2024-08-19T10:22:59Z

Here's my output :

Successful connection with gripper
Successful connection with FrankaHardwareInterface
Controller loaded and configured correctly

Here's a short demo : Robot starts in random pose and converges to equilibrium pose relative to starting point of fr3_link8 (published manually through /equilibrium_pose topic)

BarisYazici · 2024-08-19T11:36:12Z

Yes, tested on FR3 and i could access the robot interface, send commands and the robot successfully converged to equilibrium pose (set it manually). I'll provide terminal outputs once i get access to the robot.
@BarisYazici can you provide which part doesn't work, so i can help

I wrote this because the code you started the PR with was not compiling and the source code was not in the CMakelists.txt. I invested some time to fix the issues with the code and got it in a correctly compiling state. After I tested couple of time with the robot and it was throwing some communication violations. I didn't try to debug further than that. If you say you tried it on the hardware, I will give it another try. But please put all the files necessary to run the example in this PR. You mention a node to publish equilibrium pose. Could you also put it in this PR?

I noticed in the image of the terminal output your example controller name is force_impedance_control. Is it the same setup you are using in the tests? Please make sure it's exactly the same.

BarisYazici · 2024-08-12T13:36:41Z

franka_example_controllers/src/cartesian_impedance_example_controller.cpp

+  tau_d << tau_task + tau_nullspace + coriolis;
+
+  // saturate the commanded torque to joint limits
+  tau_d << saturateTorqueRate(tau_d, tau_j_d);


typo with tau_j_d -> tau_J_d

BarisYazici · 2024-08-12T13:37:41Z

franka_example_controllers/src/cartesian_impedance_example_controller.cpp

+}
+
+controller_interface::return_type CartesianImpedanceExampleController::update(
+    const rclcpp::Time& time,


comment time and period out to make linter happy

Suggested change

const rclcpp::Time& time,

controller_interface::return_type CartesianImpedanceExampleController::update(

const rclcpp::Time& /*time*/,

const rclcpp::Duration& /*period*/) {

BarisYazici · 2024-08-12T13:40:45Z

franka_bringup/config/controllers.yaml

@@ -47,6 +47,10 @@ controller_manager:
    franka_robot_state_broadcaster:
      type: franka_robot_state_broadcaster/FrankaRobotStateBroadcaster

+    cartesian_impedance_example_controller:
+      type: franka_example_controllers::CartesianImpedanceExampleController


Suggested change

type: franka_example_controllers::CartesianImpedanceExampleController

type: franka_example_controllers/CartesianImpedanceExampleController

BarisYazici · 2024-08-12T14:01:56Z

franka_example_controllers/src/cartesian_impedance_example_controller.cpp

+
+  for (int i = 0; i < num_joints; i++) {
+    command_interfaces_[i].set_value(tau_d[i]);
+    std::cout << tau_d[i] << std::endl;


remove std::cout

AllanXuRan · 2025-01-06T13:22:31Z

There is an error occurs because franka_example_controllers references a non-existent
source file equilibrium_pose_publisher.cpp in its CMakeLists.txt. As a result,
CMake fails with:

  Cannot find source file: src/equilibrium_pose_publisher.cpp
  No SOURCES given to target: equilibrium_pose_publisher

How to fix this?

AndreasKuhner · 2025-01-07T09:55:34Z

To my knowledge, there is no equilibrium_pose_publisher.cpp. So, not sure where this would come from. Maybe it was part of the old implementation?

ZackqB added 3 commits March 18, 2024 15:44

import cartesian_impedance_controller to franka_ros2 example controllers

c8ed063

clean up headers

89c8aff

saturation

edcf178

BarisYazici reviewed Jun 19, 2024

View reviewed changes

franka_example_controllers/src/cartesian_impedance_example_controller.cpp Outdated Show resolved Hide resolved

BarisYazici requested changes Jul 1, 2024

View reviewed changes

ZackqB added 3 commits July 3, 2024 17:24

Cartesian Impedance Example Controller config

a4fcfec

clenup leftover & fix equilibriumPoseCallback call & pseudo inversion…

9306231

… header

Cartesian Impedance Example launch file

d6929be

AndreasKuhner reviewed Aug 5, 2024

View reviewed changes

ZackqB added 2 commits August 5, 2024 10:59

return inverse instead of receiving parameter

dc89731

doc, variables and arm_id call fix

bc5c07f

AndreasKuhner reviewed Aug 5, 2024

View reviewed changes

franka_example_controllers/src/cartesian_impedance_example_controller.cpp Outdated Show resolved Hide resolved

overlap clean identity

1370f93

sp-sophia-labs requested a review from BarisYazici August 6, 2024 11:47

Merge branch 'frankaemika:humble' into humble

07da33e

BarisYazici reviewed Aug 12, 2024

View reviewed changes

add executable and needed adjustement for local version

1b3b4ff

sp-sophia-labs requested a review from BarisYazici August 12, 2024 14:41

BarisYazici reviewed Aug 19, 2024

View reviewed changes

add equilibrium pose publisher

64966bf

sp-sophia-labs requested a review from BarisYazici August 20, 2024 08:44

Provide feedback

Saved searches

Use saved searches to filter your results more quickly

Port cartesian_impedance_controller to franka_ros2 example controllers #51

Port cartesian_impedance_controller to franka_ros2 example controllers #51

sp-sophia-labs commented Mar 19, 2024 •

edited

Loading

sp-sophia-labs commented Mar 27, 2024

BarisYazici commented Jun 19, 2024

BarisYazici commented Jul 1, 2024 •

edited

Loading

BarisYazici Jul 1, 2024

BarisYazici Jul 1, 2024

sp-sophia-labs Jul 3, 2024

sp-sophia-labs commented Jul 3, 2024

sp-sophia-labs commented Aug 5, 2024

AndreasKuhner commented Aug 9, 2024

BarisYazici commented Aug 12, 2024

BarisYazici commented Aug 12, 2024

BarisYazici Aug 12, 2024

sp-sophia-labs commented Aug 12, 2024

AndreasKuhner commented Aug 14, 2024

BarisYazici commented Aug 14, 2024

sp-sophia-labs commented Aug 14, 2024 •

edited

Loading

sp-sophia-labs commented Aug 19, 2024

BarisYazici commented Aug 19, 2024 •

edited

Loading

BarisYazici Aug 12, 2024

BarisYazici Aug 12, 2024

BarisYazici Aug 12, 2024

BarisYazici Aug 12, 2024

AllanXuRan commented Jan 6, 2025

AndreasKuhner commented Jan 7, 2025

		@@ -0,0 +1,243 @@
		#include "franka_example_controllers/cartesian_impedance_example_controller.hpp"
		#include "franka_example_controllers/pseudo_inverse.hpp"

	type: franka_example_controllers::CartesianImpedanceExampleController
	type: franka_example_controllers/CartesianImpedanceExampleController

Port cartesian_impedance_controller to franka_ros2 example controllers #51

Are you sure you want to change the base?

Port cartesian_impedance_controller to franka_ros2 example controllers #51

Conversation

sp-sophia-labs commented Mar 19, 2024 • edited Loading

sp-sophia-labs commented Mar 27, 2024

BarisYazici commented Jun 19, 2024

BarisYazici commented Jul 1, 2024 • edited Loading

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

sp-sophia-labs commented Jul 3, 2024

sp-sophia-labs commented Aug 5, 2024

AndreasKuhner commented Aug 9, 2024

BarisYazici commented Aug 12, 2024

BarisYazici commented Aug 12, 2024

Choose a reason for hiding this comment

sp-sophia-labs commented Aug 12, 2024

AndreasKuhner commented Aug 14, 2024

BarisYazici commented Aug 14, 2024

sp-sophia-labs commented Aug 14, 2024 • edited Loading

sp-sophia-labs commented Aug 19, 2024

BarisYazici commented Aug 19, 2024 • edited Loading

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

AllanXuRan commented Jan 6, 2025

AndreasKuhner commented Jan 7, 2025

sp-sophia-labs commented Mar 19, 2024 •

edited

Loading

BarisYazici commented Jul 1, 2024 •

edited

Loading

sp-sophia-labs commented Aug 14, 2024 •

edited

Loading

BarisYazici commented Aug 19, 2024 •

edited

Loading