Skip to content

Commit fa0a67f

Browse files
Add methods for computing frame differences
1 parent 4d6bada commit fa0a67f

File tree

2 files changed

+49
-0
lines changed

2 files changed

+49
-0
lines changed

kinematics_interface/include/kinematics_interface/kinematics_interface.hpp

+16
Original file line numberDiff line numberDiff line change
@@ -95,6 +95,18 @@ class KinematicsInterface
9595
const Eigen::VectorXd & joint_pos, const std::string & link_name,
9696
Eigen::Matrix<double, 6, Eigen::Dynamic> & jacobian) = 0;
9797

98+
/**
99+
* \brief Calculates the difference between two Cartesian frames
100+
* \param[in] x_a first Cartesian frame (x, y, z, wx, wy, wz, ww)
101+
* \param[in] x_b second Cartesian frame (x, y, z, wx, wy, wz, ww)
102+
* \param[in] dt time interval over which the numerical differentiation takes place
103+
* \param[out] delta_x Cartesian deltas (x, y, z, wx, wy, wz)
104+
* \return true if successful
105+
*/
106+
virtual bool calculate_frame_difference(
107+
Eigen::Matrix<double, 7, 1> & x_a, Eigen::Matrix<double, 7, 1> & x_b, double dt,
108+
Eigen::Matrix<double, 6, 1> & delta_x) = 0;
109+
98110
bool convert_cartesian_deltas_to_joint_deltas(
99111
std::vector<double> & joint_pos_vec, const std::vector<double> & delta_x_vec,
100112
const std::string & link_name, std::vector<double> & delta_theta_vec);
@@ -110,6 +122,10 @@ class KinematicsInterface
110122
bool calculate_jacobian(
111123
const std::vector<double> & joint_pos_vec, const std::string & link_name,
112124
Eigen::Matrix<double, 6, Eigen::Dynamic> & jacobian);
125+
126+
bool calculate_frame_difference(
127+
std::vector<double> & x_a_vec, std::vector<double> & x_b_vec, double dt,
128+
std::vector<double> & delta_x_vec);
113129
};
114130

115131
} // namespace kinematics_interface

kinematics_interface/src/kinematics_interface.cpp

+33
Original file line numberDiff line numberDiff line change
@@ -80,4 +80,37 @@ bool KinematicsInterface::calculate_jacobian(
8080
return calculate_jacobian(joint_pos, link_name, jacobian);
8181
}
8282

83+
bool KinematicsInterface::calculate_frame_difference(
84+
std::vector<double> & x_a_vec, std::vector<double> & x_b_vec, double dt,
85+
std::vector<double> & delta_x_vec)
86+
{
87+
if (x_a_vec.size() != 7)
88+
{
89+
RCLCPP_ERROR(
90+
LOGGER, "The length of the first cartesian vector (%zu) must be 7.", x_a_vec.size());
91+
return false;
92+
}
93+
Eigen::Matrix<double, 7, 1> x_a(x_a_vec.data());
94+
if (x_b_vec.size() != 7)
95+
{
96+
RCLCPP_ERROR(
97+
LOGGER, "The length of the second cartesian vector (%zu) must be 7.", x_b_vec.size());
98+
return false;
99+
}
100+
Eigen::Matrix<double, 7, 1> x_b(x_b_vec.data());
101+
if (delta_x_vec.size() != 6)
102+
{
103+
RCLCPP_ERROR(
104+
LOGGER, "The length of the cartesian delta vector (%zu) must be 6.", delta_x_vec.size());
105+
return false;
106+
}
107+
Eigen::Matrix<double, 6, 1> delta_x(delta_x_vec.data());
108+
bool ret = calculate_frame_difference(x_a, x_b, dt, delta_x);
109+
for (auto i = 0ul; i < delta_x_vec.size(); i++)
110+
{
111+
delta_x_vec[i] = delta_x[i];
112+
}
113+
return ret;
114+
}
115+
83116
} // namespace kinematics_interface

0 commit comments

Comments
 (0)