diff --git a/.github/workflows/ci.yml b/.github/workflows/ci.yml index 27ee719..ae9b904 100644 --- a/.github/workflows/ci.yml +++ b/.github/workflows/ci.yml @@ -1,9 +1,9 @@ name: CI on: push: - branches: [ main ] + branches: [ humble ] pull_request: - branches: [ main ] + branches: [ humble ] jobs: CI: runs-on: ubuntu-latest diff --git a/cartesian_vic_controller/CMakeLists.txt b/cartesian_vic_controller/CMakeLists.txt index a95c4b7..a457193 100644 --- a/cartesian_vic_controller/CMakeLists.txt +++ b/cartesian_vic_controller/CMakeLists.txt @@ -49,6 +49,8 @@ add_library(cartesian_vic_controller SHARED # Vanilla VIC rules (basic admittance / impedance control laws) src/rules/vanilla_cartesian_admittance_rule.cpp src/rules/vanilla_cartesian_impedance_rule.cpp + # Impedance control for Force Dimension Haptic Interfaces + src/rules/fd_impedance_rule.cpp ) target_compile_features(cartesian_vic_controller PUBLIC cxx_std_17) target_include_directories(cartesian_vic_controller PUBLIC diff --git a/cartesian_vic_controller/cartesian_vic_rules.xml b/cartesian_vic_controller/cartesian_vic_rules.xml index 1147429..325ac63 100644 --- a/cartesian_vic_controller/cartesian_vic_rules.xml +++ b/cartesian_vic_controller/cartesian_vic_rules.xml @@ -13,4 +13,11 @@ Classical impedance control law. + + + Impedance control law for Force Dimension haptic devices (and ONLY those!!!). + + diff --git a/cartesian_vic_controller/config/example_admittance_controller_config.yaml b/cartesian_vic_controller/config/example_admittance_controller_config.yaml index 945fd18..3b338e4 100644 --- a/cartesian_vic_controller/config/example_admittance_controller_config.yaml +++ b/cartesian_vic_controller/config/example_admittance_controller_config.yaml @@ -29,6 +29,7 @@ cartesian_vic_controller: gravity: [0.0, 0.0, -9.81] ft_sensor: + is_enabled: true name: ftsensor frame: id: ft_sensor # Wrench measurements are in this frame diff --git a/cartesian_vic_controller/include/cartesian_vic_controller/cartesian_vic_controller.hpp b/cartesian_vic_controller/include/cartesian_vic_controller/cartesian_vic_controller.hpp index 570a697..e2d161e 100644 --- a/cartesian_vic_controller/include/cartesian_vic_controller/cartesian_vic_controller.hpp +++ b/cartesian_vic_controller/include/cartesian_vic_controller/cartesian_vic_controller.hpp @@ -23,6 +23,7 @@ #include #include "cartesian_vic_controller/visibility_control.h" +#include "cartesian_vic_controller/measurement_data.hpp" #include "cartesian_vic_controller/cartesian_vic_rule.hpp" #include "cartesian_vic_controller/external_torque_sensor.hpp" @@ -133,16 +134,13 @@ class CartesianVicController : public controller_interface::ControllerInterface * @brief Read values from hardware interfaces and set corresponding fields of joint_state and * ft_values */ - bool read_state_from_hardware( - trajectory_msgs::msg::JointTrajectoryPoint & joint_state, - geometry_msgs::msg::Wrench & ft_values, - std::vector & external_torques); + bool read_state_from_hardware(MeasurementData & measurement_data); /** * @brief Initialize the vic rule */ - bool initialize_vic_rule(const trajectory_msgs::msg::JointTrajectoryPoint & joint_state); + bool initialize_vic_rule(const MeasurementData & measurement_data); /** * @brief Try to retrieve the URDF from the parameter server. If this fails, it will @@ -224,13 +222,18 @@ class CartesianVicController : public controller_interface::ControllerInterface cartesian_control_msgs::msg::CompliantFrameTrajectory cartesian_state_; cartesian_control_msgs::msg::CompliantFrameTrajectory cartesian_reference_, last_cartesian_reference_; - // joint_state_: current joint readings from the hardware // joint_command_: joint reference value computed by the controller - trajectory_msgs::msg::JointTrajectoryPoint joint_state_; trajectory_msgs::msg::JointTrajectoryPoint joint_command_, last_commanded_joint_state_; - // ft_values_: values read from the force torque sensor + + // measurement data used by the vic controller + // - joint state: current joint readings from the hardware + // - measured wrench: values read from the ft sensor (optional) + // - external torques: values read from the external torque sensor (optional) + MeasurementData measurement_data_{0}; + + // ft_values_: values read from the force torque sensor (temporary storage) geometry_msgs::msg::Wrench ft_values_; - // ext_torque_values_ : values read from the external torque sensor (optional, zero if disabled) + // ext_torque_values_ : values read from the external torque sensor (temporary storage) std::vector ext_torque_values_; }; diff --git a/cartesian_vic_controller/include/cartesian_vic_controller/cartesian_vic_rule.hpp b/cartesian_vic_controller/include/cartesian_vic_controller/cartesian_vic_rule.hpp index 170a6ff..32fd691 100644 --- a/cartesian_vic_controller/include/cartesian_vic_controller/cartesian_vic_rule.hpp +++ b/cartesian_vic_controller/include/cartesian_vic_controller/cartesian_vic_rule.hpp @@ -43,6 +43,7 @@ #include "cartesian_vic_controller_parameters.hpp" // include data structures +#include "cartesian_vic_controller/measurement_data.hpp" #include "cartesian_vic_controller/cartesian_vic_state.hpp" #include "cartesian_vic_controller/compliance_frame_trajectory.hpp" @@ -84,51 +85,55 @@ class CartesianVicRule * Compute joint command from the current cartesian tracking errors * and the desired interaction parameters (M, K, D). This function is * to be used when no external torque sensor is available. + * Internally calls update_input_data() and then compute_controls(). + * + * Note: call only ONE of the update functions. * * \param[in] period time in seconds since last controller update - * \param[in] current_joint_state current joint state of the robot - * \param[in] measured_wrench most recent measured wrench from force torque sensor + * \param[in] measurement_data most recent measurement data, including at + * least the joint position and velocity * \param[out] joint_state_command computed joint state command */ controller_interface::return_type update( const rclcpp::Duration & period, - const trajectory_msgs::msg::JointTrajectoryPoint & current_joint_state, - const geometry_msgs::msg::Wrench & measured_wrench, + const MeasurementData & measurement_data, trajectory_msgs::msg::JointTrajectoryPoint & joint_state_command ); + /** + * Update VIC input data (i.e., kinematics and parameters) + * + * Note: call only ONE of the update functions. + * + * \param[in] period time in seconds since last controller update + * \param[in] measurement_data most recent measurement data, including at + * least the joint position and velocity + */ + controller_interface::return_type update_input_data( + const rclcpp::Duration & period, + const MeasurementData & measurement_data + ); + /** * Compute joint command from the current cartesian tracking errors * and the desired interaction parameters (M, K, D). This function is - * to be used when an external torque sensor is available. + * to be used when no external torque sensor is available. * * \param[in] period time in seconds since last controller update - * \param[in] current_joint_state current joint state of the robot - * \param[in] measured_wrench most recent measured wrench from force torque sensor - * \param[in] measured_external_torques most recent measured external torques * \param[out] joint_state_command computed joint state command */ - controller_interface::return_type update( + controller_interface::return_type compute_controls( const rclcpp::Duration & period, - const trajectory_msgs::msg::JointTrajectoryPoint & current_joint_state, - const geometry_msgs::msg::Wrench & measured_wrench, - const std::vector & measured_external_torques, trajectory_msgs::msg::JointTrajectoryPoint & joint_state_command ); /// Get current control mode (ADMITTANCE / IMPEDANCE / INVALID) ControlMode get_control_mode() const {return control_mode_;} -protected: - /// Internal wrapper for the VIC logic - controller_interface::return_type internal_update( - const rclcpp::Duration & period, - const trajectory_msgs::msg::JointTrajectoryPoint & current_joint_state, - const geometry_msgs::msg::Wrench & measured_wrench, - trajectory_msgs::msg::JointTrajectoryPoint & joint_state_command, - bool use_external_torques = false - ); + /// Get current input data (call update_input_data() first) + const VicInputData & get_input_data() const {return vic_state_.input_data;} +protected: /// Manual setting of inertia, damping, and stiffness (diagonal matrices) void set_interaction_parameters( const Eigen::Matrix & desired_inertia, @@ -159,9 +164,11 @@ class CartesianVicRule std::shared_ptr parameter_handler_; cartesian_vic_controller::Params parameters_; +protected: + // ROS2 logging + rclcpp::Logger logger_; rclcpp::Clock internal_clock_; -protected: template void vec_to_eigen(const std::vector & data, T2 & matrix); @@ -193,6 +200,8 @@ class CartesianVicRule private: /// Filtered wrench expressed in world frame Eigen::Matrix wrench_world_; + /// Filtered wrench expressed in robot base frame + Eigen::Matrix wrench_base_; /// Filtered external torques Eigen::VectorXd filtered_external_torques_; diff --git a/cartesian_vic_controller/include/cartesian_vic_controller/cartesian_vic_state.hpp b/cartesian_vic_controller/include/cartesian_vic_controller/cartesian_vic_state.hpp index a74cac7..22133a2 100644 --- a/cartesian_vic_controller/include/cartesian_vic_controller/cartesian_vic_state.hpp +++ b/cartesian_vic_controller/include/cartesian_vic_controller/cartesian_vic_state.hpp @@ -63,6 +63,9 @@ class VicInputData natural_joint_space_inertia = \ Eigen::Matrix::Zero(num_joints, num_joints); */ + // Reset data availability + has_ft_sensor_ = false; + has_external_torque_sensor_ = false; } bool set_joint_state_external_torques(const Eigen::VectorXd & joint_state_external_torques) { @@ -75,28 +78,42 @@ class VicInputData return true; } - bool get_joint_state_external_torques(Eigen::VectorXd & joint_state_external_torques) const + bool set_ft_sensor_wrench(const Eigen::Matrix & ft_sensor_wrench) { - if (!has_external_torque_sensor_) { - joint_state_external_torques.setZero(); - return false; - } - if (joint_state_external_torques.size() != joint_state_external_torques_.size()) { - joint_state_external_torques.setZero(); - return false; - } - joint_state_external_torques = joint_state_external_torques_; + robot_current_wrench_at_ft_frame_ = ft_sensor_wrench; + has_ft_sensor_ = true; return true; } + + const Eigen::VectorXd & get_joint_state_external_torques() const + { + return joint_state_external_torques_; + } + + const Eigen::Matrix & get_ft_sensor_wrench() const + { + return robot_current_wrench_at_ft_frame_; + } + bool reset_joint_state_external_torques() { has_external_torque_sensor_ = false; return true; } + + bool reset_ft_sensor_wrench() + { + has_ft_sensor_ = false; + return true; + } bool has_external_torque_sensor() const { return has_external_torque_sensor_; } + bool has_ft_sensor() const + { + return has_ft_sensor_; + } public: // General parameters @@ -133,7 +150,6 @@ class VicInputData // Cartesian state (control frame w.r.t. robot base frame) Eigen::Isometry3d robot_current_pose; Eigen::Matrix robot_current_velocity; - Eigen::Matrix robot_current_wrench_at_ft_frame; // ft_frame w.r.t. base /// Natural inertia matrix (from dynamic model) // Eigen::Matrix natural_cartesian_inertia; @@ -141,6 +157,8 @@ class VicInputData protected: Eigen::VectorXd joint_state_external_torques_; + Eigen::Matrix robot_current_wrench_at_ft_frame_; // ft_frame w.r.t. base + bool has_ft_sensor_ = false; bool has_external_torque_sensor_ = false; }; diff --git a/cartesian_vic_controller/include/cartesian_vic_controller/measurement_data.hpp b/cartesian_vic_controller/include/cartesian_vic_controller/measurement_data.hpp new file mode 100644 index 0000000..3baf385 --- /dev/null +++ b/cartesian_vic_controller/include/cartesian_vic_controller/measurement_data.hpp @@ -0,0 +1,123 @@ +// Copyright 2024 ICUBE Laboratory, University of Strasbourg +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. +// +/// \authors: Thibault Poignonec + +#ifndef CARTESIAN_VIC_CONTROLLER__MEASUREMENT_DATA_HPP_ +#define CARTESIAN_VIC_CONTROLLER__MEASUREMENT_DATA_HPP_ + +#include +#include + +#include +#include +#include +#include + +#include "trajectory_msgs/msg/joint_trajectory_point.hpp" +#include "geometry_msgs/msg/wrench_stamped.hpp" + +namespace cartesian_vic_controller +{ + +/** @brief Class used to store the measurement data + * + * Used to pass the data to the Vic rule. The measurement data is + * at least the joint position and velocity. Optionally, the external + * joint torques and the ft sensor wrench can be passed. + * + */ +class MeasurementData +{ +private: + size_t num_joints_; + bool has_external_torques_data_; + bool has_ft_sensor_data_; + + geometry_msgs::msg::Wrench measured_wrench_; + std::vector external_torques_; + +public: + trajectory_msgs::msg::JointTrajectoryPoint joint_state; + +public: + explicit MeasurementData(size_t num_joints) + { + num_joints_ = num_joints; + joint_state.positions.assign(num_joints_, 0.0); // std::nan); + joint_state.velocities.assign(num_joints_, 0.0); // std::nan); + joint_state.accelerations.assign(num_joints_, 0.0); // std::nan); + joint_state.effort.reserve(num_joints_); // not used + external_torques_.assign(num_joints_, 0.0); + reset_data_availability(); + } + ~MeasurementData() = default; + + const trajectory_msgs::msg::JointTrajectoryPoint & get_joint_state() const + { + return joint_state; + } + const geometry_msgs::msg::Wrench & get_ft_sensor_wrench() const + { + return measured_wrench_; + } + const std::vector & get_external_torques() const + { + return external_torques_; + } + bool has_ft_sensor_data() const + { + return has_ft_sensor_data_; + } + bool has_external_torques_data() const + { + return has_external_torques_data_; + } + + /// Reset all values back to default + bool reset_data_availability() + { + has_external_torques_data_ = false; + has_ft_sensor_data_ = false; + return true; + } + + bool update_joint_state(const trajectory_msgs::msg::JointTrajectoryPoint & joint_state_values) + { + if (joint_state_values.positions.size() != num_joints_ || + joint_state_values.velocities.size() != num_joints_) + { + return false; + } + joint_state = joint_state_values; + return true; + } + bool update_ft_sensor_wrench(const geometry_msgs::msg::Wrench & measured_wrench) + { + measured_wrench_ = measured_wrench; + has_ft_sensor_data_ = true; + return true; + } + bool update_external_torques(const std::vector & external_torques) + { + external_torques_ = external_torques; + has_external_torques_data_ = true; + return true; + } +}; + +} // namespace cartesian_vic_controller + + +#endif // CARTESIAN_VIC_CONTROLLER__MEASUREMENT_DATA_HPP_ diff --git a/cartesian_vic_controller/include/cartesian_vic_controller/rules/fd_impedance_rule.hpp b/cartesian_vic_controller/include/cartesian_vic_controller/rules/fd_impedance_rule.hpp new file mode 100644 index 0000000..679427d --- /dev/null +++ b/cartesian_vic_controller/include/cartesian_vic_controller/rules/fd_impedance_rule.hpp @@ -0,0 +1,72 @@ +// Copyright 2024 ICUBE Laboratory, University of Strasbourg +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. +// +/// \authors: Thibault Poignonec +/// \description: Plugin for CartesianVicRule implementing "classical" impedance control. + +#ifndef CARTESIAN_VIC_CONTROLLER__RULES__FD_IMPEDANCE_RULE_HPP_ +#define CARTESIAN_VIC_CONTROLLER__RULES__FD_IMPEDANCE_RULE_HPP_ + + +#include + +#include "cartesian_vic_controller/cartesian_vic_rule.hpp" + +namespace cartesian_vic_controller +{ + +class FdImpedanceRule : public CartesianVicRule +{ +public: + controller_interface::return_type init( + const std::shared_ptr & parameter_handler + ) override; + + /// Configure admittance solver + controller_interface::return_type configure( + const std::shared_ptr & node, + const size_t num_joints + ) override; + + /// Reset all values back to default + controller_interface::return_type reset(const size_t num_joints) override; + +protected: + /// Actual vic (admittance) control logic + bool compute_controls( + double dt /*period in seconds*/, + const VicInputData & vic_input_data, + VicCommandData & vic_command_data) override; + +private: + bool reset_rule__internal_storage(const size_t num_joints); + + // Internal data for this rule + Eigen::VectorXd raw_joint_command_effort_; + + Eigen::Matrix J_; + Eigen::Matrix J_pinv_; + Eigen::Matrix J_dot_; + + Eigen::Matrix I_joint_space_; + Eigen::Matrix M_joint_space_; + + Eigen::Matrix M_cartesian_space_; + + double alpha_pinv_ = 0.000005; +}; + +} // namespace cartesian_vic_controller + +#endif // CARTESIAN_VIC_CONTROLLER__RULES__FD_IMPEDANCE_RULE_HPP_ diff --git a/cartesian_vic_controller/src/cartesian_vic_controller.cpp b/cartesian_vic_controller/src/cartesian_vic_controller.cpp index fe20d53..7725954 100644 --- a/cartesian_vic_controller/src/cartesian_vic_controller.cpp +++ b/cartesian_vic_controller/src/cartesian_vic_controller.cpp @@ -34,6 +34,7 @@ namespace cartesian_vic_controller { + controller_interface::CallbackReturn CartesianVicController::on_init() { // Try to retrieve urdf (used by kinematics / dynamics plugin) @@ -70,12 +71,9 @@ controller_interface::CallbackReturn CartesianVicController::on_init() } // allocate dynamic memory - joint_state_.positions.assign(num_joints_, 0.0); // std::nan); - joint_state_.velocities.assign(num_joints_, 0.0); // std::nan); - joint_state_.accelerations.assign(num_joints_, 0.0); // std::nan); - - joint_command_ = joint_state_; - last_commanded_joint_state_ = joint_state_; + measurement_data_ = MeasurementData(num_joints_); + joint_command_ = measurement_data_.get_joint_state(); + last_commanded_joint_state_ = measurement_data_.get_joint_state(); return controller_interface::CallbackReturn::SUCCESS; } @@ -137,7 +135,7 @@ controller_interface::return_type CartesianVicController::update( } // get all controller inputs - bool is_state_valid = read_state_from_hardware(joint_state_, ft_values_, ext_torque_values_); + bool is_state_valid = read_state_from_hardware(measurement_data_); bool all_ok = true; // make sure impedance was initialized @@ -146,7 +144,7 @@ controller_interface::return_type CartesianVicController::update( return controller_interface::return_type::OK; } else if (!is_vic_initialized_ && is_state_valid) { // Init current desired pose from current joint position - if (!initialize_vic_rule(joint_state_)) { + if (!initialize_vic_rule(measurement_data_)) { return controller_interface::return_type::ERROR; } } else if (!is_state_valid) { @@ -166,25 +164,11 @@ controller_interface::return_type CartesianVicController::update( } // apply vic control to reference to determine desired state - controller_interface::return_type ret_vic = controller_interface::return_type::ERROR; - if (external_torque_sensor_ && vic_->parameters_.external_torque_sensor.is_enabled) { - // Update vic with external torques - ret_vic = vic_->update( - period, - joint_state_, - ft_values_, - ext_torque_values_, - joint_command_ - ); - } else { - // Update vic WITHOUT external torques - ret_vic = vic_->update( + auto ret_vic = vic_->update( period, - joint_state_, - ft_values_, + measurement_data_, joint_command_ - ); - } + ); if (ret_vic != controller_interface::return_type::OK) { std::fill( joint_command_.accelerations.begin(), joint_command_.accelerations.end(), 0.0); @@ -361,8 +345,17 @@ controller_interface::CallbackReturn CartesianVicController::on_configure( state_publisher_->unlock(); // Initialize F/T sensor semantic_component - force_torque_sensor_ = std::make_unique( - semantic_components::ForceTorqueSensor(vic_->parameters_.ft_sensor.name)); + if (vic_->parameters_.ft_sensor.is_enabled) { + RCLCPP_INFO(get_node()->get_logger(), "Force / torque sensor is enabled"); + if (vic_->parameters_.ft_sensor.name.empty()) { + RCLCPP_ERROR( + get_node()->get_logger(), + "Force / torque sensor is enabled, but sensor name is empty!"); + return CallbackReturn::FAILURE; + } + force_torque_sensor_ = std::make_unique( + semantic_components::ForceTorqueSensor(vic_->parameters_.ft_sensor.name)); + } // Initialize external torque sensor semantic_component if (vic_->parameters_.external_torque_sensor.is_enabled) { @@ -444,8 +437,8 @@ controller_interface::CallbackReturn CartesianVicController::on_activate( } // initialize states - read_state_from_hardware(joint_state_, ft_values_, ext_torque_values_); - for (auto val : joint_state_.positions) { + read_state_from_hardware(measurement_data_); + for (auto val : measurement_data_.joint_state.positions) { if (std::isnan(val)) { RCLCPP_ERROR(get_node()->get_logger(), "Failed to read joint positions from the hardware.\n"); return controller_interface::CallbackReturn::ERROR; @@ -502,10 +495,11 @@ controller_interface::CallbackReturn CartesianVicController::on_error( } bool CartesianVicController::read_state_from_hardware( - trajectory_msgs::msg::JointTrajectoryPoint & state_current, - geometry_msgs::msg::Wrench & ft_values, - std::vector & external_torques) + MeasurementData & measurement_data) { + // reset data availability in case of error + measurement_data.reset_data_availability(); + // if any interface has nan values, assume state_current is the last command state bool all_ok = true; bool nan_position = false; @@ -515,6 +509,9 @@ bool CartesianVicController::read_state_from_hardware( size_t pos_ind = 0; // Mandatory state interface size_t vel_ind = pos_ind + has_velocity_state_interface_; size_t acc_ind = vel_ind + has_acceleration_state_interface_; + + auto & state_current = measurement_data.joint_state; + for (size_t joint_ind = 0; joint_ind < num_joints_; ++joint_ind) { if (has_position_state_interface_) { state_current.positions[joint_ind] = @@ -551,46 +548,46 @@ bool CartesianVicController::read_state_from_hardware( } // if any ft_values are nan, assume values are zero - force_torque_sensor_->get_values_as_message(ft_values); - if ( - std::isnan(ft_values.force.x) || std::isnan(ft_values.force.y) || - std::isnan(ft_values.force.z) || std::isnan(ft_values.torque.x) || - std::isnan(ft_values.torque.y) || std::isnan(ft_values.torque.z)) - { + if (!force_torque_sensor_ && vic_->parameters_.ft_sensor.is_enabled) { + RCLCPP_WARN_THROTTLE( + get_node()->get_logger(), *clock, 1000, "Force/Torque sensor unavailable!"); all_ok &= false; - ft_values = geometry_msgs::msg::Wrench(); } - - // if any external_torques are nan, assume values are zero - if (external_torque_sensor_ && vic_->parameters_.external_torque_sensor.is_enabled) { - external_torques = external_torque_sensor_->get_torques(); - } else { - external_torques = std::vector(num_joints_, 0.0); + if (force_torque_sensor_ && vic_->parameters_.ft_sensor.is_enabled) { + force_torque_sensor_->get_values_as_message(ft_values_); + if ( + std::isnan(ft_values_.force.x) || std::isnan(ft_values_.force.y) || + std::isnan(ft_values_.force.z) || std::isnan(ft_values_.torque.x) || + std::isnan(ft_values_.torque.y) || std::isnan(ft_values_.torque.z)) + { + all_ok &= false; + ft_values_ = geometry_msgs::msg::Wrench(); + RCLCPP_WARN_THROTTLE(get_node()->get_logger(), *clock, 1000, "Force/Torque sensor is NaN!"); + } else { + all_ok &= measurement_data.update_ft_sensor_wrench(ft_values_); + } } + if (!external_torque_sensor_ && vic_->parameters_.external_torque_sensor.is_enabled) { RCLCPP_WARN_THROTTLE( get_node()->get_logger(), *clock, 1000, "External torque sensor unavailable!"); all_ok &= false; } - if (vic_->parameters_.external_torque_sensor.is_enabled && - external_torques.size() != num_joints_) - { - RCLCPP_WARN_THROTTLE( - get_node()->get_logger(), *clock, 1000, - "External torque sensor has incorrect size! Expected %lu, got %lu. Setting to zero", - num_joints_, external_torques.size()); - external_torques = std::vector(num_joints_, 0.0); - all_ok &= false; - } - bool ext_torque_has_nan = false; - for (size_t i = 0; i < num_joints_; ++i) { - ext_torque_has_nan |= std::isnan(external_torques[i]); - } - if (ext_torque_has_nan) { - all_ok &= false; - external_torques = std::vector(num_joints_, 0.0); - RCLCPP_WARN_THROTTLE( - get_node()->get_logger(), *clock, 1000, "External torque contains one or more NaN!"); + if (external_torque_sensor_ && vic_->parameters_.external_torque_sensor.is_enabled) { + ext_torque_values_ = external_torque_sensor_->get_torques(); + bool ext_torque_has_nan = false; + for (size_t i = 0; i < num_joints_; ++i) { + ext_torque_has_nan |= std::isnan(ext_torque_values_[i]); + } + if (ext_torque_has_nan || ext_torque_values_.size() != num_joints_) { + all_ok &= false; + ext_torque_values_ = std::vector(num_joints_, 0.0); + RCLCPP_WARN_THROTTLE( + get_node()->get_logger(), *clock, 1000, + "Invalid external torque measurements (%s)!", ext_torque_has_nan ? "NaN" : "wrong size"); + } else { + all_ok &= measurement_data.update_external_torques(ext_torque_values_); + } } return all_ok; @@ -708,24 +705,12 @@ bool CartesianVicController::write_admittance_state_to_hardware( } bool CartesianVicController::initialize_vic_rule( - const trajectory_msgs::msg::JointTrajectoryPoint & joint_state) + const MeasurementData & measurement_data) { bool all_ok = true; - RCLCPP_INFO( - get_node()->get_logger(), - "Initializing VIC rule with joint positions: [%.4f, %.4f, %.4f, %.4f, %.4f, %.4f, %.4f]\n", - joint_state.positions[0], - joint_state.positions[1], - joint_state.positions[2], - joint_state.positions[3], - joint_state.positions[4], - joint_state.positions[5], - joint_state.positions[6] - ); - // Use current joint_state as a default reference - auto ret = vic_->init_reference_frame_trajectory(joint_state); + auto ret = vic_->init_reference_frame_trajectory(measurement_data.get_joint_state()); if (ret != controller_interface::return_type::OK) { all_ok = false; RCLCPP_ERROR( @@ -733,8 +718,8 @@ bool CartesianVicController::initialize_vic_rule( "Failed to initialize the reference compliance frame trajectory.\n"); return false; } - joint_command_ = joint_state; - last_commanded_joint_state_ = joint_state; + joint_command_ = measurement_data.get_joint_state(); + last_commanded_joint_state_ = measurement_data.get_joint_state(); is_vic_initialized_ = all_ok; return all_ok; } diff --git a/cartesian_vic_controller/src/cartesian_vic_controller_parameters.yaml b/cartesian_vic_controller/src/cartesian_vic_controller_parameters.yaml index f735f7c..2c7b553 100644 --- a/cartesian_vic_controller/src/cartesian_vic_controller_parameters.yaml +++ b/cartesian_vic_controller/src/cartesian_vic_controller_parameters.yaml @@ -80,6 +80,10 @@ cartesian_vic_controller: } ft_sensor: + is_enabled: { + type: bool, + description: "Specifies if the force torque sensor is enabled." + } name: { type: string, description: "Specifies the name of the force torque sensor in the robot description which will be used in the vic calculation." @@ -167,10 +171,16 @@ cartesian_vic_controller: } activate_gravity_compensation: { type: bool, - default_value: true, + default_value: false, read_only: true, description: "Specifies if the coriolis and gravity compensation should be activated." } + use_natural_robot_inertia: { + type: bool, + default_value: false, + read_only: true, + description: "If true, the natural inertia of the robot is used as desired inertia in VIC computations." + } inertia: { type: double_array, diff --git a/cartesian_vic_controller/src/cartesian_vic_rule.cpp b/cartesian_vic_controller/src/cartesian_vic_rule.cpp index 6bb9387..3aa1299 100644 --- a/cartesian_vic_controller/src/cartesian_vic_rule.cpp +++ b/cartesian_vic_controller/src/cartesian_vic_rule.cpp @@ -34,6 +34,7 @@ namespace cartesian_vic_controller CartesianVicRule::CartesianVicRule() : num_joints_(0), vic_state_(0, ControlMode::INVALID), + logger_(rclcpp::get_logger("cartesian_vic_rule")), dynamics_loader_(nullptr) { // Nothing to do, see init(). @@ -76,7 +77,7 @@ CartesianVicRule::configure( } } catch (pluginlib::PluginlibException & ex) { RCLCPP_ERROR( - rclcpp::get_logger("CartesianVicRule"), + logger_, "Exception while loading the IK plugin '%s': '%s'", parameters_.dynamics.plugin_name.c_str(), ex.what() ); @@ -84,7 +85,7 @@ CartesianVicRule::configure( } } else { RCLCPP_ERROR( - rclcpp::get_logger("CartesianVicRule"), + logger_, "A differential IK plugin name was not specified in the config file."); return controller_interface::return_type::ERROR; } @@ -106,7 +107,7 @@ CartesianVicRule::init_reference_frame_trajectory( // Update kinematics state if (!update_kinematics(-1.0, current_joint_state)) { RCLCPP_ERROR( - rclcpp::get_logger("CartesianVicRule"), + logger_, "Failed to update internal state in 'init_reference_frame_trajectory()'!"); return controller_interface::return_type::ERROR; } @@ -122,7 +123,7 @@ CartesianVicRule::init_reference_frame_trajectory( std::stringstream ss_inital_joint_positions; ss_inital_joint_positions << initial_joint_positions_.transpose(); RCLCPP_INFO( - rclcpp::get_logger("CartesianVicRule"), + logger_, "Initial joint positions set to : %s", ss_inital_joint_positions.str().c_str()); @@ -143,7 +144,7 @@ CartesianVicRule::init_reference_frame_trajectory( ); if (!success) { RCLCPP_ERROR( - rclcpp::get_logger("CartesianVicRule"), + logger_, "Failed to fill the desired robot state for index=%u!", i); return controller_interface::return_type::ERROR; @@ -160,7 +161,7 @@ controller_interface::return_type CartesianVicRule::reset(const size_t num_joints) { if (control_mode_ == ControlMode::INVALID) { - RCLCPP_ERROR(rclcpp::get_logger("CartesianVicRule"), "Invalid control mode!"); + RCLCPP_ERROR(logger_, "Invalid control mode!"); return controller_interface::return_type::ERROR; } // Reset vic state @@ -223,7 +224,7 @@ void CartesianVicRule::apply_parameters_update() parameters_.nullspace_control.joint_inertia[i]; } else { RCLCPP_ERROR( - rclcpp::get_logger("CartesianVicRule"), + logger_, "Invalid size for nullspace_inertia vector!"); vic_state_.input_data.nullspace_joint_inertia(i) = default_nullspace_inertia; } @@ -236,7 +237,7 @@ void CartesianVicRule::apply_parameters_update() parameters_.nullspace_control.joint_stiffness[i]; } else { RCLCPP_ERROR( - rclcpp::get_logger("CartesianVicRule"), + logger_, "Invalid size for nullspace_stiffness vector!"); vic_state_.input_data.nullspace_joint_stiffness(i) = default_nullspace_stiffness; } @@ -249,7 +250,7 @@ void CartesianVicRule::apply_parameters_update() parameters_.nullspace_control.joint_damping[i]; } else { RCLCPP_ERROR( - rclcpp::get_logger("CartesianVicRule"), + logger_, "Invalid size for nullspace_damping vector!"); vic_state_.input_data.nullspace_joint_damping(i) = default_nullspace_damping; } @@ -265,7 +266,7 @@ void CartesianVicRule::apply_parameters_update() ); } else { RCLCPP_ERROR( - rclcpp::get_logger("CartesianVicRule"), + logger_, "Invalid size for desired_joint_position vector!"); vic_state_.input_data.nullspace_desired_joint_positions = initial_joint_positions_; } @@ -346,119 +347,122 @@ CartesianVicRule::controller_state_to_msg( } } - controller_interface::return_type CartesianVicRule::update( const rclcpp::Duration & period, - const trajectory_msgs::msg::JointTrajectoryPoint & current_joint_state, - const geometry_msgs::msg::Wrench & measured_wrench, - const std::vector & measured_external_torques, + const MeasurementData & measurement_data, trajectory_msgs::msg::JointTrajectoryPoint & joint_state_command) { - // Update parameters - if (parameters_.enable_parameter_update_without_reactivation) { - apply_parameters_update(); - } - - // Process external torques - bool has_valid_external_torques = true; - if (!process_external_torques_measurements(period.seconds(), measured_external_torques)) { - has_valid_external_torques = false; - RCLCPP_WARN_THROTTLE( - rclcpp::get_logger("CartesianVicRule"), - internal_clock_, 1000, - "Invalid external torques data!"); - } - // Update VIC and compute joint command - auto ret = internal_update( - period, - current_joint_state, - measured_wrench, - joint_state_command, - has_valid_external_torques); - if (ret != controller_interface::return_type::OK) { + if (update_input_data(period, measurement_data) != controller_interface::return_type::OK) { RCLCPP_ERROR( - rclcpp::get_logger("CartesianVicRule"), - "Failed to update CartesianVicRule"); + logger_, + "Failed to update VIC input data!" + ); + // Set commanded position to the previous one + joint_state_command.positions = measurement_data.get_joint_state().positions; + // Set commanded velocity/acc to zero + std::fill( + joint_state_command.velocities.begin(), + joint_state_command.velocities.end(), + 0 + ); + std::fill( + joint_state_command.accelerations.begin(), + joint_state_command.accelerations.end(), + 0 + ); + // Set efforts to zero + std::fill( + joint_state_command.effort.begin(), + joint_state_command.effort.end(), + 0 + ); + return controller_interface::return_type::ERROR; } - return ret; + return compute_controls(period, joint_state_command); } controller_interface::return_type -CartesianVicRule::update( +CartesianVicRule::update_input_data( const rclcpp::Duration & period, - const trajectory_msgs::msg::JointTrajectoryPoint & current_joint_state, - const geometry_msgs::msg::Wrench & measured_wrench, - trajectory_msgs::msg::JointTrajectoryPoint & joint_state_command) + const MeasurementData & measurement_data) { - // Update parameters + bool success = true; // return flag + const double dt = period.seconds(); + + // Update parameters if (parameters_.enable_parameter_update_without_reactivation) { apply_parameters_update(); } - // Update VIC and compute joint command - auto ret = internal_update( - period, - current_joint_state, - measured_wrench, - joint_state_command, - false /*use_external_torques*/); - if (ret != controller_interface::return_type::OK) { - RCLCPP_ERROR( - rclcpp::get_logger("CartesianVicRule"), - "Failed to update CartesianVicRule"); + // Process F/T sensor data + bool has_valid_ft_wrench = measurement_data.has_ft_sensor_data(); + if (has_valid_ft_wrench) { + has_valid_ft_wrench &= process_wrench_measurements( + period.seconds(), measurement_data.get_ft_sensor_wrench()); } - return ret; -} - -controller_interface::return_type -CartesianVicRule::internal_update( - const rclcpp::Duration & period, - const trajectory_msgs::msg::JointTrajectoryPoint & current_joint_state, - const geometry_msgs::msg::Wrench & measured_wrench, - trajectory_msgs::msg::JointTrajectoryPoint & joint_state_command, - bool use_external_torques) -{ - bool success = true; // return flag - const double dt = period.seconds(); - - // Check external torques availability - if (!use_external_torques) { - // No external torque sensor, reset flag and set data to zero - vic_state_.input_data.reset_joint_state_external_torques(); + if (!has_valid_ft_wrench) { + vic_state_.input_data.reset_ft_sensor_wrench(); + if (measurement_data.has_ft_sensor_data()) { + success = false; + RCLCPP_WARN_THROTTLE( + logger_, + internal_clock_, + 1000, + "Invalid F/T sensor data provided to VIC rule!"); + } } - if (use_external_torques && !vic_state_.input_data.has_external_torque_sensor()) { - RCLCPP_ERROR( - rclcpp::get_logger("CartesianVicRule"), - "External torque sensor requested, but not available!" - ); - success = false; + // Process external torques + bool has_valid_external_torques = measurement_data.has_external_torques_data(); + if (has_valid_external_torques) { + has_valid_external_torques &= process_external_torques_measurements( + period.seconds(), measurement_data.get_external_torques()); + } + if (!has_valid_external_torques) { + vic_state_.input_data.reset_joint_state_external_torques(); + if (measurement_data.has_external_torques_data()) { + success = false; + RCLCPP_WARN_THROTTLE( + logger_, + internal_clock_, + 1000, + "Invalid external torques provided to VIC rule!"); + } } - // Update current robot kinematic state + // Update current robot kinematic state success &= update_kinematics( - dt, - current_joint_state + dt, + measurement_data.get_joint_state() ); - // Process wrench measurement - success &= process_wrench_measurements(dt, measured_wrench); - - // Process external torques - // TODO(tpoignonec): success &= process_external_torques(dt, measured_external_torques); + if (!success) { + return controller_interface::return_type::ERROR; + } + return controller_interface::return_type::OK; +} +controller_interface::return_type +CartesianVicRule::compute_controls( + const rclcpp::Duration & period, + trajectory_msgs::msg::JointTrajectoryPoint & joint_state_command) +{ + bool success = true; // return flag + const double dt = period.seconds(); // Compute controls success &= compute_controls(dt, vic_state_.input_data, vic_state_.command_data); // If an error is detected, set commanded velocity to zero if (!success) { RCLCPP_ERROR( - rclcpp::get_logger("CartesianVicRule"), + logger_, "Failed to compute the controls!" ); // Set commanded position to the previous one - joint_state_command.positions = current_joint_state.positions; + for (size_t i = 0; i < parameters_.joints.size(); ++i) { + joint_state_command.positions[i] = vic_state_.input_data.joint_state_position(i); + } // Set commanded velocity/acc to zero std::fill( @@ -519,6 +523,8 @@ CartesianVicRule::internal_update( return controller_interface::return_type::OK; } +// Internal functions + bool CartesianVicRule::update_kinematics( double dt, const trajectory_msgs::msg::JointTrajectoryPoint & current_joint_state) @@ -573,11 +579,18 @@ bool CartesianVicRule::update_kinematics( ); // Pre-compute commonly used transformations - success &= dynamics_->calculate_link_transform( - vic_state_.input_data.joint_state_position, - vic_state_.input_data.ft_sensor_frame, - vic_transforms_.base_ft_ - ); + if (parameters_.ft_sensor.is_enabled) { + success &= dynamics_->calculate_link_transform( + vic_state_.input_data.joint_state_position, + vic_state_.input_data.ft_sensor_frame, + vic_transforms_.base_ft_ + ); + } else { + // TODO(tpoignonec): how to make sure it is not used in that case ? + // In theory, there is no reason to use it if no wrench is available, but you never know... + vic_transforms_.base_ft_.setIdentity(); + } + success &= dynamics_->calculate_link_transform( vic_state_.input_data.joint_state_position, parameters_.dynamics.tip, @@ -661,12 +674,12 @@ bool CartesianVicRule::process_wrench_measurements( wrench_world_(i) = new_wrench_world(i); } } - // Transform wrench_world_ into base frame - vic_state_.input_data.robot_current_wrench_at_ft_frame.head(3) = + wrench_base_.head(3) = \ vic_transforms_.world_base_.rotation().transpose() * wrench_world_.head(3); - vic_state_.input_data.robot_current_wrench_at_ft_frame.tail(3) = + wrench_base_.tail(3) = \ vic_transforms_.world_base_.rotation().transpose() * wrench_world_.tail(3); + vic_state_.input_data.set_ft_sensor_wrench(wrench_base_); return true; } @@ -678,7 +691,7 @@ bool CartesianVicRule::process_external_torques_measurements( // Check data validity if (static_cast(measured_external_torques.size()) != num_joints_) { RCLCPP_ERROR( - rclcpp::get_logger("CartesianVicRule"), + logger_, "Invalid size for measured_external_torques vector!"); filtered_external_torques_.setZero(); vic_state_.input_data.set_joint_state_external_torques(filtered_external_torques_); diff --git a/cartesian_vic_controller/src/cartesian_vic_state.cpp b/cartesian_vic_controller/src/cartesian_vic_state.cpp index 68457f1..f494141 100644 --- a/cartesian_vic_controller/src/cartesian_vic_state.cpp +++ b/cartesian_vic_controller/src/cartesian_vic_state.cpp @@ -67,7 +67,16 @@ VicState::to_msg(cartesian_control_msgs::msg::VicControllerState & vic_state_msg // Fill robot state vic_state_msg.pose = Eigen::toMsg(input_data.robot_current_pose); vic_state_msg.velocity = Eigen::toMsg(input_data.robot_current_velocity); - vic_state_msg.wrench = WrenchToMsg(input_data.robot_current_wrench_at_ft_frame); + if (input_data.has_ft_sensor()) { + vic_state_msg.wrench = WrenchToMsg(input_data.get_ft_sensor_wrench()); + } else { + vic_state_msg.wrench.force.x = 0.0; + vic_state_msg.wrench.force.y = 0.0; + vic_state_msg.wrench.force.z = 0.0; + vic_state_msg.wrench.torque.x = 0.0; + vic_state_msg.wrench.torque.y = 0.0; + vic_state_msg.wrench.torque.z = 0.0; + } // matrixEigenToMsg(input_data.natural_cartesian_inertia, vic_state_msg.natural_inertia); // Fill rendered impedance diff --git a/cartesian_vic_controller/src/rules/fd_impedance_rule.cpp b/cartesian_vic_controller/src/rules/fd_impedance_rule.cpp new file mode 100644 index 0000000..cc50dc8 --- /dev/null +++ b/cartesian_vic_controller/src/rules/fd_impedance_rule.cpp @@ -0,0 +1,262 @@ +// Copyright 2024 ICUBE Laboratory, University of Strasbourg +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. +// +/// \authors: Thibault Poignonec + +#include "cartesian_vic_controller/rules/fd_impedance_rule.hpp" + +#include +#include // for debug purposes... + +#include "control_toolbox/filters.hpp" + + +namespace cartesian_vic_controller +{ + +controller_interface::return_type FdImpedanceRule::init( + const std::shared_ptr & parameter_handler) +{ + logger_ = rclcpp::get_logger("fd_impedance_rule"); + // Initialize CartesianVicRule + control_mode_ = ControlMode::IMPEDANCE; + auto ret = CartesianVicRule::init(parameter_handler); + return ret; +} + +controller_interface::return_type FdImpedanceRule::configure( + const std::shared_ptr & node, + const size_t num_joints) +{ + if (num_joints != 3 && num_joints != 6) { + RCLCPP_ERROR( + logger_, + "For a Force Dimension device, either 3 or 6 joints are expected (got %li)", + num_joints + ); + return controller_interface::return_type::ERROR; + } + reset_rule__internal_storage(num_joints); + return CartesianVicRule::configure(node, num_joints); +} + +controller_interface::return_type FdImpedanceRule::reset(const size_t num_joints) +{ + reset_rule__internal_storage(num_joints); + return CartesianVicRule::reset(num_joints); +} + +bool FdImpedanceRule::compute_controls( + double dt /*period in seconds*/, + const VicInputData & vic_input_data, + VicCommandData & vic_command_data) +{ + bool success = true; + // auto num_joints = vic_input_data.joint_state_position.size(); + + // Get reference compliant frame at t_k + const CompliantFrame & reference_compliant_frame = + vic_input_data.reference_compliant_frames.get_compliant_frame(0); + + // No passivation of impedance parameters + vic_command_data.inertia = reference_compliant_frame.inertia; + vic_command_data.stiffness = reference_compliant_frame.stiffness; + vic_command_data.damping = reference_compliant_frame.damping; + + // Prepare VIC data + // -------------------------------- + + // auto rot_base_control = vic_transforms_.base_control_.rotation(); + auto rot_base_impedance = vic_transforms_.base_vic_.rotation(); + // Express M, K, D matrices in base (provided in base_vic frame) + + Eigen::Matrix K = Eigen::Matrix::Zero(); + K.block<3, 3>(0, 0) = + rot_base_impedance * \ + vic_command_data.stiffness.block<3, 3>(0, 0) * \ + rot_base_impedance.transpose(); + K.block<3, 3>(3, 3) = + rot_base_impedance * \ + vic_command_data.stiffness.block<3, 3>(3, 3) * \ + rot_base_impedance.transpose(); + + Eigen::Matrix D = Eigen::Matrix::Zero(); + D.block<3, 3>(0, 0) = + rot_base_impedance * \ + vic_command_data.damping.block<3, 3>(0, 0) * \ + rot_base_impedance.transpose(); + D.block<3, 3>(3, 3) = + rot_base_impedance * \ + vic_command_data.damping.block<3, 3>(3, 3) * \ + rot_base_impedance.transpose(); + + Eigen::Matrix M = Eigen::Matrix::Zero(); + M.block<3, 3>(0, 0) = + rot_base_impedance * \ + vic_command_data.inertia.block<3, 3>(0, 0) * \ + rot_base_impedance.transpose(); + M.block<3, 3>(3, 3) = + rot_base_impedance * \ + vic_command_data.inertia.block<3, 3>(3, 3) * \ + rot_base_impedance.transpose(); + + Eigen::Matrix M_inv = M.inverse(); + + // Compute pose tracking errors + Eigen::Matrix error_pose; + error_pose.head(3) = + reference_compliant_frame.pose.translation() - \ + vic_input_data.robot_current_pose.translation(); + + auto R_angular_error = \ + reference_compliant_frame.pose.rotation() * \ + vic_input_data.robot_current_pose.rotation().transpose(); + auto angle_axis = Eigen::AngleAxisd(R_angular_error); + error_pose.tail(3) = angle_axis.angle() * angle_axis.axis(); + + // Compute velocity tracking errors in ft frame + Eigen::Matrix error_velocity = + reference_compliant_frame.velocity - \ + vic_input_data.robot_current_velocity; + + // Compute Kinematics and Dynamics + bool model_is_ok = dynamics_->calculate_jacobian( + vic_input_data.joint_state_position, + vic_input_data.control_frame, + J_ + ); + model_is_ok &= dynamics_->calculate_jacobian_derivative( + vic_input_data.joint_state_position, + vic_input_data.joint_state_velocity, + vic_input_data.control_frame, + J_dot_ + ); + J_pinv_ = (J_.transpose() * J_ + alpha_pinv_ * I_joint_space_).inverse() * J_.transpose(); + + model_is_ok &= dynamics_->calculate_inertia( + vic_input_data.joint_state_position, + M_joint_space_ + ); + + // Check if the model is ok + if (!model_is_ok) { + success = false; + RCLCPP_ERROR( + logger_, + "Failed to calculate kinematic / dynamic model!" + ); + } + + // External force at interaction frame (assumed to be control frame) + Eigen::Matrix F_ext = Eigen::Matrix::Zero(); + if (parameters_.vic.use_natural_robot_inertia) { + M_cartesian_space_ = (J_ * M_joint_space_.inverse() * J_.transpose()).inverse(); + M = M_cartesian_space_; + } else { + if (vic_input_data.has_ft_sensor()) { + F_ext = -vic_input_data.get_ft_sensor_wrench(); + } else { + RCLCPP_ERROR( + logger_, + "F/T sensor is required for inertia shaping! Setting wrenches to zero!" + ); + success &= false; + } + } + + if (parameters_.vic.use_natural_robot_inertia) { + // Simplified impedance controller without F/T sensor + // see https://www.diag.uniroma1.it/~deluca/rob2_en/15_ImpedanceControl.pdf (page 13) + raw_joint_command_effort_ = \ + M_joint_space_ * J_pinv_ * + (reference_compliant_frame.acceleration - J_dot_ * vic_input_data.joint_state_velocity) + \ + J_.transpose() * (K * error_pose + D * error_velocity); + } else { + // Implement "normal" impedance control + Eigen::Matrix commanded_cartesian_acc = reference_compliant_frame.acceleration + \ + M_inv * (K * error_pose + D * error_velocity - F_ext); + + // Compute joint command accelerations + vic_command_data.joint_command_acceleration = \ + J_pinv_ * (commanded_cartesian_acc - J_dot_ * vic_input_data.joint_state_velocity); + + // Compute joint command effort from desired joint acc. + raw_joint_command_effort_ = M_joint_space_.diagonal().asDiagonal() * + vic_command_data.joint_command_acceleration - \ + J_.transpose() * F_ext; + } + + // Filter joint command effort + // ------------------------------------------------ + double cutoff_freq_cmd = parameters_.filters.command_filter_cuttoff_freq; + if (cutoff_freq_cmd > 0.0) { + double cmd_filter_coefficient = 1.0 - exp(-dt * 2 * 3.14 * cutoff_freq_cmd); + for (size_t i = 0; i < static_cast( + vic_command_data.joint_command_effort.size()); i++) + { + vic_command_data.joint_command_effort(i) = filters::exponentialSmoothing( + vic_command_data.joint_command_effort(i), + raw_joint_command_effort_(i), + cmd_filter_coefficient + ); + } + } else { + // No smoothing otherwise + vic_command_data.joint_command_effort = raw_joint_command_effort_; + } + + // Set flags for available commands + // ------------------------------------------------ + vic_command_data.has_position_command = false; + vic_command_data.has_velocity_command = false; + vic_command_data.has_acceleration_command = false; + vic_command_data.has_effort_command = true; + + // Just to be safe + vic_command_data.joint_command_position = \ + vic_input_data.joint_state_position; + vic_command_data.joint_command_velocity.setZero(); + vic_command_data.joint_command_acceleration.setZero(); + + // Logging + // ------------------------------------------------ + + return success; +} + +bool FdImpedanceRule::reset_rule__internal_storage(const size_t num_joints) +{ + raw_joint_command_effort_ = Eigen::VectorXd::Zero(num_joints); + + J_ = Eigen::Matrix::Zero(6, num_joints); + J_pinv_ = Eigen::Matrix::Zero(num_joints, 6); + J_dot_ = Eigen::Matrix::Zero(6, num_joints); + + I_joint_space_ = \ + Eigen::Matrix::Identity(num_joints, num_joints); + M_joint_space_ = \ + Eigen::Matrix::Zero(num_joints, num_joints); + + M_cartesian_space_.setZero(); + return true; +} + +} // namespace cartesian_vic_controller + +#include "pluginlib/class_list_macros.hpp" + +PLUGINLIB_EXPORT_CLASS( + cartesian_vic_controller::FdImpedanceRule, + cartesian_vic_controller::CartesianVicRule +) diff --git a/cartesian_vic_controller/src/rules/vanilla_cartesian_admittance_rule.cpp b/cartesian_vic_controller/src/rules/vanilla_cartesian_admittance_rule.cpp index 299dc55..4e5dfdc 100644 --- a/cartesian_vic_controller/src/rules/vanilla_cartesian_admittance_rule.cpp +++ b/cartesian_vic_controller/src/rules/vanilla_cartesian_admittance_rule.cpp @@ -27,6 +27,7 @@ namespace cartesian_vic_controller controller_interface::return_type VanillaCartesianAdmittanceRule::init( const std::shared_ptr & parameter_handler) { + logger_ = rclcpp::get_logger("vanilla_cartesian_admittance_rule"); // Initialize CartesianVicRule control_mode_ = ControlMode::ADMITTANCE; auto ret = CartesianVicRule::init(parameter_handler); @@ -63,6 +64,9 @@ bool VanillaCartesianAdmittanceRule::compute_controls( const VicInputData & vic_input_data, VicCommandData & vic_command_data) { + bool success = true; + + // Get reference compliant frame at t_k const CompliantFrame & reference_compliant_frame = vic_input_data.reference_compliant_frames.get_compliant_frame(0); @@ -128,24 +132,32 @@ bool VanillaCartesianAdmittanceRule::compute_controls( reference_compliant_frame.velocity - \ vic_input_data.robot_current_velocity; - // External force at interaction frame (assumed to be control frame), expressed in the base frame - // (note that this is the measured force, the the generalized wrench used in VIC papers...) - Eigen::Matrix F_ext = vic_input_data.robot_current_wrench_at_ft_frame; + // External force at interaction frame (assumed to be control frame) + Eigen::Matrix F_ext = Eigen::Matrix::Zero(); + if (vic_input_data.has_ft_sensor()) { + F_ext = -vic_input_data.get_ft_sensor_wrench(); + } else { + success &= false; + RCLCPP_ERROR( + logger_, + "F/T sensor is required for inertia shaping! Setting wrenches to zero!" + ); + } // Compute admittance control law in the base frame // ------------------------------------------------ - // commanded_acc = p_ddot_desired + inv(M) * (K * err_p + D * err_p_dot + f_ext) + // commanded_acc = p_ddot_desired + inv(M) * (K * err_p + D * err_p_dot - f_ext) // where err_p = p_desired - p_current Eigen::Matrix commanded_cartesian_acc = reference_compliant_frame.acceleration + \ - M_inv * (K * error_pose + D * error_velocity + F_ext); + M_inv * (K * error_pose + D * error_velocity - F_ext); robot_command_twist_ += last_robot_commanded_twist_ + commanded_cartesian_acc * dt; auto previous_jnt_cmd_velocity = vic_command_data.joint_command_velocity; - bool success = dynamics_->convert_cartesian_deltas_to_joint_deltas( + success &= dynamics_->convert_cartesian_deltas_to_joint_deltas( vic_input_data.joint_state_position, robot_command_twist_, vic_input_data.control_frame, diff --git a/cartesian_vic_controller/src/rules/vanilla_cartesian_impedance_rule.cpp b/cartesian_vic_controller/src/rules/vanilla_cartesian_impedance_rule.cpp index 5eb1190..11140e6 100644 --- a/cartesian_vic_controller/src/rules/vanilla_cartesian_impedance_rule.cpp +++ b/cartesian_vic_controller/src/rules/vanilla_cartesian_impedance_rule.cpp @@ -28,6 +28,7 @@ namespace cartesian_vic_controller controller_interface::return_type VanillaCartesianImpedanceRule::init( const std::shared_ptr & parameter_handler) { + logger_ = rclcpp::get_logger("vanilla_cartesian_impedance_rule"); // Initialize CartesianVicRule control_mode_ = ControlMode::IMPEDANCE; auto ret = CartesianVicRule::init(parameter_handler); @@ -55,7 +56,6 @@ bool VanillaCartesianImpedanceRule::compute_controls( { bool success = true; // auto num_joints = vic_input_data.joint_state_position.size(); - auto logger = rclcpp::get_logger("VanillaCartesianImpedanceRule"); // Get reference compliant frame at t_k const CompliantFrame & reference_compliant_frame = @@ -103,8 +103,6 @@ bool VanillaCartesianImpedanceRule::compute_controls( vic_command_data.inertia.block<3, 3>(3, 3) * \ rot_base_impedance.transpose(); - Eigen::Matrix M_inv = M.inverse(); - // Compute pose tracking errors Eigen::Matrix error_pose; error_pose.head(3) = @@ -123,7 +121,16 @@ bool VanillaCartesianImpedanceRule::compute_controls( vic_input_data.robot_current_velocity; // External force at interaction frame (assumed to be control frame), expressed in the base frame - Eigen::Matrix F_ext = -vic_input_data.robot_current_wrench_at_ft_frame; + Eigen::Matrix F_ext = Eigen::Matrix::Zero(); + if (vic_input_data.has_ft_sensor()) { + F_ext = -vic_input_data.get_ft_sensor_wrench(); + } else { + success &= false; + RCLCPP_ERROR( + logger_, + "F/T sensor is required for inertia shaping! Setting wrenches to zero!" + ); + } // Compute Kinematics and Dynamics bool model_is_ok = dynamics_->calculate_jacobian( @@ -155,11 +162,25 @@ bool VanillaCartesianImpedanceRule::compute_controls( if (!model_is_ok) { success = false; RCLCPP_ERROR( - logger, + logger_, "Failed to calculate kinematic / dynamic model!" ); } + Eigen::Matrix M_inv; + if (parameters_.vic.use_natural_robot_inertia) { + M_inv = J_ * M_joint_space_.inverse() * J_.transpose(); + M = M_inv.inverse(); + RCLCPP_INFO_THROTTLE( + logger_, + internal_clock_, + 5000, + "Using natural robot inertia as desired inertia matrix." + ); + } else { + M_inv = M.inverse(); + } + // Compute impedance control law in the base frame // commanded_acc = p_ddot_desired + inv(M) * (K * err_p + D * err_p_dot - f_ext) // commanded_torques = M_joint_space @ J_pinv @ (commanded_acc - J_dot @ q_dot) @@ -192,7 +213,7 @@ bool VanillaCartesianImpedanceRule::compute_controls( } else { // Pure (small) damping in nullspace for stability RCLCPP_WARN_THROTTLE( - logger, + logger_, internal_clock_, 10000, // every 10 seconds "WARNING! nullspace impedance control is disabled!" @@ -218,7 +239,7 @@ bool VanillaCartesianImpedanceRule::compute_controls( // std::cout << "coriolis = " << coriolis_.transpose() << std::endl; } else { RCLCPP_WARN_THROTTLE( - logger, + logger_, internal_clock_, 10000, // every 10 seconds "WARNING! gravity compensation is disabled!" diff --git a/cartesian_vic_controller/test/test_cartesian_admittance_controller.cpp b/cartesian_vic_controller/test/test_cartesian_admittance_controller.cpp index 1c9a44c..4f432f3 100644 --- a/cartesian_vic_controller/test/test_cartesian_admittance_controller.cpp +++ b/cartesian_vic_controller/test/test_cartesian_admittance_controller.cpp @@ -34,7 +34,8 @@ INSTANTIATE_TEST_SUITE_P( ::testing::Values( "admittance.inertia", "admittance.selected_axes", "admittance.stiffness", "command_interfaces", "control.frame.id", "admittance.frame.id", - "fixed_world_frame.frame.id", "ft_sensor.frame.id", "ft_sensor.name", + "fixed_world_frame.frame.id", + "ft_sensor.is_enabled", "ft_sensor.frame.id", "ft_sensor.name", "gravity_compensation.CoG.pos", "gravity_compensation.frame.id", "joints", "kinematics.base", "kinematics.plugin_name", "kinematics.plugin_package", "kinematics.tip", "state_interfaces"));