Skip to content

Commit f674476

Browse files
pac48pac48
authored
Fix failing KDL test (#5)
* add verification for all inputs * add underscore to end_effector * add test to ensure inverse then forward calculation is approximately unit
1 parent c7d415f commit f674476

File tree

3 files changed

+181
-82
lines changed

3 files changed

+181
-82
lines changed

‎kinematics_interface_kdl/include/kinematics_interface_kdl/kinematics_interface_kdl.hpp

+17-11
Original file line numberDiff line numberDiff line change
@@ -37,8 +37,8 @@ namespace kinematics_interface_kdl
3737
/**
3838
* \brief KDL implementation of ros2_control kinematics interface
3939
*/
40-
virtual bool
41-
initialize(std::shared_ptr<rclcpp_lifecycle::LifecycleNode> node, const std::string &end_effector_name);
40+
bool
41+
initialize(std::shared_ptr<rclcpp_lifecycle::LifecycleNode> node, const std::string &end_effector_name) override;
4242

4343
/**
4444
* \brief Convert Cartesian delta-x to joint delta-theta, using the Jacobian.
@@ -48,11 +48,11 @@ namespace kinematics_interface_kdl
4848
* \param[out] delta_theta_vec output vector with joint states
4949
* \return true if successful
5050
*/
51-
virtual bool
51+
bool
5252
convert_cartesian_deltas_to_joint_deltas(const std::vector<double> &joint_pos,
5353
const std::vector<double> &delta_x_vec,
5454
const std::string &link_name,
55-
std::vector<double> &delta_theta_vec);
55+
std::vector<double> &delta_theta_vec) override;
5656

5757
/**
5858
* \brief Convert joint delta-theta to Cartesian delta-x.
@@ -62,11 +62,11 @@ namespace kinematics_interface_kdl
6262
* \param[out] delta_x_vec Cartesian deltas (x, y, z, wx, wy, wz)
6363
* \return true if successful
6464
*/
65-
virtual bool
65+
bool
6666
convert_joint_deltas_to_cartesian_deltas(const std::vector<double> &joint_pos,
6767
const std::vector<double> &delta_theta_vec,
6868
const std::string &link_name,
69-
std::vector<double> &delta_x_vec);
69+
std::vector<double> &delta_x_vec) override;
7070

7171
/**
7272
* \brief Calculates the joint transform for a specified link using provided joint positions.
@@ -75,9 +75,9 @@ namespace kinematics_interface_kdl
7575
* \param[out] transform_vec transformation matrix of the specified link in column major format.
7676
* \return true if successful
7777
*/
78-
virtual bool
78+
bool
7979
calculate_link_transform(const std::vector<double> &joint_pos, const std::string &link_name,
80-
std::vector<double> &transform_vec);
80+
std::vector<double> &transform_vec) override;
8181

8282
/**
8383
* \brief Calculates the joint transform for a specified link using provided joint positions.
@@ -86,17 +86,23 @@ namespace kinematics_interface_kdl
8686
* \param[out] jacobian Jacobian matrix of the specified link in column major format.
8787
* \return true if successful
8888
*/
89-
virtual bool
89+
bool
9090
calculate_jacobian(const std::vector<double> &joint_pos, const std::string &link_name,
91-
std::vector<double> &jacobian);
91+
std::vector<double> &jacobian) override;
9292

9393

9494
private:
9595
bool update_joint_array(const std::vector<double>& joint_pos);
96+
97+
//verification methods
98+
bool verify_initialized();
9699
bool verify_link_name(const std::string& link_name);
100+
bool verify_transform_vector(const std::vector<double> &transform);
101+
bool verify_cartesian_vector(const std::vector<double> &cartesian_vector);
102+
bool verify_joint_vector(const std::vector<double> &joint_vector);
103+
bool verify_jacobian(const std::vector<double> &jacobian_vector);
97104

98105
bool initialized = false;
99-
std::string end_effector_name_;
100106
std::string root_name_;
101107
size_t num_joints_;
102108
KDL::Chain chain_;

‎kinematics_interface_kdl/src/kinematics_interface_kdl.cpp

