diff --git a/admittance_controller/include/admittance_controller/admittance_controller.hpp b/admittance_controller/include/admittance_controller/admittance_controller.hpp index 9b2a22abf7..58a403d8e0 100644 --- a/admittance_controller/include/admittance_controller/admittance_controller.hpp +++ b/admittance_controller/include/admittance_controller/admittance_controller.hpp @@ -143,6 +143,7 @@ class AdmittanceController : public controller_interface::ChainableControllerInt // ROS messages std::shared_ptr joint_command_msg_; std::shared_ptr goal_pose_msg_; + geometry_msgs::msg::PoseStamped last_goal_pose_; // real-time buffer realtime_tools::RealtimeBuffer> @@ -159,9 +160,17 @@ class AdmittanceController : public controller_interface::ChainableControllerInt // joint_state_: current joint readings from the hardware // reference_admittance_: reference value used by the controller after the admittance values are // applied ft_values_: values read from the force torque sensor - trajectory_msgs::msg::JointTrajectoryPoint reference_, joint_state_, reference_admittance_; + trajectory_msgs::msg::JointTrajectoryPoint reference_, joint_state_, reference_admittance_, joint_space_goal_; geometry_msgs::msg::Wrench ft_values_; + /** + * @brief Check whether two poses differ within a certain threshold. + */ + bool is_same_pose( + const geometry_msgs::msg::Pose & a, + const geometry_msgs::msg::Pose & b, + double eps = 1e-5); + /** * @brief Read values from hardware interfaces and set corresponding fields of state_current and * ft_values diff --git a/admittance_controller/include/admittance_controller/admittance_rule.hpp b/admittance_controller/include/admittance_controller/admittance_rule.hpp index cdc39af07e..b14077b4bf 100644 --- a/admittance_controller/include/admittance_controller/admittance_rule.hpp +++ b/admittance_controller/include/admittance_controller/admittance_rule.hpp @@ -111,31 +111,14 @@ class AdmittanceRule controller_interface::return_type reset(const size_t num_joints); /** - * Calculate transforms needed for admittance control using the loader kinematics plugin. If + * 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[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_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 + * 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_reference_state_transforms( + bool get_all_transforms( + const trajectory_msgs::msg::JointTrajectoryPoint & current_joint_state, const trajectory_msgs::msg::JointTrajectoryPoint & reference_joint_state); /** @@ -145,24 +128,6 @@ class AdmittanceRule */ void apply_parameters_update(); - /** - * 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 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 - */ - controller_interface::return_type 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_states); - /** * Calculate 'desired joint states' based on the 'measured force', 'reference joint state', and * 'current_joint_state'. @@ -192,10 +157,20 @@ class AdmittanceRule /** * Explanation - uses kinematics * - * \param[in] state_message message - * \param[out] state_message message + * \param[in] current_joint_state message */ geometry_msgs::msg::Pose initialize_goal_pose(const trajectory_msgs::msg::JointTrajectoryPoint & current_joint_state); + + /** + * @brief Convert cartesian goal pose to joint-space goal. + * \param[in] input PoseStamped goal expressed as ft_sensor link pose in base_link frame + * \param[in] current_state current joint state + * \param[out] output = current_state + delta_theta required to reach the goal pose + */ + void cartesian_goal_to_joint_space( + const geometry_msgs::msg::PoseStamped & input, + trajectory_msgs::msg::JointTrajectoryPoint & current_state, + trajectory_msgs::msg::JointTrajectoryPoint & output); public: // admittance config parameters diff --git a/admittance_controller/include/admittance_controller/admittance_rule_impl.hpp b/admittance_controller/include/admittance_controller/admittance_rule_impl.hpp index 421171e62b..7595a9010e 100644 --- a/admittance_controller/include/admittance_controller/admittance_rule_impl.hpp +++ b/admittance_controller/include/admittance_controller/admittance_rule_impl.hpp @@ -126,6 +126,58 @@ geometry_msgs::msg::Pose AdmittanceRule::initialize_goal_pose( return tf2::toMsg(admittance_transforms_.ref_base_ft_); } +void AdmittanceRule::cartesian_goal_to_joint_space( + const geometry_msgs::msg::PoseStamped & input, + trajectory_msgs::msg::JointTrajectoryPoint & current_state, + trajectory_msgs::msg::JointTrajectoryPoint & output) +{ + // calculate difference between `tool0` current and goal pose, + // both expressed in `base_link` frame. + Eigen::Isometry3d goal_base_ft; + tf2::fromMsg(input.pose, goal_base_ft); + + Eigen::Matrix delta_X; + delta_X.block<3, 1>(0, 0) = + goal_base_ft.translation() - admittance_transforms_.base_ft_.translation(); + + const auto R_current = admittance_transforms_.base_ft_.rotation(); + const auto R_desired = goal_base_ft.rotation(); + const auto R = R_desired * R_current.transpose(); + const auto angle_axis = Eigen::AngleAxisd(R); + delta_X.block<3, 1>(3, 0) = angle_axis.angle() * angle_axis.axis(); + + + std::cout<convert_cartesian_deltas_to_joint_deltas( + admittance_state_.current_joint_pos, delta_X, parameters_.control.frame.id, delta_joints); + + /**/ + if(success){ + std::cout<<"VectorXd: "<is_old(parameters_)) @@ -148,11 +200,17 @@ void AdmittanceRule::apply_parameters_update() } -bool AdmittanceRule::get_current_state_transforms( - const trajectory_msgs::msg::JointTrajectoryPoint & current_joint_state) +bool AdmittanceRule::get_all_transforms( + const trajectory_msgs::msg::JointTrajectoryPoint & current_joint_state, + const trajectory_msgs::msg::JointTrajectoryPoint & reference_joint_state) { - // get transforms at current configuration + // get reference transforms bool success = kinematics_->calculate_link_transform( + reference_joint_state.positions, parameters_.ft_sensor.frame.id, + admittance_transforms_.ref_base_ft_); + + // get transforms at current configuration + 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_); @@ -168,20 +226,6 @@ bool AdmittanceRule::get_current_state_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( @@ -196,8 +240,7 @@ controller_interface::return_type AdmittanceRule::update( { apply_parameters_update(); } - bool success = get_current_state_transforms(current_joint_state); - success &= get_reference_state_transforms(reference_joint_state); + bool success = get_all_transforms(current_joint_state, reference_joint_state); get_admittance_state_from_transforms(current_joint_state, measured_wrench); success &= calculate_admittance_rule(admittance_state_, dt); @@ -222,42 +265,6 @@ controller_interface::return_type AdmittanceRule::update( 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) diff --git a/admittance_controller/src/admittance_controller.cpp b/admittance_controller/src/admittance_controller.cpp index e11a376799..b15c170988 100644 --- a/admittance_controller/src/admittance_controller.cpp +++ b/admittance_controller/src/admittance_controller.cpp @@ -360,10 +360,12 @@ controller_interface::CallbackReturn AdmittanceController::on_activate( RCLCPP_ERROR(get_node()->get_logger(), "Failed to initialize goal_pose from current joint positions.\n"); return controller_interface::CallbackReturn::ERROR; } + last_goal_pose_.pose = goal_pose_msg_->pose; // Use current joint_state as a default reference last_reference_ = joint_state_; last_commanded_ = joint_state_; + joint_space_goal_ = joint_state_; reference_ = joint_state_; reference_admittance_ = joint_state_; @@ -394,11 +396,25 @@ controller_interface::return_type AdmittanceController::update_reference_from_su } } - // after initializing goal_pose_msg_, update it from subscribers only - // if another message exists + // if another message exists... if(*input_goal_pose_.readFromRT()) { goal_pose_msg_ = *input_goal_pose_.readFromRT(); + // ... and its a different goal... + if(is_same_pose(last_goal_pose_.pose, goal_pose_msg_->pose) == false) + { + // ... convert goal to joint-space and apply it. + admittance_->cartesian_goal_to_joint_space(*goal_pose_msg_, joint_state_, joint_space_goal_); + for (size_t i = 0; i < joint_space_goal_.positions.size(); ++i) + { + position_reference_[i].get() = joint_space_goal_.positions[i]; + } + for (size_t i = 0; i < joint_space_goal_.velocities.size(); ++i) + { + velocity_reference_[i].get() = joint_space_goal_.velocities[i]; + } + } + last_goal_pose_.pose = goal_pose_msg_->pose; } @@ -427,18 +443,7 @@ 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 - - /* 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_); + admittance_->update(joint_state_, ft_values_, reference_, period, reference_admittance_); // write calculated values to joint interfaces write_state_to_hardware(reference_admittance_); @@ -610,6 +615,25 @@ void AdmittanceController::read_state_reference_interfaces( last_reference_.velocities = state_reference.velocities; } +bool AdmittanceController::is_same_pose(const geometry_msgs::msg::Pose & a, + const geometry_msgs::msg::Pose & b, + double eps) +{ + // compare position + bool pos = std::abs(a.position.x - b.position.x) <= eps && + std::abs(a.position.y - b.position.y) <= eps && + std::abs(a.position.z - b.position.z) <= eps; + if (!pos) return false; + + // compare quaternion + bool rot = std::abs(a.orientation.w - b.orientation.w) <= eps && + std::abs(a.orientation.x - b.orientation.x) <= eps && + std::abs(a.orientation.y - b.orientation.y) <= eps && + std::abs(a.orientation.z - b.orientation.z) <= eps; + + return rot; +} + } // namespace admittance_controller #include "pluginlib/class_list_macros.hpp"