diff --git a/admittance_controller/CMakeLists.txt b/admittance_controller/CMakeLists.txt index a8a8832fce..0697ea0c1b 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 6ff6cdae7a..ced713c9bc 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 36c027491c..2e67962883 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" @@ -52,11 +52,6 @@ struct AdmittanceTransforms // transformation from end effector frame to base link frame at reference + 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 @@ -98,9 +93,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_); @@ -117,9 +114,10 @@ class AdmittanceRule /** * Calculate all transforms needed for admittance control using the loader kinematics plugin. If * the transform does not exist in the kinematics model, then TF will be used for lookup. The - * return value is true if all transformation are calculated without an error \param[in] - * current_joint_state current joint state of the robot \param[in] reference_joint_state input - * joint state reference \param[out] success true if no calls to the kinematics interface fail + * return value is true if all transformation are calculated without an error + * \param[in] current_joint_state current joint state of the robot + * \param[in] reference_joint_state input joint state reference + * \param[out] success true if no calls to the kinematics interface fail */ bool get_all_transforms( const trajectory_msgs::msg::JointTrajectoryPoint & current_joint_state, @@ -162,29 +160,29 @@ 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: /** * 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 the admittance offset \param[in] dt controller period + * indicating if any of the kinematics plugin calls failed. + * \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 */ 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 \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 + * 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[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); @@ -197,8 +195,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}; @@ -206,12 +209,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 9b03924882..3c0fee36c0 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 @@ -103,8 +106,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(); @@ -119,8 +121,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); @@ -147,12 +147,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_); @@ -176,18 +170,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, expect the result in ft_sensor.frame (=FK-accessible frame) + 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); @@ -228,8 +229,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(); @@ -269,9 +270,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); @@ -304,32 +310,28 @@ 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) + // 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 + 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) { - wrench_world_(i) = filters::exponentialSmoothing( - new_wrench_base(i), wrench_world_(i), parameters_.ft_sensor.filter_coefficient); + 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.cpp b/admittance_controller/src/admittance_controller.cpp index c6a8168736..81d978a76d 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) { @@ -148,6 +153,7 @@ controller_interface::CallbackReturn AdmittanceController::on_configure( const rclcpp_lifecycle::State & /*previous_state*/) { command_joint_names_ = admittance_->parameters_.command_joints; + if (command_joint_names_.empty()) { command_joint_names_ = admittance_->parameters_.joints; @@ -262,6 +268,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 ee1efa67ab..0fcb269e45 100644 --- a/admittance_controller/src/admittance_controller_parameters.yaml +++ b/admittance_controller/src/admittance_controller_parameters.yaml @@ -64,44 +64,17 @@ 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: + meas_frame: id: { type: string, - description: "Specifies the control frame used for admittance calculation." + description: "Specifies the measurement frame of the force torque sensor (depends on sensor thickness)." } - 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: + control: 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." + description: "Specifies the control frame used for admittance calculation." } admittance: diff --git a/admittance_controller/test/test_admittance_controller.cpp b/admittance_controller/test/test_admittance_controller.cpp index fe1d3214e0..13a390b8f8 100644 --- a/admittance_controller/test/test_admittance_controller.cpp +++ b/admittance_controller/test/test_admittance_controller.cpp @@ -31,18 +31,28 @@ INSTANTIATE_TEST_SUITE_P( MissingMandatoryParameterDuringInit, 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", - "kinematics.plugin_name", "kinematics.plugin_package", "kinematics.tip", "state_interfaces")); + "chainable_command_interfaces", "command_interfaces", "control.frame.id", "ft_sensor.frame.id", + "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( - InvalidParameterDuringConfiguration, AdmittanceControllerTestParameterizedInvalidParameters, + MissingMandatoryParameterDuringConfiguration, + AdmittanceControllerTestParameterizedMissingConfigParameters, + ::testing::Values( + "sensor_filter_chain.filter2.params.CoG.pos", + "sensor_filter_chain.filter2.params.sensor_frame")); + +INSTANTIATE_TEST_SUITE_P( + InvalidParameterDuringInit, AdmittanceControllerTestParameterizedInvalidParameters, ::testing::Values( - // wrong length COG - std::make_tuple( - std::string("gravity_compensation.CoG.pos"), - rclcpp::ParameterValue(std::vector() = {1, 2, 3, 4})), // wrong length stiffness std::make_tuple( std::string("admittance.stiffness"), @@ -112,6 +122,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( @@ -230,14 +241,15 @@ 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_); + // 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++) @@ -249,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()); @@ -281,7 +328,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 8888cd700a..322781a993 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,18 +241,21 @@ 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_; - 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_; @@ -265,14 +268,19 @@ 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_; + // no sensor_frame transform, as it must be one kinematic link it is attached to + // 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); } @@ -280,8 +288,10 @@ class AdmittanceControllerTest : public ::testing::Test { // create a new subscriber auto subs_callback = [&](const ControllerStateMsg::SharedPtr) {}; - auto subscription = test_subscription_node_->create_subscription( - "/test_admittance_controller/status", 10, subs_callback); + std::string topic = controller_->get_node()->get_name(); + topic += "/status"; + auto subscription = + test_subscription_node_->create_subscription(topic, 10, subs_callback); // call update to publish the test value ASSERT_EQ( @@ -376,7 +386,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; @@ -385,6 +395,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}; @@ -393,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_; @@ -445,6 +457,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, diff --git a/admittance_controller/test/test_params.yaml b/admittance_controller/test/test_params.yaml index 22cbda2df9..0d67cb0636 100644 --- a/admittance_controller/test/test_params.yaml +++ b/admittance_controller/test/test_params.yaml @@ -49,31 +49,134 @@ 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 - filter_coefficient: 0.005 + 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: + # this filter converges almost immediately (similar to expSmoothing 0.995) + 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: "fixed_world_frame" + 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 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) + 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 + +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: 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 + 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 - gravity_compensation: + control: 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 + 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: