Expose joint Jacobian functions to bindings.

kylc · Feb 28, 2024 · e643e87 · e643e87
1 parent bddfbe1
commit e643e87
Show file tree

Hide file tree

Showing 4 changed files with 54 additions and 1 deletion.
diff --git a/crates/optik-cpp/include/optik.hpp b/crates/optik-cpp/include/optik.hpp
@@ -64,6 +64,13 @@ class Robot final {
   //! subject to the joint limits.
   Eigen::VectorXd RandomConfiguration() const noexcept;
 
+  //! Compute the joint Jacobian matrix of the end-effector in the given joint
+  //! configuration.
+  //!
+  //! Panics if `q` does not match the number of generalized positions.
+  Eigen::Matrix<double, 6, Eigen::Dynamic> JointJacobian(
+      const Eigen::VectorXd& q) const;
+
   //! Compute the pose of the end-effector link w.r.t. the base link, expressed
   //! in the base link.
   //!

diff --git a/crates/optik-cpp/src/lib.cpp b/crates/optik-cpp/src/lib.cpp
@@ -13,10 +13,13 @@ extern optik::detail::robot* optik_robot_from_urdf_str(const char* urdf,
 extern void optik_robot_free(optik::detail::robot* robot);
 
 extern void optik_robot_set_parallelism(optik::detail::robot*, unsigned int n);
-extern unsigned int optik_robot_num_positions(const optik::detail::robot* robot);
+extern unsigned int optik_robot_num_positions(
+    const optik::detail::robot* robot);
 extern double* optik_robot_joint_limits(const optik::detail::robot* robot);
 extern double* optik_robot_random_configuration(
     const optik::detail::robot* robot);
+extern double* optik_robot_joint_jacobian(const optik::detail::robot* robot,
+                                          const double* x);
 extern double* optik_robot_fk(const optik::detail::robot* robot,
                               const double* x);
 extern double* optik_robot_ik(const optik::detail::robot* robot,
@@ -56,6 +59,21 @@ Eigen::VectorXd Robot::RandomConfiguration() const noexcept {
   return q;
 }
 
+Eigen::Matrix<double, 6, Eigen::Dynamic> Robot::JointJacobian(
+    const Eigen::VectorXd& q) const {
+  using JacobianMatrix = Eigen::Matrix<double, 6, Eigen::Dynamic>;
+
+  if (q.size() != num_positions()) {
+    throw std::runtime_error("dof mismatch");
+  }
+
+  double* jac_data = optik_robot_joint_jacobian(inner_, q.data());
+  JacobianMatrix jac = Eigen::Map<JacobianMatrix>(jac_data, 6, num_positions());
+  free(jac_data);
+
+  return jac;
+}
+
 Eigen::Isometry3d Robot::DoFk(const Eigen::VectorXd& q) const {
   if (q.size() != num_positions()) {
     throw std::runtime_error("dof mismatch");

diff --git a/crates/optik-cpp/src/lib.rs b/crates/optik-cpp/src/lib.rs
@@ -85,6 +85,22 @@ extern "C" fn optik_robot_joint_limits(robot: *const Robot) -> *const c_double {
     }
 }
 
+#[no_mangle]
+extern "C" fn optik_robot_joint_jacobian(
+    robot: *const Robot,
+    x: *const c_double,
+) -> *const c_double {
+    unsafe {
+        let robot = robot.as_ref().unwrap();
+        let x = std::slice::from_raw_parts(x, robot.num_positions());
+
+        let fk = robot.fk(x);
+        let jac = robot.joint_jacobian(&fk);
+
+        jac.data.as_slice().to_vec().leak().as_ptr()
+    }
+}
+
 #[no_mangle]
 extern "C" fn optik_robot_fk(robot: *const Robot, x: *const c_double) -> *const c_double {
     unsafe {

diff --git a/crates/optik-py/src/lib.rs b/crates/optik-py/src/lib.rs
@@ -69,6 +69,18 @@ impl PyRobot {
         )
     }
 
+    fn joint_jacobian(&self, x: Vec<f64>) -> Vec<Vec<f64>> {
+        let robot = &self.0;
+
+        assert_eq!(x.len(), robot.num_positions());
+
+        let fk = robot.fk(&x);
+        let jac = robot.joint_jacobian(&fk);
+        jac.row_iter()
+            .map(|row| row.iter().copied().collect())
+            .collect()
+    }
+
     fn fk(&self, x: Vec<f64>) -> Vec<Vec<f64>> {
         let robot = &self.0;