Skip to content

Commit

Permalink
add verification for all inputs
Browse files Browse the repository at this point in the history
  • Loading branch information
@pac48
pac48 committed Aug 5, 2022
1 parent edc5c95 commit 6cb886b
Show file tree
Hide file tree
Showing 3 changed files with 172 additions and 79 deletions.
28 changes: 17 additions & 11 deletions kinematics_interface_kdl/include/kinematics_interface_kdl/kinematics_interface_kdl.hpp
View file
Expand Up @@ -37,8 +37,8 @@ namespace kinematics_interface_kdl
/**
* \brief KDL implementation of ros2_control kinematics interface
*/
virtual bool
initialize(std::shared_ptr<rclcpp_lifecycle::LifecycleNode> node, const std::string &end_effector_name);
bool
initialize(std::shared_ptr<rclcpp_lifecycle::LifecycleNode> node, const std::string &end_effector_name) override;

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

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

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

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


private:
bool update_joint_array(const std::vector<double>& joint_pos);

//verification methods
bool verify_initialized();
bool verify_link_name(const std::string& link_name);
bool verify_transform_vector(const std::vector<double> &transform);
bool verify_cartesian_vector(const std::vector<double> &cartesian_vector);
bool verify_joint_vector(const std::vector<double> &joint_vector);
bool verify_jacobian(const std::vector<double> &jacobian_vector);

