Skip to content
New issue

Have a question about this project? Sign up for a free GitHub account to open an issue and contact its maintainers and the community.

Sign up for GitHub

By clicking “Sign up for GitHub”, you agree to our terms of service and privacy statement. We’ll occasionally send you account related emails.

Already on GitHub? Sign in to your account

Calculate frame difference #93

Open
wants to merge 4 commits into
base: master
Choose a base branch
from
Open

Calculate frame difference #93

Show file tree
Hide file tree
Changes from all commits
Commits
Show all changes
4 commits
Select commit Hold shift + click to select a range
fa0a67f
Add methods for computing frame differences
francescodonofrio Nov 14, 2024
210a57f
Add KDL implementation for computing frame differences
francescodonofrio Nov 14, 2024
279c61c
Merge branch 'master' into feature/calculate_frame_difference
francescodonofrio Nov 21, 2024
ac8a8c3
Merge branch 'master' into feature/calculate_frame_difference
francescodonofrio Dec 21, 2024
File filter

Filter by extension

Filter by extension
Conversations
Failed to load comments.
Loading
Jump to
Jump to file
Failed to load files.
Loading
Diff view
Diff view
16 changes: 16 additions & 0 deletions kinematics_interface/include/kinematics_interface/kinematics_interface.hpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -95,6 +95,18 @@ class KinematicsInterface
const Eigen::VectorXd & joint_pos, const std::string & link_name,
Eigen::Matrix<double, 6, Eigen::Dynamic> & jacobian) = 0;

/**
* \brief Calculates the difference between two Cartesian frames
* \param[in] x_a first Cartesian frame (x, y, z, wx, wy, wz, ww)
* \param[in] x_b second Cartesian frame (x, y, z, wx, wy, wz, ww)
* \param[in] dt time interval over which the numerical differentiation takes place
* \param[out] delta_x Cartesian deltas (x, y, z, wx, wy, wz)
* \return true if successful
*/
virtual bool calculate_frame_difference(
Eigen::Matrix<double, 7, 1> & x_a, Eigen::Matrix<double, 7, 1> & x_b, double dt,
Eigen::Matrix<double, 6, 1> & delta_x) = 0;

bool convert_cartesian_deltas_to_joint_deltas(
std::vector<double> & joint_pos_vec, const std::vector<double> & delta_x_vec,
const std::string & link_name, std::vector<double> & delta_theta_vec);
Expand All @@ -110,6 +122,10 @@ class KinematicsInterface
bool calculate_jacobian(
const std::vector<double> & joint_pos_vec, const std::string & link_name,
Eigen::Matrix<double, 6, Eigen::Dynamic> & jacobian);

bool calculate_frame_difference(
std::vector<double> & x_a_vec, std::vector<double> & x_b_vec, double dt,
std::vector<double> & delta_x_vec);
Comment on lines +127 to +128
Copy link
Member

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

Why not imbibe the size constraints?

Suggested change
std::vector<double> & x_a_vec, std::vector<double> & x_b_vec, double dt,
std::vector<double> & delta_x_vec);
std::array<double, 7> & x_a_vec, std::array<double, 7> & x_b_vec, double dt,
std::array<double, 6> & delta_x_vec);

Copy link
Contributor Author

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

I used std::vector<double> to maintain consistency with the convert_joint_deltas_to_cartesian_deltas method, where std::vector<double> is used for the delta_x parameter.

};

} // namespace kinematics_interface
Expand Down
33 changes: 33 additions & 0 deletions kinematics_interface/src/kinematics_interface.cpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -80,4 +80,37 @@ bool KinematicsInterface::calculate_jacobian(
return calculate_jacobian(joint_pos, link_name, jacobian);
}