+89-48
Original file line numberDiff line numberDiff line change
@@ -22,8 +22,9 @@ namespace kinematics_interface_kdl {
2222

2323
bool KDLKinematics::initialize(std::shared_ptr<rclcpp_lifecycle::LifecycleNode> node,
2424
const std::string &end_effector_name) {
25+
// track initialization plugin
2526
initialized = true;
26-
end_effector_name_ = end_effector_name;
27+
2728
// get robot description
2829
auto robot_param = rclcpp::Parameter();
2930
if (!node->get_parameter("robot_description", robot_param)) {
@@ -69,36 +70,40 @@ namespace kinematics_interface_kdl {
6970
const std::vector<double> &delta_theta_vec,
7071
const std::string &link_name,
7172
std::vector<double> &delta_x_vec) {
72-
// get joint array and check dimensions
73-
if (!update_joint_array(joint_pos)) {
74-
return false;
75-
}
76-
if (!verify_link_name(link_name)){
73+
// verify inputs
74+
if (!verify_initialized() || !verify_joint_vector(joint_pos) || !verify_link_name(link_name) ||
75+
!verify_cartesian_vector(delta_x_vec) || !verify_joint_vector(delta_theta_vec)) {
7776
return false;
7877
}
78+
79+
// get joint array
80+
update_joint_array(joint_pos);
81+
7982
// copy vector to eigen type
80-
memcpy(delta_theta.data(), delta_theta_vec.data(), delta_theta_vec.size()*sizeof(double));
83+
memcpy(delta_theta.data(), delta_theta_vec.data(), delta_theta_vec.size() * sizeof(double));
8184
// calculate Jacobian
8285
jac_solver_->JntToJac(q_, *jacobian_, link_name_map_[link_name]);
8386
delta_x = jacobian_->data * delta_theta;
8487
// copy eigen type to vector
85-
memcpy(delta_x_vec.data(), delta_x.data(), 6*sizeof(double));
88+
memcpy(delta_x_vec.data(), delta_x.data(), 6 * sizeof(double));
8689
return true;
8790
}
8891

8992
bool KDLKinematics::convert_cartesian_deltas_to_joint_deltas(const std::vector<double> &joint_pos,
9093
const std::vector<double> &delta_x_vec,
9194
const std::string &link_name,
9295
std::vector<double> &delta_theta_vec) {
93-
// get joint array and check dimensions
94-
if (!update_joint_array(joint_pos)) {
95-
return false;
96-
}
97-
if (!verify_link_name(link_name)){
96+
// verify inputs
97+
if (!verify_initialized() || !verify_joint_vector(joint_pos) || !verify_link_name(link_name) ||
98+
!verify_cartesian_vector(delta_x_vec) || !verify_joint_vector(delta_theta_vec)) {
9899
return false;
99100
}
101+
102+
// get joint array
103+
update_joint_array(joint_pos);
104+
100105
// copy vector to eigen type
101-
memcpy(delta_x.data(), delta_x_vec.data(), delta_x_vec.size()*sizeof(double));
106+
memcpy(delta_x.data(), delta_x_vec.data(), delta_x_vec.size() * sizeof(double));
102107
// calculate Jacobian
103108
jac_solver_->JntToJac(q_, *jacobian_, link_name_map_[link_name]);
104109
// TODO this dynamic allocation needs to be replaced
@@ -107,48 +112,44 @@ namespace kinematics_interface_kdl {
107112
Eigen::Matrix<double, Eigen::Dynamic, 6> J_inverse = (J.transpose() * J + alpha * I).inverse() * J.transpose();
108113
delta_theta = J_inverse * delta_x;
109114
// copy eigen type to vector
110-
memcpy(delta_theta_vec.data(), delta_theta.data(), num_joints_*sizeof(double));
115+
memcpy(delta_theta_vec.data(), delta_theta.data(), num_joints_ * sizeof(double));
111116
return true;
112117
}
113118

114119
bool KDLKinematics::calculate_jacobian(const std::vector<double> &joint_pos,
115-
const std::string &link_name,
116-
std::vector<double> &jacobian) {
117-
// get joint array and check dimensions
118-
if (!update_joint_array(joint_pos)) {
119-
return false;
120-
}
121-
if (!verify_link_name(link_name)){
122-
return false;
123-
}
124-
if (jacobian.size() != 6*num_joints_) {
125-
RCLCPP_ERROR(LOGGER, "The size of the jacobian argument (%zu) does not match the required size of (%zu)",
126-
jacobian.size(), 6*num_joints_);
120+
const std::string &link_name,
121+
std::vector<double> &jacobian_vector) {
122+
// verify inputs
123+
if (!verify_initialized() || !verify_joint_vector(joint_pos) || !verify_link_name(link_name) ||
124+
!verify_jacobian(jacobian_vector)) {
127125
return false;
128126
}
129127

128+
// get joint array
129+
update_joint_array(joint_pos);
130+
130131
// calculate Jacobian
131132
jac_solver_->JntToJac(q_, *jacobian_, link_name_map_[link_name]);
132133

133-
memcpy(jacobian.data(), jacobian_->data.data(), 6*num_joints_*sizeof(double));
134+
memcpy(jacobian_vector.data(), jacobian_->data.data(), 6 * num_joints_ * sizeof(double));
134135

135136
return true;
136137
}
137138

138139
bool KDLKinematics::calculate_link_transform(const std::vector<double> &joint_pos,
139140
const std::string &link_name,
140-
std::vector<double> &transform_vec
141-
) {
142-
// get joint array and check dimensions
143-
if (!update_joint_array(joint_pos)) {
144-
return false;
145-
}
146-
if (!verify_link_name(link_name)){
141+
std::vector<double> &transform_vec) {
142+
// verify inputs
143+
if (!verify_initialized() || !verify_joint_vector(joint_pos) || !verify_link_name(link_name) ||
144+
!verify_transform_vector(transform_vec)) {
147145
return false;
148146
}
149147

148+
// get joint array
149+
update_joint_array(joint_pos);
150+
150151
// reset transform_vec
151-
memset(transform_vec.data(), 0, 16*sizeof(double));
152+
memset(transform_vec.data(), 0, 16 * sizeof(double));
152153

153154
// special case: since the root is not in the robot tree, need to return identity transform
154155
if (link_name == root_name_) {
@@ -158,6 +159,8 @@ namespace kinematics_interface_kdl {
158159
transform_vec[15] = 1.0;
159160
return true;
160161
}
162+
163+
// create forward kinematics solver
161164
fk_pos_solver_->JntToCart(q_, frame_, link_name_map_[link_name]);
162165
double tmp[] = {frame_.p.x(), frame_.p.y(), frame_.p.z()};
163166
// KDL::Rotation stores data in row-major format. e.g Xx, Yx, Zx, Xy... = data index at 0, 1, 2, 3, 4...
@@ -172,18 +175,7 @@ namespace kinematics_interface_kdl {
172175
}
173176

174177
bool KDLKinematics::update_joint_array(const std::vector<double> &joint_pos) {
175-
// check if interface is initialized
176-
if (!initialized) {
177-
RCLCPP_ERROR(LOGGER, "The KDL kinematics plugin was not initialized. Ensure you called the initialize method.");
178-
return false;
179-
}
180-
// check that joint_pos_ is the correct size
181-
if (joint_pos.size() != num_joints_) {
182-
RCLCPP_ERROR(LOGGER, "The size of joint_pos_ (%zu) does not match that of the robot model (%zu)",
183-
joint_pos.size(), num_joints_);
184-
return false;
185-
}
186-
memcpy(q_.data.data(), joint_pos.data(), joint_pos.size()*sizeof(double));
178+
memcpy(q_.data.data(), joint_pos.data(), joint_pos.size() * sizeof(double));
187179
return true;
188180
}
189181

@@ -202,6 +194,55 @@ namespace kinematics_interface_kdl {
202194
}
203195
return true;
204196
}
197+
198+
bool KDLKinematics::verify_transform_vector(const std::vector<double> &transform) {
199+
if (transform.size() != 16) {
200+
RCLCPP_ERROR(LOGGER,
201+
"Invalid size (%zu) for the transformation vector. Expected size is 16.",
202+
transform.size());
203+
return false;
204+
}
205+
return true;
206+
}
207+
208+
bool KDLKinematics::verify_cartesian_vector(const std::vector<double> &cartesian_vector) {
209+
if (cartesian_vector.size() != jacobian_->rows()) {
210+
RCLCPP_ERROR(LOGGER,
211+
"Invalid size (%zu) for the cartesian vector. Expected size is %d.",
212+
cartesian_vector.size(), jacobian_->rows());
213+
return false;
214+
}
215+
return true;
216+
}
217+
218+
bool KDLKinematics::verify_joint_vector(const std::vector<double> &joint_vector) {
219+
if (joint_vector.size() != num_joints_) {
220+
RCLCPP_ERROR(LOGGER,
221+
"Invalid size (%zu) for the joint vector. Expected size is %d.",
222+
joint_vector.size(), num_joints_);
223+
return false;
224+
}
225+
return true;
226+
}
227+
228+
bool KDLKinematics::verify_initialized() {
229+
// check if interface is initialized
230+
if (!initialized) {
231+
RCLCPP_ERROR(LOGGER, "The KDL kinematics plugin was not initialized. Ensure you called the initialize method.");
232+
return false;
233+
}
234+
return true;
235+
}
236+
237+
bool KDLKinematics::verify_jacobian(const std::vector<double> &jacobian_vector) {
238+
if (jacobian_vector.size() != jacobian_->rows() * jacobian_->columns()) {
239+
RCLCPP_ERROR(LOGGER, "The size of the jacobian argument (%zu) does not match the required size of (%zu)",
240+
jacobian_vector.size(), jacobian_->rows() * jacobian_->columns());
241+
return false;
242+
}
243+
return true;
244+
}
245+
205246
}
206247

207248
#include "pluginlib/class_list_macros.hpp"

0 commit comments

Comments
 (0)