@@ -22,8 +22,9 @@ namespace kinematics_interface_kdl {
22
22
23
23
bool KDLKinematics::initialize (std::shared_ptr<rclcpp_lifecycle::LifecycleNode> node,
24
24
const std::string &end_effector_name) {
25
+ // track initialization plugin
25
26
initialized = true ;
26
- end_effector_name_ = end_effector_name;
27
+
27
28
// get robot description
28
29
auto robot_param = rclcpp::Parameter ();
29
30
if (!node->get_parameter (" robot_description" , robot_param)) {
@@ -69,36 +70,40 @@ namespace kinematics_interface_kdl {
69
70
const std::vector<double > &delta_theta_vec,
70
71
const std::string &link_name,
71
72
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)) {
77
76
return false ;
78
77
}
78
+
79
+ // get joint array
80
+ update_joint_array (joint_pos);
81
+
79
82
// 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 ));
81
84
// calculate Jacobian
82
85
jac_solver_->JntToJac (q_, *jacobian_, link_name_map_[link_name]);
83
86
delta_x = jacobian_->data * delta_theta;
84
87
// 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 ));
86
89
return true ;
87
90
}
88
91
89
92
bool KDLKinematics::convert_cartesian_deltas_to_joint_deltas (const std::vector<double > &joint_pos,
90
93
const std::vector<double > &delta_x_vec,
91
94
const std::string &link_name,
92
95
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)) {
98
99
return false ;
99
100
}
101
+
102
+ // get joint array
103
+ update_joint_array (joint_pos);
104
+
100
105
// 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 ));
102
107
// calculate Jacobian
103
108
jac_solver_->JntToJac (q_, *jacobian_, link_name_map_[link_name]);
104
109
// TODO this dynamic allocation needs to be replaced
@@ -107,48 +112,44 @@ namespace kinematics_interface_kdl {
107
112
Eigen::Matrix<double , Eigen::Dynamic, 6 > J_inverse = (J.transpose () * J + alpha * I).inverse () * J.transpose ();
108
113
delta_theta = J_inverse * delta_x;
109
114
// 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 ));
111
116
return true ;
112
117
}
113
118
114
119
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)) {
127
125
return false ;
128
126
}
129
127
128
+ // get joint array
129
+ update_joint_array (joint_pos);
130
+
130
131
// calculate Jacobian
131
132
jac_solver_->JntToJac (q_, *jacobian_, link_name_map_[link_name]);
132
133
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 ));
134
135
135
136
return true ;
136
137
}
137
138
138
139
bool KDLKinematics::calculate_link_transform (const std::vector<double > &joint_pos,
139
140
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)) {
147
145
return false ;
148
146
}
149
147
148
+ // get joint array
149
+ update_joint_array (joint_pos);
150
+
150
151
// reset transform_vec
151
- memset (transform_vec.data (), 0 , 16 * sizeof (double ));
152
+ memset (transform_vec.data (), 0 , 16 * sizeof (double ));
152
153
153
154
// special case: since the root is not in the robot tree, need to return identity transform
154
155
if (link_name == root_name_) {
@@ -158,6 +159,8 @@ namespace kinematics_interface_kdl {
158
159
transform_vec[15 ] = 1.0 ;
159
160
return true ;
160
161
}
162
+
163
+ // create forward kinematics solver
161
164
fk_pos_solver_->JntToCart (q_, frame_, link_name_map_[link_name]);
162
165
double tmp[] = {frame_.p .x (), frame_.p .y (), frame_.p .z ()};
163
166
// 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 {
172
175
}
173
176
174
177
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 ));
187
179
return true ;
188
180
}
189
181
@@ -202,6 +194,55 @@ namespace kinematics_interface_kdl {
202
194
}
203
195
return true ;
204
196
}
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
+
205
246
}
206
247
207
248
#include " pluginlib/class_list_macros.hpp"
0 commit comments