bool KinematicsInterface::calculate_frame_difference(
std::vector<double> & x_a_vec, std::vector<double> & x_b_vec, double dt,
std::vector<double> & delta_x_vec)
{
if (x_a_vec.size() != 7)
{
RCLCPP_ERROR(
LOGGER, "The length of the first cartesian vector (%zu) must be 7.", x_a_vec.size());
return false;
}
Eigen::Matrix<double, 7, 1> x_a(x_a_vec.data());
if (x_b_vec.size() != 7)
{
RCLCPP_ERROR(
LOGGER, "The length of the second cartesian vector (%zu) must be 7.", x_b_vec.size());
return false;
}
Eigen::Matrix<double, 7, 1> x_b(x_b_vec.data());
if (delta_x_vec.size() != 6)
{
RCLCPP_ERROR(
LOGGER, "The length of the cartesian delta vector (%zu) must be 6.", delta_x_vec.size());
return false;
}
Eigen::Matrix<double, 6, 1> delta_x(delta_x_vec.data());
bool ret = calculate_frame_difference(x_a, x_b, dt, delta_x);
for (auto i = 0ul; i < delta_x_vec.size(); i++)
{
delta_x_vec[i] = delta_x[i];
}
return ret;
}

} // namespace kinematics_interface
8 changes: 7 additions & 1 deletion kinematics_interface_kdl/include/kinematics_interface_kdl/kinematics_interface_kdl.hpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -60,20 +60,26 @@ class KinematicsInterfaceKDL : public kinematics_interface::KinematicsInterface
const Eigen::VectorXd & joint_pos, const std::string & link_name,
Eigen::Matrix<double, 6, Eigen::Dynamic> & jacobian) override;

bool calculate_frame_difference(
Eigen::Matrix<double, 7, 1> & x_a, Eigen::Matrix<double, 7, 1> & x_b, double dt,
Eigen::Matrix<double, 6, 1> & delta_x) override;

private:
// verification methods
bool verify_initialized();
bool verify_link_name(const std::string & link_name);
bool verify_joint_vector(const Eigen::VectorXd & joint_vector);
bool verify_jacobian(const Eigen::Matrix<double, 6, Eigen::Dynamic> & jacobian);
bool verify_period(const double dt);

bool initialized = false;
std::string root_name_;
size_t num_joints_;
KDL::Chain chain_;
std::shared_ptr<KDL::ChainFkSolverPos_recursive> fk_pos_solver_;
KDL::JntArray q_;
KDL::Frame frame_;
Eigen::Matrix<KDL::Frame, 2, 1> frames_;
KDL::Twist delta_x_;
std::shared_ptr<KDL::Jacobian> jacobian_;
std::shared_ptr<KDL::ChainJntToJacSolver> jac_solver_;
std::shared_ptr<rclcpp::node_interfaces::NodeParametersInterface> parameters_interface_;
Expand Down
40 changes: 38 additions & 2 deletions kinematics_interface_kdl/src/kinematics_interface_kdl.cpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -205,8 +205,34 @@ bool KinematicsInterfaceKDL::calculate_link_transform(
}

// create forward kinematics solver
fk_pos_solver_->JntToCart(q_, frame_, link_name_map_[link_name]);
tf2::transformKDLToEigen(frame_, transform);
fk_pos_solver_->JntToCart(q_, frames_(0), link_name_map_[link_name]);
tf2::transformKDLToEigen(frames_(0), transform);
return true;
}

bool KinematicsInterfaceKDL::calculate_frame_difference(
Eigen::Matrix<double, 7, 1> & x_a, Eigen::Matrix<double, 7, 1> & x_b, double dt,
Eigen::Matrix<double, 6, 1> & delta_x)
{
// verify inputs
if (!verify_initialized() || !verify_period(dt))
{
return false;
}

// get frames
frames_(0) = KDL::Frame(
KDL::Rotation::Quaternion(x_a(3), x_a(4), x_a(5), x_a(6)), KDL::Vector(x_a(0), x_a(1), x_a(2)));
frames_(1) = KDL::Frame(
KDL::Rotation::Quaternion(x_b(3), x_b(4), x_b(5), x_b(6)), KDL::Vector(x_b(0), x_b(1), x_b(2)));

// compute the difference
delta_x_ = KDL::diff(frames_(0), frames_(1), dt);
for (size_t i = 0; i < 6; ++i)
{
delta_x(i) = delta_x_[i];
}

return true;
}