bool initialized = false;
std::string end_effector_name_;
std::string root_name_;
size_t num_joints_;
KDL::Chain chain_;
Expand Down
137 changes: 89 additions & 48 deletions kinematics_interface_kdl/src/kinematics_interface_kdl.cpp
View file
Expand Up @@ -22,8 +22,9 @@ namespace kinematics_interface_kdl {

bool KDLKinematics::initialize(std::shared_ptr<rclcpp_lifecycle::LifecycleNode> node,
const std::string &end_effector_name) {
// track initialization plugin
initialized = true;
end_effector_name_ = end_effector_name;

// get robot description
auto robot_param = rclcpp::Parameter();
if (!node->get_parameter("robot_description", robot_param)) {
Expand Down Expand Up @@ -69,36 +70,40 @@ namespace kinematics_interface_kdl {
const std::vector<double> &delta_theta_vec,
const std::string &link_name,
std::vector<double> &delta_x_vec) {
// get joint array and check dimensions
if (!update_joint_array(joint_pos)) {
return false;
}
if (!verify_link_name(link_name)){
// verify inputs
if (!verify_initialized() || !verify_joint_vector(joint_pos) || !verify_link_name(link_name) ||
!verify_cartesian_vector(delta_x_vec) || !verify_joint_vector(delta_theta_vec)) {
return false;
}

// get joint array
update_joint_array(joint_pos);

// copy vector to eigen type
memcpy(delta_theta.data(), delta_theta_vec.data(), delta_theta_vec.size()*sizeof(double));
memcpy(delta_theta.data(), delta_theta_vec.data(), delta_theta_vec.size() * sizeof(double));
// calculate Jacobian
jac_solver_->JntToJac(q_, *jacobian_, link_name_map_[link_name]);
delta_x = jacobian_->data * delta_theta;
// copy eigen type to vector
memcpy(delta_x_vec.data(), delta_x.data(), 6*sizeof(double));
memcpy(delta_x_vec.data(), delta_x.data(), 6 * sizeof(double));
return true;
}

bool KDLKinematics::convert_cartesian_deltas_to_joint_deltas(const std::vector<double> &joint_pos,
const std::vector<double> &delta_x_vec,
const std::string &link_name,
std::vector<double> &delta_theta_vec) {
// get joint array and check dimensions
if (!update_joint_array(joint_pos)) {
return false;
}
if (!verify_link_name(link_name)){
// verify inputs
if (!verify_initialized() || !verify_joint_vector(joint_pos) || !verify_link_name(link_name) ||
!verify_cartesian_vector(delta_x_vec) || !verify_joint_vector(delta_theta_vec)) {
return false;
}

// get joint array
update_joint_array(joint_pos);

// copy vector to eigen type
memcpy(delta_x.data(), delta_x_vec.data(), delta_x_vec.size()*sizeof(double));
memcpy(delta_x.data(), delta_x_vec.data(), delta_x_vec.size() * sizeof(double));
// calculate Jacobian
jac_solver_->JntToJac(q_, *jacobian_, link_name_map_[link_name]);
// TODO this dynamic allocation needs to be replaced
Expand All @@ -107,48 +112,44 @@ namespace kinematics_interface_kdl {
Eigen::Matrix<double, Eigen::Dynamic, 6> J_inverse = (J.transpose() * J + alpha * I).inverse() * J.transpose();
delta_theta = J_inverse * delta_x;
// copy eigen type to vector
memcpy(delta_theta_vec.data(), delta_theta.data(), num_joints_*sizeof(double));
memcpy(delta_theta_vec.data(), delta_theta.data(), num_joints_ * sizeof(double));
return true;
}

bool KDLKinematics::calculate_jacobian(const std::vector<double> &joint_pos,
const std::string &link_name,
std::vector<double> &jacobian) {
// get joint array and check dimensions
if (!update_joint_array(joint_pos)) {
return false;
}
if (!verify_link_name(link_name)){
return false;
}
if (jacobian.size() != 6*num_joints_) {
RCLCPP_ERROR(LOGGER, "The size of the jacobian argument (%zu) does not match the required size of (%zu)",
jacobian.size(), 6*num_joints_);
const std::string &link_name,
std::vector<double> &jacobian_vector) {
// verify inputs
if (!verify_initialized() || !verify_joint_vector(joint_pos) || !verify_link_name(link_name) ||
!verify_jacobian(jacobian_vector)) {
return false;
}

// get joint array
update_joint_array(joint_pos);

// calculate Jacobian
jac_solver_->JntToJac(q_, *jacobian_, link_name_map_[link_name]);

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

return true;
}

bool KDLKinematics::calculate_link_transform(const std::vector<double> &joint_pos,
const std::string &link_name,
std::vector<double> &transform_vec
) {
// get joint array and check dimensions
if (!update_joint_array(joint_pos)) {
return false;
}
if (!verify_link_name(link_name)){
std::vector<double> &transform_vec) {
// verify inputs
if (!verify_initialized() || !verify_joint_vector(joint_pos) || !verify_link_name(link_name) ||
!verify_transform_vector(transform_vec)) {
return false;
}

// get joint array
update_joint_array(joint_pos);

// reset transform_vec
memset(transform_vec.data(), 0, 16*sizeof(double));
memset(transform_vec.data(), 0, 16 * sizeof(double));

// special case: since the root is not in the robot tree, need to return identity transform
if (link_name == root_name_) {
Expand All @@ -158,6 +159,8 @@ namespace kinematics_interface_kdl {
transform_vec[15] = 1.0;
return true;
}

// create forward kinematics solver
fk_pos_solver_->JntToCart(q_, frame_, link_name_map_[link_name]);
double tmp[] = {frame_.p.x(), frame_.p.y(), frame_.p.z()};
// KDL::Rotation stores data in row-major format. e.g Xx, Yx, Zx, Xy... = data index at 0, 1, 2, 3, 4...
Expand All @@ -172,18 +175,7 @@ namespace kinematics_interface_kdl {
}

bool KDLKinematics::update_joint_array(const std::vector<double> &joint_pos) {
// check if interface is initialized
if (!initialized) {
RCLCPP_ERROR(LOGGER, "The KDL kinematics plugin was not initialized. Ensure you called the initialize method.");
return false;
}
// check that joint_pos_ is the correct size
if (joint_pos.size() != num_joints_) {
RCLCPP_ERROR(LOGGER, "The size of joint_pos_ (%zu) does not match that of the robot model (%zu)",
joint_pos.size(), num_joints_);
return false;
}
memcpy(q_.data.data(), joint_pos.data(), joint_pos.size()*sizeof(double));
memcpy(q_.data.data(), joint_pos.data(), joint_pos.size() * sizeof(double));
return true;
}

Expand All @@ -202,6 +194,55 @@ namespace kinematics_interface_kdl {
}
return true;
}

bool KDLKinematics::verify_transform_vector(const std::vector<double> &transform) {
if (transform.size() != 16) {
RCLCPP_ERROR(LOGGER,
"Invalid size (%zu) for the transformation vector. Expected size is 16.",
transform.size());
return false;
}
return true;
}

bool KDLKinematics::verify_cartesian_vector(const std::vector<double> &cartesian_vector) {
if (cartesian_vector.size() != jacobian_->rows()) {
RCLCPP_ERROR(LOGGER,
"Invalid size (%zu) for the cartesian vector. Expected size is %d.",
cartesian_vector.size(), jacobian_->rows());
return false;
}
return true;
}

bool KDLKinematics::verify_joint_vector(const std::vector<double> &joint_vector) {
if (joint_vector.size() != num_joints_) {
RCLCPP_ERROR(LOGGER,
"Invalid size (%zu) for the joint vector. Expected size is %d.",
joint_vector.size(), num_joints_);
return false;
}
return true;
}

bool KDLKinematics::verify_initialized() {
// check if interface is initialized
if (!initialized) {
RCLCPP_ERROR(LOGGER, "The KDL kinematics plugin was not initialized. Ensure you called the initialize method.");
return false;
}
return true;
}

bool KDLKinematics::verify_jacobian(const std::vector<double> &jacobian_vector) {
if (jacobian_vector.size() != jacobian_->rows() * jacobian_->columns()) {
RCLCPP_ERROR(LOGGER, "The size of the jacobian argument (%zu) does not match the required size of (%zu)",
jacobian_vector.size(), jacobian_->rows() * jacobian_->columns());
return false;
}
return true;
}

}

#include "pluginlib/class_list_macros.hpp"
Expand Down

0 comments on commit 6cb886b

Please sign in to comment.