From 3778da6a6474a39aa9d06e2a53c6d9f433047bb2 Mon Sep 17 00:00:00 2001 From: Nikola Banovic Date: Wed, 31 Jul 2024 12:44:29 +0200 Subject: [PATCH] enable pose-only goal in AdmittanceRule --- .../admittance_controller/admittance_rule.hpp | 21 +++++++++++---- .../admittance_rule_impl.hpp | 26 ++++++++++++------- .../src/admittance_controller.cpp | 11 +++++++- 3 files changed, 43 insertions(+), 15 deletions(-) diff --git a/admittance_controller/include/admittance_controller/admittance_rule.hpp b/admittance_controller/include/admittance_controller/admittance_rule.hpp index a326b663d0..87c0ff96ff 100644 --- a/admittance_controller/include/admittance_controller/admittance_rule.hpp +++ b/admittance_controller/include/admittance_controller/admittance_rule.hpp @@ -29,6 +29,7 @@ #include "kinematics_interface/kinematics_interface.hpp" #include "pluginlib/class_loader.hpp" #include "trajectory_msgs/msg/joint_trajectory_point.hpp" +#include "geometry_msgs/msg/pose.hpp" namespace admittance_controller { @@ -111,13 +112,14 @@ 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_pose input ft sensor reference pose + * \param[out] success true if no calls to the kinematics interface fail */ bool get_all_transforms( const trajectory_msgs::msg::JointTrajectoryPoint & current_joint_state, - const trajectory_msgs::msg::JointTrajectoryPoint & reference_joint_state); + const geometry_msgs::msg::Pose & reference_pose); /** * Updates parameter_ struct if any parameters have changed since last update. Parameter dependent @@ -132,7 +134,7 @@ class AdmittanceRule * * \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] reference_joint_state input joint state reference + * \param[in] reference_pose input joint state reference * \param[in] period time in seconds since last controller update * \param[out] desired_joint_state joint state reference after the admittance offset is applied to * the input reference @@ -140,6 +142,7 @@ class AdmittanceRule controller_interface::return_type update( const trajectory_msgs::msg::JointTrajectoryPoint & current_joint_state, const geometry_msgs::msg::Wrench & measured_wrench, + const geometry_msgs::msg::Pose & reference_pose, const trajectory_msgs::msg::JointTrajectoryPoint & reference_joint_state, const rclcpp::Duration & period, trajectory_msgs::msg::JointTrajectoryPoint & desired_joint_states); @@ -152,6 +155,14 @@ class AdmittanceRule */ const control_msgs::msg::AdmittanceControllerState & get_controller_state(); + /** + * Explanation - uses kinematics + * + * \param[in] state_message message + * \param[out] state_message message + */ + geometry_msgs::msg::Pose initialize_goal_pose(const trajectory_msgs::msg::JointTrajectoryPoint & current_joint_state); + public: // admittance config parameters std::shared_ptr parameter_handler_; diff --git a/admittance_controller/include/admittance_controller/admittance_rule_impl.hpp b/admittance_controller/include/admittance_controller/admittance_rule_impl.hpp index 0e132c0761..1dcfc0431d 100644 --- a/admittance_controller/include/admittance_controller/admittance_rule_impl.hpp +++ b/admittance_controller/include/admittance_controller/admittance_rule_impl.hpp @@ -115,6 +115,15 @@ controller_interface::return_type AdmittanceRule::reset(const size_t num_joints) return controller_interface::return_type::OK; } +geometry_msgs::msg::Pose AdmittanceRule::initialize_goal_pose( + const trajectory_msgs::msg::JointTrajectoryPoint & current_joint_state) +{ + kinematics_->calculate_link_transform( + current_joint_state.positions, parameters_.ft_sensor.frame.id, + admittance_transforms_.ref_base_ft_); + return tf2::toMsg(admittance_transforms_.ref_base_ft_); +} + void AdmittanceRule::apply_parameters_update() { if (parameter_handler_->is_old(parameters_)) @@ -136,14 +145,14 @@ void AdmittanceRule::apply_parameters_update() } } + bool AdmittanceRule::get_all_transforms( const trajectory_msgs::msg::JointTrajectoryPoint & current_joint_state, - const trajectory_msgs::msg::JointTrajectoryPoint & reference_joint_state) + const geometry_msgs::msg::Pose & reference_pose) { - // get reference transforms - bool success = kinematics_->calculate_link_transform( - reference_joint_state.positions, parameters_.ft_sensor.frame.id, - admittance_transforms_.ref_base_ft_); + // get reference transforms + bool success=true; + tf2::fromMsg(reference_pose, admittance_transforms_.ref_base_ft_); // get transforms at current configuration success &= kinematics_->calculate_link_transform( @@ -167,7 +176,8 @@ bool AdmittanceRule::get_all_transforms( controller_interface::return_type AdmittanceRule::update( const trajectory_msgs::msg::JointTrajectoryPoint & current_joint_state, const geometry_msgs::msg::Wrench & measured_wrench, - const trajectory_msgs::msg::JointTrajectoryPoint & reference_joint_state, + const geometry_msgs::msg::Pose & reference_pose, + const trajectory_msgs::msg::JointTrajectoryPoint & reference_joint_state, const rclcpp::Duration & period, trajectory_msgs::msg::JointTrajectoryPoint & desired_joint_state) { const double dt = period.seconds(); @@ -176,9 +186,7 @@ controller_interface::return_type AdmittanceRule::update( { apply_parameters_update(); } - - bool success = get_all_transforms(current_joint_state, reference_joint_state); - + bool success = get_all_transforms(current_joint_state, reference_pose); // apply filter and update wrench_world_ vector Eigen::Matrix rot_world_sensor = admittance_transforms_.world_base_.rotation() * admittance_transforms_.base_ft_.rotation(); diff --git a/admittance_controller/src/admittance_controller.cpp b/admittance_controller/src/admittance_controller.cpp index 1718ded8e3..9770685810 100644 --- a/admittance_controller/src/admittance_controller.cpp +++ b/admittance_controller/src/admittance_controller.cpp @@ -404,8 +404,17 @@ controller_interface::return_type AdmittanceController::update_and_write_command // get all controller inputs read_state_from_hardware(joint_state_, ft_values_); + auto goal_pose = geometry_msgs::msg::Pose(); + /* world-tool0*/ + goal_pose.position.x = 1.53; + goal_pose.position.y = 0.9; + goal_pose.position.z = 1.5; + goal_pose.orientation.w = 0.0; + goal_pose.orientation.x = 0.707; + goal_pose.orientation.y = 0.0; + goal_pose.orientation.z = 0.707; // apply admittance control to reference to determine desired state - admittance_->update(joint_state_, ft_values_, reference_, period, reference_admittance_); + admittance_->update(joint_state_, ft_values_, goal_pose, reference_, period, reference_admittance_); // write calculated values to joint interfaces write_state_to_hardware(reference_admittance_);