RSDK-13546- compute jacobian from model table

CMakeLists.txt

@@ -104,15 +104,22 @@ set(CMAKE_EXPORT_COMPILE_COMMANDS ON)
 if(TRAJEX_STANDALONE_BUILD)
   include(FetchContent)
 
+  # TEMPORARY: pinned to commit 35be728 on the SDK branch
+  # 20250514-RSDK-13982-construct-model-table because trajex's jacobian
+  # module consumes sdk::tensor_to_model_table / sdk::ModelTable, which are
+  # not yet in any release. That branch lives only on the nfranczak fork
+  # (not viamrobotics/viam-cpp-sdk), so GIT_REPOSITORY points at the fork.
+  # GIT_SHALLOW is FALSE so the non-tip commit resolves. Switch back to
+  # viamrobotics/viam-cpp-sdk + a release tag once the work ships.
+  # See RSDK-13546.
   FetchContent_Declare(
     viam-cpp-sdk
-    GIT_REPOSITORY https://github.com/viamrobotics/viam-cpp-sdk
-    GIT_TAG        releases/v0.31.0
-    GIT_SHALLOW    TRUE
+    GIT_REPOSITORY https://github.com/nfranczak/viam-cpp-sdk
+    GIT_TAG        35be728
+    GIT_SHALLOW    FALSE
     SYSTEM
     # Delete conflicting protobuf headers
     PATCH_COMMAND find ./src/viam/api -name "*.pb.h" -type f -exec rm {} +
-    FIND_PACKAGE_ARGS
   )
 
   FetchContent_MakeAvailable(viam-cpp-sdk)
@@ -169,6 +176,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/json_serialization.cpp
     src/viam/trajex/totg/observers.cpp
     src/viam/trajex/totg/path.cpp
