From 165de4ce7db355947127b775149d4e9becf5b795 Mon Sep 17 00:00:00 2001 From: Nikola Banovic Date: Fri, 2 Aug 2024 13:43:55 +0200 Subject: [PATCH] overload admittance->update() to account for both joint_state and pose_stamped goals --- .../admittance_controller/admittance_rule.hpp | 52 ++++++++- .../admittance_rule_impl.hpp | 109 +++++++++++++----- .../src/admittance_controller.cpp | 13 ++- 3 files changed, 139 insertions(+), 35 deletions(-) diff --git a/admittance_controller/include/admittance_controller/admittance_rule.hpp b/admittance_controller/include/admittance_controller/admittance_rule.hpp index 2646eccd3b..cdc39af07e 100644 --- a/admittance_controller/include/admittance_controller/admittance_rule.hpp +++ b/admittance_controller/include/admittance_controller/admittance_rule.hpp @@ -111,16 +111,32 @@ class AdmittanceRule controller_interface::return_type reset(const size_t num_joints); /** - * Calculate all transforms needed for admittance control using the loader kinematics plugin. If + * Calculate 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[out] success true if no calls to the kinematics interface fail + */ + bool get_current_state_transforms( + const trajectory_msgs::msg::JointTrajectoryPoint & current_joint_state); + /** + * Calculate ft_sensor_link -> base_link transform 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] 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, + bool get_reference_state_transforms( const geometry_msgs::msg::PoseStamped & reference_pose); + /** + * Calculate ft_sensor_link -> base_link transform 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] reference_joint_state input reference joint state + * \param[out] success true if no calls to the kinematics interface fail + */ + bool get_reference_state_transforms( + const trajectory_msgs::msg::JointTrajectoryPoint & reference_joint_state); /** * Updates parameter_ struct if any parameters have changed since last update. Parameter dependent @@ -130,12 +146,12 @@ class AdmittanceRule void apply_parameters_update(); /** - * Calculate 'desired joint states' based on the 'measured force', 'reference joint state', and + * Calculate 'desired joint states' based on the 'measured force', 'reference goal pose', and * 'current_joint_state'. * * \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_pose input joint state reference + * \param[in] reference_pose input reference pose * \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 @@ -144,6 +160,23 @@ class AdmittanceRule const trajectory_msgs::msg::JointTrajectoryPoint & current_joint_state, const geometry_msgs::msg::Wrench & measured_wrench, const geometry_msgs::msg::PoseStamped & reference_pose, + const rclcpp::Duration & period, + trajectory_msgs::msg::JointTrajectoryPoint & desired_joint_states); + + /** + * Calculate 'desired joint states' based on the 'measured force', 'reference joint state', and + * 'current_joint_state'. + * + * \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] 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 + */ + controller_interface::return_type 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 rclcpp::Duration & period, trajectory_msgs::msg::JointTrajectoryPoint & desired_joint_states); @@ -170,6 +203,15 @@ class AdmittanceRule admittance_controller::Params parameters_; protected: + /** + * Populate the AdmittanceState member using the current AdmittanceTransforms + * \param[in] current_joint_state + * \param[in] measured_wrench + */ + void get_admittance_state_from_transforms( + const trajectory_msgs::msg::JointTrajectoryPoint & current_joint_state, + const geometry_msgs::msg::Wrench & measured_wrench); + /** * 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 diff --git a/admittance_controller/include/admittance_controller/admittance_rule_impl.hpp b/admittance_controller/include/admittance_controller/admittance_rule_impl.hpp index 2d077268c6..421171e62b 100644 --- a/admittance_controller/include/admittance_controller/admittance_rule_impl.hpp +++ b/admittance_controller/include/admittance_controller/admittance_rule_impl.hpp @@ -148,16 +148,11 @@ void AdmittanceRule::apply_parameters_update() } -bool AdmittanceRule::get_all_transforms( - const trajectory_msgs::msg::JointTrajectoryPoint & current_joint_state, - const geometry_msgs::msg::PoseStamped & reference_pose) +bool AdmittanceRule::get_current_state_transforms( + const trajectory_msgs::msg::JointTrajectoryPoint & current_joint_state) { - // get reference transforms - bool success=true; - tf2::fromMsg(reference_pose.pose, admittance_transforms_.ref_base_ft_); - // get transforms at current configuration - success &= kinematics_->calculate_link_transform( + bool success = kinematics_->calculate_link_transform( 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_); @@ -173,13 +168,26 @@ bool AdmittanceRule::get_all_transforms( return success; } +bool AdmittanceRule::get_reference_state_transforms( + const geometry_msgs::msg::PoseStamped & reference_pose) +{ + tf2::fromMsg(reference_pose.pose, admittance_transforms_.ref_base_ft_); + return true; +} +bool AdmittanceRule::get_reference_state_transforms( + const trajectory_msgs::msg::JointTrajectoryPoint & reference_joint_state) +{ + bool success = kinematics_->calculate_link_transform( + reference_joint_state.positions, parameters_.ft_sensor.frame.id, + admittance_transforms_.ref_base_ft_); + return success; +} // Update from reference joint states controller_interface::return_type AdmittanceRule::update( const trajectory_msgs::msg::JointTrajectoryPoint & current_joint_state, const geometry_msgs::msg::Wrench & measured_wrench, - const geometry_msgs::msg::PoseStamped & reference_pose, - const trajectory_msgs::msg::JointTrajectoryPoint & reference_joint_state, + const trajectory_msgs::msg::JointTrajectoryPoint & reference_joint_state, const rclcpp::Duration & period, trajectory_msgs::msg::JointTrajectoryPoint & desired_joint_state) { const double dt = period.seconds(); @@ -188,25 +196,9 @@ controller_interface::return_type AdmittanceRule::update( { apply_parameters_update(); } - 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(); - 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 - admittance_state_.wrench_base.block<3, 1>(0, 0) = - admittance_transforms_.world_base_.rotation().transpose() * wrench_world_.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); - - // Compute admittance control law - vec_to_eigen(current_joint_state.positions, admittance_state_.current_joint_pos); - admittance_state_.rot_base_control = admittance_transforms_.base_control_.rotation(); - admittance_state_.ref_trans_base_ft = admittance_transforms_.ref_base_ft_; - admittance_state_.ft_sensor_frame = parameters_.ft_sensor.frame.id; + bool success = get_current_state_transforms(current_joint_state); + success &= get_reference_state_transforms(reference_joint_state); + get_admittance_state_from_transforms(current_joint_state, measured_wrench); success &= calculate_admittance_rule(admittance_state_, dt); // if a failure occurred during any kinematics interface calls, return an error and don't @@ -227,10 +219,69 @@ controller_interface::return_type AdmittanceRule::update( desired_joint_state.accelerations[i] = reference_joint_state.accelerations[i] + admittance_state_.joint_acc[i]; } + return controller_interface::return_type::OK; +} + +controller_interface::return_type AdmittanceRule::update( + const trajectory_msgs::msg::JointTrajectoryPoint & current_joint_state, + const geometry_msgs::msg::Wrench & measured_wrench, + const geometry_msgs::msg::PoseStamped & reference_pose, + const rclcpp::Duration & period, trajectory_msgs::msg::JointTrajectoryPoint & desired_joint_state) +{ + const double dt = period.seconds(); + + if (parameters_.enable_parameter_update_without_reactivation) + { + apply_parameters_update(); + } + bool success = get_current_state_transforms(current_joint_state); + success &= get_reference_state_transforms(reference_pose); + get_admittance_state_from_transforms(current_joint_state, measured_wrench); + success &= calculate_admittance_rule(admittance_state_, dt); + + // if a failure occurred during any kinematics interface calls, return an error and + // remain at current state + if (!success) + { + desired_joint_state = current_joint_state; + return controller_interface::return_type::ERROR; + } + + // update joint desired joint state + for (size_t i = 0; i < num_joints_; ++i) + { + desired_joint_state.positions[i] = admittance_state_.joint_pos[i]; + desired_joint_state.velocities[i] = admittance_state_.joint_vel[i]; + desired_joint_state.accelerations[i] = admittance_state_.joint_acc[i]; + } return controller_interface::return_type::OK; } +void AdmittanceRule::get_admittance_state_from_transforms( + const trajectory_msgs::msg::JointTrajectoryPoint & current_joint_state, + const geometry_msgs::msg::Wrench & measured_wrench) +{ + // 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 + admittance_state_.wrench_base.block<3, 1>(0, 0) = + admittance_transforms_.world_base_.rotation().transpose() * wrench_world_.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); + + // Compute admittance control law + vec_to_eigen(current_joint_state.positions, admittance_state_.current_joint_pos); + admittance_state_.rot_base_control = admittance_transforms_.base_control_.rotation(); + admittance_state_.ref_trans_base_ft = admittance_transforms_.ref_base_ft_; + admittance_state_.ft_sensor_frame = parameters_.ft_sensor.frame.id; +} + bool AdmittanceRule::calculate_admittance_rule(AdmittanceState & admittance_state, double dt) { // Create stiffness matrix in base frame. The user-provided values of admittance_state.stiffness diff --git a/admittance_controller/src/admittance_controller.cpp b/admittance_controller/src/admittance_controller.cpp index 8caa517983..e11a376799 100644 --- a/admittance_controller/src/admittance_controller.cpp +++ b/admittance_controller/src/admittance_controller.cpp @@ -427,7 +427,18 @@ controller_interface::return_type AdmittanceController::update_and_write_command read_state_from_hardware(joint_state_, ft_values_); // apply admittance control to reference to determine desired state - admittance_->update(joint_state_, ft_values_, *goal_pose_msg_, reference_, period, reference_admittance_); + + /* some flag that determines whether we call the update() with goal_pose or with joint reference. + How can we know here whether the reference was updated from subscribers or from itnerfaces? + + if(updated_from_subscribers) + admittance_->update(joint_state_, ft_values_, *goal_pose_msg_, period, reference_admittance_); + else + admittance_->update(joint_state_, ft_values_, reference_, period, reference_admittance_); + + */ + + admittance_->update(joint_state_, ft_values_, *goal_pose_msg_, period, reference_admittance_); // write calculated values to joint interfaces write_state_to_hardware(reference_admittance_);