Skip to content

Commit 652ec2c

Browse files
Add the methods for the computation of the jacobian inverse
1 parent 4d6bada commit 652ec2c

File tree

2 files changed

+24
-0
lines changed

2 files changed

+24
-0
lines changed

kinematics_interface/include/kinematics_interface/kinematics_interface.hpp

+15
Original file line numberDiff line numberDiff line change
@@ -95,6 +95,17 @@ 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 jacobian inverse for a specified link using provided joint positions.
100+
* \param[in] joint_pos joint positions of the robot in radians
101+
* \param[in] link_name the name of the link to find the transform for
102+
* \param[out] jacobian_inverse Jacobian inverse matrix of the specified link in row major format.
103+
* \return true if successful
104+
*/
105+
virtual bool calculate_jacobian_inverse(
106+
const Eigen::VectorXd & joint_pos, const std::string & link_name,
107+
Eigen::Matrix<double, Eigen::Dynamic, 6> & jacobian_inverse) = 0;
108+
98109
bool convert_cartesian_deltas_to_joint_deltas(
99110
std::vector<double> & joint_pos_vec, const std::vector<double> & delta_x_vec,
100111
const std::string & link_name, std::vector<double> & delta_theta_vec);
@@ -110,6 +121,10 @@ class KinematicsInterface
110121
bool calculate_jacobian(
111122
const std::vector<double> & joint_pos_vec, const std::string & link_name,
112123
Eigen::Matrix<double, 6, Eigen::Dynamic> & jacobian);
124+
125+
bool calculate_jacobian_inverse(
126+
const std::vector<double> & joint_pos_vec, const std::string & link_name,
127+
Eigen::Matrix<double, Eigen::Dynamic, 6> & jacobian_inverse);
113128
};
114129

115130
} // namespace kinematics_interface

kinematics_interface/src/kinematics_interface.cpp

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

83+
bool KinematicsInterface::calculate_jacobian_inverse(
84+
const std::vector<double> & joint_pos_vec, const std::string & link_name,
85+
Eigen::Matrix<double, Eigen::Dynamic, 6> & jacobian_inverse)
86+
{
87+
auto joint_pos = Eigen::Map<const Eigen::VectorXd>(joint_pos_vec.data(), joint_pos_vec.size());
88+
89+
return calculate_jacobian_inverse(joint_pos, link_name, jacobian_inverse);
90+
}
91+
8392
} // namespace kinematics_interface

0 commit comments

Comments
 (0)