Skip to content

Commit dae64a8

Browse files
mhubiimhubii
committed
added a kinematics class
1 parent 0115ba2 commit dae64a8

File tree

5 files changed

+139
-46
lines changed

5 files changed

+139
-46
lines changed

‎lbr_fri_ros2/CMakeLists.txt

+1
Original file line numberDiff line numberDiff line change
@@ -35,6 +35,7 @@ add_library(lbr_fri_ros2
3535
src/command_guard.cpp
3636
src/filters.cpp
3737
src/ft_estimator.cpp
38+
src/kinematics.cpp
3839
)
3940

4041
target_include_directories(lbr_fri_ros2

‎lbr_fri_ros2/include/lbr_fri_ros2/ft_estimator.hpp

+8-22
Original file line numberDiff line numberDiff line change
@@ -4,20 +4,17 @@
44
#include <algorithm>
55
#include <array>
66
#include <cmath>
7+
#include <memory>
78
#include <string>
89

910
#include "eigen3/Eigen/Core"
10-
#include "kdl/chain.hpp"
11-
#include "kdl/chainfksolverpos_recursive.hpp"
12-
#include "kdl/chainjnttojacsolver.hpp"
13-
#include "kdl/tree.hpp"
14-
#include "kdl_parser/kdl_parser.hpp"
1511
#include "rclcpp/logger.hpp"
1612
#include "rclcpp/logging.hpp"
1713

1814
#include "friLBRState.h"
1915

2016
#include "lbr_fri_idl/msg/lbr_state.hpp"
17+
#include "lbr_fri_ros2/kinematics.hpp"
2118
#include "lbr_fri_ros2/pinv.hpp"
2219

2320
namespace lbr_fri_ros2 {
@@ -30,8 +27,7 @@ class FTEstimator {
3027
using const_ext_tau_array_t_ref = const ext_tau_array_t &;
3128

3229
public:
33-
static constexpr uint8_t CARTESIAN_DOF = 6;
34-
using cart_array_t = std::array<double, CARTESIAN_DOF>;
30+
using cart_array_t = std::array<double, Kinematics::CARTESIAN_DOF>;
3531
using cart_array_t_ref = cart_array_t &;
3632
using const_cart_array_t_ref = const cart_array_t &;
3733

@@ -47,24 +43,14 @@ class FTEstimator {
4743
// force threshold
4844
cart_array_t f_ext_th_;
4945

50-
KDL::Tree tree_;
51-
KDL::Chain chain_;
52-
53-
// solvers
54-
std::unique_ptr<KDL::ChainJntToJacSolver> jacobian_solver_;
55-
std::unique_ptr<KDL::ChainFkSolverPos_recursive> fk_solver_;
56-
57-
// robot state
58-
KDL::JntArray q_;
59-
60-
// forward kinematics
61-
KDL::Frame chain_tip_frame_;
46+
// kinematics
47+
std::unique_ptr<Kinematics> kinematics_;
6248

6349
// force estimation
64-
KDL::Jacobian jacobian_;
65-
Eigen::Matrix<double, KUKA::FRI::LBRState::NUMBER_OF_JOINTS, CARTESIAN_DOF> jacobian_inv_;
50+
Eigen::Matrix<double, KUKA::FRI::LBRState::NUMBER_OF_JOINTS, Kinematics::CARTESIAN_DOF>
51+
jacobian_inv_;
6652
Eigen::Matrix<double, KUKA::FRI::LBRState::NUMBER_OF_JOINTS, 1> tau_ext_;
67-
Eigen::Matrix<double, CARTESIAN_DOF, 1> f_ext_;
53+
Eigen::Matrix<double, Kinematics::CARTESIAN_DOF, 1> f_ext_;
6854
};
6955
} // namespace lbr_fri_ros2
7056
#endif // LBR_FRI_ROS2__FT_ESTIMATOR_HPP_

‎lbr_fri_ros2/include/lbr_fri_ros2/kinematics.hpp

+58
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,58 @@
1+
#ifndef LBR_FRI_ROS2__KINEMATICS_HPP_
2+
#define LBR_FRI_ROS2__KINEMATICS_HPP_
3+
4+
#include <string>
5+
6+
#include "eigen3/Eigen/Core"
7+
#include "kdl/chain.hpp"
8+
#include "kdl/chainfksolverpos_recursive.hpp"
9+
#include "kdl/chainjnttojacsolver.hpp"
10+
#include "kdl/tree.hpp"
11+
#include "kdl_parser/kdl_parser.hpp"
12+
#include "rclcpp/logger.hpp"
13+
#include "rclcpp/logging.hpp"
14+
15+
#include "friLBRState.h"
16+
17+
#include "lbr_fri_idl/msg/lbr_state.hpp"
18+
19+
namespace lbr_fri_ros2 {
20+
class Kinematics {
21+
protected:
22+
static constexpr char LOGGER_NAME[] = "lbr_fri_ros2::Kinematics";
23+
24+
public:
25+
static constexpr uint8_t CARTESIAN_DOF = 6;
26+
using jnt_pos_array_t = lbr_fri_idl::msg::LBRState::_measured_joint_position_type;
27+
using const_jnt_pos_array_t_ref = const jnt_pos_array_t &;
28+
29+
Kinematics(const std::string &robot_description, const std::string &chain_root = "lbr_link_0",
30+
const std::string &chain_tip = "lbr_link_ee");
31+
32+
// internally computes the jacobian and return a reference to it
33+
const KDL::Jacobian &compute_jacobian(const_jnt_pos_array_t_ref q);
34+
const KDL::Jacobian &compute_jacobian(const std::vector<double> &q);
35+
36+
// forward kinematics
37+
const KDL::Frame &compute_fk(const_jnt_pos_array_t_ref q);
38+
const KDL::Frame &compute_fk(const std::vector<double> &q);
39+
40+
protected:
41+
KDL::Tree tree_;
42+
KDL::Chain chain_;
43+
44+
// solvers
45+
std::unique_ptr<KDL::ChainJntToJacSolver> jacobian_solver_;
46+
std::unique_ptr<KDL::ChainFkSolverPos_recursive> fk_solver_;
47+
48+
// robot state
49+
KDL::JntArray q_;
50+
51+
// forward kinematics
52+
KDL::Frame chain_tip_frame_;
53+
54+
// jacobian
55+
KDL::Jacobian jacobian_;
56+
};
57+
} // namespace lbr_fri_ros2
58+
#endif // LBR_FRI_ROS2__KINEMATICS_HPP_

‎lbr_fri_ros2/src/ft_estimator.cpp

+7-24
Original file line numberDiff line numberDiff line change
@@ -4,41 +4,25 @@ namespace lbr_fri_ros2 {
44
FTEstimator::FTEstimator(const std::string &robot_description, const std::string &chain_root,
55
const std::string &chain_tip, const_cart_array_t_ref f_ext_th)
66
: f_ext_th_(f_ext_th) {
7-
if (!kdl_parser::treeFromString(robot_description, tree_)) {
8-
std::string err = "Failed to construct kdl tree from robot description.";
9-
RCLCPP_ERROR(rclcpp::get_logger(LOGGER_NAME), err.c_str());
10-
throw std::runtime_error(err);
11-
}
12-
if (!tree_.getChain(chain_root, chain_tip, chain_)) {
13-
std::string err = "Failed to construct kdl chain from robot description.";
14-
RCLCPP_ERROR(rclcpp::get_logger(LOGGER_NAME), err.c_str());
15-
throw std::runtime_error(err);
16-
}
17-
jacobian_solver_ = std::make_unique<KDL::ChainJntToJacSolver>(chain_);
18-
fk_solver_ = std::make_unique<KDL::ChainFkSolverPos_recursive>(chain_);
19-
jacobian_.resize(KUKA::FRI::LBRState::NUMBER_OF_JOINTS);
20-
q_.resize(KUKA::FRI::LBRState::NUMBER_OF_JOINTS);
21-
7+
kinematics_ = std::make_unique<Kinematics>(robot_description, chain_root, chain_tip);
228
reset();
239
}
2410

2511
void FTEstimator::compute(const_jnt_pos_array_t_ref measured_joint_position,
2612
const_ext_tau_array_t_ref external_torque, cart_array_t_ref f_ext,
2713
const double &damping) {
28-
q_.data = Eigen::Map<const Eigen::Matrix<double, KUKA::FRI::LBRState::NUMBER_OF_JOINTS, 1>>(
29-
measured_joint_position.data());
3014
tau_ext_ = Eigen::Map<const Eigen::Matrix<double, KUKA::FRI::LBRState::NUMBER_OF_JOINTS, 1>>(
3115
external_torque.data());
32-
jacobian_solver_->JntToJac(q_, jacobian_);
33-
jacobian_inv_ = pinv(jacobian_.data, damping);
16+
auto jacobian = kinematics_->compute_jacobian(measured_joint_position);
17+
jacobian_inv_ = pinv(jacobian.data, damping);
3418
f_ext_ = jacobian_inv_.transpose() * tau_ext_;
3519

3620
// rotate into chain tip frame
37-
fk_solver_->JntToCart(q_, chain_tip_frame_);
38-
f_ext_.topRows(3) = Eigen::Matrix3d::Map(chain_tip_frame_.M.data) * f_ext_.topRows(3);
39-
f_ext_.bottomRows(3) = Eigen::Matrix3d::Map(chain_tip_frame_.M.data) * f_ext_.bottomRows(3);
21+
auto chain_tip_frame = kinematics_->compute_fk(measured_joint_position);
22+
f_ext_.topRows(3) = Eigen::Matrix3d::Map(chain_tip_frame.M.data) * f_ext_.topRows(3);
23+
f_ext_.bottomRows(3) = Eigen::Matrix3d::Map(chain_tip_frame.M.data) * f_ext_.bottomRows(3);
4024

41-
Eigen::Map<Eigen::Matrix<double, CARTESIAN_DOF, 1>>(f_ext.data()) = f_ext_;
25+
Eigen::Map<Eigen::Matrix<double, Kinematics::CARTESIAN_DOF, 1>>(f_ext.data()) = f_ext_;
4226

4327
// threshold force-torque
4428
std::transform(f_ext.begin(), f_ext.end(), f_ext_th_.begin(), f_ext.begin(),
@@ -52,7 +36,6 @@ void FTEstimator::compute(const_jnt_pos_array_t_ref measured_joint_position,
5236
}
5337

5438
void FTEstimator::reset() {
55-
q_.data.setZero();
5639
tau_ext_.setZero();
5740
f_ext_.setZero();
5841
}

‎lbr_fri_ros2/src/kinematics.cpp

+65
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,65 @@
1+
#include "lbr_fri_ros2/kinematics.hpp"
2+
3+
namespace lbr_fri_ros2 {
4+
Kinematics::Kinematics(const std::string &robot_description, const std::string &chain_root,
5+
const std::string &chain_tip) {
6+
if (!kdl_parser::treeFromString(robot_description, tree_)) {
7+
std::string err = "Failed to construct kdl tree from robot description.";
8+
RCLCPP_ERROR(rclcpp::get_logger(LOGGER_NAME), err.c_str());
9+
throw std::runtime_error(err);
10+
}
11+
if (!tree_.getChain(chain_root, chain_tip, chain_)) {
12+
std::string err = "Failed to construct kdl chain from robot description.";
13+
RCLCPP_ERROR(rclcpp::get_logger(LOGGER_NAME), err.c_str());
14+
throw std::runtime_error(err);
15+
}
16+
if (chain_.getNrOfJoints() != KUKA::FRI::LBRState::NUMBER_OF_JOINTS) {
17+
std::string err = "Invalid number of joints in chain.";
18+
RCLCPP_ERROR(rclcpp::get_logger(LOGGER_NAME), err.c_str());
19+
throw std::runtime_error(err);
20+
}
21+
jacobian_solver_ = std::make_unique<KDL::ChainJntToJacSolver>(chain_);
22+
fk_solver_ = std::make_unique<KDL::ChainFkSolverPos_recursive>(chain_);
23+
jacobian_.resize(KUKA::FRI::LBRState::NUMBER_OF_JOINTS);
24+
q_.resize(KUKA::FRI::LBRState::NUMBER_OF_JOINTS);
25+
q_.data.setZero();
26+
}
27+
28+
const KDL::Jacobian &Kinematics::compute_jacobian(const_jnt_pos_array_t_ref q) {
29+
q_.data =
30+
Eigen::Map<const Eigen::Matrix<double, KUKA::FRI::LBRState::NUMBER_OF_JOINTS, 1>>(q.data());
31+
jacobian_solver_->JntToJac(q_, jacobian_);
32+
return jacobian_;
33+
}
34+
35+
const KDL::Jacobian &Kinematics::compute_jacobian(const std::vector<double> &q) {
36+
if (q.size() != KUKA::FRI::LBRState::NUMBER_OF_JOINTS) {
37+
std::string err = "Invalid number of joint positions.";
38+
RCLCPP_ERROR(rclcpp::get_logger(LOGGER_NAME), err.c_str());
39+
throw std::runtime_error(err);
40+
}
41+
q_.data =
42+
Eigen::Map<const Eigen::Matrix<double, KUKA::FRI::LBRState::NUMBER_OF_JOINTS, 1>>(q.data());
43+
jacobian_solver_->JntToJac(q_, jacobian_);
44+
return jacobian_;
45+
}
46+
47+
const KDL::Frame &Kinematics::compute_fk(const_jnt_pos_array_t_ref q) {
48+
q_.data =
49+
Eigen::Map<const Eigen::Matrix<double, KUKA::FRI::LBRState::NUMBER_OF_JOINTS, 1>>(q.data());
50+
fk_solver_->JntToCart(q_, chain_tip_frame_);
51+
return chain_tip_frame_;
52+
}
53+
54+
const KDL::Frame &Kinematics::compute_fk(const std::vector<double> &q) {
55+
if (q.size() != KUKA::FRI::LBRState::NUMBER_OF_JOINTS) {
56+
std::string err = "Invalid number of joint positions.";
57+
RCLCPP_ERROR(rclcpp::get_logger(LOGGER_NAME), err.c_str());
58+
throw std::runtime_error(err);
59+
}
60+
q_.data =
61+
Eigen::Map<const Eigen::Matrix<double, KUKA::FRI::LBRState::NUMBER_OF_JOINTS, 1>>(q.data());
62+
fk_solver_->JntToCart(q_, chain_tip_frame_);
63+
return chain_tip_frame_;
64+
}
65+
} // namespace lbr_fri_ros2

0 commit comments

Comments
 (0)