-
Notifications
You must be signed in to change notification settings - Fork 43
Commit
This commit does not belong to any branch on this repository, and may belong to a fork outside of the repository.
moved the FT estimation into an asynchronous thread (#213)
- Loading branch information
Showing
13 changed files
with
202 additions
and
96 deletions.
There are no files selected for viewing
This file contains bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
This file contains bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
This file contains bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
This file contains bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
This file contains bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
This file contains bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
This file contains bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
| Original file line number | Diff line number | Diff line change |
|---|---|---|
| @@ -1,41 +1,54 @@ | ||
| #include "lbr_fri_ros2/ft_estimator.hpp" | ||
|
|
||
| namespace lbr_fri_ros2 { | ||
| FTEstimator::FTEstimator(const std::string &robot_description, const std::string &chain_root, | ||
| const std::string &chain_tip, const_cart_array_t_ref f_ext_th) | ||
| : f_ext_th_(f_ext_th) { | ||
| FTEstimatorImpl::FTEstimatorImpl(const std::string &robot_description, | ||
| const std::string &chain_root, const std::string &chain_tip, | ||
| const_cart_array_t_ref f_ext_th, const double &damping) | ||
| : f_ext_th_(f_ext_th), damping_(damping) { | ||
| kinematics_ptr_ = std::make_unique<Kinematics>(robot_description, chain_root, chain_tip); | ||
| reset(); | ||
| } | ||
|
|
||
| void FTEstimator::compute(const_jnt_array_t_ref measured_joint_position, | ||
| const_jnt_array_t_ref external_torque, cart_array_t_ref f_ext, | ||
| const double &damping) { | ||
| tau_ext_ = Eigen::Map<const Eigen::Matrix<double, N_JNTS, 1>>(external_torque.data()); | ||
| auto jacobian = kinematics_ptr_->compute_jacobian(measured_joint_position); | ||
| jacobian_inv_ = pinv(jacobian.data, damping); | ||
| f_ext_ = jacobian_inv_.transpose() * tau_ext_; | ||
| void FTEstimatorImpl::compute() { | ||
| auto jacobian = kinematics_ptr_->compute_jacobian(q_); | ||
| jacobian_inv_ = pinv(jacobian.data, damping_); | ||
| f_ext_raw_ = jacobian_inv_.transpose() * tau_ext_; | ||
| int i = -1; | ||
| f_ext_ = f_ext_raw_.unaryExpr([&](double v) { | ||
| ++i; | ||
| if (std::abs(v) < f_ext_th_[i]) { | ||
| return 0.; | ||
| } else { | ||
| return std::copysign(1., v) * (std::abs(v) - f_ext_th_[i]); | ||
| } | ||
| }); | ||
|
|
||
| // rotate into chain tip frame | ||
| auto chain_tip_frame = kinematics_ptr_->compute_fk(measured_joint_position); | ||
| f_ext_.topRows(3) = Eigen::Matrix3d::Map(chain_tip_frame.M.data) * f_ext_.topRows(3); | ||
| f_ext_.bottomRows(3) = Eigen::Matrix3d::Map(chain_tip_frame.M.data) * f_ext_.bottomRows(3); | ||
|
|
||
| Eigen::Map<Eigen::Matrix<double, CARTESIAN_DOF, 1>>(f_ext.data()) = f_ext_; | ||
|
|
||
| // threshold force-torque | ||
| std::transform(f_ext.begin(), f_ext.end(), f_ext_th_.begin(), f_ext.begin(), | ||
| [](const double &f_ext_i, const double &f_ext_th_i) { | ||
| if (std::abs(f_ext_i) < f_ext_th_i) { | ||
| return 0.; | ||
| } else { | ||
| return std::copysign(1., f_ext_i) * (std::abs(f_ext_i) - f_ext_th_i); | ||
| } | ||
| }); | ||
| auto chain_tip_frame = kinematics_ptr_->compute_fk(q_); | ||
| f_ext_tf_.topRows(3) = Eigen::Matrix3d::Map(chain_tip_frame.M.data) * f_ext_.topRows(3); | ||
| f_ext_tf_.bottomRows(3) = Eigen::Matrix3d::Map(chain_tip_frame.M.data) * f_ext_.bottomRows(3); | ||
| } | ||
|
|
||
| void FTEstimator::reset() { | ||
| void FTEstimatorImpl::reset() { | ||
| std::for_each(q_.begin(), q_.end(), [](double &q_i) { q_i = 0.; }); | ||
| tau_ext_.setZero(); | ||
| f_ext_raw_.setZero(); | ||
| f_ext_.setZero(); | ||
| f_ext_tf_.setZero(); | ||
| jacobian_inv_.setZero(); | ||
| } | ||
|
|
||
| FTEstimator::FTEstimator(const std::shared_ptr<FTEstimatorImpl> ft_estimator_impl_ptr, | ||
| const std::uint16_t &update_rate) | ||
| : ft_estimator_impl_ptr_(ft_estimator_impl_ptr), update_rate_(update_rate) {} | ||
|
|
||
| void FTEstimator::perform_work_() { | ||
| running_ = true; | ||
| while (rclcpp::ok() && !should_stop_) { | ||
| auto start = std::chrono::high_resolution_clock::now(); | ||
| ft_estimator_impl_ptr_->compute(); | ||
| std::this_thread::sleep_until(start + std::chrono::nanoseconds(static_cast<int>( | ||
| 1.e9 / static_cast<double>(update_rate_)))); | ||
| } | ||
| }; | ||
| } // namespace lbr_fri_ros2 |
Oops, something went wrong.