From c8ed063a3ca6e9ffb99f4ddbe9c897ce003de05a Mon Sep 17 00:00:00 2001 From: Zakaria Bouzit Date: Mon, 18 Mar 2024 15:44:07 +0100 Subject: [PATCH 01/11] import cartesian_impedance_controller to franka_ros2 example controllers --- .../franka_example_controllers.xml | 6 + ...cartesian_impedance_example_controller.hpp | 104 ++++++++ .../pseudo_inversion.hpp | 20 ++ ...cartesian_impedance_example_controller.cpp | 247 ++++++++++++++++++ 4 files changed, 377 insertions(+) create mode 100644 franka_example_controllers/include/franka_example_controllers/cartesian_impedance_example_controller.hpp create mode 100644 franka_example_controllers/include/franka_example_controllers/pseudo_inversion.hpp create mode 100644 franka_example_controllers/src/cartesian_impedance_example_controller.cpp diff --git a/franka_example_controllers/franka_example_controllers.xml b/franka_example_controllers/franka_example_controllers.xml index a144070a..cf79da41 100644 --- a/franka_example_controllers/franka_example_controllers.xml +++ b/franka_example_controllers/franka_example_controllers.xml @@ -71,4 +71,10 @@ The franka model example controller evaluates and prints the dynamic model ofFranka Robotics Robots. + + + A controller that renders a spring damper system in cartesian space. Compliance parameters and the equilibrium pose can be modified online with dynamic reconfigure and an interactive marker respectively. + + diff --git a/franka_example_controllers/include/franka_example_controllers/cartesian_impedance_example_controller.hpp b/franka_example_controllers/include/franka_example_controllers/cartesian_impedance_example_controller.hpp new file mode 100644 index 00000000..f27d7164 --- /dev/null +++ b/franka_example_controllers/include/franka_example_controllers/cartesian_impedance_example_controller.hpp @@ -0,0 +1,104 @@ +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include + +#include "rclcpp/rclcpp.hpp" +#include "rclcpp/node.hpp" +#include +#include "rclcpp_action/server.hpp" +#include "rclcpp_action/rclcpp_action.hpp" +#include "geometry_msgs/msg/pose_stamped.hpp" +#include "sensor_msgs/msg/joint_state.hpp" +#include "control_msgs/action/follow_joint_trajectory.hpp" + +#include + +#include "hardware_interface/types/hardware_interface_type_values.hpp" +#include "hardware_interface/types/hardware_interface_return_values.hpp" +#include "hardware_interface/macros.hpp" +#include +#include +#include + +#include "franka_hardware/franka_hardware_interface.hpp" +#include "franka_semantic_components/franka_robot_model.hpp" +#include "franka_semantic_components/franka_robot_state.hpp" +#include "franka_semantic_components/franka_cartesian_pose_interface.hpp" +#include +#include + +#include "franka_example_controllers/visibility_control.h" + + +#define IDENTITY Eigen::MatrixXd::Identity(6,6) + +namespace franka_example_controllers +{ +class CartesianImpedanceExampleController : public controller_interface::ControllerInterface +{ + public: + using CallbackReturn = rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn; + + // Config methods + [[nodiscard]] controller_interface::InterfaceConfiguration command_interface_configuration() const override; + [[nodiscard]] controller_interface::InterfaceConfiguration state_interface_configuration() const override; + + // ROS2 lifecycle related methods + 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; + + // Main real-time method + controller_interface::return_type update(const rclcpp::Time & time, const rclcpp::Duration & period) override; + + + private: + std::unique_ptr franka_robot_model_; + std::unique_ptr franka_robot_state_; + + bool k_elbow_activated{true}; + + franka_msgs::msg::FrankaRobotState robot_state_, init_robot_state_; + + const std::string k_robot_state_interface_name{"robot_state"}; + const std::string k_robot_model_interface_name{"robot_model"}; + + std::string arm_id_{"fr3"}; + int num_joints{7}; + + // Saturation + Eigen::Matrix saturateTorqueRate( + const Eigen::Matrix& tau_d_calculated, + const Eigen::Matrix& tau_J_d); + + // Classic cartesian controller + double filter_params_{0.005}; + double nullspace_stiffness_{20.0}; + double nullspace_stiffness_target_{20.0}; + const double delta_tau_max_{1.0}; + + Eigen::Matrix cartesian_stiffness_; + Eigen::Matrix cartesian_stiffness_target_; + Eigen::Matrix cartesian_damping_; + Eigen::Matrix cartesian_damping_target_; + Eigen::Matrix q_d_nullspace_; + Eigen::Vector3d position_d_; + Eigen::Quaterniond orientation_d_; + std::mutex position_and_orientation_d_target_mutex_; + Eigen::Vector3d position_d_target_; + Eigen::Quaterniond orientation_d_target_; + + rclcpp::Subscription::SharedPtr sub_equilibrium_pose_; + void equilibriumPoseCallback(const geometry_msgs::msg::PoseStamped::SharedPtr msg); +}; + +} + \ No newline at end of file diff --git a/franka_example_controllers/include/franka_example_controllers/pseudo_inversion.hpp b/franka_example_controllers/include/franka_example_controllers/pseudo_inversion.hpp new file mode 100644 index 00000000..bc16644b --- /dev/null +++ b/franka_example_controllers/include/franka_example_controllers/pseudo_inversion.hpp @@ -0,0 +1,20 @@ +#pragma once +#include + +namespace franka_example_controllers { + +inline void pseudoInverse(const Eigen::MatrixXd& M_, Eigen::MatrixXd& M_pinv_, bool damped = true) { + double lambda_ = damped ? 0.2 : 0.0; + + Eigen::JacobiSVD svd(M_, Eigen::ComputeFullU | Eigen::ComputeFullV); + Eigen::JacobiSVD::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_); + + M_pinv_ = Eigen::MatrixXd(svd.matrixV() * S_.transpose() * svd.matrixU().transpose()); +} + +} \ No newline at end of file diff --git a/franka_example_controllers/src/cartesian_impedance_example_controller.cpp b/franka_example_controllers/src/cartesian_impedance_example_controller.cpp new file mode 100644 index 00000000..34acb09f --- /dev/null +++ b/franka_example_controllers/src/cartesian_impedance_example_controller.cpp @@ -0,0 +1,247 @@ +#include "franka_example_controllers/cartesian_impedance_example_controller.hpp" +#include "franka_example_controllers/pseudo_inverse.hpp" + +#include +#include +#include + +#include +#include + +namespace franka_example_controllers +{ + + CallbackReturn CartesianImpedanceExampleController::on_init() + { + // Equilibrium pose subscription + sub_equilibrium_pose_ = get_node()->create_subscription( + "equilibrium_pose", 20, + std::bind(&FSMImpedanceController::equilibriumPoseCallback, this, std::placeholders::_1)); + + position_d_.setZero(); + orientation_d_.coeffs() << 0.0, 0.0, 0.0, 1.0; + position_d_target_.setZero(); + orientation_d_target_.coeffs() << 0.0, 0.0, 0.0, 1.0; + + // Compliance parameters + const double translational_stiffness{150.0}; + const double rotational_stiffness{10.0}; + cartesian_stiffness_.setZero(); + cartesian_stiffness_.topLeftCorner(3, 3) << translational_stiffness * Eigen::MatrixXd::Identity(3, 3); + cartesian_stiffness_.bottomRightCorner(3, 3) << rotational_stiffness * Eigen::MatrixXd::Identity(3, 3); + cartesian_damping_.setZero(); + cartesian_damping_.topLeftCorner(3, 3) << 2.0 * sqrt(translational_stiffness) * + Eigen::MatrixXd::Identity(3, 3); + cartesian_damping_.bottomRightCorner(3, 3) << 2.0 * sqrt(rotational_stiffness) * + Eigen::MatrixXd::Identity(3, 3); + return CallbackReturn::SUCCESS; + } + + controller_interface::InterfaceConfiguration CartesianImpedanceExampleController::command_interface_configuration() const + { + // Define command interfaces + controller_interface::InterfaceConfiguration command_interfaces_config; + command_interfaces_config.type = controller_interface::interface_configuration_type::INDIVIDUAL; + + // get command config + for (int i = 1; i <= num_joints; i++) { + command_interfaces_config.names.push_back(arm_id_ + "_joint" + std::to_string(i) + "/effort"); + } + return command_interfaces_config; + } + + controller_interface::InterfaceConfiguration CartesianImpedanceExampleController::state_interface_configuration()const + { + // Define state interfaces + controller_interface::InterfaceConfiguration state_interfaces_config; + state_interfaces_config.type = controller_interface::interface_configuration_type::INDIVIDUAL; + + // Creates state interface for robot state + for (const auto& franka_robot_model_name : franka_robot_model_->get_state_interface_names()) { + state_interfaces_config.names.push_back(franka_robot_model_name); + } + // Creates state interface for robot model + for (const auto& franka_robot_state_name : franka_robot_state_->get_state_interface_names()) { + state_interfaces_config.names.push_back(franka_robot_state_name); + } + return state_interfaces_config; + } + + CallbackReturn CartesianImpedanceExampleController::on_configure(const rclcpp_lifecycle::State& /*previous_state*/) + { + franka_robot_state_ = std::make_unique(franka_semantic_components::FrankaRobotState(arm_id_ + "/" + k_robot_state_interface_name)); + franka_robot_model_ = std::make_unique(franka_semantic_components::FrankaRobotModel(arm_id_ + "/" + k_robot_model_interface_name, arm_id_ + "/" + k_robot_state_interface_name)); + return CallbackReturn::SUCCESS; + } + + // In ROS 2, the equivalent functionality to the starting() method in ROS 1 is typically handled in the on_activate() method. + // The on_activate() method 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. + CallbackReturn CartesianImpedanceExampleController::on_activate(const rclcpp_lifecycle::State& /*previous_state*/) + { + franka_robot_state_->assign_loaned_state_interfaces(state_interfaces_); + franka_robot_model_->assign_loaned_state_interfaces(state_interfaces_); + + // Starting() + // compute initial velocity with jacobian and set x_attractor and q_d_nullspace + init_robot_state_ = franka_msgs::msg::FrankaRobotState(); + franka_robot_state_->get_values_as_message(init_robot_state_); + + Eigen::Map> q_initial(init_robot_state_.measured_joint_state.position.data()); + + Eigen::Vector3d position_init( + init_robot_state_.o_t_ee.pose.position.x, + init_robot_state_.o_t_ee.pose.position.y, + init_robot_state_.o_t_ee.pose.position.z); + Eigen::Quaterniond orientation_init( + init_robot_state_.o_t_ee.pose.orientation.w, + init_robot_state_.o_t_ee.pose.orientation.x, + init_robot_state_.o_t_ee.pose.orientation.y, + init_robot_state_.o_t_ee.pose.orientation.z); + Eigen::Affine3d initial_transform = Eigen::Affine3d::Identity(); + initial_transform.translation() = position_init; + initial_transform.rotate(orientation_init.toRotationMatrix()); + position_d_ = initial_transform.translation(); + orientation_d_ = initial_transform.rotation(); + position_d_target_ = initial_transform.translation(); + orientation_d_target_ = initial_transform.rotation(); + + q_d_nullspace_ = q_initial; + return CallbackReturn::SUCCESS; + } + + controller_interface::CallbackReturn CartesianImpedanceExampleController::on_deactivate(const rclcpp_lifecycle::State& /*previous_state*/) + { + franka_robot_state_->release_interfaces(); + franka_robot_model_->release_interfaces(); + return CallbackReturn::SUCCESS; + } + + + controller_interface::return_type CartesianImpedanceExampleController::update(const rclcpp::Time & time, const rclcpp::Duration & period) + { + // get state variables + robot_state_ = franka_msgs::msg::FrankaRobotState(); + franka_robot_state_->get_values_as_message(robot_state_); + + std::array mass = franka_robot_model_->getMassMatrix(); + std::array coriolis_array = franka_robot_model_->getCoriolisForceVector(); + std::array jacobian_array = franka_robot_model_->getZeroJacobian(franka::Frame::kEndEffector); + + Eigen::Map> M(mass.data()); + Eigen::Map> coriolis(coriolis_array.data()); + Eigen::Map> jacobian(jacobian_array.data()); + + Eigen::Map> q(robot_state_.measured_joint_state.position.data()); + Eigen::Map> dq(robot_state_.measured_joint_state.velocity.data()); + Eigen::Map> tau_J_d(robot_state_.measured_joint_state.effort.data()); + Eigen::Vector3d position( + robot_state_.o_t_ee.pose.position.x, + robot_state_.o_t_ee.pose.position.y, + robot_state_.o_t_ee.pose.position.z); + Eigen::Quaterniond orientation( + robot_state_.o_t_ee.pose.orientation.w, + robot_state_.o_t_ee.pose.orientation.x, + robot_state_.o_t_ee.pose.orientation.y, + robot_state_.o_t_ee.pose.orientation.z); + Eigen::Affine3d transform = Eigen::Affine3d::Identity(); + transform.translation() = position; + transform.rotate(orientation.toRotationMatrix()); + + // compute error to desired pose + // position error + Eigen::Matrix error; + error.head(3) << position - position_d_; + + // orientation error + if(orientation_d_.coeffs().dot(orientation.coeffs()) < 0.0) + { + orientation.coeffs() << -orientation.coeffs(); + } + + // "difference" quaternion + Eigen::Quaterniond error_quaternion(orientation.inverse() * orientation_d_); + error.tail(3) << error_quaternion.x(), error_quaternion.y(), error_quaternion.z(); + // Transform to base frame + error.tail(3) << -transform.rotation() * error.tail(3); + + // compute control + // allocate variables + Eigen::VectorXd tau_task(7), tau_nullspace(7), tau_d(7); + + // pseudoinverse for nullspace handling + // kinematic pseuoinverse + Eigen::MatrixXd jacobian_transpose_pinv; + + try { + // Release Build : colcon build --cmake-args -DCMAKE_BUILD_TYPE=Release + pseudoInverse(jacobian.transpose(), jacobian_transpose_pinv); + + } catch (const std::exception& e) { + std::cerr << "Error during pseudo-inverse computation: " << e.what() << std::endl; + } + + // Cartesian PD control with damping ratio = 1 + tau_task << jacobian.transpose() * + (-cartesian_stiffness_ * error - cartesian_damping_ * (jacobian * dq)); + // nullspace PD control with damping ratio = 1 + tau_nullspace << (Eigen::MatrixXd::Identity(7, 7) - + jacobian.transpose()* jacobian_transpose_pinv) * + (nullspace_stiffness_ * (q_d_nullspace_ - q) - + (2.0 * sqrt(nullspace_stiffness_)) * dq); + + // Desired torque + tau_d << tau_task + coriolis; + + for (int i = 0; i < num_joints; i++) { + command_interfaces_[i].set_value(tau_d[i]); + std::cout< position_d_target_mutex_lock( + position_and_orientation_d_target_mutex_); + position_d_ = filter_params_ * position_d_target_ + (1.0 - filter_params_) * position_d_; + orientation_d_ = orientation_d_.slerp(filter_params_, orientation_d_target_); + + return controller_interface::return_type::OK; + } + + void CartesianImpedanceExampleController::equilibriumPoseCallback(const geometry_msgs::msg::PoseStamped::SharedPtr msg) + { + std::lock_guard position_d_target_mutex_lock(position_and_orientation_d_target_mutex_); + position_d_target_ << msg->pose.position.x, msg->pose.position.y, msg->pose.position.z; + Eigen::Quaterniond last_orientation_d_target(orientation_d_target_); + orientation_d_target_.coeffs() << msg->pose.orientation.x, msg->pose.orientation.y, msg->pose.orientation.z, msg->pose.orientation.w; + if (last_orientation_d_target.coeffs().dot(orientation_d_target_.coeffs()) < 0.0) { + orientation_d_target_.coeffs() << -orientation_d_target_.coeffs(); + } + } + + Eigen::Matrix CartesianImpedanceExampleController::saturateTorqueRate( + const Eigen::Matrix& tau_d_calculated, + const Eigen::Matrix& tau_J_d) { // NOLINT (readability-identifier-naming) + Eigen::Matrix tau_d_saturated{}; + for (size_t i = 0; i < 7; i++) { + double difference = tau_d_calculated[i] - tau_J_d[i]; + tau_d_saturated[i] = + tau_J_d[i] + std::max(std::min(difference, delta_tau_max_), -delta_tau_max_); + } + return tau_d_saturated; + } +} + +// Expose the controller as visible to the rest of ros2_control +#include "pluginlib/class_list_macros.hpp" + +PLUGINLIB_EXPORT_CLASS( + franka_example_controllers::CartesianImpedanceExampleController, + controller_interface::ControllerInterface +) \ No newline at end of file From 89c8aff2cffa81d5a18bdac4a55e78737d4acc20 Mon Sep 17 00:00:00 2001 From: Zakaria Bouzit Date: Tue, 19 Mar 2024 10:23:11 +0100 Subject: [PATCH 02/11] clean up headers --- .../cartesian_impedance_example_controller.hpp | 18 +++--------------- .../pseudo_inversion.hpp | 3 ++- .../cartesian_impedance_example_controller.cpp | 7 ------- 3 files changed, 5 insertions(+), 23 deletions(-) diff --git a/franka_example_controllers/include/franka_example_controllers/cartesian_impedance_example_controller.hpp b/franka_example_controllers/include/franka_example_controllers/cartesian_impedance_example_controller.hpp index f27d7164..3597df4b 100644 --- a/franka_example_controllers/include/franka_example_controllers/cartesian_impedance_example_controller.hpp +++ b/franka_example_controllers/include/franka_example_controllers/cartesian_impedance_example_controller.hpp @@ -1,10 +1,12 @@ +#pragma once + #include #include #include #include #include #include -#include +#include #include #include #include @@ -12,25 +14,11 @@ #include "rclcpp/rclcpp.hpp" #include "rclcpp/node.hpp" #include -#include "rclcpp_action/server.hpp" -#include "rclcpp_action/rclcpp_action.hpp" #include "geometry_msgs/msg/pose_stamped.hpp" -#include "sensor_msgs/msg/joint_state.hpp" -#include "control_msgs/action/follow_joint_trajectory.hpp" #include - -#include "hardware_interface/types/hardware_interface_type_values.hpp" -#include "hardware_interface/types/hardware_interface_return_values.hpp" -#include "hardware_interface/macros.hpp" -#include -#include -#include - -#include "franka_hardware/franka_hardware_interface.hpp" #include "franka_semantic_components/franka_robot_model.hpp" #include "franka_semantic_components/franka_robot_state.hpp" -#include "franka_semantic_components/franka_cartesian_pose_interface.hpp" #include #include diff --git a/franka_example_controllers/include/franka_example_controllers/pseudo_inversion.hpp b/franka_example_controllers/include/franka_example_controllers/pseudo_inversion.hpp index bc16644b..04ff9908 100644 --- a/franka_example_controllers/include/franka_example_controllers/pseudo_inversion.hpp +++ b/franka_example_controllers/include/franka_example_controllers/pseudo_inversion.hpp @@ -1,5 +1,6 @@ #pragma once -#include + +#include namespace franka_example_controllers { diff --git a/franka_example_controllers/src/cartesian_impedance_example_controller.cpp b/franka_example_controllers/src/cartesian_impedance_example_controller.cpp index 34acb09f..18cc4755 100644 --- a/franka_example_controllers/src/cartesian_impedance_example_controller.cpp +++ b/franka_example_controllers/src/cartesian_impedance_example_controller.cpp @@ -1,13 +1,6 @@ #include "franka_example_controllers/cartesian_impedance_example_controller.hpp" #include "franka_example_controllers/pseudo_inverse.hpp" -#include -#include -#include - -#include -#include - namespace franka_example_controllers { From edcf1784edd048c9434cbfa753c64f5b479054ac Mon Sep 17 00:00:00 2001 From: Zakaria Bouzit Date: Tue, 19 Mar 2024 10:27:56 +0100 Subject: [PATCH 03/11] saturation --- .../src/cartesian_impedance_example_controller.cpp | 5 ++++- 1 file changed, 4 insertions(+), 1 deletion(-) diff --git a/franka_example_controllers/src/cartesian_impedance_example_controller.cpp b/franka_example_controllers/src/cartesian_impedance_example_controller.cpp index 18cc4755..386959bf 100644 --- a/franka_example_controllers/src/cartesian_impedance_example_controller.cpp +++ b/franka_example_controllers/src/cartesian_impedance_example_controller.cpp @@ -184,7 +184,10 @@ namespace franka_example_controllers (2.0 * sqrt(nullspace_stiffness_)) * dq); // Desired torque - tau_d << tau_task + coriolis; + tau_d << tau_task + tau_nullspace + coriolis; + + // saturate the commanded torque to joint limits + tau_d << saturateTorqueRate(tau_d, tau_j_d); for (int i = 0; i < num_joints; i++) { command_interfaces_[i].set_value(tau_d[i]); From a4fcfeca7c62aa9346076588252ff313743c271d Mon Sep 17 00:00:00 2001 From: Zakaria Bouzit Date: Wed, 3 Jul 2024 17:24:43 +0200 Subject: [PATCH 04/11] Cartesian Impedance Example Controller config --- franka_bringup/config/controllers.yaml | 4 ++++ 1 file changed, 4 insertions(+) diff --git a/franka_bringup/config/controllers.yaml b/franka_bringup/config/controllers.yaml index 4ca3d451..ec92b96a 100644 --- a/franka_bringup/config/controllers.yaml +++ b/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 + + joint_impedance_with_ik_example_controller: ros__parameters: arm_id: panda From 930623125c595ff7991e1df6cadf4a7973c6505f Mon Sep 17 00:00:00 2001 From: Zakaria Bouzit Date: Wed, 3 Jul 2024 17:26:30 +0200 Subject: [PATCH 05/11] clenup leftover & fix equilibriumPoseCallback call & pseudo inversion header --- ...cartesian_impedance_example_controller.cpp | 19 ++++--------------- 1 file changed, 4 insertions(+), 15 deletions(-) diff --git a/franka_example_controllers/src/cartesian_impedance_example_controller.cpp b/franka_example_controllers/src/cartesian_impedance_example_controller.cpp index 386959bf..598c50dc 100644 --- a/franka_example_controllers/src/cartesian_impedance_example_controller.cpp +++ b/franka_example_controllers/src/cartesian_impedance_example_controller.cpp @@ -1,5 +1,5 @@ #include "franka_example_controllers/cartesian_impedance_example_controller.hpp" -#include "franka_example_controllers/pseudo_inverse.hpp" +#include "franka_example_controllers/pseudo_inversion.hpp" namespace franka_example_controllers { @@ -9,7 +9,7 @@ namespace franka_example_controllers // Equilibrium pose subscription sub_equilibrium_pose_ = get_node()->create_subscription( "equilibrium_pose", 20, - std::bind(&FSMImpedanceController::equilibriumPoseCallback, this, std::placeholders::_1)); + std::bind(&CartesianImpedanceExampleController::equilibriumPoseCallback, this, std::placeholders::_1)); position_d_.setZero(); orientation_d_.coeffs() << 0.0, 0.0, 0.0, 1.0; @@ -67,15 +67,12 @@ namespace franka_example_controllers return CallbackReturn::SUCCESS; } - // In ROS 2, the equivalent functionality to the starting() method in ROS 1 is typically handled in the on_activate() method. - // The on_activate() method 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. + CallbackReturn CartesianImpedanceExampleController::on_activate(const rclcpp_lifecycle::State& /*previous_state*/) { franka_robot_state_->assign_loaned_state_interfaces(state_interfaces_); franka_robot_model_->assign_loaned_state_interfaces(state_interfaces_); - // Starting() // compute initial velocity with jacobian and set x_attractor and q_d_nullspace init_robot_state_ = franka_msgs::msg::FrankaRobotState(); franka_robot_state_->get_values_as_message(init_robot_state_); @@ -163,17 +160,9 @@ namespace franka_example_controllers Eigen::VectorXd tau_task(7), tau_nullspace(7), tau_d(7); // pseudoinverse for nullspace handling - // kinematic pseuoinverse Eigen::MatrixXd jacobian_transpose_pinv; + pseudoInverse(jacobian.transpose(), jacobian_transpose_pinv); - try { - // Release Build : colcon build --cmake-args -DCMAKE_BUILD_TYPE=Release - pseudoInverse(jacobian.transpose(), jacobian_transpose_pinv); - - } catch (const std::exception& e) { - std::cerr << "Error during pseudo-inverse computation: " << e.what() << std::endl; - } - // Cartesian PD control with damping ratio = 1 tau_task << jacobian.transpose() * (-cartesian_stiffness_ * error - cartesian_damping_ * (jacobian * dq)); From d6929be4eeb7b7c5058796743e83c595cad6217b Mon Sep 17 00:00:00 2001 From: Zakaria Bouzit Date: Wed, 3 Jul 2024 17:27:05 +0200 Subject: [PATCH 06/11] Cartesian Impedance Example launch file --- ...ian_impedance_example_controller.launch.py | 62 +++++++++++++++++++ 1 file changed, 62 insertions(+) create mode 100644 franka_bringup/launch/cartesian_impedance_example_controller.launch.py diff --git a/franka_bringup/launch/cartesian_impedance_example_controller.launch.py b/franka_bringup/launch/cartesian_impedance_example_controller.launch.py new file mode 100644 index 00000000..00efbffc --- /dev/null +++ b/franka_bringup/launch/cartesian_impedance_example_controller.launch.py @@ -0,0 +1,62 @@ +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(), + ), + + Node( + package='controller_manager', + executable='spawner', + arguments=['cartesian_impedance_example_controller'], + output='screen', + ), + ]) \ No newline at end of file From dc8973103d069ce42a8dfcc4109ea4d6ca00bda2 Mon Sep 17 00:00:00 2001 From: Zakaria Bouzit Date: Mon, 5 Aug 2024 10:59:43 +0200 Subject: [PATCH 07/11] return inverse instead of receiving parameter --- .../include/franka_example_controllers/pseudo_inversion.hpp | 6 +++--- 1 file changed, 3 insertions(+), 3 deletions(-) diff --git a/franka_example_controllers/include/franka_example_controllers/pseudo_inversion.hpp b/franka_example_controllers/include/franka_example_controllers/pseudo_inversion.hpp index 04ff9908..aa04e25e 100644 --- a/franka_example_controllers/include/franka_example_controllers/pseudo_inversion.hpp +++ b/franka_example_controllers/include/franka_example_controllers/pseudo_inversion.hpp @@ -4,7 +4,7 @@ namespace franka_example_controllers { -inline void pseudoInverse(const Eigen::MatrixXd& M_, Eigen::MatrixXd& M_pinv_, bool damped = true) { +inline Eigen::MatrixXd pseudoInverse(const Eigen::MatrixXd& M_, bool damped = true) { double lambda_ = damped ? 0.2 : 0.0; Eigen::JacobiSVD svd(M_, Eigen::ComputeFullU | Eigen::ComputeFullV); @@ -15,7 +15,7 @@ inline void pseudoInverse(const Eigen::MatrixXd& M_, Eigen::MatrixXd& M_pinv_, b for (int i = 0; i < sing_vals_.size(); i++) S_(i, i) = (sing_vals_(i)) / (sing_vals_(i) * sing_vals_(i) + lambda_ * lambda_); - M_pinv_ = Eigen::MatrixXd(svd.matrixV() * S_.transpose() * svd.matrixU().transpose()); + return svd.matrixV() * S_.transpose() * svd.matrixU().transpose(); } -} \ No newline at end of file +} // namespace franka_example_controllers \ No newline at end of file From bc5c07f1912015759b37bce217c28a363bccc95a Mon Sep 17 00:00:00 2001 From: Zakaria Bouzit Date: Mon, 5 Aug 2024 11:02:12 +0200 Subject: [PATCH 08/11] doc, variables and arm_id call fix --- ...cartesian_impedance_example_controller.hpp | 149 +++--- ...cartesian_impedance_example_controller.cpp | 431 +++++++++--------- 2 files changed, 299 insertions(+), 281 deletions(-) diff --git a/franka_example_controllers/include/franka_example_controllers/cartesian_impedance_example_controller.hpp b/franka_example_controllers/include/franka_example_controllers/cartesian_impedance_example_controller.hpp index 3597df4b..16be344e 100644 --- a/franka_example_controllers/include/franka_example_controllers/cartesian_impedance_example_controller.hpp +++ b/franka_example_controllers/include/franka_example_controllers/cartesian_impedance_example_controller.hpp @@ -1,92 +1,91 @@ #pragma once +#include +#include +#include +#include #include #include -#include -#include -#include #include -#include -#include +#include #include -#include +#include -#include "rclcpp/rclcpp.hpp" -#include "rclcpp/node.hpp" -#include +#include #include "geometry_msgs/msg/pose_stamped.hpp" +#include "rclcpp/node.hpp" +#include "rclcpp/rclcpp.hpp" -#include +#include +#include +#include #include "franka_semantic_components/franka_robot_model.hpp" #include "franka_semantic_components/franka_robot_state.hpp" -#include -#include #include "franka_example_controllers/visibility_control.h" +namespace franka_example_controllers { +class CartesianImpedanceExampleController : public controller_interface::ControllerInterface { + public: + using CallbackReturn = rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn; + + // Config methods + + [[nodiscard]] controller_interface::InterfaceConfiguration command_interface_configuration() + const override; + [[nodiscard]] controller_interface::InterfaceConfiguration state_interface_configuration() + const override; + + // ROS2 lifecycle related methods + 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; + + // Main real-time method + controller_interface::return_type update(const rclcpp::Time& time, + const rclcpp::Duration& period) override; + + private: + std::unique_ptr franka_robot_model_; + std::unique_ptr franka_robot_state_; + + bool k_elbow_activated{true}; + + franka_msgs::msg::FrankaRobotState robot_state_; + franka_msgs::msg::FrankaRobotState init_robot_state_; + + const std::string k_robot_state_interface_name{"robot_state"}; + const std::string k_robot_model_interface_name{"robot_model"}; + + std::string arm_id_; + int num_joints{7}; + + // Saturation + Eigen::Matrix saturateTorqueRate( + const Eigen::Matrix& tau_d_calculated, + const Eigen::Matrix& tau_J_d); + + // Classic cartesian controller + double filter_params_{0.005}; + double nullspace_stiffness_{20.0}; + double nullspace_stiffness_target_{20.0}; + const double delta_tau_max_{1.0}; + + Eigen::Matrix cartesian_stiffness_; + Eigen::Matrix cartesian_stiffness_target_; + Eigen::Matrix cartesian_damping_; + Eigen::Matrix cartesian_damping_target_; + Eigen::Matrix q_d_nullspace_; + Eigen::Vector3d position_d_; + Eigen::Quaterniond orientation_d_; + std::mutex position_and_orientation_d_target_mutex_; + Eigen::Vector3d position_d_target_; + Eigen::Quaterniond orientation_d_target_; + + rclcpp::Subscription::SharedPtr sub_equilibrium_pose_; -#define IDENTITY Eigen::MatrixXd::Identity(6,6) - -namespace franka_example_controllers -{ -class CartesianImpedanceExampleController : public controller_interface::ControllerInterface -{ - public: - using CallbackReturn = rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn; - - // Config methods - [[nodiscard]] controller_interface::InterfaceConfiguration command_interface_configuration() const override; - [[nodiscard]] controller_interface::InterfaceConfiguration state_interface_configuration() const override; - - // ROS2 lifecycle related methods - 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; - - // Main real-time method - controller_interface::return_type update(const rclcpp::Time & time, const rclcpp::Duration & period) override; - - - private: - std::unique_ptr franka_robot_model_; - std::unique_ptr franka_robot_state_; - - bool k_elbow_activated{true}; - - franka_msgs::msg::FrankaRobotState robot_state_, init_robot_state_; - - const std::string k_robot_state_interface_name{"robot_state"}; - const std::string k_robot_model_interface_name{"robot_model"}; - - std::string arm_id_{"fr3"}; - int num_joints{7}; - - // Saturation - Eigen::Matrix saturateTorqueRate( - const Eigen::Matrix& tau_d_calculated, - const Eigen::Matrix& tau_J_d); - - // Classic cartesian controller - double filter_params_{0.005}; - double nullspace_stiffness_{20.0}; - double nullspace_stiffness_target_{20.0}; - const double delta_tau_max_{1.0}; - - Eigen::Matrix cartesian_stiffness_; - Eigen::Matrix cartesian_stiffness_target_; - Eigen::Matrix cartesian_damping_; - Eigen::Matrix cartesian_damping_target_; - Eigen::Matrix q_d_nullspace_; - Eigen::Vector3d position_d_; - Eigen::Quaterniond orientation_d_; - std::mutex position_and_orientation_d_target_mutex_; - Eigen::Vector3d position_d_target_; - Eigen::Quaterniond orientation_d_target_; - - rclcpp::Subscription::SharedPtr sub_equilibrium_pose_; - void equilibriumPoseCallback(const geometry_msgs::msg::PoseStamped::SharedPtr msg); + void equilibriumPoseCallback(const geometry_msgs::msg::PoseStamped::SharedPtr msg); }; -} - \ No newline at end of file +} // namespace franka_example_controllers diff --git a/franka_example_controllers/src/cartesian_impedance_example_controller.cpp b/franka_example_controllers/src/cartesian_impedance_example_controller.cpp index 598c50dc..378bde61 100644 --- a/franka_example_controllers/src/cartesian_impedance_example_controller.cpp +++ b/franka_example_controllers/src/cartesian_impedance_example_controller.cpp @@ -1,232 +1,251 @@ #include "franka_example_controllers/cartesian_impedance_example_controller.hpp" #include "franka_example_controllers/pseudo_inversion.hpp" -namespace franka_example_controllers -{ - - CallbackReturn CartesianImpedanceExampleController::on_init() - { - // Equilibrium pose subscription - sub_equilibrium_pose_ = get_node()->create_subscription( - "equilibrium_pose", 20, - std::bind(&CartesianImpedanceExampleController::equilibriumPoseCallback, this, std::placeholders::_1)); - - position_d_.setZero(); - orientation_d_.coeffs() << 0.0, 0.0, 0.0, 1.0; - position_d_target_.setZero(); - orientation_d_target_.coeffs() << 0.0, 0.0, 0.0, 1.0; - - // Compliance parameters - const double translational_stiffness{150.0}; - const double rotational_stiffness{10.0}; - cartesian_stiffness_.setZero(); - cartesian_stiffness_.topLeftCorner(3, 3) << translational_stiffness * Eigen::MatrixXd::Identity(3, 3); - cartesian_stiffness_.bottomRightCorner(3, 3) << rotational_stiffness * Eigen::MatrixXd::Identity(3, 3); - cartesian_damping_.setZero(); - cartesian_damping_.topLeftCorner(3, 3) << 2.0 * sqrt(translational_stiffness) * - Eigen::MatrixXd::Identity(3, 3); - cartesian_damping_.bottomRightCorner(3, 3) << 2.0 * sqrt(rotational_stiffness) * - Eigen::MatrixXd::Identity(3, 3); - return CallbackReturn::SUCCESS; - } +const Eigen::MatrixXd IDENTITY = Eigen::MatrixXd::Identity(6, 6); + +namespace franka_example_controllers { - controller_interface::InterfaceConfiguration CartesianImpedanceExampleController::command_interface_configuration() const - { - // Define command interfaces - controller_interface::InterfaceConfiguration command_interfaces_config; - command_interfaces_config.type = controller_interface::interface_configuration_type::INDIVIDUAL; - - // get command config - for (int i = 1; i <= num_joints; i++) { - command_interfaces_config.names.push_back(arm_id_ + "_joint" + std::to_string(i) + "/effort"); - } - return command_interfaces_config; +CallbackReturn CartesianImpedanceExampleController::on_init() { + try { + auto_declare("arm_id", "fr3"); + } catch (const std::exception& e) { + fprintf(stderr, "Exception thrown during init stage with message: %s \n", e.what()); + return CallbackReturn::ERROR; } + // Equilibrium pose subscription + sub_equilibrium_pose_ = get_node()->create_subscription( + "equilibrium_pose", 20, + std::bind(&CartesianImpedanceExampleController::equilibriumPoseCallback, this, + std::placeholders::_1)); + + position_d_.setZero(); + orientation_d_.coeffs() << 0.0, 0.0, 0.0, 1.0; + position_d_target_.setZero(); + orientation_d_target_.coeffs() << 0.0, 0.0, 0.0, 1.0; + + // Compliance parameters + const double translational_stiffness{150.0}; + const double rotational_stiffness{10.0}; + cartesian_stiffness_.setZero(); + cartesian_stiffness_.topLeftCorner(3, 3) + << translational_stiffness * Eigen::MatrixXd::Identity(3, 3); + cartesian_stiffness_.bottomRightCorner(3, 3) + << rotational_stiffness * Eigen::MatrixXd::Identity(3, 3); + cartesian_damping_.setZero(); + cartesian_damping_.topLeftCorner(3, 3) + << 2.0 * sqrt(translational_stiffness) * Eigen::MatrixXd::Identity(3, 3); + cartesian_damping_.bottomRightCorner(3, 3) + << 2.0 * sqrt(rotational_stiffness) * Eigen::MatrixXd::Identity(3, 3); + return CallbackReturn::SUCCESS; +} - controller_interface::InterfaceConfiguration CartesianImpedanceExampleController::state_interface_configuration()const - { - // Define state interfaces - controller_interface::InterfaceConfiguration state_interfaces_config; - state_interfaces_config.type = controller_interface::interface_configuration_type::INDIVIDUAL; - - // Creates state interface for robot state - for (const auto& franka_robot_model_name : franka_robot_model_->get_state_interface_names()) { - state_interfaces_config.names.push_back(franka_robot_model_name); - } - // Creates state interface for robot model - for (const auto& franka_robot_state_name : franka_robot_state_->get_state_interface_names()) { - state_interfaces_config.names.push_back(franka_robot_state_name); - } - return state_interfaces_config; +controller_interface::InterfaceConfiguration +CartesianImpedanceExampleController::command_interface_configuration() const { + // Define command interfaces + controller_interface::InterfaceConfiguration command_interfaces_config; + command_interfaces_config.type = controller_interface::interface_configuration_type::INDIVIDUAL; + + // get command config + for (int i = 1; i <= num_joints; i++) { + command_interfaces_config.names.push_back(arm_id_ + "_joint" + std::to_string(i) + "/effort"); } + return command_interfaces_config; +} - CallbackReturn CartesianImpedanceExampleController::on_configure(const rclcpp_lifecycle::State& /*previous_state*/) - { - franka_robot_state_ = std::make_unique(franka_semantic_components::FrankaRobotState(arm_id_ + "/" + k_robot_state_interface_name)); - franka_robot_model_ = std::make_unique(franka_semantic_components::FrankaRobotModel(arm_id_ + "/" + k_robot_model_interface_name, arm_id_ + "/" + k_robot_state_interface_name)); - return CallbackReturn::SUCCESS; +controller_interface::InterfaceConfiguration +CartesianImpedanceExampleController::state_interface_configuration() const { + // Define state interfaces + controller_interface::InterfaceConfiguration state_interfaces_config; + state_interfaces_config.type = controller_interface::interface_configuration_type::INDIVIDUAL; + + // Creates state interface for robot state + for (const auto& franka_robot_model_name : franka_robot_model_->get_state_interface_names()) { + state_interfaces_config.names.push_back(franka_robot_model_name); + } + // Creates state interface for robot model + for (const auto& franka_robot_state_name : franka_robot_state_->get_state_interface_names()) { + state_interfaces_config.names.push_back(franka_robot_state_name); } + return state_interfaces_config; +} + +CallbackReturn CartesianImpedanceExampleController::on_configure( + const rclcpp_lifecycle::State& /*previous_state*/) { + arm_id_ = get_node()->get_parameter("arm_id").as_string(); + franka_robot_state_ = std::make_unique( + franka_semantic_components::FrankaRobotState(arm_id_ + "/" + k_robot_state_interface_name)); + franka_robot_model_ = std::make_unique( + franka_semantic_components::FrankaRobotModel(arm_id_ + "/" + k_robot_model_interface_name, + arm_id_ + "/" + k_robot_state_interface_name)); + return CallbackReturn::SUCCESS; +} +CallbackReturn CartesianImpedanceExampleController::on_activate( + const rclcpp_lifecycle::State& /*previous_state*/) { + franka_robot_state_->assign_loaned_state_interfaces(state_interfaces_); + franka_robot_model_->assign_loaned_state_interfaces(state_interfaces_); + + // compute initial velocity with jacobian and set x_attractor and q_d_nullspace + init_robot_state_ = franka_msgs::msg::FrankaRobotState(); + franka_robot_state_->get_values_as_message(init_robot_state_); + + Eigen::Map> q_initial( + init_robot_state_.measured_joint_state.position.data()); + + Eigen::Vector3d position_init(init_robot_state_.o_t_ee.pose.position.x, + init_robot_state_.o_t_ee.pose.position.y, + init_robot_state_.o_t_ee.pose.position.z); + Eigen::Quaterniond orientation_init( + init_robot_state_.o_t_ee.pose.orientation.w, init_robot_state_.o_t_ee.pose.orientation.x, + init_robot_state_.o_t_ee.pose.orientation.y, init_robot_state_.o_t_ee.pose.orientation.z); + Eigen::Affine3d initial_transform = Eigen::Affine3d::Identity(); + initial_transform.translation() = position_init; + initial_transform.rotate(orientation_init.toRotationMatrix()); + position_d_ = initial_transform.translation(); + orientation_d_ = initial_transform.rotation(); + position_d_target_ = initial_transform.translation(); + orientation_d_target_ = initial_transform.rotation(); + + q_d_nullspace_ = q_initial; + return CallbackReturn::SUCCESS; +} - CallbackReturn CartesianImpedanceExampleController::on_activate(const rclcpp_lifecycle::State& /*previous_state*/) - { - franka_robot_state_->assign_loaned_state_interfaces(state_interfaces_); - franka_robot_model_->assign_loaned_state_interfaces(state_interfaces_); - - // compute initial velocity with jacobian and set x_attractor and q_d_nullspace - init_robot_state_ = franka_msgs::msg::FrankaRobotState(); - franka_robot_state_->get_values_as_message(init_robot_state_); - - Eigen::Map> q_initial(init_robot_state_.measured_joint_state.position.data()); - - Eigen::Vector3d position_init( - init_robot_state_.o_t_ee.pose.position.x, - init_robot_state_.o_t_ee.pose.position.y, - init_robot_state_.o_t_ee.pose.position.z); - Eigen::Quaterniond orientation_init( - init_robot_state_.o_t_ee.pose.orientation.w, - init_robot_state_.o_t_ee.pose.orientation.x, - init_robot_state_.o_t_ee.pose.orientation.y, - init_robot_state_.o_t_ee.pose.orientation.z); - Eigen::Affine3d initial_transform = Eigen::Affine3d::Identity(); - initial_transform.translation() = position_init; - initial_transform.rotate(orientation_init.toRotationMatrix()); - position_d_ = initial_transform.translation(); - orientation_d_ = initial_transform.rotation(); - position_d_target_ = initial_transform.translation(); - orientation_d_target_ = initial_transform.rotation(); - - q_d_nullspace_ = q_initial; - return CallbackReturn::SUCCESS; +controller_interface::CallbackReturn CartesianImpedanceExampleController::on_deactivate( + const rclcpp_lifecycle::State& /*previous_state*/) { + franka_robot_state_->release_interfaces(); + franka_robot_model_->release_interfaces(); + return CallbackReturn::SUCCESS; +} + +controller_interface::return_type CartesianImpedanceExampleController::update( + const rclcpp::Time& time, + const rclcpp::Duration& period) { + // get state variables + robot_state_ = franka_msgs::msg::FrankaRobotState(); + franka_robot_state_->get_values_as_message(robot_state_); + + std::array mass = franka_robot_model_->getMassMatrix(); + std::array coriolis_array = franka_robot_model_->getCoriolisForceVector(); + std::array jacobian_array = + franka_robot_model_->getZeroJacobian(franka::Frame::kEndEffector); + + Eigen::Map> M(mass.data()); + Eigen::Map> coriolis(coriolis_array.data()); + Eigen::Map> jacobian(jacobian_array.data()); + + Eigen::Map> q(robot_state_.measured_joint_state.position.data()); + Eigen::Map> dq(robot_state_.measured_joint_state.velocity.data()); + Eigen::Map> tau_J_d(robot_state_.measured_joint_state.effort.data()); + Eigen::Vector3d position(robot_state_.o_t_ee.pose.position.x, robot_state_.o_t_ee.pose.position.y, + robot_state_.o_t_ee.pose.position.z); + Eigen::Quaterniond orientation( + robot_state_.o_t_ee.pose.orientation.w, robot_state_.o_t_ee.pose.orientation.x, + robot_state_.o_t_ee.pose.orientation.y, robot_state_.o_t_ee.pose.orientation.z); + Eigen::Affine3d transform = Eigen::Affine3d::Identity(); + transform.translation() = position; + transform.rotate(orientation.toRotationMatrix()); + + // compute error to desired pose + // position error + Eigen::Matrix position_error; + position_error.head(3) << position - position_d_; + + // orientation error + if (orientation_d_.coeffs().dot(orientation.coeffs()) < 0.0) { + orientation.coeffs() << -orientation.coeffs(); } - controller_interface::CallbackReturn CartesianImpedanceExampleController::on_deactivate(const rclcpp_lifecycle::State& /*previous_state*/) - { - franka_robot_state_->release_interfaces(); - franka_robot_model_->release_interfaces(); - return CallbackReturn::SUCCESS; + // "difference" quaternion + Eigen::Quaterniond error_quaternion(orientation.inverse() * orientation_d_); + position_error.tail(3) << error_quaternion.x(), error_quaternion.y(), error_quaternion.z(); + // Transform to base frame + position_error.tail(3) << -transform.rotation() * position_error.tail(3); + + // compute control + // allocate variables + Eigen::VectorXd tau_task(7), tau_nullspace(7), tau_d(7); + + // pseudoinverse for nullspace handling + Eigen::MatrixXd jacobian_transpose_pinv; + pseudoInverse(jacobian.transpose(), jacobian_transpose_pinv); + + // Cartesian PD control with damping ratio = 1 + tau_task << jacobian.transpose() * + (-cartesian_stiffness_ * position_error - cartesian_damping_ * (jacobian * dq)); + // nullspace PD control with damping ratio = 1 + tau_nullspace << (Eigen::MatrixXd::Identity(7, 7) - + jacobian.transpose() * jacobian_transpose_pinv) * + (nullspace_stiffness_ * (q_d_nullspace_ - q) - + (2.0 * sqrt(nullspace_stiffness_)) * dq); + + // Desired torque + tau_d << tau_task + tau_nullspace + coriolis; + + // saturate the commanded torque to joint limits + tau_d << saturateTorqueRate(tau_d, tau_j_d); + + for (int i = 0; i < num_joints; i++) { + command_interfaces_[i].set_value(tau_d[i]); + std::cout << tau_d[i] << std::endl; } + // update parameters changed online either through dynamic reconfigure or through the interactive + // target by filtering + cartesian_stiffness_ = + filter_params_ * cartesian_stiffness_target_ + (1.0 - filter_params_) * cartesian_stiffness_; + cartesian_damping_ = + filter_params_ * cartesian_damping_target_ + (1.0 - filter_params_) * cartesian_damping_; + nullspace_stiffness_ = + filter_params_ * nullspace_stiffness_target_ + (1.0 - filter_params_) * nullspace_stiffness_; - controller_interface::return_type CartesianImpedanceExampleController::update(const rclcpp::Time & time, const rclcpp::Duration & period) - { - // get state variables - robot_state_ = franka_msgs::msg::FrankaRobotState(); - franka_robot_state_->get_values_as_message(robot_state_); - - std::array mass = franka_robot_model_->getMassMatrix(); - std::array coriolis_array = franka_robot_model_->getCoriolisForceVector(); - std::array jacobian_array = franka_robot_model_->getZeroJacobian(franka::Frame::kEndEffector); - - Eigen::Map> M(mass.data()); - Eigen::Map> coriolis(coriolis_array.data()); - Eigen::Map> jacobian(jacobian_array.data()); - - Eigen::Map> q(robot_state_.measured_joint_state.position.data()); - Eigen::Map> dq(robot_state_.measured_joint_state.velocity.data()); - Eigen::Map> tau_J_d(robot_state_.measured_joint_state.effort.data()); - Eigen::Vector3d position( - robot_state_.o_t_ee.pose.position.x, - robot_state_.o_t_ee.pose.position.y, - robot_state_.o_t_ee.pose.position.z); - Eigen::Quaterniond orientation( - robot_state_.o_t_ee.pose.orientation.w, - robot_state_.o_t_ee.pose.orientation.x, - robot_state_.o_t_ee.pose.orientation.y, - robot_state_.o_t_ee.pose.orientation.z); - Eigen::Affine3d transform = Eigen::Affine3d::Identity(); - transform.translation() = position; - transform.rotate(orientation.toRotationMatrix()); - - // compute error to desired pose - // position error - Eigen::Matrix error; - error.head(3) << position - position_d_; - - // orientation error - if(orientation_d_.coeffs().dot(orientation.coeffs()) < 0.0) - { - orientation.coeffs() << -orientation.coeffs(); - } - - // "difference" quaternion - Eigen::Quaterniond error_quaternion(orientation.inverse() * orientation_d_); - error.tail(3) << error_quaternion.x(), error_quaternion.y(), error_quaternion.z(); - // Transform to base frame - error.tail(3) << -transform.rotation() * error.tail(3); - - // compute control - // allocate variables - Eigen::VectorXd tau_task(7), tau_nullspace(7), tau_d(7); - - // pseudoinverse for nullspace handling - Eigen::MatrixXd jacobian_transpose_pinv; - pseudoInverse(jacobian.transpose(), jacobian_transpose_pinv); - - // Cartesian PD control with damping ratio = 1 - tau_task << jacobian.transpose() * - (-cartesian_stiffness_ * error - cartesian_damping_ * (jacobian * dq)); - // nullspace PD control with damping ratio = 1 - tau_nullspace << (Eigen::MatrixXd::Identity(7, 7) - - jacobian.transpose()* jacobian_transpose_pinv) * - (nullspace_stiffness_ * (q_d_nullspace_ - q) - - (2.0 * sqrt(nullspace_stiffness_)) * dq); - - // Desired torque - tau_d << tau_task + tau_nullspace + coriolis; - - // saturate the commanded torque to joint limits - tau_d << saturateTorqueRate(tau_d, tau_j_d); - - for (int i = 0; i < num_joints; i++) { - command_interfaces_[i].set_value(tau_d[i]); - std::cout< position_d_target_mutex_lock( + std::lock_guard position_d_target_mutex_lock( position_and_orientation_d_target_mutex_); - position_d_ = filter_params_ * position_d_target_ + (1.0 - filter_params_) * position_d_; - orientation_d_ = orientation_d_.slerp(filter_params_, orientation_d_target_); - - return controller_interface::return_type::OK; - } + position_d_ = filter_params_ * position_d_target_ + (1.0 - filter_params_) * position_d_; + orientation_d_ = orientation_d_.slerp(filter_params_, orientation_d_target_); - void CartesianImpedanceExampleController::equilibriumPoseCallback(const geometry_msgs::msg::PoseStamped::SharedPtr msg) - { - std::lock_guard position_d_target_mutex_lock(position_and_orientation_d_target_mutex_); - position_d_target_ << msg->pose.position.x, msg->pose.position.y, msg->pose.position.z; - Eigen::Quaterniond last_orientation_d_target(orientation_d_target_); - orientation_d_target_.coeffs() << msg->pose.orientation.x, msg->pose.orientation.y, msg->pose.orientation.z, msg->pose.orientation.w; - if (last_orientation_d_target.coeffs().dot(orientation_d_target_.coeffs()) < 0.0) { - orientation_d_target_.coeffs() << -orientation_d_target_.coeffs(); - } + return controller_interface::return_type::OK; +} + +/** + * @brief Callback function to update the target equilibrium pose. + * + * This method listens to an equilibrium pose message of type `geometry_msgs::msg::PoseStamped` + * and updates the target position (`position_d_target_`) and orientation (`orientation_d_target_`) + * accordingly. These targets are used to update the desired position (`position_d_`) and + * orientation (`orientation_d_`), which the robot converges to for achieving equilibrium. + * + * @param msg Shared pointer to a `geometry_msgs::msg::PoseStamped` message containing the new + * target pose. + */ + +void CartesianImpedanceExampleController::equilibriumPoseCallback( + const geometry_msgs::msg::PoseStamped::SharedPtr msg) { + std::lock_guard position_d_target_mutex_lock( + position_and_orientation_d_target_mutex_); + position_d_target_ << msg->pose.position.x, msg->pose.position.y, msg->pose.position.z; + Eigen::Quaterniond last_orientation_d_target(orientation_d_target_); + orientation_d_target_.coeffs() << msg->pose.orientation.x, msg->pose.orientation.y, + msg->pose.orientation.z, msg->pose.orientation.w; + if (last_orientation_d_target.coeffs().dot(orientation_d_target_.coeffs()) < 0.0) { + orientation_d_target_.coeffs() << -orientation_d_target_.coeffs(); } +} - Eigen::Matrix CartesianImpedanceExampleController::saturateTorqueRate( - const Eigen::Matrix& tau_d_calculated, - const Eigen::Matrix& tau_J_d) { // NOLINT (readability-identifier-naming) - Eigen::Matrix tau_d_saturated{}; - for (size_t i = 0; i < 7; i++) { - double difference = tau_d_calculated[i] - tau_J_d[i]; - tau_d_saturated[i] = - tau_J_d[i] + std::max(std::min(difference, delta_tau_max_), -delta_tau_max_); - } - return tau_d_saturated; +Eigen::Matrix CartesianImpedanceExampleController::saturateTorqueRate( + const Eigen::Matrix& tau_d_calculated, + const Eigen::Matrix& tau_J_d) { // NOLINT (readability-identifier-naming) + Eigen::Matrix tau_d_saturated{}; + for (size_t i = 0; i < 7; i++) { + double difference = tau_d_calculated[i] - tau_J_d[i]; + tau_d_saturated[i] = + tau_J_d[i] + std::max(std::min(difference, delta_tau_max_), -delta_tau_max_); } + return tau_d_saturated; } +} // namespace franka_example_controllers // Expose the controller as visible to the rest of ros2_control #include "pluginlib/class_list_macros.hpp" -PLUGINLIB_EXPORT_CLASS( - franka_example_controllers::CartesianImpedanceExampleController, - controller_interface::ControllerInterface -) \ No newline at end of file +PLUGINLIB_EXPORT_CLASS(franka_example_controllers::CartesianImpedanceExampleController, + controller_interface::ControllerInterface) \ No newline at end of file From 1370f933ae4885b74f922a9fb658714e7b23e1ba Mon Sep 17 00:00:00 2001 From: Zakaria Bouzit Date: Tue, 6 Aug 2024 10:01:45 +0200 Subject: [PATCH 09/11] overlap clean identity --- .../src/cartesian_impedance_example_controller.cpp | 2 -- 1 file changed, 2 deletions(-) diff --git a/franka_example_controllers/src/cartesian_impedance_example_controller.cpp b/franka_example_controllers/src/cartesian_impedance_example_controller.cpp index 378bde61..e54507da 100644 --- a/franka_example_controllers/src/cartesian_impedance_example_controller.cpp +++ b/franka_example_controllers/src/cartesian_impedance_example_controller.cpp @@ -1,8 +1,6 @@ #include "franka_example_controllers/cartesian_impedance_example_controller.hpp" #include "franka_example_controllers/pseudo_inversion.hpp" -const Eigen::MatrixXd IDENTITY = Eigen::MatrixXd::Identity(6, 6); - namespace franka_example_controllers { CallbackReturn CartesianImpedanceExampleController::on_init() { From 1b3b4ffc4034f02bbfe4d3b56b22aa28913de250 Mon Sep 17 00:00:00 2001 From: Zakaria Bouzit Date: Mon, 12 Aug 2024 16:40:32 +0200 Subject: [PATCH 10/11] add executable and needed adjustement for local version --- ...artesian_impedance_example_controller.launch.py | 7 +++++++ franka_example_controllers/CMakeLists.txt | 1 + .../src/cartesian_impedance_example_controller.cpp | 14 ++++++++------ 3 files changed, 16 insertions(+), 6 deletions(-) diff --git a/franka_bringup/launch/cartesian_impedance_example_controller.launch.py b/franka_bringup/launch/cartesian_impedance_example_controller.launch.py index 00efbffc..0007f958 100644 --- a/franka_bringup/launch/cartesian_impedance_example_controller.launch.py +++ b/franka_bringup/launch/cartesian_impedance_example_controller.launch.py @@ -8,12 +8,14 @@ def generate_launch_description(): robot_ip_parameter_name = 'robot_ip' + arm_id_parameter_name = 'arm_id' 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) + arm_id = LaunchConfiguration(arm_id_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) @@ -23,6 +25,10 @@ def generate_launch_description(): DeclareLaunchArgument( robot_ip_parameter_name, description='Hostname or IP address of the robot.'), + DeclareLaunchArgument( + arm_id_parameter_name, + default_value='fr3', + description='ID of the type of arm used. Supported values: fer, fr3, fp3'), DeclareLaunchArgument( use_rviz_parameter_name, default_value='false', @@ -46,6 +52,7 @@ def generate_launch_description(): PythonLaunchDescriptionSource([PathJoinSubstitution( [FindPackageShare('franka_bringup'), 'launch', 'franka.launch.py'])]), launch_arguments={robot_ip_parameter_name: robot_ip, + arm_id_parameter_name: arm_id, load_gripper_parameter_name: load_gripper, use_fake_hardware_parameter_name: use_fake_hardware, fake_sensor_commands_parameter_name: fake_sensor_commands, diff --git a/franka_example_controllers/CMakeLists.txt b/franka_example_controllers/CMakeLists.txt index 566b6b17..1c9c8aec 100644 --- a/franka_example_controllers/CMakeLists.txt +++ b/franka_example_controllers/CMakeLists.txt @@ -36,6 +36,7 @@ add_library( src/cartesian_velocity_example_controller.cpp src/cartesian_pose_example_controller.cpp src/joint_impedance_with_ik_example_controller.cpp + src/cartesian_impedance_example_controller.cpp src/cartesian_elbow_example_controller.cpp src/cartesian_orientation_example_controller.cpp src/elbow_example_controller.cpp diff --git a/franka_example_controllers/src/cartesian_impedance_example_controller.cpp b/franka_example_controllers/src/cartesian_impedance_example_controller.cpp index e54507da..04ac8383 100644 --- a/franka_example_controllers/src/cartesian_impedance_example_controller.cpp +++ b/franka_example_controllers/src/cartesian_impedance_example_controller.cpp @@ -3,7 +3,7 @@ namespace franka_example_controllers { -CallbackReturn CartesianImpedanceExampleController::on_init() { +CartesianImpedanceExampleController::CallbackReturn CartesianImpedanceExampleController::on_init() { try { auto_declare("arm_id", "fr3"); } catch (const std::exception& e) { @@ -67,18 +67,20 @@ CartesianImpedanceExampleController::state_interface_configuration() const { return state_interfaces_config; } -CallbackReturn CartesianImpedanceExampleController::on_configure( +CartesianImpedanceExampleController::CallbackReturn CartesianImpedanceExampleController::on_configure( const rclcpp_lifecycle::State& /*previous_state*/) { arm_id_ = get_node()->get_parameter("arm_id").as_string(); + auto robot_description = get_node()->get_parameter("robot_description").as_string(); franka_robot_state_ = std::make_unique( - franka_semantic_components::FrankaRobotState(arm_id_ + "/" + k_robot_state_interface_name)); + franka_semantic_components::FrankaRobotState(arm_id_ + "/" + k_robot_state_interface_name, + robot_description)); franka_robot_model_ = std::make_unique( franka_semantic_components::FrankaRobotModel(arm_id_ + "/" + k_robot_model_interface_name, arm_id_ + "/" + k_robot_state_interface_name)); return CallbackReturn::SUCCESS; } -CallbackReturn CartesianImpedanceExampleController::on_activate( +CartesianImpedanceExampleController::CallbackReturn CartesianImpedanceExampleController::on_activate( const rclcpp_lifecycle::State& /*previous_state*/) { franka_robot_state_->assign_loaned_state_interfaces(state_interfaces_); franka_robot_model_->assign_loaned_state_interfaces(state_interfaces_); @@ -165,7 +167,7 @@ controller_interface::return_type CartesianImpedanceExampleController::update( // pseudoinverse for nullspace handling Eigen::MatrixXd jacobian_transpose_pinv; - pseudoInverse(jacobian.transpose(), jacobian_transpose_pinv); + jacobian_transpose_pinv = pseudoInverse(jacobian.transpose(), true); // Cartesian PD control with damping ratio = 1 tau_task << jacobian.transpose() * @@ -180,7 +182,7 @@ controller_interface::return_type CartesianImpedanceExampleController::update( tau_d << tau_task + tau_nullspace + coriolis; // saturate the commanded torque to joint limits - tau_d << saturateTorqueRate(tau_d, tau_j_d); + tau_d << saturateTorqueRate(tau_d, tau_J_d); for (int i = 0; i < num_joints; i++) { command_interfaces_[i].set_value(tau_d[i]); From 64966bf59f95f9bf25446bb26ee5e2908d3c6b77 Mon Sep 17 00:00:00 2001 From: Zakaria Bouzit Date: Tue, 20 Aug 2024 10:42:50 +0200 Subject: [PATCH 11/11] add equilibrium pose publisher --- franka_bringup/config/controllers.yaml | 2 +- ...artesian_impedance_example_controller.launch.py | 5 +++++ franka_example_controllers/CMakeLists.txt | 9 +++++++++ .../cartesian_impedance_example_controller.hpp | 2 +- .../src/cartesian_impedance_example_controller.cpp | 14 +++++++------- 5 files changed, 23 insertions(+), 9 deletions(-) diff --git a/franka_bringup/config/controllers.yaml b/franka_bringup/config/controllers.yaml index 9deedc6f..53b724cd 100644 --- a/franka_bringup/config/controllers.yaml +++ b/franka_bringup/config/controllers.yaml @@ -48,7 +48,7 @@ controller_manager: type: franka_robot_state_broadcaster/FrankaRobotStateBroadcaster cartesian_impedance_example_controller: - type: franka_example_controllers::CartesianImpedanceExampleController + type: franka_example_controllers/CartesianImpedanceExampleController joint_impedance_with_ik_example_controller: diff --git a/franka_bringup/launch/cartesian_impedance_example_controller.launch.py b/franka_bringup/launch/cartesian_impedance_example_controller.launch.py index 0007f958..850aa658 100644 --- a/franka_bringup/launch/cartesian_impedance_example_controller.launch.py +++ b/franka_bringup/launch/cartesian_impedance_example_controller.launch.py @@ -66,4 +66,9 @@ def generate_launch_description(): arguments=['cartesian_impedance_example_controller'], output='screen', ), + Node( + package='franka_example_controllers', + executable='equilibrium_pose_publisher', + output='screen', + ), ]) \ No newline at end of file diff --git a/franka_example_controllers/CMakeLists.txt b/franka_example_controllers/CMakeLists.txt index 1c9c8aec..9ed7d50e 100644 --- a/franka_example_controllers/CMakeLists.txt +++ b/franka_example_controllers/CMakeLists.txt @@ -25,6 +25,15 @@ find_package(Eigen3 REQUIRED) find_package(franka_semantic_components REQUIRED) find_package(moveit_core REQUIRED) find_package(moveit_msgs REQUIRED) +find_package(geometry_msgs REQUIRED) + + +add_executable(equilibrium_pose_publisher src/equilibrium_pose_publisher.cpp) +# Link the executable with the necessary libraries +ament_target_dependencies(equilibrium_pose_publisher + rclcpp + geometry_msgs +) add_library( ${PROJECT_NAME} diff --git a/franka_example_controllers/include/franka_example_controllers/cartesian_impedance_example_controller.hpp b/franka_example_controllers/include/franka_example_controllers/cartesian_impedance_example_controller.hpp index 16be344e..6182f490 100644 --- a/franka_example_controllers/include/franka_example_controllers/cartesian_impedance_example_controller.hpp +++ b/franka_example_controllers/include/franka_example_controllers/cartesian_impedance_example_controller.hpp @@ -64,7 +64,7 @@ class CartesianImpedanceExampleController : public controller_interface::Control // Saturation Eigen::Matrix saturateTorqueRate( const Eigen::Matrix& tau_d_calculated, - const Eigen::Matrix& tau_J_d); + const Eigen::Matrix& tau_j_d); // Classic cartesian controller double filter_params_{0.005}; diff --git a/franka_example_controllers/src/cartesian_impedance_example_controller.cpp b/franka_example_controllers/src/cartesian_impedance_example_controller.cpp index 04ac8383..57b22212 100644 --- a/franka_example_controllers/src/cartesian_impedance_example_controller.cpp +++ b/franka_example_controllers/src/cartesian_impedance_example_controller.cpp @@ -119,7 +119,8 @@ controller_interface::CallbackReturn CartesianImpedanceExampleController::on_dea controller_interface::return_type CartesianImpedanceExampleController::update( const rclcpp::Time& time, - const rclcpp::Duration& period) { + const rclcpp::Duration& period + ) { // get state variables robot_state_ = franka_msgs::msg::FrankaRobotState(); franka_robot_state_->get_values_as_message(robot_state_); @@ -135,7 +136,7 @@ controller_interface::return_type CartesianImpedanceExampleController::update( Eigen::Map> q(robot_state_.measured_joint_state.position.data()); Eigen::Map> dq(robot_state_.measured_joint_state.velocity.data()); - Eigen::Map> tau_J_d(robot_state_.measured_joint_state.effort.data()); + Eigen::Map> tau_j_d(robot_state_.measured_joint_state.effort.data()); Eigen::Vector3d position(robot_state_.o_t_ee.pose.position.x, robot_state_.o_t_ee.pose.position.y, robot_state_.o_t_ee.pose.position.z); Eigen::Quaterniond orientation( @@ -182,11 +183,10 @@ controller_interface::return_type CartesianImpedanceExampleController::update( tau_d << tau_task + tau_nullspace + coriolis; // saturate the commanded torque to joint limits - tau_d << saturateTorqueRate(tau_d, tau_J_d); + tau_d << saturateTorqueRate(tau_d, tau_j_d); for (int i = 0; i < num_joints; i++) { command_interfaces_[i].set_value(tau_d[i]); - std::cout << tau_d[i] << std::endl; } // update parameters changed online either through dynamic reconfigure or through the interactive @@ -233,12 +233,12 @@ void CartesianImpedanceExampleController::equilibriumPoseCallback( Eigen::Matrix CartesianImpedanceExampleController::saturateTorqueRate( const Eigen::Matrix& tau_d_calculated, - const Eigen::Matrix& tau_J_d) { // NOLINT (readability-identifier-naming) + const Eigen::Matrix& tau_j_d) { // NOLINT (readability-identifier-naming) Eigen::Matrix tau_d_saturated{}; for (size_t i = 0; i < 7; i++) { - double difference = tau_d_calculated[i] - tau_J_d[i]; + double difference = tau_d_calculated[i] - tau_j_d[i]; tau_d_saturated[i] = - tau_J_d[i] + std::max(std::min(difference, delta_tau_max_), -delta_tau_max_); + tau_j_d[i] + std::max(std::min(difference, delta_tau_max_), -delta_tau_max_); } return tau_d_saturated; }