Expand Down Expand Up @@ -269,6 +295,16 @@ bool KinematicsInterfaceKDL::verify_jacobian(
return true;
}

bool KinematicsInterfaceKDL::verify_period(const double dt)
{
if (dt < 0)
{
RCLCPP_ERROR(LOGGER, "The period (%f) must be a non-negative number", dt);
return false;
}
return true;
}

} // namespace kinematics_interface_kdl

#include "pluginlib/class_list_macros.hpp"
Expand Down
37 changes: 37 additions & 0 deletions kinematics_interface_kdl/test/test_kinematics_interface_kdl.cpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -108,6 +108,21 @@ TEST_F(TestKDLPlugin, KDL_plugin_function)
{
ASSERT_NEAR(delta_x[i], delta_x_est[i], 0.02);
}

// compute the difference between two cartesian frames
Eigen::Matrix<double, 7, 1> x_a, x_b;
x_a << 0, 1, 0, 0, 0, 0, 1;
x_b << 2, 3, 0, 0, 1, 0, 0;
double dt = 1.0;
delta_x = Eigen::Matrix<double, 6, 1>::Zero();
delta_x_est << 2, 2, 0, 0, 3.14, 0;
ASSERT_TRUE(ik_->calculate_frame_difference(x_a, x_b, dt, delta_x));

// ensure that difference math is correct
for (size_t i = 0; i < static_cast<size_t>(delta_x.size()); ++i)
{
ASSERT_NEAR(delta_x(i), delta_x_est(i), 0.02);
}
}

TEST_F(TestKDLPlugin, KDL_plugin_function_std_vector)
Expand Down Expand Up @@ -142,6 +157,21 @@ TEST_F(TestKDLPlugin, KDL_plugin_function_std_vector)
{
ASSERT_NEAR(delta_x[i], delta_x_est[i], 0.02);
}

// compute the difference between two cartesian frames
std::vector<double> x_a(7), x_b(7);
x_a = {0, 1, 0, 0, 0, 0, 1};
x_b = {2, 3, 0, 0, 1, 0, 0};
double dt = 1.0;
delta_x = {0, 0, 0, 0, 0, 0};
delta_x_est = {2, 2, 0, 0, 3.14, 0};
ASSERT_TRUE(ik_->calculate_frame_difference(x_a, x_b, dt, delta_x));

// ensure that difference math is correct
for (size_t i = 0; i < static_cast<size_t>(delta_x.size()); ++i)
{
ASSERT_NEAR(delta_x[i], delta_x_est[i], 0.02);
}
}

TEST_F(TestKDLPlugin, incorrect_input_sizes)
Expand All @@ -161,10 +191,14 @@ TEST_F(TestKDLPlugin, incorrect_input_sizes)
delta_x[2] = 1;
Eigen::Matrix<double, Eigen::Dynamic, 1> delta_theta = Eigen::Matrix<double, 2, 1>::Zero();
Eigen::Matrix<double, 6, 1> delta_x_est;
Eigen::Matrix<double, 7, 1> x_a, x_b;

// wrong size input vector
Eigen::Matrix<double, Eigen::Dynamic, 1> vec_5 = Eigen::Matrix<double, 5, 1>::Zero();

// wrong value for period
double dt = -0.1;

// calculate transform
ASSERT_FALSE(ik_->calculate_link_transform(vec_5, end_effector_, end_effector_transform));
ASSERT_FALSE(ik_->calculate_link_transform(pos, "link_not_in_model", end_effector_transform));
Expand All @@ -183,6 +217,9 @@ TEST_F(TestKDLPlugin, incorrect_input_sizes)
ik_->convert_joint_deltas_to_cartesian_deltas(pos, vec_5, end_effector_, delta_x_est));
ASSERT_FALSE(ik_->convert_joint_deltas_to_cartesian_deltas(
pos, delta_theta, "link_not_in_model", delta_x_est));

// compute the difference between two cartesian frames
ASSERT_FALSE(ik_->calculate_frame_difference(x_a, x_b, dt, delta_x));
}

TEST_F(TestKDLPlugin, KDL_plugin_no_robot_description)
Expand Down
Loading