@@ -95,6 +95,18 @@ class KinematicsInterface
95
95
const Eigen::VectorXd & joint_pos, const std::string & link_name,
96
96
Eigen::Matrix<double , 6 , Eigen::Dynamic> & jacobian) = 0;
97
97
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
+
98
110
bool convert_cartesian_deltas_to_joint_deltas (
99
111
std::vector<double > & joint_pos_vec, const std::vector<double > & delta_x_vec,
100
112
const std::string & link_name, std::vector<double > & delta_theta_vec);
@@ -110,6 +122,10 @@ class KinematicsInterface
110
122
bool calculate_jacobian (
111
123
const std::vector<double > & joint_pos_vec, const std::string & link_name,
112
124
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);
113
129
};
114
130
115
131
} // namespace kinematics_interface
0 commit comments