RSDK-13546- compute jacobian from model table

CMakeLists.txt

@@ -260,6 +260,7 @@ add_library(viam-trajex-totg)
 target_sources(viam-trajex-totg
   PRIVATE
     src/viam/trajex/trajex.cpp
+    src/viam/trajex/jacobian/jacobian.cpp
     src/viam/trajex/totg/observers.cpp
     src/viam/trajex/totg/path.cpp
     src/viam/trajex/totg/trajectory.cpp
@@ -494,6 +495,7 @@ if (VIAM_TRAJEX_BUILD_TESTS)
       src/viam/trajex/totg/test/uniform_sampler.cpp
       src/viam/trajex/totg/test/integration.cpp
       src/viam/trajex/totg/test/observers.cpp
+      src/viam/trajex/jacobian/test/jacobian.cpp
     )
 
     target_link_libraries(viam-trajex-totg-test

src/viam/trajex/jacobian/jacobian.cpp

@@ -0,0 +1,235 @@
+#include <viam/trajex/jacobian/jacobian.hpp>
+
+#include <array>
+#include <cmath>
+#include <cstddef>
+#include <stdexcept>
+#include <string>
+#include <utility>
+#include <vector>
+
+namespace viam::trajex::jacobian {
+
+namespace {
+
+using vec3 = std::array<double, 3>;
+
+double dot(const vec3& a, const vec3& b) {
+    return (a[0] * b[0]) + (a[1] * b[1]) + (a[2] * b[2]);
+}
+vec3 cross(const vec3& a, const vec3& b) {
+    return {(a[1] * b[2]) - (a[2] * b[1]), (a[2] * b[0]) - (a[0] * b[2]), (a[0] * b[1]) - (a[1] * b[0])};
+}
+double norm(const vec3& a) {
+    return std::sqrt(dot(a, a));
+}
+vec3 normalized(const vec3& a) {
+    const double n = norm(a);
+    return {a[0] / n, a[1] / n, a[2] / n};
+}
+
+xt::xarray<double> identity4() {
+    xt::xarray<double> t = xt::zeros<double>({std::size_t{4}, std::size_t{4}});
+    for (std::size_t i = 0; i < 4; ++i) {
+        t(i, i) = 1.0;
+    }
+    return t;
+}
+
+xt::xarray<double> matmul4(const xt::xarray<double>& a, const xt::xarray<double>& b) {
+    xt::xarray<double> c = xt::zeros<double>({std::size_t{4}, std::size_t{4}});
+    for (std::size_t i = 0; i < 4; ++i) {
+        for (std::size_t j = 0; j < 4; ++j) {
+            double s = 0.0;
+            for (std::size_t k = 0; k < 4; ++k) {
+                s += a(i, k) * b(k, j);
+            }
+            c(i, j) = s;
+        }
+    }
+    return c;
+}
+
+// 4x4 rotation about a unit axis by theta radians (Rodrigues).
+xt::xarray<double> axis_rotation4(const vec3& axis, double theta) {
+    const double c = std::cos(theta);
+    const double s = std::sin(theta);
+    const double t = 1.0 - c;
+    const double x = axis[0];
+    const double y = axis[1];
+    const double z = axis[2];
+
+    xt::xarray<double> r = identity4();
+    r(0, 0) = (t * x * x) + c;
+    r(0, 1) = (t * x * y) - (s * z);
+    r(0, 2) = (t * x * z) + (s * y);
+    r(1, 0) = (t * x * y) + (s * z);
+    r(1, 1) = (t * y * y) + c;
+    r(1, 2) = (t * y * z) - (s * x);
+    r(2, 0) = (t * x * z) - (s * y);
+    r(2, 1) = (t * y * z) + (s * x);
+    r(2, 2) = (t * z * z) + c;
+    return r;
+}
+
+// 4x4 transform for a URDF link: rotation Rz(yaw) * Ry(pitch) * Rx(roll) with
+// translation xyz.
+xt::xarray<double> link_transform(const vec3& xyz, const vec3& rpy) {
+    const xt::xarray<double> rx = axis_rotation4({1.0, 0.0, 0.0}, rpy[0]);
+    const xt::xarray<double> ry = axis_rotation4({0.0, 1.0, 0.0}, rpy[1]);
+    const xt::xarray<double> rz = axis_rotation4({0.0, 0.0, 1.0}, rpy[2]);
+    xt::xarray<double> out = matmul4(matmul4(rz, ry), rx);
+    out(0, 3) = xyz[0];
+    out(1, 3) = xyz[1];
+    out(2, 3) = xyz[2];
+    return out;
+}
+
+// Rotate a 3-vector by the rotation part of a 4x4 transform.
+vec3 rotate(const xt::xarray<double>& transform, const vec3& v) {
+    return {
+        (transform(0, 0) * v[0]) + (transform(0, 1) * v[1]) + (transform(0, 2) * v[2]),
+        (transform(1, 0) * v[0]) + (transform(1, 1) * v[1]) + (transform(1, 2) * v[2]),
+        (transform(2, 0) * v[0]) + (transform(2, 1) * v[1]) + (transform(2, 2) * v[2]),
+    };
+}
+
+vec3 translation(const xt::xarray<double>& transform) {
+    return {transform(0, 3), transform(1, 3), transform(2, 3)};
+}
+
+}  // namespace
+
+// Per-revolute-joint world-frame axis and origin, plus the end-effector
+// position, captured while evaluating the forward kinematics. With these in
+// hand, Jacobian column i is J_v_i = axes[i] x (p_e - positions[i]) (linear)
+// stacked on J_w_i = axes[i] (angular).
+struct kinematic_chain::chain_state {
+    std::vector<vec3> axes;
+    std::vector<vec3> positions;
+    vec3 p_e;
+};
+
+// Evaluate the forward kinematics row by row (base to end-effector),
+// accumulating the running link transform. For each revolute joint, capture
+// its world-frame axis and origin before applying joint motion.
+kinematic_chain::chain_state kinematic_chain::compute_chain_state(const xt::xarray<double>& q) const {
+    if (q.size() != actuated_count_) {
+        throw std::invalid_argument("viam::trajex::jacobian: q size mismatch: expected " + std::to_string(actuated_count_) +
+                                    " (actuated joints), got " + std::to_string(q.size()));
+    }
+
+    chain_state state;
+    state.axes.reserve(actuated_count_);
+    state.positions.reserve(actuated_count_);
+
+    xt::xarray<double> T = identity4();
+    std::size_t qi = 0;
+    for (const joint_row& row : rows_) {
+        T = matmul4(T, link_transform(row.xyz, row.rpy));
+
+        if (row.type == joint_type::k_revolute) {
+            const vec3 axis_local = normalized(row.axis);
+            state.axes.push_back(rotate(T, axis_local));
+            state.positions.push_back(translation(T));
+
+            T = matmul4(T, axis_rotation4(axis_local, q(qi)));
+            ++qi;
+        }
+        // fixed: no motion to apply.
+    }
+    state.p_e = translation(T);
+    return state;
+}
+
+kinematic_chain::kinematic_chain(std::vector<joint_row> rows) : rows_(std::move(rows)) {
+    if (rows_.empty()) {
+        throw std::invalid_argument("viam::trajex::jacobian: empty model table");
+    }
+    for (std::size_t i = 0; i < rows_.size(); ++i) {
+        const joint_row& row = rows_[i];
+        switch (row.type) {
+            case joint_type::k_revolute:
+                if (dot(row.axis, row.axis) < 1e-24) {
+                    throw std::invalid_argument("viam::trajex::jacobian: row " + std::to_string(i) +
+                                                " is a revolute joint with zero-magnitude axis");
+                }
+                ++actuated_count_;
+                break;
+            case joint_type::k_fixed:
+                break;
+            case joint_type::k_continuous:
+            case joint_type::k_prismatic:
+                throw std::invalid_argument("viam::trajex::jacobian: row " + std::to_string(i) +
+                                            " has unsupported joint type (only revolute and fixed are supported)");
+            default:
+                throw std::invalid_argument("viam::trajex::jacobian: row " + std::to_string(i) +
+                                            " joint type does not match any joint_type value");
+        }
+    }
+}
+
+kinematic_chain kinematic_chain::from(const xt::xarray<double>& tensor) {
+    if (tensor.dimension() != 2) {
+        throw std::invalid_argument("viam::trajex::jacobian: expected 2D model-table tensor, got " + std::to_string(tensor.dimension()) +
+                                    "D");
+    }
+    if (tensor.shape()[1] != 10) {
+        throw std::invalid_argument("viam::trajex::jacobian: expected model-table shape (n, 10), got (n, " +
+                                    std::to_string(tensor.shape()[1]) + ")");
+    }
+
+    std::vector<joint_row> rows;
+    rows.reserve(tensor.shape()[0]);
+    for (std::size_t i = 0; i < tensor.shape()[0]; ++i) {
+        const double type_value = tensor(i, 9);
+        const int type_int = static_cast<int>(type_value);
+        if (static_cast<double>(type_int) != type_value) {
+            throw std::invalid_argument("viam::trajex::jacobian: row " + std::to_string(i) + " joint type value " +
+                                        std::to_string(type_value) + " is not an integer");
+        }
+
+        joint_row row;
+        row.xyz = {tensor(i, 0), tensor(i, 1), tensor(i, 2)};
+        row.rpy = {tensor(i, 3), tensor(i, 4), tensor(i, 5)};
+        row.axis = {tensor(i, 6), tensor(i, 7), tensor(i, 8)};
+        row.type = static_cast<joint_type>(type_int);
+        rows.push_back(row);
+    }
+    return kinematic_chain(std::move(rows));
+}
+
+xt::xarray<double> kinematic_chain::jacobian(const xt::xarray<double>& q) const {
+    const chain_state state = compute_chain_state(q);
+
+    xt::xarray<double> J = xt::zeros<double>({std::size_t{6}, actuated_count_});
+    for (std::size_t i = 0; i < actuated_count_; ++i) {
+        const vec3& w = state.axes[i];
+        const vec3& p = state.positions[i];
+        const vec3 jv = cross(w, {state.p_e[0] - p[0], state.p_e[1] - p[1], state.p_e[2] - p[2]});
+        J(0, i) = jv[0];
+        J(1, i) = jv[1];
+        J(2, i) = jv[2];
+        J(3, i) = w[0];
+        J(4, i) = w[1];
+        J(5, i) = w[2];
+    }
+    return J;
+}
+
+xt::xarray<double> kinematic_chain::linear_jacobian(const xt::xarray<double>& q) const {
+    const chain_state state = compute_chain_state(q);
+
+    xt::xarray<double> J = xt::zeros<double>({std::size_t{3}, actuated_count_});
+    for (std::size_t i = 0; i < actuated_count_; ++i) {
+        const vec3& w = state.axes[i];
+        const vec3& p = state.positions[i];
+        const vec3 jv = cross(w, {state.p_e[0] - p[0], state.p_e[1] - p[1], state.p_e[2] - p[2]});
+        J(0, i) = jv[0];
+        J(1, i) = jv[1];
+        J(2, i) = jv[2];
+    }
+    return J;
+}
+
+}  // namespace viam::trajex::jacobian

src/viam/trajex/jacobian/jacobian.hpp

@@ -0,0 +1,100 @@
+#pragma once
+
+#include <array>
+#include <cstddef>
+#include <cstdint>
+#include <vector>
+
+#if __has_include(<xtensor/containers/xarray.hpp>)
+#include <xtensor/containers/xarray.hpp>
+#else
+#include <xtensor/xarray.hpp>
+#endif
+
+namespace viam::trajex::jacobian {
+
+// TODO(RSDK-14104): Remove duplicated code that exists in the viam-cpp-sdk
+
+///
+/// A validated URDF-style serial kinematic chain, parsed from an (n, 10)
+/// model-table tensor and held in chain order.
+///
+class kinematic_chain {
+   public:
+    ///
+    /// Parses an (n, 10) tensor in the viam::sdk::ModelTable format into a
+    /// kinematic chain.
+    ///
+    /// @param tensor (n, 10) tensor in the viam::sdk::ModelTable format
+    /// @return Validated kinematic chain
+    /// @throws std::invalid_argument on non-2D input, wrong column count, a
+    ///         non-integer joint-type encoding, an empty table, an unsupported
+    ///         joint type (continuous or prismatic), or a revolute row with
+    ///         zero-magnitude axis
+    ///
+    [[nodiscard]] static kinematic_chain from(const xt::xarray<double>& tensor);
+
+    ///
+    /// Computes the geometric Jacobian at joint positions q.
+    ///
+    /// @param q (N_actuated,) vector with one element per revolute row in the
+    ///        table, in chain order. Fixed rows do not consume a q entry.
+    /// @return A (6, N_actuated) xarray where rows 0..2 are the
+    ///         linear-velocity columns J_v_i = w_i x (p_e - p_i) and rows 3..5
+    ///         are the angular-velocity columns J_w_i = w_i, with w_i the
+    ///         world-frame axis of revolute joint i, p_i its world position,
+    ///         and p_e the end-effector position.
+    /// @throws std::invalid_argument on q-size mismatch
+    ///
+    [[nodiscard]] xt::xarray<double> jacobian(const xt::xarray<double>& q) const;
+
+    ///
+    /// Computes the linear-velocity block of the geometric Jacobian at joint
+    /// positions q.
+    ///
+    /// @param q (N_actuated,) vector with one element per revolute row in the
+    ///        table, in chain order. Fixed rows do not consume a q entry.
+    /// @return A (3, N_actuated) xarray of linear-velocity columns.
+    /// @throws std::invalid_argument on q-size mismatch
+    ///
+    [[nodiscard]] xt::xarray<double> linear_jacobian(const xt::xarray<double>& q) const;
+
+   private:
+    // URDF joint type, restricted to arm-relevant joints. Underlying values
+    // are the column-9 wire encoding accepted by `from`, and match
+    // viam::sdk::ModelTable::JointType.
+    enum class joint_type : std::uint8_t {
+        k_revolute = 0,
+        k_continuous = 1,
+        k_prismatic = 2,
+        k_fixed = 3,
+    };
+
+    // One row of the model table: the per-joint URDF fields. xyz/rpy are the
+    // joint origin relative to the parent link (rpy is fixed-axis XYZ); axis
+    // is the joint axis in the local frame.
+    struct joint_row {
+        std::array<double, 3> xyz{};
+        std::array<double, 3> rpy{};
+        std::array<double, 3> axis{};
+        joint_type type = joint_type::k_fixed;
+    };
+
+    // Per-revolute-joint world-frame axes and origins plus the end-effector
+    // position.
+    struct chain_state;
+
+    // Validates the rows (joint types, axes) and counts actuated joints; all
+    // public construction funnels through here via `from`.
+    explicit kinematic_chain(std::vector<joint_row> rows);
+
+    // Evaluates the forward kinematics at joint positions q, capturing the
+    // per-joint quantities the Jacobian assemblies need. Throws
+    // std::invalid_argument on q-size mismatch.
+    chain_state compute_chain_state(const xt::xarray<double>& q) const;
+
+    std::vector<joint_row> rows_;
+    std::size_t actuated_count_ = 0;
+};
+
+}  // namespace viam::trajex::jacobian