From a6c573dc0ad8aa41e2d6524c073197ebbbe2e2d8 Mon Sep 17 00:00:00 2001 From: Guillaume Walck Date: Wed, 15 Mar 2023 11:16:58 +0100 Subject: [PATCH 1/5] Switched to plugin-based filters --- admittance_controller/CMakeLists.txt | 1 + .../admittance_controller.hpp | 4 + .../admittance_controller/admittance_rule.hpp | 49 +++++------ .../admittance_rule_impl.hpp | 81 ++++++++----------- .../src/admittance_controller.cpp | 32 +++++--- .../src/admittance_controller_parameters.yaml | 32 -------- .../test/test_admittance_controller.cpp | 20 +++-- .../test/test_admittance_controller.hpp | 8 +- admittance_controller/test/test_params.yaml | 38 ++++----- 9 files changed, 125 insertions(+), 140 deletions(-) diff --git a/admittance_controller/CMakeLists.txt b/admittance_controller/CMakeLists.txt index 9ece5e7fb0..f1564ef37b 100644 --- a/admittance_controller/CMakeLists.txt +++ b/admittance_controller/CMakeLists.txt @@ -11,6 +11,7 @@ set(THIS_PACKAGE_INCLUDE_DEPENDS control_toolbox controller_interface Eigen3 + filters generate_parameter_library geometry_msgs hardware_interface diff --git a/admittance_controller/include/admittance_controller/admittance_controller.hpp b/admittance_controller/include/admittance_controller/admittance_controller.hpp index e4aef2cf3d..1e1f9fe61c 100644 --- a/admittance_controller/include/admittance_controller/admittance_controller.hpp +++ b/admittance_controller/include/admittance_controller/admittance_controller.hpp @@ -29,6 +29,7 @@ #include "admittance_controller/visibility_control.h" #include "control_msgs/msg/admittance_controller_state.hpp" #include "controller_interface/chainable_controller_interface.hpp" +#include "filters/filter_chain.hpp" #include "geometry_msgs/msg/pose_stamped.hpp" #include "geometry_msgs/msg/transform_stamped.hpp" #include "geometry_msgs/msg/wrench_stamped.hpp" @@ -146,6 +147,9 @@ class AdmittanceController : public controller_interface::ChainableControllerInt // admittance parameters std::shared_ptr parameter_handler_; + // filter chain for Wrench data + std::shared_ptr> filter_chain_; + // ROS messages std::shared_ptr joint_command_msg_; diff --git a/admittance_controller/include/admittance_controller/admittance_rule.hpp b/admittance_controller/include/admittance_controller/admittance_rule.hpp index 0f0aabb063..a49360473d 100644 --- a/admittance_controller/include/admittance_controller/admittance_rule.hpp +++ b/admittance_controller/include/admittance_controller/admittance_rule.hpp @@ -25,8 +25,8 @@ #include #include "control_msgs/msg/admittance_controller_state.hpp" -#include "control_toolbox/filters.hpp" #include "controller_interface/controller_interface.hpp" +#include "filters/filter_chain.hpp" #include "geometry_msgs/msg/wrench_stamped.hpp" #include "kinematics_interface/kinematics_interface.hpp" #include "pluginlib/class_loader.hpp" @@ -41,18 +41,14 @@ namespace admittance_controller { struct AdmittanceTransforms { - // transformation from force torque sensor frame to base link frame at reference joint angles + // transformation from FT sensor frame to base frame at ref joint angles Eigen::Isometry3d ref_base_ft_; - // transformation from force torque sensor frame to base link frame at reference + admittance offset joint angles + // transformation from FT sensor frame to base frame at ref + admittance offset joint angles Eigen::Isometry3d base_ft_; - // transformation from control frame to base link frame at reference + admittance offset joint angles + // transformation from control frame to base frame at ref + admittance offset joint angles Eigen::Isometry3d base_control_; - // transformation from end effector frame to base link frame at reference + admittance offset joint angles + // transformation from end effector frame to base frame at ref + admittance offset joint angles Eigen::Isometry3d base_tip_; - // transformation from center of gravity frame to base link frame at reference + admittance offset joint angles - Eigen::Isometry3d base_cog_; - // transformation from world frame to base link frame - Eigen::Isometry3d world_base_; }; struct AdmittanceState @@ -94,9 +90,11 @@ class AdmittanceRule { public: explicit AdmittanceRule( - const std::shared_ptr & parameter_handler) + const std::shared_ptr & parameter_handler, + const std::shared_ptr> & filter_chain) { parameter_handler_ = parameter_handler; + filter_chain_ = filter_chain; parameters_ = parameter_handler_->get_params(); num_joints_ = parameters_.joints.size(); admittance_state_ = AdmittanceState(num_joints_); @@ -156,6 +154,8 @@ class AdmittanceRule // admittance config parameters std::shared_ptr parameter_handler_; admittance_controller::Params parameters_; + // Filter chain for Wrench data + std::shared_ptr> filter_chain_; protected: /** @@ -169,17 +169,13 @@ class AdmittanceRule bool calculate_admittance_rule(AdmittanceState & admittance_state, double dt); /** - * Updates internal estimate of wrench in world frame `wrench_world_` given the new measurement `measured_wrench`, - * the sensor to base frame rotation `sensor_world_rot`, and the center of gravity frame to base frame rotation `cog_world_rot`. - * The `wrench_world_` estimate includes gravity compensation + * Updates internal estimate of wrench in ft frame `measured_wrench_filtered_` + * given the new measurement `measured_wrench` + * The `measured_wrench_filtered_` might include gravity compensation if such a filter is loaded * \param[in] measured_wrench most recent measured wrench from force torque sensor - * \param[in] sensor_world_rot rotation matrix from world frame to sensor frame - * \param[in] cog_world_rot rotation matrix from world frame to center of gravity frame + * \param[out] success false if processing failed (=filter update failed) */ - void process_wrench_measurements( - const geometry_msgs::msg::Wrench & measured_wrench, - const Eigen::Matrix & sensor_world_rot, - const Eigen::Matrix & cog_world_rot); + bool process_wrench_measurements(const geometry_msgs::msg::Wrench & measured_wrench); template void vec_to_eigen(const std::vector & data, T2 & matrix); @@ -192,8 +188,13 @@ class AdmittanceRule kinematics_loader_; std::unique_ptr kinematics_; - // filtered wrench in world frame - Eigen::Matrix wrench_world_; + // filtered wrench in base frame + Eigen::Matrix wrench_base_; + // input wrench in sensor frame + geometry_msgs::msg::WrenchStamped measured_wrench_; + // filtered wrench in sensor_frame + geometry_msgs::msg::WrenchStamped measured_wrench_filtered_; + Eigen::Matrix wrench_filtered_ft_; // admittance controllers internal state AdmittanceState admittance_state_{0}; @@ -201,12 +202,6 @@ class AdmittanceRule // transforms needed for admittance update AdmittanceTransforms admittance_transforms_; - // position of center of gravity in cog_frame - Eigen::Vector3d cog_pos_; - - // force applied to sensor due to weight of end effector - Eigen::Vector3d end_effector_weight_; - // ROS control_msgs::msg::AdmittanceControllerState state_message_; }; diff --git a/admittance_controller/include/admittance_controller/admittance_rule_impl.hpp b/admittance_controller/include/admittance_controller/admittance_rule_impl.hpp index 87c16e4787..557a385a02 100644 --- a/admittance_controller/include/admittance_controller/admittance_rule_impl.hpp +++ b/admittance_controller/include/admittance_controller/admittance_rule_impl.hpp @@ -22,8 +22,11 @@ #include #include +#include "geometry_msgs/msg/transform_stamped.hpp" #include "rclcpp/duration.hpp" #include "rclcpp/utilities.hpp" +#include "tf2_eigen/tf2_eigen.hpp" +#include "tf2_geometry_msgs/tf2_geometry_msgs.hpp" #include "tf2_ros/transform_listener.h" namespace admittance_controller @@ -100,8 +103,7 @@ controller_interface::return_type AdmittanceRule::reset(const size_t num_joints) admittance_transforms_ = AdmittanceTransforms(); // reset forces - wrench_world_.setZero(); - end_effector_weight_.setZero(); + wrench_base_.setZero(); // load/initialize Eigen types from parameters apply_parameters_update(); @@ -116,8 +118,6 @@ void AdmittanceRule::apply_parameters_update() parameters_ = parameter_handler_->get_params(); } // update param values - end_effector_weight_[2] = -parameters_.gravity_compensation.CoG.force; - vec_to_eigen(parameters_.gravity_compensation.CoG.pos, cog_pos_); vec_to_eigen(parameters_.admittance.mass, admittance_state_.mass); vec_to_eigen(parameters_.admittance.stiffness, admittance_state_.stiffness); vec_to_eigen(parameters_.admittance.selected_axes, admittance_state_.selected_axes); @@ -144,12 +144,6 @@ bool AdmittanceRule::get_all_transforms( current_joint_state.positions, parameters_.ft_sensor.frame.id, admittance_transforms_.base_ft_); success &= kinematics_->calculate_link_transform( current_joint_state.positions, parameters_.kinematics.tip, admittance_transforms_.base_tip_); - success &= kinematics_->calculate_link_transform( - current_joint_state.positions, parameters_.fixed_world_frame.frame.id, - admittance_transforms_.world_base_); - success &= kinematics_->calculate_link_transform( - current_joint_state.positions, parameters_.gravity_compensation.frame.id, - admittance_transforms_.base_cog_); success &= kinematics_->calculate_link_transform( current_joint_state.positions, parameters_.control.frame.id, admittance_transforms_.base_control_); @@ -173,18 +167,25 @@ controller_interface::return_type AdmittanceRule::update( bool success = get_all_transforms(current_joint_state, reference_joint_state); - // apply filter and update wrench_world_ vector - Eigen::Matrix rot_world_sensor = - admittance_transforms_.world_base_.rotation() * admittance_transforms_.base_ft_.rotation(); - Eigen::Matrix rot_world_cog = - admittance_transforms_.world_base_.rotation() * admittance_transforms_.base_cog_.rotation(); - process_wrench_measurements(measured_wrench, rot_world_sensor, rot_world_cog); - - // transform wrench_world_ into base frame + // process the wrench measurements + if (!process_wrench_measurements(measured_wrench)) + { + desired_joint_state = reference_joint_state; + return controller_interface::return_type::ERROR; + } + // fill the Wrench (there is no fromMsg for wrench) + wrench_filtered_ft_(0) = measured_wrench_filtered_.wrench.force.x; + wrench_filtered_ft_(1) = measured_wrench_filtered_.wrench.force.y; + wrench_filtered_ft_(2) = measured_wrench_filtered_.wrench.force.z; + wrench_filtered_ft_(3) = measured_wrench_filtered_.wrench.torque.x; + wrench_filtered_ft_(4) = measured_wrench_filtered_.wrench.torque.y; + wrench_filtered_ft_(5) = measured_wrench_filtered_.wrench.torque.z; + + // Rotate (and not transform) to the base frame, explanation lower admittance_state_.wrench_base.block<3, 1>(0, 0) = - admittance_transforms_.world_base_.rotation().transpose() * wrench_world_.block<3, 1>(0, 0); + admittance_transforms_.base_ft_.rotation() * wrench_filtered_ft_.block<3, 1>(0, 0); admittance_state_.wrench_base.block<3, 1>(3, 0) = - admittance_transforms_.world_base_.rotation().transpose() * wrench_world_.block<3, 1>(3, 0); + admittance_transforms_.base_ft_.rotation() * wrench_filtered_ft_.block<3, 1>(3, 0); // Compute admittance control law vec_to_eigen(current_joint_state.positions, admittance_state_.current_joint_pos); @@ -225,8 +226,8 @@ bool AdmittanceRule::calculate_admittance_rule(AdmittanceState & admittance_stat Eigen::Matrix K_rot = Eigen::Matrix::Zero(); K_pos.diagonal() = admittance_state.stiffness.block<3, 1>(0, 0); K_rot.diagonal() = admittance_state.stiffness.block<3, 1>(3, 0); - // Transform to the control frame - // A reference is here: https://users.wpi.edu/~jfu2/rbe502/files/force_control.pdf + // Rotate (and not transform) to the control frame, explanation can be found + // in the reference here: https://users.wpi.edu/~jfu2/rbe502/files/force_control.pdf // Force Control by Luigi Villani and Joris De Schutter // Page 200 K_pos = rot_base_control * K_pos * rot_base_control.transpose(); @@ -266,9 +267,14 @@ bool AdmittanceRule::calculate_admittance_rule(AdmittanceState & admittance_stat // zero out any forces in the control frame Eigen::Matrix F_control; + // rotate to control frame F_control.block<3, 1>(0, 0) = rot_base_control.transpose() * F_base.block<3, 1>(0, 0); F_control.block<3, 1>(3, 0) = rot_base_control.transpose() * F_base.block<3, 1>(3, 0); + + // select the axis to apply the zeroing to F_control = F_control.cwiseProduct(admittance_state.selected_axes); + + // rotate to base frame F_base.block<3, 1>(0, 0) = rot_base_control * F_control.block<3, 1>(0, 0); F_base.block<3, 1>(3, 0) = rot_base_control * F_control.block<3, 1>(3, 0); @@ -301,32 +307,13 @@ bool AdmittanceRule::calculate_admittance_rule(AdmittanceState & admittance_stat return success; } -void AdmittanceRule::process_wrench_measurements( - const geometry_msgs::msg::Wrench & measured_wrench, - const Eigen::Matrix & sensor_world_rot, - const Eigen::Matrix & cog_world_rot) +bool AdmittanceRule::process_wrench_measurements(const geometry_msgs::msg::Wrench & measured_wrench) { - Eigen::Matrix new_wrench; - new_wrench(0, 0) = measured_wrench.force.x; - new_wrench(1, 0) = measured_wrench.force.y; - new_wrench(2, 0) = measured_wrench.force.z; - new_wrench(0, 1) = measured_wrench.torque.x; - new_wrench(1, 1) = measured_wrench.torque.y; - new_wrench(2, 1) = measured_wrench.torque.z; - - // transform to world frame - Eigen::Matrix new_wrench_base = sensor_world_rot * new_wrench; - - // apply gravity compensation - new_wrench_base(2, 0) -= end_effector_weight_[2]; - new_wrench_base.block<3, 1>(0, 1) -= (cog_world_rot * cog_pos_).cross(end_effector_weight_); - - // apply smoothing filter - for (size_t i = 0; i < 6; ++i) - { - wrench_world_(i) = filters::exponentialSmoothing( - new_wrench_base(i), wrench_world_(i), parameters_.ft_sensor.filter_coefficient); - } + // pass the measured_wrench in its original frame, output will be in the same frame + measured_wrench_.header.frame_id = parameters_.ft_sensor.frame.id; + measured_wrench_.wrench = measured_wrench; + // apply the filter + return filter_chain_->update(measured_wrench_, measured_wrench_filtered_); } const control_msgs::msg::AdmittanceControllerState & AdmittanceRule::get_controller_state() diff --git a/admittance_controller/src/admittance_controller.cpp b/admittance_controller/src/admittance_controller.cpp index 573d8ba06a..f06546bcf9 100644 --- a/admittance_controller/src/admittance_controller.cpp +++ b/admittance_controller/src/admittance_controller.cpp @@ -24,7 +24,9 @@ #include #include "admittance_controller/admittance_rule_impl.hpp" +#include "filters/filter_chain.hpp" #include "geometry_msgs/msg/wrench.hpp" +#include "geometry_msgs/msg/wrench_stamped.hpp" #include "rcutils/logging_macros.h" #include "tf2_ros/buffer.h" #include "trajectory_msgs/msg/joint_trajectory_point.hpp" @@ -37,7 +39,10 @@ controller_interface::CallbackReturn AdmittanceController::on_init() try { parameter_handler_ = std::make_shared(get_node()); - admittance_ = std::make_unique(parameter_handler_); + filter_chain_ = std::make_unique>( + "geometry_msgs::msg::WrenchStamped"); + admittance_ = + std::make_unique(parameter_handler_, filter_chain_); } catch (const std::exception & e) { @@ -147,19 +152,16 @@ AdmittanceController::on_export_reference_interfaces() controller_interface::CallbackReturn AdmittanceController::on_configure( const rclcpp_lifecycle::State & /*previous_state*/) { - try - { - parameter_handler_ = std::make_shared(get_node()); - admittance_ = std::make_unique(parameter_handler_); - } - catch (const std::exception & e) + if (!admittance_) { RCLCPP_ERROR( - get_node()->get_logger(), "Exception thrown during init stage with message: %s \n", e.what()); - return controller_interface::CallbackReturn::ERROR; + get_node()->get_logger(), + "on_configure should not be called unless Init successfully completed"); + return CallbackReturn::ERROR; } command_joint_names_ = admittance_->parameters_.command_joints; + if (command_joint_names_.empty()) { command_joint_names_ = admittance_->parameters_.joints; @@ -274,6 +276,18 @@ controller_interface::CallbackReturn AdmittanceController::on_configure( get_interface_list(admittance_->parameters_.command_interfaces).c_str(), get_interface_list(admittance_->parameters_.state_interfaces).c_str()); + // configure filters + if (!filter_chain_->configure( + "sensor_filter_chain", get_node()->get_node_logging_interface(), + get_node()->get_node_parameters_interface())) + { + RCLCPP_ERROR( + get_node()->get_logger(), + "Could not configure sensor filter chain, please check if the " + "parameters are provided correctly."); + return CallbackReturn::FAILURE; + } + // setup subscribers and publishers auto joint_command_callback = [this](const std::shared_ptr msg) diff --git a/admittance_controller/src/admittance_controller_parameters.yaml b/admittance_controller/src/admittance_controller_parameters.yaml index 5bdc40e398..5347270a5e 100644 --- a/admittance_controller/src/admittance_controller_parameters.yaml +++ b/admittance_controller/src/admittance_controller_parameters.yaml @@ -64,11 +64,6 @@ admittance_controller: type: string, description: "Specifies the frame/link name of the force torque sensor." } - filter_coefficient: { - type: double, - default_value: 0.05, - description: "Specifies the filter coefficient for the sensor's exponential filter." - } control: frame: @@ -77,33 +72,6 @@ admittance_controller: description: "Specifies the control frame used for admittance calculation." } - fixed_world_frame: # Gravity points down (neg. Z) in this frame (Usually: world or base_link) - frame: - id: { - type: string, - description: "Specifies the world frame use for admittance calculation. Gravity must point down in this frame." - } - - gravity_compensation: - frame: - id: { - type: string, - description: "Specifies the frame which center of gravity (CoG) is defined in. Normally, the force torque sensor frame should be used." - } - CoG: - pos: { - type: double_array, - description: "Specifies the position of the center of gravity (CoG) of the end effector in the gravity compensation frame.", - validation: { - fixed_size<>: 3 - } - } - force: { - type: double, - default_value: 0.0, - description: "Specifies the weight of the end effector, e.g mass * 9.81." - } - admittance: selected_axes: { diff --git a/admittance_controller/test/test_admittance_controller.cpp b/admittance_controller/test/test_admittance_controller.cpp index d056e0406c..5a4beb5043 100644 --- a/admittance_controller/test/test_admittance_controller.cpp +++ b/admittance_controller/test/test_admittance_controller.cpp @@ -24,8 +24,16 @@ // Test on_configure returns ERROR when a required parameter is missing TEST_P(AdmittanceControllerTestParameterizedMissingParameters, one_parameter_is_missing) { - ASSERT_EQ(SetUpController(GetParam()), controller_interface::return_type::ERROR); - ASSERT_EQ(controller_->on_configure(rclcpp_lifecycle::State()), NODE_ERROR); + auto ret = SetUpController(GetParam()); + // additionally, test params required during configure only if init worked + if (ret == controller_interface::return_type::OK) + { + ASSERT_EQ(controller_->on_configure(rclcpp_lifecycle::State()), NODE_FAILURE); + } + else + { + ASSERT_EQ(ret, controller_interface::return_type::ERROR); + } } INSTANTIATE_TEST_SUITE_P( @@ -33,9 +41,9 @@ INSTANTIATE_TEST_SUITE_P( AdmittanceControllerTestParameterizedMissingParameters, ::testing::Values( "admittance.mass", "admittance.selected_axes", "admittance.stiffness", - "chainable_command_interfaces", "command_interfaces", "control.frame.id", - "fixed_world_frame.frame.id", "ft_sensor.frame.id", "ft_sensor.name", - "gravity_compensation.CoG.pos", "gravity_compensation.frame.id", "joints", "kinematics.base", + "chainable_command_interfaces", "command_interfaces", "control.frame.id", "ft_sensor.frame.id", + "ft_sensor.name", "sensor_filter_chain.filter2.params.CoG.pos", + "sensor_filter_chain.filter2.params.sensor_frame", "joints", "kinematics.base", "kinematics.plugin_name", "kinematics.plugin_package", "kinematics.tip", "state_interfaces")); INSTANTIATE_TEST_SUITE_P( @@ -43,7 +51,7 @@ INSTANTIATE_TEST_SUITE_P( ::testing::Values( // wrong length COG std::make_tuple( - std::string("gravity_compensation.CoG.pos"), + std::string("sensor_filter_chain.filter2.params.CoG.pos"), rclcpp::ParameterValue(std::vector() = {1, 2, 3, 4})), // wrong length stiffness std::make_tuple( diff --git a/admittance_controller/test/test_admittance_controller.hpp b/admittance_controller/test/test_admittance_controller.hpp index 499aec7de9..052c38cfe3 100644 --- a/admittance_controller/test/test_admittance_controller.hpp +++ b/admittance_controller/test/test_admittance_controller.hpp @@ -53,6 +53,8 @@ const double COMMON_THRESHOLD = 0.001; constexpr auto NODE_SUCCESS = rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn::SUCCESS; +constexpr auto NODE_FAILURE = + rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn::FAILURE; constexpr auto NODE_ERROR = rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn::ERROR; @@ -248,9 +250,10 @@ class AdmittanceControllerTest : public ::testing::Test transform_stamped.transform.rotation.w = 1; transform_stamped.child_frame_id = ik_base_frame_; + // world to kinematic base frame br.sendTransform(transform_stamped); - transform_stamped.child_frame_id = ik_tip_frame_; + // world to kinematic tip frame br.sendTransform(transform_stamped); transform_stamped.header.frame_id = ik_tip_frame_; @@ -263,14 +266,17 @@ class AdmittanceControllerTest : public ::testing::Test transform_stamped.transform.rotation.w = 1; transform_stamped.child_frame_id = control_frame_; + // kinematic tip to control frame br.sendTransform(transform_stamped); transform_stamped.transform.translation.z = 0.05; transform_stamped.child_frame_id = sensor_frame_; + // kinematic tip to sensor frame br.sendTransform(transform_stamped); transform_stamped.transform.translation.z = 0.2; transform_stamped.child_frame_id = endeffector_frame_; + // kinematic tip to end-effector frame br.sendTransform(transform_stamped); } diff --git a/admittance_controller/test/test_params.yaml b/admittance_controller/test/test_params.yaml index 22cbda2df9..3bd1355def 100644 --- a/admittance_controller/test/test_params.yaml +++ b/admittance_controller/test/test_params.yaml @@ -51,30 +51,32 @@ test_admittance_controller: frame: id: link_6 # tool0 Wrench measurements are in this frame external: false # force torque frame exists within URDF kinematic chain - filter_coefficient: 0.005 + sensor_filter_chain: + filter1: + type: "control_filters/LowPassFilterWrench" + name: "low_pass_filter" + params: + # coefficients make up for same value of alpha = 0.005of the exponentialSmooth filter + sampling_frequency: 100.0 + damping_frequency: 134.0 + damping_intensity: 1.0 + divider: 1 + filter2: + type: "control_filters/GravityCompensationWrench" + name: "gravity_compensation" + params: + world_frame: "base_link" + sensor_frame: "tool0" # tool0 Wrench measurements are in this frame + force_frame: "base_link" # gravity is expressed in this frame + CoG: + pos: [0.1, 0.0, 0.0] + force: [0.0, 0.0, -23.0] # -gravity_acc * mass control: frame: id: tool0 # Admittance calcs (displacement etc) are done in this frame. Usually the tool or end-effector external: false # control frame exists within URDF kinematic chain - fixed_world_frame: # Gravity points down (neg. Z) in this frame (Usually: world or base_link) - frame: - id: base_link # Admittance calcs (displacement etc) are done in this frame. Usually the tool or end-effector - external: false # control frame exists within URDF kinematic chain - - gravity_compensation: - frame: - id: tool0 - external: false - - CoG: # specifies the center of gravity of the end effector - pos: - - 0.1 # x - - 0.0 # y - - 0.0 # z - force: 23.0 # mass * 9.81 - admittance: selected_axes: - true # x From 8a1e58828433ff07c0a229676965d33ac38eb8b0 Mon Sep 17 00:00:00 2001 From: Guillaume Walck Date: Sat, 18 Mar 2023 17:58:03 +0100 Subject: [PATCH 2/5] Handled offset from ft sensor frame to meas frame Test currently fails at joint state matching commands, but this should be expected as there is gravity compensation with a 2.3 kg mass --- .../admittance_rule_impl.hpp | 23 +++++++++++++++---- .../src/admittance_controller_parameters.yaml | 5 ++++ .../test/test_admittance_controller.cpp | 22 ++++++++++-------- .../test/test_admittance_controller.hpp | 9 +++++--- admittance_controller/test/test_params.yaml | 11 +++++---- 5 files changed, 50 insertions(+), 20 deletions(-) diff --git a/admittance_controller/include/admittance_controller/admittance_rule_impl.hpp b/admittance_controller/include/admittance_controller/admittance_rule_impl.hpp index 557a385a02..f125baf57b 100644 --- a/admittance_controller/include/admittance_controller/admittance_rule_impl.hpp +++ b/admittance_controller/include/admittance_controller/admittance_rule_impl.hpp @@ -167,7 +167,7 @@ controller_interface::return_type AdmittanceRule::update( bool success = get_all_transforms(current_joint_state, reference_joint_state); - // process the wrench measurements + // process the wrench measurements, expect the result in ft_sensor.frame (=FK-accessible frame) if (!process_wrench_measurements(measured_wrench)) { desired_joint_state = reference_joint_state; @@ -309,11 +309,26 @@ bool AdmittanceRule::calculate_admittance_rule(AdmittanceState & admittance_stat bool AdmittanceRule::process_wrench_measurements(const geometry_msgs::msg::Wrench & measured_wrench) { - // pass the measured_wrench in its original frame, output will be in the same frame - measured_wrench_.header.frame_id = parameters_.ft_sensor.frame.id; + // pass the measured_wrench in its ft_sensor meas_frame + measured_wrench_.header.frame_id = parameters_.ft_sensor.meas_frame.id; measured_wrench_.wrench = measured_wrench; + // output should be ft_sensor frame (because kinematics is only up to there) + measured_wrench_filtered_.header.frame_id = parameters_.ft_sensor.frame.id; // apply the filter - return filter_chain_->update(measured_wrench_, measured_wrench_filtered_); + auto ret = filter_chain_->update(measured_wrench_, measured_wrench_filtered_); + // check the output wrench was correctly transformed into the desired frame + if (measured_wrench_filtered_.header.frame_id != parameters_.ft_sensor.frame.id) + { + RCLCPP_ERROR_ONCE( + rclcpp::get_logger("AdmittanceRule"), + "Wrench frame transformation missing.\n" + "If ft_sensor.frame != ft_sensor.meas_frame, kinematics has no idea about " + "ft_sensor.meas_frame.\n" + "Fix the frames or provide a filter that transforms the wrench from ft_sensor.meas_frame" + " to ft_sensor.frame"); + return false; + } + return ret; } const control_msgs::msg::AdmittanceControllerState & AdmittanceRule::get_controller_state() diff --git a/admittance_controller/src/admittance_controller_parameters.yaml b/admittance_controller/src/admittance_controller_parameters.yaml index 5347270a5e..63f3fd612f 100644 --- a/admittance_controller/src/admittance_controller_parameters.yaml +++ b/admittance_controller/src/admittance_controller_parameters.yaml @@ -64,6 +64,11 @@ admittance_controller: type: string, description: "Specifies the frame/link name of the force torque sensor." } + meas_frame: + id: { + type: string, + description: "Specifies the measurement frame of the force torque sensor (depends on sensor thickness)." + } control: frame: diff --git a/admittance_controller/test/test_admittance_controller.cpp b/admittance_controller/test/test_admittance_controller.cpp index 5a4beb5043..cb868bd812 100644 --- a/admittance_controller/test/test_admittance_controller.cpp +++ b/admittance_controller/test/test_admittance_controller.cpp @@ -114,6 +114,7 @@ TEST_F(AdmittanceControllerTest, all_parameters_set_configure_success) ASSERT_EQ(controller_->admittance_->parameters_.ft_sensor.name, ft_sensor_name_); ASSERT_EQ(controller_->admittance_->parameters_.kinematics.base, ik_base_frame_); ASSERT_EQ(controller_->admittance_->parameters_.ft_sensor.frame.id, sensor_frame_); + ASSERT_EQ(controller_->admittance_->parameters_.ft_sensor.meas_frame.id, sensor_meas_frame_); ASSERT_TRUE(!controller_->admittance_->parameters_.admittance.selected_axes.empty()); ASSERT_TRUE( @@ -232,14 +233,14 @@ TEST_F(AdmittanceControllerTest, publish_status_success) ControllerStateMsg msg; subscribe_and_get_messages(msg); - // // Check that wrench command are all zero since not used - // ASSERT_EQ(msg.wrench_base.header.frame_id, ik_base_frame_); - // ASSERT_EQ(msg.wrench_base.wrench.force.x, 0.0); - // ASSERT_EQ(msg.wrench_base.wrench.force.y, 0.0); - // ASSERT_TRUE(msg.wrench_base.wrench.force.z > 0.15); - // ASSERT_TRUE(msg.wrench_base.wrench.torque.x != 0.0); - // ASSERT_TRUE(msg.wrench_base.wrench.torque.y != 0.0); - // ASSERT_EQ(msg.wrench_base.wrench.torque.z, 0.0); + // // Check that wrench command match the compensation for -m*g =-23 and CoG 0.1 x + ASSERT_EQ(msg.wrench_base.header.frame_id, ik_base_frame_); + EXPECT_NEAR(msg.wrench_base.wrench.force.x, -20.713, COMMON_THRESHOLD); + EXPECT_NEAR(msg.wrench_base.wrench.force.y, 3.3487, COMMON_THRESHOLD); + EXPECT_NEAR(msg.wrench_base.wrench.force.z, -9.42043, COMMON_THRESHOLD); + EXPECT_NEAR(msg.wrench_base.wrench.torque.x, 0.7209, COMMON_THRESHOLD); + EXPECT_NEAR(msg.wrench_base.wrench.torque.y, -1.001, COMMON_THRESHOLD); + EXPECT_NEAR(msg.wrench_base.wrench.torque.z, -1.94115, COMMON_THRESHOLD); // // Check joint command message // for (auto i = 0ul; i < joint_names_.size(); i++) @@ -283,7 +284,10 @@ TEST_F(AdmittanceControllerTest, receive_message_and_publish_updated_status) controller_->update(rclcpp::Time(0), rclcpp::Duration::from_seconds(0.01)), controller_interface::return_type::OK); - EXPECT_NEAR(joint_command_values_[0], joint_state_values_[0], COMMON_THRESHOLD); + for (auto i = 0ul; i < joint_state_values_.size(); i++) + { + EXPECT_NEAR(joint_state_values_[i], joint_command_values_[i], COMMON_THRESHOLD); + } subscribe_and_get_messages(msg); } diff --git a/admittance_controller/test/test_admittance_controller.hpp b/admittance_controller/test/test_admittance_controller.hpp index 052c38cfe3..2dbec367d4 100644 --- a/admittance_controller/test/test_admittance_controller.hpp +++ b/admittance_controller/test/test_admittance_controller.hpp @@ -269,9 +269,11 @@ class AdmittanceControllerTest : public ::testing::Test // kinematic tip to control frame br.sendTransform(transform_stamped); + // no sensor_frame transform, as it must be one kinematic link it is attached to + transform_stamped.transform.translation.z = 0.05; - transform_stamped.child_frame_id = sensor_frame_; - // kinematic tip to sensor frame + transform_stamped.child_frame_id = sensor_meas_frame_; + // kinematic tip to sensor measurement frame br.sendTransform(transform_stamped); transform_stamped.transform.translation.z = 0.2; @@ -380,7 +382,7 @@ class AdmittanceControllerTest : public ::testing::Test bool hardware_state_has_offset_ = false; const std::string ik_base_frame_ = "base_link"; - const std::string ik_tip_frame_ = "tool0"; + const std::string ik_tip_frame_ = "link_6"; const std::string ik_group_name_ = "arm"; // const std::string robot_description_ = ros2_control_test_assets::valid_6d_robot_urdf; // const std::string robot_description_semantic_ = ros2_control_test_assets::valid_6d_robot_srdf; @@ -389,6 +391,7 @@ class AdmittanceControllerTest : public ::testing::Test const std::string endeffector_frame_ = "endeffector_frame"; const std::string fixed_world_frame_ = "fixed_world_frame"; const std::string sensor_frame_ = "link_6"; + const std::string sensor_meas_frame_ = "ft_mount"; std::array admittance_selected_axes_ = {true, true, true, true, true, true}; std::array admittance_mass_ = {5.5, 6.6, 7.7, 8.8, 9.9, 10.10}; diff --git a/admittance_controller/test/test_params.yaml b/admittance_controller/test/test_params.yaml index 3bd1355def..5cedea3a36 100644 --- a/admittance_controller/test/test_params.yaml +++ b/admittance_controller/test/test_params.yaml @@ -49,8 +49,11 @@ test_admittance_controller: ft_sensor: name: ft_sensor_name frame: - id: link_6 # tool0 Wrench measurements are in this frame + id: link_6 # Where the sensor is located in the kinematics (usually tool frame) external: false # force torque frame exists within URDF kinematic chain + meas_frame: + id: ft_mount # Wrench measurements are in this frame + external: true # not part of robot URDF, (depends on sensor thickness for instance) sensor_filter_chain: filter1: @@ -66,12 +69,12 @@ test_admittance_controller: type: "control_filters/GravityCompensationWrench" name: "gravity_compensation" params: - world_frame: "base_link" - sensor_frame: "tool0" # tool0 Wrench measurements are in this frame - force_frame: "base_link" # gravity is expressed in this frame + world_frame: "fixed_world_frame" + sensor_frame: "ft_mount" # CoG is defined in this frame in this frame CoG: pos: [0.1, 0.0, 0.0] force: [0.0, 0.0, -23.0] # -gravity_acc * mass + control: frame: id: tool0 # Admittance calcs (displacement etc) are done in this frame. Usually the tool or end-effector From 40d47f3b32aee93e1b555e77c48080e0a998cc09 Mon Sep 17 00:00:00 2001 From: Guillaume Walck Date: Mon, 3 Apr 2023 12:08:16 +0200 Subject: [PATCH 3/5] Split tests for init and config missing params --- .../test/test_admittance_controller.cpp | 38 ++++++++++--------- .../test/test_admittance_controller.hpp | 5 +++ 2 files changed, 26 insertions(+), 17 deletions(-) diff --git a/admittance_controller/test/test_admittance_controller.cpp b/admittance_controller/test/test_admittance_controller.cpp index cb868bd812..69d114dee8 100644 --- a/admittance_controller/test/test_admittance_controller.cpp +++ b/admittance_controller/test/test_admittance_controller.cpp @@ -21,30 +21,34 @@ #include #include -// Test on_configure returns ERROR when a required parameter is missing -TEST_P(AdmittanceControllerTestParameterizedMissingParameters, one_parameter_is_missing) +// Test on_init returns ERROR when a required parameter is missing +TEST_P(AdmittanceControllerTestParameterizedMissingParameters, one_control_parameter_is_missing) { - auto ret = SetUpController(GetParam()); - // additionally, test params required during configure only if init worked - if (ret == controller_interface::return_type::OK) - { - ASSERT_EQ(controller_->on_configure(rclcpp_lifecycle::State()), NODE_FAILURE); - } - else - { - ASSERT_EQ(ret, controller_interface::return_type::ERROR); - } + ASSERT_EQ(SetUpController(GetParam()), controller_interface::return_type::ERROR); } INSTANTIATE_TEST_SUITE_P( - MissingMandatoryParameterDuringConfiguration, - AdmittanceControllerTestParameterizedMissingParameters, + MissingMandatoryParameterDuringInit, AdmittanceControllerTestParameterizedMissingParameters, ::testing::Values( "admittance.mass", "admittance.selected_axes", "admittance.stiffness", "chainable_command_interfaces", "command_interfaces", "control.frame.id", "ft_sensor.frame.id", - "ft_sensor.name", "sensor_filter_chain.filter2.params.CoG.pos", - "sensor_filter_chain.filter2.params.sensor_frame", "joints", "kinematics.base", - "kinematics.plugin_name", "kinematics.plugin_package", "kinematics.tip", "state_interfaces")); + "ft_sensor.name", "joints", "kinematics.base", "kinematics.plugin_name", + "kinematics.plugin_package", "kinematics.tip", "state_interfaces")); + +// Test on_configure returns FAILURE when a required parameter is missing +TEST_P( + AdmittanceControllerTestParameterizedMissingConfigParameters, one_config_parameter_is_missing) +{ + SetUpController(GetParam()); + ASSERT_EQ(controller_->on_configure(rclcpp_lifecycle::State()), NODE_FAILURE); +} + +INSTANTIATE_TEST_SUITE_P( + MissingMandatoryParameterDuringConfiguration, + AdmittanceControllerTestParameterizedMissingConfigParameters, + ::testing::Values( + "sensor_filter_chain.filter2.params.CoG.pos", + "sensor_filter_chain.filter2.params.sensor_frame")); INSTANTIATE_TEST_SUITE_P( InvalidParameterDuringConfiguration, AdmittanceControllerTestParameterizedInvalidParameters, diff --git a/admittance_controller/test/test_admittance_controller.hpp b/admittance_controller/test/test_admittance_controller.hpp index 2dbec367d4..a7008d93ea 100644 --- a/admittance_controller/test/test_admittance_controller.hpp +++ b/admittance_controller/test/test_admittance_controller.hpp @@ -452,6 +452,11 @@ class AdmittanceControllerTestParameterizedMissingParameters std::map overrides_; }; +class AdmittanceControllerTestParameterizedMissingConfigParameters +: public AdmittanceControllerTestParameterizedMissingParameters +{ +}; + // From the tutorial: https://www.sandordargo.com/blog/2019/04/24/parameterized-testing-with-gtest class AdmittanceControllerTestParameterizedInvalidParameters : public AdmittanceControllerTest, From 97c889aa1f293b01ad38091abd8349d31e9f9516 Mon Sep 17 00:00:00 2001 From: Guillaume Walck Date: Fri, 4 Aug 2023 22:21:23 +0200 Subject: [PATCH 4/5] Adapted tests to pass with GravityCompensation New initial pose and matching valid frames to ease checks on GC/wrench Special parameters for test of first state with no mass to avoid deviation form GC --- .../test/test_admittance_controller.cpp | 50 +++++++-- .../test/test_admittance_controller.hpp | 35 +++--- admittance_controller/test/test_params.yaml | 106 +++++++++++++++++- 3 files changed, 165 insertions(+), 26 deletions(-) diff --git a/admittance_controller/test/test_admittance_controller.cpp b/admittance_controller/test/test_admittance_controller.cpp index 1f547d7755..9fb8076cf8 100644 --- a/admittance_controller/test/test_admittance_controller.cpp +++ b/admittance_controller/test/test_admittance_controller.cpp @@ -243,12 +243,13 @@ TEST_F(AdmittanceControllerTest, publish_status_success) // // Check that wrench command match the compensation for -m*g =-23 and CoG 0.1 x ASSERT_EQ(msg.wrench_base.header.frame_id, ik_base_frame_); - EXPECT_NEAR(msg.wrench_base.wrench.force.x, -20.713, COMMON_THRESHOLD); - EXPECT_NEAR(msg.wrench_base.wrench.force.y, 3.3487, COMMON_THRESHOLD); - EXPECT_NEAR(msg.wrench_base.wrench.force.z, -9.42043, COMMON_THRESHOLD); - EXPECT_NEAR(msg.wrench_base.wrench.torque.x, 0.7209, COMMON_THRESHOLD); - EXPECT_NEAR(msg.wrench_base.wrench.torque.y, -1.001, COMMON_THRESHOLD); - EXPECT_NEAR(msg.wrench_base.wrench.torque.z, -1.94115, COMMON_THRESHOLD); + // larger tolerance because initial pose does not make link_6 perfectly oriented like world + EXPECT_NEAR(msg.wrench_base.wrench.force.x, 0.0, 0.03); + EXPECT_NEAR(msg.wrench_base.wrench.force.y, 0.0, 0.03); + EXPECT_NEAR(msg.wrench_base.wrench.force.z, 23.0, 0.03); + EXPECT_NEAR(msg.wrench_base.wrench.torque.x, 0.1*23, 0.03); + EXPECT_NEAR(msg.wrench_base.wrench.torque.y, 0.0, 0.03); + EXPECT_NEAR(msg.wrench_base.wrench.torque.z, 0.0, 0.03); // // Check joint command message // for (auto i = 0ul; i < joint_names_.size(); i++) @@ -260,9 +261,44 @@ TEST_F(AdmittanceControllerTest, publish_status_success) // } } -TEST_F(AdmittanceControllerTest, receive_message_and_publish_updated_status) +TEST_F(AdmittanceControllerTest, compensation_success) { SetUpController(); + + ASSERT_EQ(controller_->on_configure(rclcpp_lifecycle::State()), NODE_SUCCESS); + ASSERT_EQ(controller_->on_activate(rclcpp_lifecycle::State()), NODE_SUCCESS); + // set the force on the force torque sensor to simulate gravity pulling + fts_state_values_ = {0.0, 0.0, -23.0, -0.1*23, 0, 0.0}; + broadcast_tfs(); + ASSERT_EQ( + controller_->update(rclcpp::Time(0), rclcpp::Duration::from_seconds(0.01)), + controller_interface::return_type::OK); + // after first update, Low-Pass filter output is null + ASSERT_EQ( + controller_->update(rclcpp::Time(0), rclcpp::Duration::from_seconds(0.01)), + controller_interface::return_type::OK); + // after second update, Low-Pass filter output not yet to 100 % output + ASSERT_EQ( + controller_->update(rclcpp::Time(0), rclcpp::Duration::from_seconds(0.01)), + controller_interface::return_type::OK); + + ControllerStateMsg msg; + subscribe_and_get_messages(msg); + + // // Check that wrench command match the compensation for -m*g =-23 and CoG 0.1 x + ASSERT_EQ(msg.wrench_base.header.frame_id, ik_base_frame_); + // larger tolerance because initial pose does not make link_6 perfectly oriented like world + EXPECT_NEAR(msg.wrench_base.wrench.force.x, 0.0, COMMON_THRESHOLD); + EXPECT_NEAR(msg.wrench_base.wrench.force.y, 0.0, COMMON_THRESHOLD); + EXPECT_NEAR(msg.wrench_base.wrench.force.z, 0.0, COMMON_THRESHOLD); + EXPECT_NEAR(msg.wrench_base.wrench.torque.x, 0.0, COMMON_THRESHOLD); + EXPECT_NEAR(msg.wrench_base.wrench.torque.y, 0.0, COMMON_THRESHOLD); + EXPECT_NEAR(msg.wrench_base.wrench.torque.z, 0.0, COMMON_THRESHOLD); +} + +TEST_F(AdmittanceControllerTest, receive_message_and_publish_updated_status) +{ + SetUpController("test_admittance_controller_no_mass"); rclcpp::executors::MultiThreadedExecutor executor; executor.add_node(controller_->get_node()->get_node_base_interface()); diff --git a/admittance_controller/test/test_admittance_controller.hpp b/admittance_controller/test/test_admittance_controller.hpp index fe11bcb90e..68a0a47063 100644 --- a/admittance_controller/test/test_admittance_controller.hpp +++ b/admittance_controller/test/test_admittance_controller.hpp @@ -145,11 +145,11 @@ class AdmittanceControllerTest : public ::testing::Test command_publisher_node_ = std::make_shared("command_publisher"); force_command_publisher_ = command_publisher_node_->create_publisher( - "/test_admittance_controller/force_references", rclcpp::SystemDefaultsQoS()); + "/test_admittance_controller_no_mass/force_references", rclcpp::SystemDefaultsQoS()); // pose_command_publisher_ =command_publisher_node_->create_publisher( // "/test_admittance_controller/pose_commands", rclcpp::SystemDefaultsQoS()); joint_command_publisher_ = command_publisher_node_->create_publisher( - "/test_admittance_controller/joint_references", rclcpp::SystemDefaultsQoS()); + "/test_admittance_controller_no_mass/joint_references", rclcpp::SystemDefaultsQoS()); test_subscription_node_ = std::make_shared("test_subscription_node"); test_broadcaster_node_ = std::make_shared("test_broadcaster_node"); @@ -241,17 +241,19 @@ class AdmittanceControllerTest : public ::testing::Test transform_stamped.header.stamp = test_broadcaster_node_->now(); transform_stamped.header.frame_id = fixed_world_frame_; - transform_stamped.transform.translation.x = 1.3; - transform_stamped.transform.translation.y = 0.5; - transform_stamped.transform.translation.z = 0.5; + // base should be at origin, so no offset + transform_stamped.child_frame_id = ik_base_frame_; + // world to kinematic base frame + br.sendTransform(transform_stamped); + // tip should have the same offset as tip at the chosen initial pose 0.613, -0.344, 0.924 + transform_stamped.transform.translation.x = 0.613; + transform_stamped.transform.translation.y = -0.344; + transform_stamped.transform.translation.z = 0.924; transform_stamped.transform.rotation.x = 0; transform_stamped.transform.rotation.y = 0; transform_stamped.transform.rotation.z = 0; transform_stamped.transform.rotation.w = 1; - - transform_stamped.child_frame_id = ik_base_frame_; - // world to kinematic base frame - br.sendTransform(transform_stamped); + transform_stamped.child_frame_id = ik_tip_frame_; // world to kinematic tip frame br.sendTransform(transform_stamped); @@ -270,13 +272,13 @@ class AdmittanceControllerTest : public ::testing::Test br.sendTransform(transform_stamped); // no sensor_frame transform, as it must be one kinematic link it is attached to - - transform_stamped.transform.translation.z = 0.05; + // the test robot has its last link joint rotation axis along x. so shift along x + transform_stamped.transform.translation.x = 0.0; transform_stamped.child_frame_id = sensor_meas_frame_; // kinematic tip to sensor measurement frame br.sendTransform(transform_stamped); - - transform_stamped.transform.translation.z = 0.2; + // the test robot has its last tip joint rotation axis along x. so shift along x + transform_stamped.transform.translation.x = 0.2; transform_stamped.child_frame_id = endeffector_frame_; // kinematic tip to end-effector frame br.sendTransform(transform_stamped); @@ -286,8 +288,10 @@ class AdmittanceControllerTest : public ::testing::Test { // create a new subscriber auto subs_callback = [&](const ControllerStateMsg::SharedPtr) {}; + std::string topic = controller_->get_node()->get_name(); + topic += "/status"; auto subscription = test_subscription_node_->create_subscription( - "/test_admittance_controller/status", 10, subs_callback); + topic, 10, subs_callback); // call update to publish the test value ASSERT_EQ( @@ -400,7 +404,8 @@ class AdmittanceControllerTest : public ::testing::Test std::array admittance_stiffness_ = {214.1, 214.2, 214.3, 214.4, 214.5, 214.6}; std::array joint_command_values_ = {0.0, 0.0, 0.0, 0.0, 0.0, 0.0}; - std::array joint_state_values_ = {1.1, 2.2, 3.3, 4.4, 5.5, 6.6}; + // pose that leads to link6 being almost oriented like base, at pos 0.613, -0.344, 0.924 + std::array joint_state_values_ = {0.573, -1.1, 0.9, 1.87, -0.60, -1.93}; std::array fts_state_values_ = {0.0, 0.0, 0.0, 0.0, 0.0, 0.0}; std::vector fts_state_names_; diff --git a/admittance_controller/test/test_params.yaml b/admittance_controller/test/test_params.yaml index 5cedea3a36..b83821282c 100644 --- a/admittance_controller/test/test_params.yaml +++ b/admittance_controller/test/test_params.yaml @@ -60,7 +60,7 @@ test_admittance_controller: type: "control_filters/LowPassFilterWrench" name: "low_pass_filter" params: - # coefficients make up for same value of alpha = 0.005of the exponentialSmooth filter + # this filter converges almost immediately (similar to expSmoothing 0.995) sampling_frequency: 100.0 damping_frequency: 134.0 damping_intensity: 1.0 @@ -70,10 +70,10 @@ test_admittance_controller: name: "gravity_compensation" params: world_frame: "fixed_world_frame" - sensor_frame: "ft_mount" # CoG is defined in this frame in this frame + sensor_frame: "ft_mount" CoG: - pos: [0.1, 0.0, 0.0] - force: [0.0, 0.0, -23.0] # -gravity_acc * mass + pos: [0.0, 0.1, 0.0] # in sensor_frame + force: [0.0, 0.0, -23.0] # -gravity_acc * mass in world_frame control: frame: @@ -114,3 +114,101 @@ test_admittance_controller: - 214.4 - 214.5 - 214.6 + +test_admittance_controller_no_mass: + # contains minimal needed parameters for kuka_kr6 with no mass at end-effector (causes deviations for some tests) + ros__parameters: + joints: + - joint1 + - joint2 + - joint3 + - joint4 + - joint5 + - joint6 + + command_interfaces: + - position + + state_interfaces: + - position + + chainable_command_interfaces: + - position + - velocity + + kinematics: + plugin_name: kinematics_interface_kdl/KinematicsInterfaceKDL + plugin_package: kinematics_interface + base: base_link # Assumed to be stationary + tip: tool0 + group_name: arm + alpha: 0.0005 + + ft_sensor: + name: ft_sensor_name + frame: + id: link_6 # Where the sensor is located in the kinematics (usually tool frame) + external: false # force torque frame exists within URDF kinematic chain + meas_frame: + id: ft_mount # Wrench measurements are in this frame + external: true # not part of robot URDF, (depends on sensor thickness for instance) + + sensor_filter_chain: + filter1: + type: "control_filters/LowPassFilterWrench" + name: "low_pass_filter" + params: + # coefficients make up for same value of alpha = 0.005 of the exponentialSmooth filter + sampling_frequency: 1000.0 + damping_frequency: 0.63 + damping_intensity: 1.0 + divider: 1 + filter2: + type: "control_filters/GravityCompensationWrench" + name: "gravity_compensation" + params: + world_frame: "fixed_world_frame" + sensor_frame: "ft_mount" + CoG: + pos: [0.0, 0.0, 0.0] # in sensor_frame + force: [0.0, 0.0, 0.0] # -gravity_acc * mass in world_frame + + control: + frame: + id: tool0 # Admittance calcs (displacement etc) are done in this frame. Usually the tool or end-effector + external: false # control frame exists within URDF kinematic chain + + admittance: + selected_axes: + - true # x + - true # y + - true # z + - true # rx + - true # ry + - true # rz + + # Having ".0" at the end is MUST, otherwise there is a loading error + # F = M*a + D*v + S*(x - x_d) + mass: + - 5.5 + - 6.6 + - 7.7 + - 8.8 + - 9.9 + - 10.10 + + damping_ratio: # damping can be used instead: zeta = D / (2 * sqrt( M * S )) + - 2.828427 # x + - 2.828427 # y + - 2.828427 # z + - 2.828427 # rx + - 2.828427 # ry + - 2.828427 # rz + + stiffness: + - 214.1 + - 214.2 + - 214.3 + - 214.4 + - 214.5 + - 214.6 \ No newline at end of file From aefd00d44c2c1900397989fd9fafd4de14f4322a Mon Sep 17 00:00:00 2001 From: Guillaume Walck Date: Fri, 4 Aug 2023 22:23:19 +0200 Subject: [PATCH 5/5] Lint --- .../include/admittance_controller/admittance_rule.hpp | 2 +- admittance_controller/test/test_admittance_controller.cpp | 4 ++-- admittance_controller/test/test_admittance_controller.hpp | 6 +++--- admittance_controller/test/test_params.yaml | 6 +++--- 4 files changed, 9 insertions(+), 9 deletions(-) diff --git a/admittance_controller/include/admittance_controller/admittance_rule.hpp b/admittance_controller/include/admittance_controller/admittance_rule.hpp index 627831fd28..2e67962883 100644 --- a/admittance_controller/include/admittance_controller/admittance_rule.hpp +++ b/admittance_controller/include/admittance_controller/admittance_rule.hpp @@ -168,7 +168,7 @@ class AdmittanceRule * Calculates the admittance rule from given the robot's current joint angles. The admittance * controller state input is updated with the new calculated values. A boolean value is returned * indicating if any of the kinematics plugin calls failed. - * \param[in] admittance_state contains all the information needed to calculate + * \param[in] admittance_state contains all the information needed to calculate * the admittance offset * \param[in] dt controller period * \param[out] success true if no calls to the kinematics interface fail diff --git a/admittance_controller/test/test_admittance_controller.cpp b/admittance_controller/test/test_admittance_controller.cpp index 9fb8076cf8..13a390b8f8 100644 --- a/admittance_controller/test/test_admittance_controller.cpp +++ b/admittance_controller/test/test_admittance_controller.cpp @@ -247,7 +247,7 @@ TEST_F(AdmittanceControllerTest, publish_status_success) EXPECT_NEAR(msg.wrench_base.wrench.force.x, 0.0, 0.03); EXPECT_NEAR(msg.wrench_base.wrench.force.y, 0.0, 0.03); EXPECT_NEAR(msg.wrench_base.wrench.force.z, 23.0, 0.03); - EXPECT_NEAR(msg.wrench_base.wrench.torque.x, 0.1*23, 0.03); + EXPECT_NEAR(msg.wrench_base.wrench.torque.x, 0.1 * 23, 0.03); EXPECT_NEAR(msg.wrench_base.wrench.torque.y, 0.0, 0.03); EXPECT_NEAR(msg.wrench_base.wrench.torque.z, 0.0, 0.03); @@ -268,7 +268,7 @@ TEST_F(AdmittanceControllerTest, compensation_success) ASSERT_EQ(controller_->on_configure(rclcpp_lifecycle::State()), NODE_SUCCESS); ASSERT_EQ(controller_->on_activate(rclcpp_lifecycle::State()), NODE_SUCCESS); // set the force on the force torque sensor to simulate gravity pulling - fts_state_values_ = {0.0, 0.0, -23.0, -0.1*23, 0, 0.0}; + fts_state_values_ = {0.0, 0.0, -23.0, -0.1 * 23, 0, 0.0}; broadcast_tfs(); ASSERT_EQ( controller_->update(rclcpp::Time(0), rclcpp::Duration::from_seconds(0.01)), diff --git a/admittance_controller/test/test_admittance_controller.hpp b/admittance_controller/test/test_admittance_controller.hpp index 68a0a47063..322781a993 100644 --- a/admittance_controller/test/test_admittance_controller.hpp +++ b/admittance_controller/test/test_admittance_controller.hpp @@ -253,7 +253,7 @@ class AdmittanceControllerTest : public ::testing::Test transform_stamped.transform.rotation.y = 0; transform_stamped.transform.rotation.z = 0; transform_stamped.transform.rotation.w = 1; - + transform_stamped.child_frame_id = ik_tip_frame_; // world to kinematic tip frame br.sendTransform(transform_stamped); @@ -290,8 +290,8 @@ class AdmittanceControllerTest : public ::testing::Test auto subs_callback = [&](const ControllerStateMsg::SharedPtr) {}; std::string topic = controller_->get_node()->get_name(); topic += "/status"; - auto subscription = test_subscription_node_->create_subscription( - topic, 10, subs_callback); + auto subscription = + test_subscription_node_->create_subscription(topic, 10, subs_callback); // call update to publish the test value ASSERT_EQ( diff --git a/admittance_controller/test/test_params.yaml b/admittance_controller/test/test_params.yaml index b83821282c..0d67cb0636 100644 --- a/admittance_controller/test/test_params.yaml +++ b/admittance_controller/test/test_params.yaml @@ -70,7 +70,7 @@ test_admittance_controller: name: "gravity_compensation" params: world_frame: "fixed_world_frame" - sensor_frame: "ft_mount" + sensor_frame: "ft_mount" CoG: pos: [0.0, 0.1, 0.0] # in sensor_frame force: [0.0, 0.0, -23.0] # -gravity_acc * mass in world_frame @@ -168,7 +168,7 @@ test_admittance_controller_no_mass: name: "gravity_compensation" params: world_frame: "fixed_world_frame" - sensor_frame: "ft_mount" + sensor_frame: "ft_mount" CoG: pos: [0.0, 0.0, 0.0] # in sensor_frame force: [0.0, 0.0, 0.0] # -gravity_acc * mass in world_frame @@ -211,4 +211,4 @@ test_admittance_controller_no_mass: - 214.3 - 214.4 - 214.5 - - 214.6 \ No newline at end of file + - 214.6