@@ -186,6 +194,8 @@ target_include_directories(viam-trajex-totg
 
 target_link_libraries(viam-trajex-totg
   PUBLIC
+    Eigen3::Eigen
+    viam-cpp-sdk::viamsdk
     xtensor
     xtl
   PRIVATE
@@ -317,6 +327,7 @@ target_sources(viam-trajex-totg-test
     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,148 @@
+#include <viam/trajex/jacobian/jacobian.hpp>
+
+#include <stdexcept>
+#include <string>
+#include <utility>
+#include <vector>
+
+#include <Eigen/Geometry>
+
+#include <viam/sdk/referenceframe/urdf_model_table.hpp>
+
+namespace viam::trajex::jacobian {
+
+namespace {
+
+// The SDK's ModelTable carries xyz/rpy/axis as viam::sdk::Vector3 (a thin
+// wrapper around std::array<double, 3>, no linalg ops). trajex internally
+// uses Eigen, so we adapt at the boundary.
+inline Eigen::Vector3d to_eigen(const viam::sdk::Vector3& v) {
+    return Eigen::Vector3d(v.x(), v.y(), v.z());
+}
+
+// Build the per-link 4x4 transform from URDF (xyz, rpy). URDF rpy is
+// fixed-axis XYZ, equivalent to Rz(yaw) * Ry(pitch) * Rx(roll).
+inline Eigen::Matrix4d link_transform(const Eigen::Vector3d& xyz,
+                                      const Eigen::Vector3d& rpy) {
+    const Eigen::Matrix3d R =
+        (Eigen::AngleAxisd(rpy.z(), Eigen::Vector3d::UnitZ()) *
+         Eigen::AngleAxisd(rpy.y(), Eigen::Vector3d::UnitY()) *
+         Eigen::AngleAxisd(rpy.x(), Eigen::Vector3d::UnitX()))
+            .toRotationMatrix();
+    Eigen::Matrix4d T = Eigen::Matrix4d::Identity();
+    T.block<3, 3>(0, 0) = R;
+    T.block<3, 1>(0, 3) = xyz;
+    return T;
+}
+
+// Validate types and return revolute count. Throws std::invalid_argument
+// for unsupported joint types or zero-magnitude revolute axes.
+std::size_t validate_and_count_actuated(const viam::sdk::ModelTable& table) {
+    std::size_t n_actuated = 0;
+    for (std::size_t i = 0; i < table.size(); ++i) {
+        const auto& r = table[i];
+        switch (r.type) {
+            case viam::sdk::JointType::revolute:
+                if (to_eigen(r.axis).squaredNorm() < 1e-24) {
+                    throw std::invalid_argument(
+                        "viam::trajex::jacobian: row " + std::to_string(i) +
+                        " is a revolute joint with zero-magnitude axis");
+                }
+                ++n_actuated;
+                break;
+            case viam::sdk::JointType::fixed:
+                break;
+            case viam::sdk::JointType::continuous:
+            case viam::sdk::JointType::prismatic:
+                throw std::invalid_argument(
+                    "viam::trajex::jacobian: row " + std::to_string(i) +
+                    " has unsupported joint type (only revolute and fixed are supported)");
+        }
+    }
+    return n_actuated;
+}
+
+void check_q_size(std::size_t n_actuated, std::size_t q_size) {
+    if (q_size != n_actuated) {
+        throw std::invalid_argument(
+            "viam::trajex::jacobian: q size mismatch: expected " +
+            std::to_string(n_actuated) + " (actuated joints), got " +
+            std::to_string(q_size));
+    }
+}
+
+}  // namespace
+
+xt::xarray<double> compute_jacobian(const xt::xarray<double>& model_table,
+                                    const xt::xarray<double>& q) {
+    const auto table = viam::sdk::tensor_to_model_table(model_table);
+    const std::size_t n_actuated = validate_and_count_actuated(table);
+    check_q_size(n_actuated, q.size());
+
+    // Walk the chain. For each revolute joint, capture its world-frame axis
+    // and origin BEFORE applying joint motion (equivalent to post-motion for
+    // rotation about own axis; using pre-motion is clearer). After the walk,
+    // p_e = translation of final T.
+    std::vector<std::pair<Eigen::Vector3d, Eigen::Vector3d>> per_joint;
+    per_joint.reserve(n_actuated);
+
+    Eigen::Matrix4d T = Eigen::Matrix4d::Identity();
+    std::size_t qi = 0;
+    for (const auto& row : table) {
+        T = T * link_transform(to_eigen(row.xyz), to_eigen(row.rpy));
+
+        if (row.type == viam::sdk::JointType::revolute) {
+            const Eigen::Vector3d axis_local = to_eigen(row.axis).normalized();
+            const Eigen::Vector3d w_world = T.block<3, 3>(0, 0) * axis_local;
+            const Eigen::Vector3d p_joint = T.block<3, 1>(0, 3);
+            per_joint.emplace_back(w_world, p_joint);
+
+            Eigen::Matrix4d T_motion = Eigen::Matrix4d::Identity();
+            T_motion.block<3, 3>(0, 0) =
+                Eigen::AngleAxisd(q(qi), axis_local).toRotationMatrix();
+            T = T * T_motion;
+            ++qi;
+        }
+        // fixed: no motion to apply.
+    }
+
+    const Eigen::Vector3d p_e = T.block<3, 1>(0, 3);
+
+    xt::xarray<double> J = xt::zeros<double>({std::size_t{6}, n_actuated});
+    for (std::size_t i = 0; i < n_actuated; ++i) {
+        const auto& [w, p] = per_joint[i];
+        const Eigen::Vector3d Jv = w.cross(p_e - p);
+        J(0, i) = Jv.x();
+        J(1, i) = Jv.y();
+        J(2, i) = Jv.z();
+        J(3, i) = w.x();
+        J(4, i) = w.y();
+        J(5, i) = w.z();
+    }
+    return J;
+}
+
+Eigen::Matrix4d forward_kinematics(const xt::xarray<double>& model_table,
+                                   const xt::xarray<double>& q) {
+    const auto table = viam::sdk::tensor_to_model_table(model_table);
+    const std::size_t n_actuated = validate_and_count_actuated(table);
+    check_q_size(n_actuated, q.size());
+
+    Eigen::Matrix4d T = Eigen::Matrix4d::Identity();
+    std::size_t qi = 0;
+    for (const auto& row : table) {
+        T = T * link_transform(to_eigen(row.xyz), to_eigen(row.rpy));
+
+        if (row.type == viam::sdk::JointType::revolute) {
+            const Eigen::Vector3d axis_local = to_eigen(row.axis).normalized();
+            Eigen::Matrix4d T_motion = Eigen::Matrix4d::Identity();
+            T_motion.block<3, 3>(0, 0) =
+                Eigen::AngleAxisd(q(qi), axis_local).toRotationMatrix();
+            T = T * T_motion;
+            ++qi;
+        }
+    }
+    return T;
+}
+
+}  // namespace viam::trajex::jacobian

src/viam/trajex/jacobian/jacobian.hpp

@@ -0,0 +1,41 @@
+#pragma once
+
+#include <Eigen/Core>
+
+#if __has_include(<xtensor/containers/xarray.hpp>)
+#include <xtensor/containers/xarray.hpp>
+#else
+#include <xtensor/xarray.hpp>
+#endif
+
+namespace viam::trajex::jacobian {
+
+// Compute the geometric Jacobian for a URDF-style model table at joint
+// positions q.
+//
+// model_table: (n, 10) tensor in the viam::sdk::ModelTable format
+//              (see viam/sdk/referenceframe/urdf_model_table.hpp).
+// q:           (N_actuated,) vector. One element per revolute row in the
+//              table, in chain order. Fixed rows do not consume a q entry.
+//
+// Returns a (6, N_actuated) xarray:
+//   rows 0..2: linear-velocity columns J_v_i = w_i x (p_e - p_i)
+//   rows 3..5: angular-velocity columns J_w_i = w_i
+// where w_i is the world-frame axis of revolute joint i, p_i is its world
+// position, and p_e is the end-effector position.
+//
+// Supports only revolute and fixed joints. Throws:
+//   - std::invalid_argument on q-size mismatch, unsupported joint type
+//     (continuous or prismatic), or revolute row with zero-magnitude axis.
+//   - viam::sdk::Exception on malformed tensor shape or invalid joint-type
+//     encoding (propagated from sdk::tensor_to_model_table).
+xt::xarray<double> compute_jacobian(const xt::xarray<double>& model_table,
+                                    const xt::xarray<double>& q);
+
+// Compute the end-effector pose (4x4 homogeneous transform in the base
+// frame) for a URDF-style model table at joint positions q. Same
+// arguments and same throw contract as compute_jacobian.
+Eigen::Matrix4d forward_kinematics(const xt::xarray<double>& model_table,
+                                   const xt::xarray<double>& q);
+
+}  // namespace viam::trajex::jacobian