From de61c1ab5fd10b858d8827b2b7c1221098f6a5c6 Mon Sep 17 00:00:00 2001 From: Nick Franczak Date: Mon, 20 Apr 2026 15:07:06 -0400 Subject: [PATCH 01/19] jacobian from DH params --- CMakeLists.txt | 4 + .../trajex/jacobian/forward_kinematics.cpp | 46 +++ .../trajex/jacobian/forward_kinematics.hpp | 14 + src/viam/trajex/jacobian/jacobian.cpp | 33 +++ src/viam/trajex/jacobian/jacobian.hpp | 20 ++ src/viam/trajex/jacobian/model.hpp | 43 +++ .../trajex/jacobian/private/transforms.hpp | 10 + src/viam/trajex/jacobian/test/jacobian.cpp | 274 ++++++++++++++++++ 8 files changed, 444 insertions(+) create mode 100644 src/viam/trajex/jacobian/forward_kinematics.cpp create mode 100644 src/viam/trajex/jacobian/forward_kinematics.hpp create mode 100644 src/viam/trajex/jacobian/jacobian.cpp create mode 100644 src/viam/trajex/jacobian/jacobian.hpp create mode 100644 src/viam/trajex/jacobian/model.hpp create mode 100644 src/viam/trajex/jacobian/private/transforms.hpp create mode 100644 src/viam/trajex/jacobian/test/jacobian.cpp diff --git a/CMakeLists.txt b/CMakeLists.txt index 1017109..4a038a5 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -169,6 +169,8 @@ add_library(viam-trajex-totg) target_sources(viam-trajex-totg PRIVATE src/viam/trajex/trajex.cpp + src/viam/trajex/jacobian/forward_kinematics.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 +188,7 @@ target_include_directories(viam-trajex-totg target_link_libraries(viam-trajex-totg PUBLIC + Eigen3::Eigen xtensor xtl PRIVATE @@ -317,6 +320,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 diff --git a/src/viam/trajex/jacobian/forward_kinematics.cpp b/src/viam/trajex/jacobian/forward_kinematics.cpp new file mode 100644 index 0000000..ff51a1c --- /dev/null +++ b/src/viam/trajex/jacobian/forward_kinematics.cpp @@ -0,0 +1,46 @@ +#include + +#include +#include +#include + +#include + +namespace viam::trajex::jacobian { + +namespace detail { + +Eigen::Matrix4d dh_transform(double d, double theta, double a, double alpha) { + const double ct = std::cos(theta); + const double st = std::sin(theta); + const double ca = std::cos(alpha); + const double sa = std::sin(alpha); + + Eigen::Matrix4d T; + T << ct, -st * ca, st * sa, a * ct, + st, ct * ca, -ct * sa, a * st, + 0.0, sa, ca, d, + 0.0, 0.0, 0.0, 1.0; + return T; +} + +} // namespace detail + +void compute_forward_kinematics(const model& m, const Eigen::VectorXd& q, data& d) { + if (static_cast(q.size()) != m.joints.size()) { + throw std::invalid_argument( + "q size mismatch: expected " + std::to_string(m.joints.size()) + + ", got " + std::to_string(q.size())); + } + + Eigen::Matrix4d T = Eigen::Matrix4d::Identity(); + for (size_t i = 0; i < m.joints.size(); ++i) { + d.joint_transforms[i] = T; + const auto& j = m.joints[i]; + T = T * detail::dh_transform(j.d, j.theta + q[static_cast(i)], j.a, j.alpha); + } + d.end_effector_transform = T; + d.fk_computed = true; +} + +} // namespace viam::trajex::jacobian diff --git a/src/viam/trajex/jacobian/forward_kinematics.hpp b/src/viam/trajex/jacobian/forward_kinematics.hpp new file mode 100644 index 0000000..bdf02f6 --- /dev/null +++ b/src/viam/trajex/jacobian/forward_kinematics.hpp @@ -0,0 +1,14 @@ +#pragma once + +#include + +#include + +namespace viam::trajex::jacobian { + +// Compute forward kinematics for N-DOF robot. +// q must have model.joints.size() elements. +// Writes joint_transforms and end_effector_transform into data. +void compute_forward_kinematics(const model& m, const Eigen::VectorXd& q, data& d); + +} // namespace viam::trajex::jacobian diff --git a/src/viam/trajex/jacobian/jacobian.cpp b/src/viam/trajex/jacobian/jacobian.cpp new file mode 100644 index 0000000..6338b15 --- /dev/null +++ b/src/viam/trajex/jacobian/jacobian.cpp @@ -0,0 +1,33 @@ +#include + +#include +#include + +#include + +namespace viam::trajex::jacobian { + +void compute_jacobian(const model& m, data& d) { + assert(d.fk_computed && "compute_jacobian: FK must be computed first " + "(call compute_forward_kinematics or the 3-argument overload)"); + const Eigen::Vector3d p_e = d.end_effector_transform.block<3, 1>(0, 3); + + for (size_t i = 0; i < m.joints.size(); ++i) { + const Eigen::Matrix4d& T = d.joint_transforms[i]; + // Standard DH: joint i rotates about Z of frame i-1. The 3rd column + // of the stored rotation matrix is local Z expressed in the base frame. + const Eigen::Vector3d z = T.block<3, 1>(0, 2); + const Eigen::Vector3d p = T.block<3, 1>(0, 3); + + const Eigen::Index col = static_cast(i); + d.J.block<3, 1>(0, col) = z.cross(p_e - p); + d.J.block<3, 1>(3, col) = z; + } +} + +void compute_jacobian(const model& m, const Eigen::VectorXd& q, data& d) { + compute_forward_kinematics(m, q, d); + compute_jacobian(m, d); +} + +} // namespace viam::trajex::jacobian diff --git a/src/viam/trajex/jacobian/jacobian.hpp b/src/viam/trajex/jacobian/jacobian.hpp new file mode 100644 index 0000000..cf39080 --- /dev/null +++ b/src/viam/trajex/jacobian/jacobian.hpp @@ -0,0 +1,20 @@ +#pragma once + +#include + +#include +#include + +namespace viam::trajex::jacobian { + +// Compute full Jacobian (6 x N, where N = model.joints.size()): +// [linear velocity; angular velocity]. +// Requires FK to have been computed first (data.joint_transforms and +// data.end_effector_transform must be valid). Writes result into data.J. +void compute_jacobian(const model& m, data& d); + +// Convenience: compute both FK and Jacobian. +// q must have model.joints.size() elements. +void compute_jacobian(const model& m, const Eigen::VectorXd& q, data& d); + +} // namespace viam::trajex::jacobian diff --git a/src/viam/trajex/jacobian/model.hpp b/src/viam/trajex/jacobian/model.hpp new file mode 100644 index 0000000..59fac67 --- /dev/null +++ b/src/viam/trajex/jacobian/model.hpp @@ -0,0 +1,43 @@ +#pragma once + +#include +#include + +#include + +namespace viam::trajex::jacobian { + +// One row of a standard (distal) Denavit-Hartenberg table, for a single +// revolute joint. Per-joint transform: +// T_{i-1,i}(q) = Rz(theta + q) * Tz(d) * Tx(a) * Rx(alpha) +// theta is the zero-config offset; the runtime joint angle is theta + q. +// Units: metres for d and a, radians for theta and alpha. +struct dh_joint { + std::string name; + double d; // translation along Z_{i-1} + double theta; // zero-config rotation about Z_{i-1} + double a; // translation along X_i + double alpha; // rotation about X_i +}; + +struct model { + std::string name; + std::vector joints; // one per revolute joint; N = joints.size() +}; + +struct data { + // Base-frame transform to frame i-1 for each joint i. The Jacobian + // reads z_{i-1} and p_{i-1} from these. + std::vector joint_transforms; + Eigen::Matrix4d end_effector_transform; + Eigen::MatrixXd J; // 6 x N + bool fk_computed = false; + + explicit data(const model& m) + : joint_transforms(m.joints.size(), Eigen::Matrix4d::Identity()) + , end_effector_transform(Eigen::Matrix4d::Identity()) + , J(Eigen::MatrixXd::Zero(6, static_cast(m.joints.size()))) + {} +}; + +} // namespace viam::trajex::jacobian diff --git a/src/viam/trajex/jacobian/private/transforms.hpp b/src/viam/trajex/jacobian/private/transforms.hpp new file mode 100644 index 0000000..7cae3e2 --- /dev/null +++ b/src/viam/trajex/jacobian/private/transforms.hpp @@ -0,0 +1,10 @@ +#pragma once + +#include + +namespace viam::trajex::jacobian::detail { + +// Standard (distal) DH transform: Rz(theta) * Tz(d) * Tx(a) * Rx(alpha). +Eigen::Matrix4d dh_transform(double d, double theta, double a, double alpha); + +} // namespace viam::trajex::jacobian::detail diff --git a/src/viam/trajex/jacobian/test/jacobian.cpp b/src/viam/trajex/jacobian/test/jacobian.cpp new file mode 100644 index 0000000..a6d84f2 --- /dev/null +++ b/src/viam/trajex/jacobian/test/jacobian.cpp @@ -0,0 +1,274 @@ +// Jacobian + forward-kinematics tests. +// +// Three kinds of coverage: +// 1. Velocity projection: J * q_dot produces the expected Cartesian +// velocity for hand-chosen 2-link configurations. +// 2. Ground truth: the full 6xN Jacobian matches a hand-computed +// reference for simple 2-link configurations. +// 3. Numerical consistency: the analytical Jacobian matches a +// central-difference numerical Jacobian for 2-link and UR20 configs. + +#include +#include +#include + +#include +#include + +#include + +namespace { + +using viam::trajex::jacobian::compute_forward_kinematics; +using viam::trajex::jacobian::compute_jacobian; +using viam::trajex::jacobian::data; +using viam::trajex::jacobian::model; + +// Numerical geometric Jacobian via central differences on FK. +Eigen::MatrixXd numerical_jacobian(const model& m, const Eigen::VectorXd& q, double delta = 1e-7) { + const Eigen::Index n = static_cast(m.joints.size()); + Eigen::MatrixXd J_num(6, n); + data d_plus(m); + data d_minus(m); + + for (Eigen::Index i = 0; i < n; ++i) { + Eigen::VectorXd q_plus = q; + Eigen::VectorXd q_minus = q; + q_plus[i] += delta; + q_minus[i] -= delta; + + compute_forward_kinematics(m, q_plus, d_plus); + compute_forward_kinematics(m, q_minus, d_minus); + + const Eigen::Vector3d p_plus = d_plus.end_effector_transform.block<3, 1>(0, 3); + const Eigen::Vector3d p_minus = d_minus.end_effector_transform.block<3, 1>(0, 3); + J_num.block<3, 1>(0, i) = (p_plus - p_minus) / (2.0 * delta); + + const Eigen::Matrix3d R_plus = d_plus.end_effector_transform.block<3, 3>(0, 0); + const Eigen::Matrix3d R_minus = d_minus.end_effector_transform.block<3, 3>(0, 0); + const Eigen::Matrix3d dR = R_plus * R_minus.transpose(); + + Eigen::Vector3d omega; + omega << (dR(2, 1) - dR(1, 2)), (dR(0, 2) - dR(2, 0)), (dR(1, 0) - dR(0, 1)); + omega /= (4.0 * delta); + J_num.block<3, 1>(3, i) = omega; + } + return J_num; +} + +model twolink() { + // Two 1m links, both revolute about Z, no offsets. + return model{ + "2link_planar", + { + {"joint1", 0.0, 0.0, 1.0, 0.0}, + {"joint2", 0.0, 0.0, 1.0, 0.0}, + }, + }; +} + +model ur20() { + // DH derived from urdfs/ur20FK.urdf via viam-cpp-sdk's urdf_to_dh_params, + // pruned to end at ft_frame to match the URDF parser's end-effector choice. + return model{ + "UR20", + { + {"shoulder_pan_joint", 0.23630000000000001, -1.2246467991473532e-16, 0.0, -1.570796327}, + {"shoulder_lift_joint", 0.0, -0.0, 0.86199999999999999, 0.0}, + {"elbow_joint", 5.9732854069808984e-18, -1.2188533337963318e-18, 0.72870000000000001, 0.0}, + {"wrist_1_joint", 0.20100000000000004, 1.2188533337963318e-18, -2.4615400662861804e-17, -1.570796327}, + {"wrist_2_joint", 0.1593, -1.224646798896174e-16, 8.0025735243201159e-27, 1.570796327}, + {"wrist_3_joint", 0.15429999999999999, -3.1415926535897931, 3.8756978341028043e-27, 3.1415926535897931}, + }, + }; +} + +} // namespace + +BOOST_AUTO_TEST_SUITE(jacobian_velocity_tests) + +// q=[0,0], q_dot=[1,0]: base spinning at 1 rad/s, EE is 2m along X. +// EE traces a circle of radius 2 about Z -> v = [0, 2, 0], w = [0, 0, 1]. +BOOST_AUTO_TEST_CASE(twolink_base_spin_extended) { + const model m = twolink(); + Eigen::VectorXd q = Eigen::VectorXd::Zero(2); + Eigen::VectorXd q_dot(2); + q_dot << 1.0, 0.0; + + data d(m); + compute_jacobian(m, q, d); + + const Eigen::Vector3d v = d.J.topRows(3) * q_dot; + const Eigen::Vector3d omega = d.J.bottomRows(3) * q_dot; + + BOOST_CHECK_SMALL((v - Eigen::Vector3d(0.0, 2.0, 0.0)).norm(), 1e-10); + BOOST_CHECK_SMALL((omega - Eigen::Vector3d(0.0, 0.0, 1.0)).norm(), 1e-10); +} + +// q=[pi/2, 0], q_dot=[1,0]: base spinning, arm along +Y, EE at (0,2,0). +// EE sweeps in -X -> v = [-2, 0, 0], w = [0, 0, 1]. +BOOST_AUTO_TEST_CASE(twolink_base_spin_rotated) { + const model m = twolink(); + Eigen::VectorXd q(2); + q << std::numbers::pi / 2.0, 0.0; + Eigen::VectorXd q_dot(2); + q_dot << 1.0, 0.0; + + data d(m); + compute_jacobian(m, q, d); + + const Eigen::Vector3d v = d.J.topRows(3) * q_dot; + const Eigen::Vector3d omega = d.J.bottomRows(3) * q_dot; + + BOOST_CHECK_SMALL((v - Eigen::Vector3d(-2.0, 0.0, 0.0)).norm(), 1e-10); + BOOST_CHECK_SMALL((omega - Eigen::Vector3d(0.0, 0.0, 1.0)).norm(), 1e-10); +} + +// q=[0, pi/2], q_dot=[1,0]: base spinning, link2 bent 90deg, EE at (1,1,0). +// -> v = [-1, 1, 0], w = [0, 0, 1]. +BOOST_AUTO_TEST_CASE(twolink_base_spin_bent) { + const model m = twolink(); + Eigen::VectorXd q(2); + q << 0.0, std::numbers::pi / 2.0; + Eigen::VectorXd q_dot(2); + q_dot << 1.0, 0.0; + + data d(m); + compute_jacobian(m, q, d); + + const Eigen::Vector3d v = d.J.topRows(3) * q_dot; + const Eigen::Vector3d omega = d.J.bottomRows(3) * q_dot; + + BOOST_CHECK_SMALL((v - Eigen::Vector3d(-1.0, 1.0, 0.0)).norm(), 1e-10); + BOOST_CHECK_SMALL((omega - Eigen::Vector3d(0.0, 0.0, 1.0)).norm(), 1e-10); +} + +BOOST_AUTO_TEST_SUITE_END() + + +BOOST_AUTO_TEST_SUITE(jacobian_ground_truth_tests) + +// q=[0,0]: links along X, EE at (2,0,0). +BOOST_AUTO_TEST_CASE(twolink_zero) { + const model m = twolink(); + Eigen::VectorXd q = Eigen::VectorXd::Zero(2); + data d(m); + compute_jacobian(m, q, d); + + Eigen::MatrixXd J_expected(6, 2); + J_expected << 0.0, 0.0, + 2.0, 1.0, + 0.0, 0.0, + 0.0, 0.0, + 0.0, 0.0, + 1.0, 1.0; + + BOOST_CHECK_SMALL((d.J - J_expected).norm(), 1e-10); +} + +// q=[pi/2, 0]: links along Y, EE at (0,2,0). +BOOST_AUTO_TEST_CASE(twolink_q1_ninety) { + const model m = twolink(); + Eigen::VectorXd q(2); + q << std::numbers::pi / 2.0, 0.0; + data d(m); + compute_jacobian(m, q, d); + + Eigen::MatrixXd J_expected(6, 2); + J_expected << -2.0, -1.0, + 0.0, 0.0, + 0.0, 0.0, + 0.0, 0.0, + 0.0, 0.0, + 1.0, 1.0; + + BOOST_CHECK_SMALL((d.J - J_expected).norm(), 1e-10); +} + +// q=[0, pi/2]: link1 along X, link2 along Y, EE at (1,1,0). +BOOST_AUTO_TEST_CASE(twolink_q2_ninety) { + const model m = twolink(); + Eigen::VectorXd q(2); + q << 0.0, std::numbers::pi / 2.0; + data d(m); + compute_jacobian(m, q, d); + + Eigen::MatrixXd J_expected(6, 2); + J_expected << -1.0, -1.0, + 1.0, 0.0, + 0.0, 0.0, + 0.0, 0.0, + 0.0, 0.0, + 1.0, 1.0; + + BOOST_CHECK_SMALL((d.J - J_expected).norm(), 1e-10); +} + +BOOST_AUTO_TEST_SUITE_END() + + +BOOST_AUTO_TEST_SUITE(jacobian_numerical_consistency_tests) + +namespace { + +void check_matches_numerical(const model& m, const Eigen::VectorXd& q, double tol = 1e-6) { + data d(m); + compute_jacobian(m, q, d); + const Eigen::MatrixXd J_num = numerical_jacobian(m, q); + BOOST_CHECK_SMALL((d.J - J_num).norm(), tol); +} + +} // namespace + +BOOST_AUTO_TEST_CASE(twolink_zero) { + check_matches_numerical(twolink(), Eigen::VectorXd::Zero(2)); +} + +BOOST_AUTO_TEST_CASE(twolink_q1_ninety) { + Eigen::VectorXd q(2); + q << std::numbers::pi / 2.0, 0.0; + check_matches_numerical(twolink(), q); +} + +BOOST_AUTO_TEST_CASE(twolink_q1_q2_forty_five) { + Eigen::VectorXd q(2); + q << std::numbers::pi / 4.0, std::numbers::pi / 4.0; + check_matches_numerical(twolink(), q); +} + +BOOST_AUTO_TEST_CASE(twolink_folded_back) { + Eigen::VectorXd q(2); + q << 0.0, std::numbers::pi; + check_matches_numerical(twolink(), q); +} + +BOOST_AUTO_TEST_CASE(ur20_zero) { + check_matches_numerical(ur20(), Eigen::VectorXd::Zero(6)); +} + +BOOST_AUTO_TEST_CASE(ur20_small_angles) { + Eigen::VectorXd q(6); + q << 0.1, 0.2, -0.1, 0.15, -0.05, 0.1; + check_matches_numerical(ur20(), q); +} + +BOOST_AUTO_TEST_CASE(ur20_typical) { + Eigen::VectorXd q(6); + q << 0.0, -std::numbers::pi / 2.0, std::numbers::pi / 2.0, 0.0, std::numbers::pi / 2.0, 0.0; + check_matches_numerical(ur20(), q); +} + +BOOST_AUTO_TEST_SUITE_END() + + +BOOST_AUTO_TEST_SUITE(jacobian_error_tests) + +BOOST_AUTO_TEST_CASE(fk_rejects_wrong_q_size) { + const model m = twolink(); + data d(m); + Eigen::VectorXd q_bad(3); + q_bad << 0.0, 0.0, 0.0; + BOOST_CHECK_THROW(compute_forward_kinematics(m, q_bad, d), std::invalid_argument); +} + +BOOST_AUTO_TEST_SUITE_END() From 05980a246bed5004344e388e373daa0ed6704854 Mon Sep 17 00:00:00 2001 From: Nick Franczak Date: Mon, 27 Apr 2026 10:53:36 -0400 Subject: [PATCH 02/19] use eigen --- .../trajex/jacobian/forward_kinematics.cpp | 41 +++- .../trajex/jacobian/forward_kinematics.hpp | 8 +- src/viam/trajex/jacobian/jacobian.cpp | 41 +++- src/viam/trajex/jacobian/jacobian.hpp | 8 +- src/viam/trajex/jacobian/model.hpp | 22 +- .../trajex/jacobian/private/transforms.hpp | 17 +- src/viam/trajex/jacobian/test/jacobian.cpp | 225 +++++++++++------- 7 files changed, 231 insertions(+), 131 deletions(-) diff --git a/src/viam/trajex/jacobian/forward_kinematics.cpp b/src/viam/trajex/jacobian/forward_kinematics.cpp index ff51a1c..dcce99e 100644 --- a/src/viam/trajex/jacobian/forward_kinematics.cpp +++ b/src/viam/trajex/jacobian/forward_kinematics.cpp @@ -10,34 +10,51 @@ namespace viam::trajex::jacobian { namespace detail { -Eigen::Matrix4d dh_transform(double d, double theta, double a, double alpha) { +xt::xarray dh_transform(double d, double theta, double a, double alpha) { const double ct = std::cos(theta); const double st = std::sin(theta); const double ca = std::cos(alpha); const double sa = std::sin(alpha); - Eigen::Matrix4d T; - T << ct, -st * ca, st * sa, a * ct, - st, ct * ca, -ct * sa, a * st, - 0.0, sa, ca, d, - 0.0, 0.0, 0.0, 1.0; - return T; + return xt::xarray{ + { ct, -st * ca, st * sa, a * ct }, + { st, ct * ca, -ct * sa, a * st }, + {0.0, sa, ca, d }, + {0.0, 0.0, 0.0, 1.0 }, + }; +} + +void matmul_4x4(const xt::xarray& A, + const xt::xarray& B, + xt::xarray& out) { + 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); + } + out(i, j) = s; + } + } } } // namespace detail -void compute_forward_kinematics(const model& m, const Eigen::VectorXd& q, data& d) { - if (static_cast(q.size()) != m.joints.size()) { +void compute_forward_kinematics(const model& m, const xt::xarray& q, data& d) { + if (q.size() != m.joints.size()) { throw std::invalid_argument( "q size mismatch: expected " + std::to_string(m.joints.size()) + ", got " + std::to_string(q.size())); } - Eigen::Matrix4d T = Eigen::Matrix4d::Identity(); - for (size_t i = 0; i < m.joints.size(); ++i) { + xt::xarray T = xt::eye(4); + xt::xarray next = xt::zeros({std::size_t{4}, std::size_t{4}}); + for (std::size_t i = 0; i < m.joints.size(); ++i) { d.joint_transforms[i] = T; const auto& j = m.joints[i]; - T = T * detail::dh_transform(j.d, j.theta + q[static_cast(i)], j.a, j.alpha); + const auto step = detail::dh_transform(j.d, j.theta + q(i), j.a, j.alpha); + detail::matmul_4x4(T, step, next); + T = next; } d.end_effector_transform = T; d.fk_computed = true; diff --git a/src/viam/trajex/jacobian/forward_kinematics.hpp b/src/viam/trajex/jacobian/forward_kinematics.hpp index bdf02f6..185ed5f 100644 --- a/src/viam/trajex/jacobian/forward_kinematics.hpp +++ b/src/viam/trajex/jacobian/forward_kinematics.hpp @@ -1,6 +1,10 @@ #pragma once -#include +#if __has_include() +#include +#else +#include +#endif #include @@ -9,6 +13,6 @@ namespace viam::trajex::jacobian { // Compute forward kinematics for N-DOF robot. // q must have model.joints.size() elements. // Writes joint_transforms and end_effector_transform into data. -void compute_forward_kinematics(const model& m, const Eigen::VectorXd& q, data& d); +void compute_forward_kinematics(const model& m, const xt::xarray& q, data& d); } // namespace viam::trajex::jacobian diff --git a/src/viam/trajex/jacobian/jacobian.cpp b/src/viam/trajex/jacobian/jacobian.cpp index 6338b15..7445a21 100644 --- a/src/viam/trajex/jacobian/jacobian.cpp +++ b/src/viam/trajex/jacobian/jacobian.cpp @@ -1,31 +1,48 @@ #include +#include #include #include -#include - namespace viam::trajex::jacobian { +namespace { + +// 3-element cross product written into J(0..2, col). +inline void write_cross_into_J_col( + const std::array& a, + const std::array& b, + xt::xarray& J, + std::size_t col) { + J(0, col) = a[1] * b[2] - a[2] * b[1]; + J(1, col) = a[2] * b[0] - a[0] * b[2]; + J(2, col) = a[0] * b[1] - a[1] * b[0]; +} + +} // namespace + void compute_jacobian(const model& m, data& d) { assert(d.fk_computed && "compute_jacobian: FK must be computed first " "(call compute_forward_kinematics or the 3-argument overload)"); - const Eigen::Vector3d p_e = d.end_effector_transform.block<3, 1>(0, 3); + const auto& T_e = d.end_effector_transform; + const std::array p_e{T_e(0, 3), T_e(1, 3), T_e(2, 3)}; - for (size_t i = 0; i < m.joints.size(); ++i) { - const Eigen::Matrix4d& T = d.joint_transforms[i]; + for (std::size_t i = 0; i < m.joints.size(); ++i) { + const auto& T = d.joint_transforms[i]; // Standard DH: joint i rotates about Z of frame i-1. The 3rd column // of the stored rotation matrix is local Z expressed in the base frame. - const Eigen::Vector3d z = T.block<3, 1>(0, 2); - const Eigen::Vector3d p = T.block<3, 1>(0, 3); - - const Eigen::Index col = static_cast(i); - d.J.block<3, 1>(0, col) = z.cross(p_e - p); - d.J.block<3, 1>(3, col) = z; + const std::array z{T(0, 2), T(1, 2), T(2, 2)}; + const std::array p{T(0, 3), T(1, 3), T(2, 3)}; + const std::array r{p_e[0] - p[0], p_e[1] - p[1], p_e[2] - p[2]}; + + write_cross_into_J_col(z, r, d.J, i); + d.J(3, i) = z[0]; + d.J(4, i) = z[1]; + d.J(5, i) = z[2]; } } -void compute_jacobian(const model& m, const Eigen::VectorXd& q, data& d) { +void compute_jacobian(const model& m, const xt::xarray& q, data& d) { compute_forward_kinematics(m, q, d); compute_jacobian(m, d); } diff --git a/src/viam/trajex/jacobian/jacobian.hpp b/src/viam/trajex/jacobian/jacobian.hpp index cf39080..ceeef1e 100644 --- a/src/viam/trajex/jacobian/jacobian.hpp +++ b/src/viam/trajex/jacobian/jacobian.hpp @@ -1,6 +1,10 @@ #pragma once -#include +#if __has_include() +#include +#else +#include +#endif #include #include @@ -15,6 +19,6 @@ void compute_jacobian(const model& m, data& d); // Convenience: compute both FK and Jacobian. // q must have model.joints.size() elements. -void compute_jacobian(const model& m, const Eigen::VectorXd& q, data& d); +void compute_jacobian(const model& m, const xt::xarray& q, data& d); } // namespace viam::trajex::jacobian diff --git a/src/viam/trajex/jacobian/model.hpp b/src/viam/trajex/jacobian/model.hpp index 59fac67..0b3f8ce 100644 --- a/src/viam/trajex/jacobian/model.hpp +++ b/src/viam/trajex/jacobian/model.hpp @@ -3,7 +3,11 @@ #include #include -#include +#if __has_include() +#include +#else +#include +#endif namespace viam::trajex::jacobian { @@ -26,17 +30,17 @@ struct model { }; struct data { - // Base-frame transform to frame i-1 for each joint i. The Jacobian - // reads z_{i-1} and p_{i-1} from these. - std::vector joint_transforms; - Eigen::Matrix4d end_effector_transform; - Eigen::MatrixXd J; // 6 x N + // Base-frame transform to frame i-1 for each joint i (each is 4x4). The + // Jacobian reads z_{i-1} and p_{i-1} from these. + std::vector> joint_transforms; + xt::xarray end_effector_transform; // 4x4 + xt::xarray J; // 6 x N bool fk_computed = false; explicit data(const model& m) - : joint_transforms(m.joints.size(), Eigen::Matrix4d::Identity()) - , end_effector_transform(Eigen::Matrix4d::Identity()) - , J(Eigen::MatrixXd::Zero(6, static_cast(m.joints.size()))) + : joint_transforms(m.joints.size(), xt::eye(4)) + , end_effector_transform(xt::eye(4)) + , J(xt::zeros({std::size_t{6}, m.joints.size()})) {} }; diff --git a/src/viam/trajex/jacobian/private/transforms.hpp b/src/viam/trajex/jacobian/private/transforms.hpp index 7cae3e2..532ab39 100644 --- a/src/viam/trajex/jacobian/private/transforms.hpp +++ b/src/viam/trajex/jacobian/private/transforms.hpp @@ -1,10 +1,23 @@ #pragma once -#include +#if __has_include() +#include +#else +#include +#endif namespace viam::trajex::jacobian::detail { // Standard (distal) DH transform: Rz(theta) * Tz(d) * Tx(a) * Rx(alpha). -Eigen::Matrix4d dh_transform(double d, double theta, double a, double alpha); +// Returns a 4x4 xtensor array. +xt::xarray dh_transform(double d, double theta, double a, double alpha); + +// In-place 4x4 matrix multiply: out = A * B. The three arguments must be +// distinct buffers (no aliasing). xtensor lacks a built-in matmul without +// xtensor-blas, and a hand-rolled 4x4 loop is faster than xt::sum-based +// expressions for this fixed shape. +void matmul_4x4(const xt::xarray& A, + const xt::xarray& B, + xt::xarray& out); } // namespace viam::trajex::jacobian::detail diff --git a/src/viam/trajex/jacobian/test/jacobian.cpp b/src/viam/trajex/jacobian/test/jacobian.cpp index a6d84f2..c313c93 100644 --- a/src/viam/trajex/jacobian/test/jacobian.cpp +++ b/src/viam/trajex/jacobian/test/jacobian.cpp @@ -12,9 +12,21 @@ #include #include +#include #include #include +#if __has_include() +#include +#else +#include +#endif +#if __has_include() +#include +#else +#include +#endif + #include namespace { @@ -24,34 +36,77 @@ using viam::trajex::jacobian::compute_jacobian; using viam::trajex::jacobian::data; using viam::trajex::jacobian::model; +// Frobenius norm of (A - B). Both must have identical shape. +double matrix_diff_norm(const xt::xarray& A, const xt::xarray& B) { + double s = 0.0; + auto a = A.begin(); + auto b = B.begin(); + for (; a != A.end(); ++a, ++b) { + const double d = *a - *b; + s += d * d; + } + return std::sqrt(s); +} + +double vec3_diff_norm(const std::array& a, const std::array& b) { + const double dx = a[0] - b[0]; + const double dy = a[1] - b[1]; + const double dz = a[2] - b[2]; + return std::sqrt(dx * dx + dy * dy + dz * dz); +} + +// J (6 x N) * q_dot (N) -> 6-vector returned as two 3-arrays (linear, angular). +struct twist { + std::array v; + std::array w; +}; +twist J_times_qdot(const xt::xarray& J, const xt::xarray& q_dot) { + twist t{{0, 0, 0}, {0, 0, 0}}; + const std::size_t n = q_dot.size(); + for (std::size_t j = 0; j < n; ++j) { + for (std::size_t i = 0; i < 3; ++i) t.v[i] += J(i, j) * q_dot(j); + for (std::size_t i = 0; i < 3; ++i) t.w[i] += J(3 + i, j) * q_dot(j); + } + return t; +} + // Numerical geometric Jacobian via central differences on FK. -Eigen::MatrixXd numerical_jacobian(const model& m, const Eigen::VectorXd& q, double delta = 1e-7) { - const Eigen::Index n = static_cast(m.joints.size()); - Eigen::MatrixXd J_num(6, n); +xt::xarray numerical_jacobian(const model& m, const xt::xarray& q, double delta = 1e-7) { + const std::size_t n = m.joints.size(); + xt::xarray J_num = xt::zeros({std::size_t{6}, n}); data d_plus(m); data d_minus(m); - for (Eigen::Index i = 0; i < n; ++i) { - Eigen::VectorXd q_plus = q; - Eigen::VectorXd q_minus = q; - q_plus[i] += delta; - q_minus[i] -= delta; + for (std::size_t i = 0; i < n; ++i) { + xt::xarray q_plus = q; + xt::xarray q_minus = q; + q_plus(i) += delta; + q_minus(i) -= delta; compute_forward_kinematics(m, q_plus, d_plus); compute_forward_kinematics(m, q_minus, d_minus); - const Eigen::Vector3d p_plus = d_plus.end_effector_transform.block<3, 1>(0, 3); - const Eigen::Vector3d p_minus = d_minus.end_effector_transform.block<3, 1>(0, 3); - J_num.block<3, 1>(0, i) = (p_plus - p_minus) / (2.0 * delta); - - const Eigen::Matrix3d R_plus = d_plus.end_effector_transform.block<3, 3>(0, 0); - const Eigen::Matrix3d R_minus = d_minus.end_effector_transform.block<3, 3>(0, 0); - const Eigen::Matrix3d dR = R_plus * R_minus.transpose(); - - Eigen::Vector3d omega; - omega << (dR(2, 1) - dR(1, 2)), (dR(0, 2) - dR(2, 0)), (dR(1, 0) - dR(0, 1)); - omega /= (4.0 * delta); - J_num.block<3, 1>(3, i) = omega; + const auto& Tp = d_plus.end_effector_transform; + const auto& Tm = d_minus.end_effector_transform; + for (std::size_t r = 0; r < 3; ++r) { + J_num(r, i) = (Tp(r, 3) - Tm(r, 3)) / (2.0 * delta); + } + + // dR = R_plus * R_minus^T, then extract omega from skew-symmetric part. + std::array, 3> dR{}; + for (std::size_t r = 0; r < 3; ++r) { + for (std::size_t c = 0; c < 3; ++c) { + double s = 0.0; + for (std::size_t k = 0; k < 3; ++k) { + s += Tp(r, k) * Tm(c, k); // R_minus^T(k,c) == Tm(c,k) + } + dR[r][c] = s; + } + } + const double scale = 1.0 / (4.0 * delta); + J_num(3, i) = (dR[2][1] - dR[1][2]) * scale; + J_num(4, i) = (dR[0][2] - dR[2][0]) * scale; + J_num(5, i) = (dR[1][0] - dR[0][1]) * scale; } return J_num; } @@ -91,56 +146,45 @@ BOOST_AUTO_TEST_SUITE(jacobian_velocity_tests) // EE traces a circle of radius 2 about Z -> v = [0, 2, 0], w = [0, 0, 1]. BOOST_AUTO_TEST_CASE(twolink_base_spin_extended) { const model m = twolink(); - Eigen::VectorXd q = Eigen::VectorXd::Zero(2); - Eigen::VectorXd q_dot(2); - q_dot << 1.0, 0.0; + const xt::xarray q = xt::zeros({std::size_t{2}}); + const xt::xarray q_dot = {1.0, 0.0}; data d(m); compute_jacobian(m, q, d); + const auto t = J_times_qdot(d.J, q_dot); - const Eigen::Vector3d v = d.J.topRows(3) * q_dot; - const Eigen::Vector3d omega = d.J.bottomRows(3) * q_dot; - - BOOST_CHECK_SMALL((v - Eigen::Vector3d(0.0, 2.0, 0.0)).norm(), 1e-10); - BOOST_CHECK_SMALL((omega - Eigen::Vector3d(0.0, 0.0, 1.0)).norm(), 1e-10); + BOOST_CHECK_SMALL(vec3_diff_norm(t.v, {0.0, 2.0, 0.0}), 1e-10); + BOOST_CHECK_SMALL(vec3_diff_norm(t.w, {0.0, 0.0, 1.0}), 1e-10); } // q=[pi/2, 0], q_dot=[1,0]: base spinning, arm along +Y, EE at (0,2,0). // EE sweeps in -X -> v = [-2, 0, 0], w = [0, 0, 1]. BOOST_AUTO_TEST_CASE(twolink_base_spin_rotated) { const model m = twolink(); - Eigen::VectorXd q(2); - q << std::numbers::pi / 2.0, 0.0; - Eigen::VectorXd q_dot(2); - q_dot << 1.0, 0.0; + const xt::xarray q = {std::numbers::pi / 2.0, 0.0}; + const xt::xarray q_dot = {1.0, 0.0}; data d(m); compute_jacobian(m, q, d); + const auto t = J_times_qdot(d.J, q_dot); - const Eigen::Vector3d v = d.J.topRows(3) * q_dot; - const Eigen::Vector3d omega = d.J.bottomRows(3) * q_dot; - - BOOST_CHECK_SMALL((v - Eigen::Vector3d(-2.0, 0.0, 0.0)).norm(), 1e-10); - BOOST_CHECK_SMALL((omega - Eigen::Vector3d(0.0, 0.0, 1.0)).norm(), 1e-10); + BOOST_CHECK_SMALL(vec3_diff_norm(t.v, {-2.0, 0.0, 0.0}), 1e-10); + BOOST_CHECK_SMALL(vec3_diff_norm(t.w, {0.0, 0.0, 1.0}), 1e-10); } // q=[0, pi/2], q_dot=[1,0]: base spinning, link2 bent 90deg, EE at (1,1,0). // -> v = [-1, 1, 0], w = [0, 0, 1]. BOOST_AUTO_TEST_CASE(twolink_base_spin_bent) { const model m = twolink(); - Eigen::VectorXd q(2); - q << 0.0, std::numbers::pi / 2.0; - Eigen::VectorXd q_dot(2); - q_dot << 1.0, 0.0; + const xt::xarray q = {0.0, std::numbers::pi / 2.0}; + const xt::xarray q_dot = {1.0, 0.0}; data d(m); compute_jacobian(m, q, d); + const auto t = J_times_qdot(d.J, q_dot); - const Eigen::Vector3d v = d.J.topRows(3) * q_dot; - const Eigen::Vector3d omega = d.J.bottomRows(3) * q_dot; - - BOOST_CHECK_SMALL((v - Eigen::Vector3d(-1.0, 1.0, 0.0)).norm(), 1e-10); - BOOST_CHECK_SMALL((omega - Eigen::Vector3d(0.0, 0.0, 1.0)).norm(), 1e-10); + BOOST_CHECK_SMALL(vec3_diff_norm(t.v, {-1.0, 1.0, 0.0}), 1e-10); + BOOST_CHECK_SMALL(vec3_diff_norm(t.w, {0.0, 0.0, 1.0}), 1e-10); } BOOST_AUTO_TEST_SUITE_END() @@ -151,57 +195,58 @@ BOOST_AUTO_TEST_SUITE(jacobian_ground_truth_tests) // q=[0,0]: links along X, EE at (2,0,0). BOOST_AUTO_TEST_CASE(twolink_zero) { const model m = twolink(); - Eigen::VectorXd q = Eigen::VectorXd::Zero(2); + const xt::xarray q = xt::zeros({std::size_t{2}}); data d(m); compute_jacobian(m, q, d); - Eigen::MatrixXd J_expected(6, 2); - J_expected << 0.0, 0.0, - 2.0, 1.0, - 0.0, 0.0, - 0.0, 0.0, - 0.0, 0.0, - 1.0, 1.0; + const xt::xarray J_expected = { + {0.0, 0.0}, + {2.0, 1.0}, + {0.0, 0.0}, + {0.0, 0.0}, + {0.0, 0.0}, + {1.0, 1.0}, + }; - BOOST_CHECK_SMALL((d.J - J_expected).norm(), 1e-10); + BOOST_CHECK_SMALL(matrix_diff_norm(d.J, J_expected), 1e-10); } // q=[pi/2, 0]: links along Y, EE at (0,2,0). BOOST_AUTO_TEST_CASE(twolink_q1_ninety) { const model m = twolink(); - Eigen::VectorXd q(2); - q << std::numbers::pi / 2.0, 0.0; + const xt::xarray q = {std::numbers::pi / 2.0, 0.0}; data d(m); compute_jacobian(m, q, d); - Eigen::MatrixXd J_expected(6, 2); - J_expected << -2.0, -1.0, - 0.0, 0.0, - 0.0, 0.0, - 0.0, 0.0, - 0.0, 0.0, - 1.0, 1.0; + const xt::xarray J_expected = { + {-2.0, -1.0}, + { 0.0, 0.0}, + { 0.0, 0.0}, + { 0.0, 0.0}, + { 0.0, 0.0}, + { 1.0, 1.0}, + }; - BOOST_CHECK_SMALL((d.J - J_expected).norm(), 1e-10); + BOOST_CHECK_SMALL(matrix_diff_norm(d.J, J_expected), 1e-10); } // q=[0, pi/2]: link1 along X, link2 along Y, EE at (1,1,0). BOOST_AUTO_TEST_CASE(twolink_q2_ninety) { const model m = twolink(); - Eigen::VectorXd q(2); - q << 0.0, std::numbers::pi / 2.0; + const xt::xarray q = {0.0, std::numbers::pi / 2.0}; data d(m); compute_jacobian(m, q, d); - Eigen::MatrixXd J_expected(6, 2); - J_expected << -1.0, -1.0, - 1.0, 0.0, - 0.0, 0.0, - 0.0, 0.0, - 0.0, 0.0, - 1.0, 1.0; + const xt::xarray J_expected = { + {-1.0, -1.0}, + { 1.0, 0.0}, + { 0.0, 0.0}, + { 0.0, 0.0}, + { 0.0, 0.0}, + { 1.0, 1.0}, + }; - BOOST_CHECK_SMALL((d.J - J_expected).norm(), 1e-10); + BOOST_CHECK_SMALL(matrix_diff_norm(d.J, J_expected), 1e-10); } BOOST_AUTO_TEST_SUITE_END() @@ -211,50 +256,47 @@ BOOST_AUTO_TEST_SUITE(jacobian_numerical_consistency_tests) namespace { -void check_matches_numerical(const model& m, const Eigen::VectorXd& q, double tol = 1e-6) { +void check_matches_numerical(const model& m, const xt::xarray& q, double tol = 1e-6) { data d(m); compute_jacobian(m, q, d); - const Eigen::MatrixXd J_num = numerical_jacobian(m, q); - BOOST_CHECK_SMALL((d.J - J_num).norm(), tol); + const xt::xarray J_num = numerical_jacobian(m, q); + BOOST_CHECK_SMALL(matrix_diff_norm(d.J, J_num), tol); } } // namespace BOOST_AUTO_TEST_CASE(twolink_zero) { - check_matches_numerical(twolink(), Eigen::VectorXd::Zero(2)); + check_matches_numerical(twolink(), xt::zeros({std::size_t{2}})); } BOOST_AUTO_TEST_CASE(twolink_q1_ninety) { - Eigen::VectorXd q(2); - q << std::numbers::pi / 2.0, 0.0; + const xt::xarray q = {std::numbers::pi / 2.0, 0.0}; check_matches_numerical(twolink(), q); } BOOST_AUTO_TEST_CASE(twolink_q1_q2_forty_five) { - Eigen::VectorXd q(2); - q << std::numbers::pi / 4.0, std::numbers::pi / 4.0; + const xt::xarray q = {std::numbers::pi / 4.0, std::numbers::pi / 4.0}; check_matches_numerical(twolink(), q); } BOOST_AUTO_TEST_CASE(twolink_folded_back) { - Eigen::VectorXd q(2); - q << 0.0, std::numbers::pi; + const xt::xarray q = {0.0, std::numbers::pi}; check_matches_numerical(twolink(), q); } BOOST_AUTO_TEST_CASE(ur20_zero) { - check_matches_numerical(ur20(), Eigen::VectorXd::Zero(6)); + check_matches_numerical(ur20(), xt::zeros({std::size_t{6}})); } BOOST_AUTO_TEST_CASE(ur20_small_angles) { - Eigen::VectorXd q(6); - q << 0.1, 0.2, -0.1, 0.15, -0.05, 0.1; + const xt::xarray q = {0.1, 0.2, -0.1, 0.15, -0.05, 0.1}; check_matches_numerical(ur20(), q); } BOOST_AUTO_TEST_CASE(ur20_typical) { - Eigen::VectorXd q(6); - q << 0.0, -std::numbers::pi / 2.0, std::numbers::pi / 2.0, 0.0, std::numbers::pi / 2.0, 0.0; + const xt::xarray q = { + 0.0, -std::numbers::pi / 2.0, std::numbers::pi / 2.0, + 0.0, std::numbers::pi / 2.0, 0.0}; check_matches_numerical(ur20(), q); } @@ -266,8 +308,7 @@ BOOST_AUTO_TEST_SUITE(jacobian_error_tests) BOOST_AUTO_TEST_CASE(fk_rejects_wrong_q_size) { const model m = twolink(); data d(m); - Eigen::VectorXd q_bad(3); - q_bad << 0.0, 0.0, 0.0; + const xt::xarray q_bad = {0.0, 0.0, 0.0}; BOOST_CHECK_THROW(compute_forward_kinematics(m, q_bad, d), std::invalid_argument); } From 98c534d62aa13af54fc283a6efb4790f9121ccfe Mon Sep 17 00:00:00 2001 From: Nick Franczak Date: Wed, 27 May 2026 11:32:24 -0400 Subject: [PATCH 03/19] jacobian from model table, includes SDK bump --- CMakeLists.txt | 17 +- .../trajex/jacobian/forward_kinematics.cpp | 63 ---- .../trajex/jacobian/forward_kinematics.hpp | 18 - src/viam/trajex/jacobian/jacobian.cpp | 164 +++++++-- src/viam/trajex/jacobian/jacobian.hpp | 39 +- src/viam/trajex/jacobian/model.hpp | 47 --- .../trajex/jacobian/private/transforms.hpp | 23 -- src/viam/trajex/jacobian/test/jacobian.cpp | 338 +++++++++++------- src/viam/trajex/service/mlmodel.cpp | 30 +- src/viam/trajex/service/mlmodel.hpp | 18 +- 10 files changed, 392 insertions(+), 365 deletions(-) delete mode 100644 src/viam/trajex/jacobian/forward_kinematics.cpp delete mode 100644 src/viam/trajex/jacobian/forward_kinematics.hpp delete mode 100644 src/viam/trajex/jacobian/model.hpp delete mode 100644 src/viam/trajex/jacobian/private/transforms.hpp diff --git a/CMakeLists.txt b/CMakeLists.txt index 4a038a5..0ee61ac 100644 --- a/CMakeLists.txt +++ b/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,7 +176,6 @@ add_library(viam-trajex-totg) target_sources(viam-trajex-totg PRIVATE src/viam/trajex/trajex.cpp - src/viam/trajex/jacobian/forward_kinematics.cpp src/viam/trajex/jacobian/jacobian.cpp src/viam/trajex/totg/json_serialization.cpp src/viam/trajex/totg/observers.cpp @@ -189,6 +195,7 @@ target_include_directories(viam-trajex-totg target_link_libraries(viam-trajex-totg PUBLIC Eigen3::Eigen + viam-cpp-sdk::viamsdk xtensor xtl PRIVATE diff --git a/src/viam/trajex/jacobian/forward_kinematics.cpp b/src/viam/trajex/jacobian/forward_kinematics.cpp deleted file mode 100644 index dcce99e..0000000 --- a/src/viam/trajex/jacobian/forward_kinematics.cpp +++ /dev/null @@ -1,63 +0,0 @@ -#include - -#include -#include -#include - -#include - -namespace viam::trajex::jacobian { - -namespace detail { - -xt::xarray dh_transform(double d, double theta, double a, double alpha) { - const double ct = std::cos(theta); - const double st = std::sin(theta); - const double ca = std::cos(alpha); - const double sa = std::sin(alpha); - - return xt::xarray{ - { ct, -st * ca, st * sa, a * ct }, - { st, ct * ca, -ct * sa, a * st }, - {0.0, sa, ca, d }, - {0.0, 0.0, 0.0, 1.0 }, - }; -} - -void matmul_4x4(const xt::xarray& A, - const xt::xarray& B, - xt::xarray& out) { - 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); - } - out(i, j) = s; - } - } -} - -} // namespace detail - -void compute_forward_kinematics(const model& m, const xt::xarray& q, data& d) { - if (q.size() != m.joints.size()) { - throw std::invalid_argument( - "q size mismatch: expected " + std::to_string(m.joints.size()) - + ", got " + std::to_string(q.size())); - } - - xt::xarray T = xt::eye(4); - xt::xarray next = xt::zeros({std::size_t{4}, std::size_t{4}}); - for (std::size_t i = 0; i < m.joints.size(); ++i) { - d.joint_transforms[i] = T; - const auto& j = m.joints[i]; - const auto step = detail::dh_transform(j.d, j.theta + q(i), j.a, j.alpha); - detail::matmul_4x4(T, step, next); - T = next; - } - d.end_effector_transform = T; - d.fk_computed = true; -} - -} // namespace viam::trajex::jacobian diff --git a/src/viam/trajex/jacobian/forward_kinematics.hpp b/src/viam/trajex/jacobian/forward_kinematics.hpp deleted file mode 100644 index 185ed5f..0000000 --- a/src/viam/trajex/jacobian/forward_kinematics.hpp +++ /dev/null @@ -1,18 +0,0 @@ -#pragma once - -#if __has_include() -#include -#else -#include -#endif - -#include - -namespace viam::trajex::jacobian { - -// Compute forward kinematics for N-DOF robot. -// q must have model.joints.size() elements. -// Writes joint_transforms and end_effector_transform into data. -void compute_forward_kinematics(const model& m, const xt::xarray& q, data& d); - -} // namespace viam::trajex::jacobian diff --git a/src/viam/trajex/jacobian/jacobian.cpp b/src/viam/trajex/jacobian/jacobian.cpp index 7445a21..50f3bd2 100644 --- a/src/viam/trajex/jacobian/jacobian.cpp +++ b/src/viam/trajex/jacobian/jacobian.cpp @@ -1,50 +1,148 @@ #include -#include -#include -#include +#include +#include +#include +#include + +#include + +#include namespace viam::trajex::jacobian { namespace { -// 3-element cross product written into J(0..2, col). -inline void write_cross_into_J_col( - const std::array& a, - const std::array& b, - xt::xarray& J, - std::size_t col) { - J(0, col) = a[1] * b[2] - a[2] * b[1]; - J(1, col) = a[2] * b[0] - a[0] * b[2]; - J(2, col) = a[0] * b[1] - a[1] * b[0]; +// The SDK's ModelTable carries xyz/rpy/axis as viam::sdk::Vector3 (a thin +// wrapper around std::array, 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 -void compute_jacobian(const model& m, data& d) { - assert(d.fk_computed && "compute_jacobian: FK must be computed first " - "(call compute_forward_kinematics or the 3-argument overload)"); - const auto& T_e = d.end_effector_transform; - const std::array p_e{T_e(0, 3), T_e(1, 3), T_e(2, 3)}; - - for (std::size_t i = 0; i < m.joints.size(); ++i) { - const auto& T = d.joint_transforms[i]; - // Standard DH: joint i rotates about Z of frame i-1. The 3rd column - // of the stored rotation matrix is local Z expressed in the base frame. - const std::array z{T(0, 2), T(1, 2), T(2, 2)}; - const std::array p{T(0, 3), T(1, 3), T(2, 3)}; - const std::array r{p_e[0] - p[0], p_e[1] - p[1], p_e[2] - p[2]}; - - write_cross_into_J_col(z, r, d.J, i); - d.J(3, i) = z[0]; - d.J(4, i) = z[1]; - d.J(5, i) = z[2]; +xt::xarray compute_jacobian(const xt::xarray& model_table, + const xt::xarray& 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> 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 J = xt::zeros({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; } -void compute_jacobian(const model& m, const xt::xarray& q, data& d) { - compute_forward_kinematics(m, q, d); - compute_jacobian(m, d); +Eigen::Matrix4d forward_kinematics(const xt::xarray& model_table, + const xt::xarray& 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 diff --git a/src/viam/trajex/jacobian/jacobian.hpp b/src/viam/trajex/jacobian/jacobian.hpp index ceeef1e..1804255 100644 --- a/src/viam/trajex/jacobian/jacobian.hpp +++ b/src/viam/trajex/jacobian/jacobian.hpp @@ -1,24 +1,41 @@ #pragma once +#include + #if __has_include() #include #else #include #endif -#include -#include - namespace viam::trajex::jacobian { -// Compute full Jacobian (6 x N, where N = model.joints.size()): -// [linear velocity; angular velocity]. -// Requires FK to have been computed first (data.joint_transforms and -// data.end_effector_transform must be valid). Writes result into data.J. -void compute_jacobian(const model& m, data& d); +// 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 compute_jacobian(const xt::xarray& model_table, + const xt::xarray& q); -// Convenience: compute both FK and Jacobian. -// q must have model.joints.size() elements. -void compute_jacobian(const model& m, const xt::xarray& q, data& d); +// 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& model_table, + const xt::xarray& q); } // namespace viam::trajex::jacobian diff --git a/src/viam/trajex/jacobian/model.hpp b/src/viam/trajex/jacobian/model.hpp deleted file mode 100644 index 0b3f8ce..0000000 --- a/src/viam/trajex/jacobian/model.hpp +++ /dev/null @@ -1,47 +0,0 @@ -#pragma once - -#include -#include - -#if __has_include() -#include -#else -#include -#endif - -namespace viam::trajex::jacobian { - -// One row of a standard (distal) Denavit-Hartenberg table, for a single -// revolute joint. Per-joint transform: -// T_{i-1,i}(q) = Rz(theta + q) * Tz(d) * Tx(a) * Rx(alpha) -// theta is the zero-config offset; the runtime joint angle is theta + q. -// Units: metres for d and a, radians for theta and alpha. -struct dh_joint { - std::string name; - double d; // translation along Z_{i-1} - double theta; // zero-config rotation about Z_{i-1} - double a; // translation along X_i - double alpha; // rotation about X_i -}; - -struct model { - std::string name; - std::vector joints; // one per revolute joint; N = joints.size() -}; - -struct data { - // Base-frame transform to frame i-1 for each joint i (each is 4x4). The - // Jacobian reads z_{i-1} and p_{i-1} from these. - std::vector> joint_transforms; - xt::xarray end_effector_transform; // 4x4 - xt::xarray J; // 6 x N - bool fk_computed = false; - - explicit data(const model& m) - : joint_transforms(m.joints.size(), xt::eye(4)) - , end_effector_transform(xt::eye(4)) - , J(xt::zeros({std::size_t{6}, m.joints.size()})) - {} -}; - -} // namespace viam::trajex::jacobian diff --git a/src/viam/trajex/jacobian/private/transforms.hpp b/src/viam/trajex/jacobian/private/transforms.hpp deleted file mode 100644 index 532ab39..0000000 --- a/src/viam/trajex/jacobian/private/transforms.hpp +++ /dev/null @@ -1,23 +0,0 @@ -#pragma once - -#if __has_include() -#include -#else -#include -#endif - -namespace viam::trajex::jacobian::detail { - -// Standard (distal) DH transform: Rz(theta) * Tz(d) * Tx(a) * Rx(alpha). -// Returns a 4x4 xtensor array. -xt::xarray dh_transform(double d, double theta, double a, double alpha); - -// In-place 4x4 matrix multiply: out = A * B. The three arguments must be -// distinct buffers (no aliasing). xtensor lacks a built-in matmul without -// xtensor-blas, and a hand-rolled 4x4 loop is faster than xt::sum-based -// expressions for this fixed shape. -void matmul_4x4(const xt::xarray& A, - const xt::xarray& B, - xt::xarray& out); - -} // namespace viam::trajex::jacobian::detail diff --git a/src/viam/trajex/jacobian/test/jacobian.cpp b/src/viam/trajex/jacobian/test/jacobian.cpp index c313c93..79c2bb9 100644 --- a/src/viam/trajex/jacobian/test/jacobian.cpp +++ b/src/viam/trajex/jacobian/test/jacobian.cpp @@ -1,42 +1,54 @@ -// Jacobian + forward-kinematics tests. -// -// Three kinds of coverage: -// 1. Velocity projection: J * q_dot produces the expected Cartesian -// velocity for hand-chosen 2-link configurations. -// 2. Ground truth: the full 6xN Jacobian matches a hand-computed -// reference for simple 2-link configurations. -// 3. Numerical consistency: the analytical Jacobian matches a -// central-difference numerical Jacobian for 2-link and UR20 configs. - -#include +// Tests for the simplified jacobian module (Option B: plain-return API, +// model table consumed as an xt::xarray tensor in the +// viam::sdk::ModelTable format). + #include -#include #include #include #include +#include +#include + +#include #if __has_include() #include #else #include #endif -#if __has_include() -#include -#else -#include -#endif #include +#include + namespace { -using viam::trajex::jacobian::compute_forward_kinematics; using viam::trajex::jacobian::compute_jacobian; -using viam::trajex::jacobian::data; -using viam::trajex::jacobian::model; +using viam::trajex::jacobian::forward_kinematics; + +// Joint-type encodings for column 9 of the model-table tensor. +// These match viam::sdk::JointType: revolute=0, continuous=1, +// prismatic=2, fixed=3. +constexpr double kRev = 0.0; +constexpr double kCont = 1.0; +constexpr double kPris = 2.0; +constexpr double kFix = 3.0; + +xt::xarray make_table(std::initializer_list> rows) { + const std::size_t n = rows.size(); + xt::xarray t = xt::zeros({n, std::size_t{10}}); + std::size_t i = 0; + for (const auto& r : rows) { + std::size_t j = 0; + for (double v : r) { + t(i, j++) = v; + } + ++i; + } + return t; +} -// Frobenius norm of (A - B). Both must have identical shape. double matrix_diff_norm(const xt::xarray& A, const xt::xarray& B) { double s = 0.0; auto a = A.begin(); @@ -55,7 +67,6 @@ double vec3_diff_norm(const std::array& a, const std::array 6-vector returned as two 3-arrays (linear, angular). struct twist { std::array v; std::array w; @@ -70,118 +81,118 @@ twist J_times_qdot(const xt::xarray& J, const xt::xarray& q_dot) return t; } -// Numerical geometric Jacobian via central differences on FK. -xt::xarray numerical_jacobian(const model& m, const xt::xarray& q, double delta = 1e-7) { - const std::size_t n = m.joints.size(); - xt::xarray J_num = xt::zeros({std::size_t{6}, n}); - data d_plus(m); - data d_minus(m); +// Two 1m planar links rotating about z, ending in a 1m fixed flange. +xt::xarray twolink_table() { + return xt::xarray{ + {0, 0, 0, 0, 0, 0, 0, 0, 1, kRev}, + {1, 0, 0, 0, 0, 0, 0, 0, 1, kRev}, + {1, 0, 0, 0, 0, 0, 0, 0, 0, kFix}, + }; +} + +// 3 revolute joints separated by fixed 1m spacers, ending in a 1m flange. +xt::xarray threelink_with_spacers_table() { + return xt::xarray{ + {0, 0, 0, 0, 0, 0, 0, 0, 1, kRev}, + {1, 0, 0, 0, 0, 0, 0, 0, 0, kFix}, + {0, 0, 0, 0, 0, 0, 0, 0, 1, kRev}, + {1, 0, 0, 0, 0, 0, 0, 0, 0, kFix}, + {0, 0, 0, 0, 0, 0, 0, 0, 1, kRev}, + {1, 0, 0, 0, 0, 0, 0, 0, 0, kFix}, + }; +} + +// 6-revolute spatial chain mimicking a UR-like structure. +xt::xarray sixdof_arm_table() { + return xt::xarray{ + {0, 0, 0.10, 0, 0, 0, 0, 0, 1, kRev}, + {0, 0, 0.15, 0, 0, 0, 0, 1, 0, kRev}, + {0.4, 0, 0, 0, 0, 0, 0, 1, 0, kRev}, + {0.4, 0, 0, 0, 0, 0, 0, 1, 0, kRev}, + {0, 0, 0.10, 0, 0, 0, 1, 0, 0, kRev}, + {0, 0, 0.10, 0, 0, 0, 0, 0, 1, kRev}, + {0, 0, 0.05, 0, 0, 0, 0, 0, 0, kFix}, + }; +} + +// Numerical geometric Jacobian via central differences on forward_kinematics. +xt::xarray numerical_jacobian(const xt::xarray& table, + const xt::xarray& q, + double delta = 1e-7) { + const std::size_t n_actuated = q.size(); + xt::xarray J_num = xt::zeros({std::size_t{6}, n_actuated}); - for (std::size_t i = 0; i < n; ++i) { + for (std::size_t i = 0; i < n_actuated; ++i) { xt::xarray q_plus = q; xt::xarray q_minus = q; q_plus(i) += delta; q_minus(i) -= delta; - compute_forward_kinematics(m, q_plus, d_plus); - compute_forward_kinematics(m, q_minus, d_minus); + const Eigen::Matrix4d Tp = forward_kinematics(table, q_plus); + const Eigen::Matrix4d Tm = forward_kinematics(table, q_minus); - const auto& Tp = d_plus.end_effector_transform; - const auto& Tm = d_minus.end_effector_transform; for (std::size_t r = 0; r < 3; ++r) { J_num(r, i) = (Tp(r, 3) - Tm(r, 3)) / (2.0 * delta); } - // dR = R_plus * R_minus^T, then extract omega from skew-symmetric part. - std::array, 3> dR{}; - for (std::size_t r = 0; r < 3; ++r) { - for (std::size_t c = 0; c < 3; ++c) { - double s = 0.0; - for (std::size_t k = 0; k < 3; ++k) { - s += Tp(r, k) * Tm(c, k); // R_minus^T(k,c) == Tm(c,k) - } - dR[r][c] = s; - } - } - const double scale = 1.0 / (4.0 * delta); - J_num(3, i) = (dR[2][1] - dR[1][2]) * scale; - J_num(4, i) = (dR[0][2] - dR[2][0]) * scale; - J_num(5, i) = (dR[1][0] - dR[0][1]) * scale; + // dR = R_plus * R_minus^T, extract omega from skew-symmetric part. + const Eigen::Matrix3d dR = + Tp.block<3, 3>(0, 0) * Tm.block<3, 3>(0, 0).transpose(); + const double scale = 1.0 / (2.0 * delta); + J_num(3, i) = 0.5 * (dR(2, 1) - dR(1, 2)) * scale; + J_num(4, i) = 0.5 * (dR(0, 2) - dR(2, 0)) * scale; + J_num(5, i) = 0.5 * (dR(1, 0) - dR(0, 1)) * scale; } return J_num; } -model twolink() { - // Two 1m links, both revolute about Z, no offsets. - return model{ - "2link_planar", - { - {"joint1", 0.0, 0.0, 1.0, 0.0}, - {"joint2", 0.0, 0.0, 1.0, 0.0}, - }, - }; -} - -model ur20() { - // DH derived from urdfs/ur20FK.urdf via viam-cpp-sdk's urdf_to_dh_params, - // pruned to end at ft_frame to match the URDF parser's end-effector choice. - return model{ - "UR20", - { - {"shoulder_pan_joint", 0.23630000000000001, -1.2246467991473532e-16, 0.0, -1.570796327}, - {"shoulder_lift_joint", 0.0, -0.0, 0.86199999999999999, 0.0}, - {"elbow_joint", 5.9732854069808984e-18, -1.2188533337963318e-18, 0.72870000000000001, 0.0}, - {"wrist_1_joint", 0.20100000000000004, 1.2188533337963318e-18, -2.4615400662861804e-17, -1.570796327}, - {"wrist_2_joint", 0.1593, -1.224646798896174e-16, 8.0025735243201159e-27, 1.570796327}, - {"wrist_3_joint", 0.15429999999999999, -3.1415926535897931, 3.8756978341028043e-27, 3.1415926535897931}, - }, - }; +void check_matches_numerical(const xt::xarray& table, + const xt::xarray& q, + double tol = 1e-6) { + const auto J = compute_jacobian(table, q); + const auto J_num = numerical_jacobian(table, q); + BOOST_CHECK_SMALL(matrix_diff_norm(J, J_num), tol); } } // namespace +// ============================================================================ +// Velocity tests: J * q_dot produces the expected Cartesian velocity. +// ============================================================================ + BOOST_AUTO_TEST_SUITE(jacobian_velocity_tests) -// q=[0,0], q_dot=[1,0]: base spinning at 1 rad/s, EE is 2m along X. -// EE traces a circle of radius 2 about Z -> v = [0, 2, 0], w = [0, 0, 1]. BOOST_AUTO_TEST_CASE(twolink_base_spin_extended) { - const model m = twolink(); + const auto table = twolink_table(); const xt::xarray q = xt::zeros({std::size_t{2}}); const xt::xarray q_dot = {1.0, 0.0}; - data d(m); - compute_jacobian(m, q, d); - const auto t = J_times_qdot(d.J, q_dot); + const auto J = compute_jacobian(table, q); + const auto t = J_times_qdot(J, q_dot); BOOST_CHECK_SMALL(vec3_diff_norm(t.v, {0.0, 2.0, 0.0}), 1e-10); BOOST_CHECK_SMALL(vec3_diff_norm(t.w, {0.0, 0.0, 1.0}), 1e-10); } -// q=[pi/2, 0], q_dot=[1,0]: base spinning, arm along +Y, EE at (0,2,0). -// EE sweeps in -X -> v = [-2, 0, 0], w = [0, 0, 1]. BOOST_AUTO_TEST_CASE(twolink_base_spin_rotated) { - const model m = twolink(); + const auto table = twolink_table(); const xt::xarray q = {std::numbers::pi / 2.0, 0.0}; const xt::xarray q_dot = {1.0, 0.0}; - data d(m); - compute_jacobian(m, q, d); - const auto t = J_times_qdot(d.J, q_dot); + const auto J = compute_jacobian(table, q); + const auto t = J_times_qdot(J, q_dot); BOOST_CHECK_SMALL(vec3_diff_norm(t.v, {-2.0, 0.0, 0.0}), 1e-10); BOOST_CHECK_SMALL(vec3_diff_norm(t.w, {0.0, 0.0, 1.0}), 1e-10); } -// q=[0, pi/2], q_dot=[1,0]: base spinning, link2 bent 90deg, EE at (1,1,0). -// -> v = [-1, 1, 0], w = [0, 0, 1]. BOOST_AUTO_TEST_CASE(twolink_base_spin_bent) { - const model m = twolink(); + const auto table = twolink_table(); const xt::xarray q = {0.0, std::numbers::pi / 2.0}; const xt::xarray q_dot = {1.0, 0.0}; - data d(m); - compute_jacobian(m, q, d); - const auto t = J_times_qdot(d.J, q_dot); + const auto J = compute_jacobian(table, q); + const auto t = J_times_qdot(J, q_dot); BOOST_CHECK_SMALL(vec3_diff_norm(t.v, {-1.0, 1.0, 0.0}), 1e-10); BOOST_CHECK_SMALL(vec3_diff_norm(t.w, {0.0, 0.0, 1.0}), 1e-10); @@ -190,14 +201,16 @@ BOOST_AUTO_TEST_CASE(twolink_base_spin_bent) { BOOST_AUTO_TEST_SUITE_END() +// ============================================================================ +// Ground-truth tests: hand-computed Jacobian values. +// ============================================================================ + BOOST_AUTO_TEST_SUITE(jacobian_ground_truth_tests) -// q=[0,0]: links along X, EE at (2,0,0). BOOST_AUTO_TEST_CASE(twolink_zero) { - const model m = twolink(); + const auto table = twolink_table(); const xt::xarray q = xt::zeros({std::size_t{2}}); - data d(m); - compute_jacobian(m, q, d); + const auto J = compute_jacobian(table, q); const xt::xarray J_expected = { {0.0, 0.0}, @@ -207,16 +220,13 @@ BOOST_AUTO_TEST_CASE(twolink_zero) { {0.0, 0.0}, {1.0, 1.0}, }; - - BOOST_CHECK_SMALL(matrix_diff_norm(d.J, J_expected), 1e-10); + BOOST_CHECK_SMALL(matrix_diff_norm(J, J_expected), 1e-10); } -// q=[pi/2, 0]: links along Y, EE at (0,2,0). BOOST_AUTO_TEST_CASE(twolink_q1_ninety) { - const model m = twolink(); + const auto table = twolink_table(); const xt::xarray q = {std::numbers::pi / 2.0, 0.0}; - data d(m); - compute_jacobian(m, q, d); + const auto J = compute_jacobian(table, q); const xt::xarray J_expected = { {-2.0, -1.0}, @@ -226,16 +236,13 @@ BOOST_AUTO_TEST_CASE(twolink_q1_ninety) { { 0.0, 0.0}, { 1.0, 1.0}, }; - - BOOST_CHECK_SMALL(matrix_diff_norm(d.J, J_expected), 1e-10); + BOOST_CHECK_SMALL(matrix_diff_norm(J, J_expected), 1e-10); } -// q=[0, pi/2]: link1 along X, link2 along Y, EE at (1,1,0). BOOST_AUTO_TEST_CASE(twolink_q2_ninety) { - const model m = twolink(); + const auto table = twolink_table(); const xt::xarray q = {0.0, std::numbers::pi / 2.0}; - data d(m); - compute_jacobian(m, q, d); + const auto J = compute_jacobian(table, q); const xt::xarray J_expected = { {-1.0, -1.0}, @@ -245,71 +252,126 @@ BOOST_AUTO_TEST_CASE(twolink_q2_ninety) { { 0.0, 0.0}, { 1.0, 1.0}, }; - - BOOST_CHECK_SMALL(matrix_diff_norm(d.J, J_expected), 1e-10); + BOOST_CHECK_SMALL(matrix_diff_norm(J, J_expected), 1e-10); } BOOST_AUTO_TEST_SUITE_END() -BOOST_AUTO_TEST_SUITE(jacobian_numerical_consistency_tests) +// ============================================================================ +// FK basic checks. The numerical-consistency suite depends on FK being +// correct, so verify a few easy cases directly. +// ============================================================================ -namespace { +BOOST_AUTO_TEST_SUITE(fk_tests) -void check_matches_numerical(const model& m, const xt::xarray& q, double tol = 1e-6) { - data d(m); - compute_jacobian(m, q, d); - const xt::xarray J_num = numerical_jacobian(m, q); - BOOST_CHECK_SMALL(matrix_diff_norm(d.J, J_num), tol); +BOOST_AUTO_TEST_CASE(twolink_zero_ee_at_2_0_0) { + const auto table = twolink_table(); + const Eigen::Matrix4d T = forward_kinematics(table, xt::zeros({std::size_t{2}})); + BOOST_CHECK_CLOSE(T(0, 3), 2.0, 1e-9); + BOOST_CHECK_SMALL(std::abs(T(1, 3)), 1e-12); + BOOST_CHECK_SMALL(std::abs(T(2, 3)), 1e-12); } -} // namespace +BOOST_AUTO_TEST_CASE(twolink_q1_pi_over_2_ee_at_0_2_0) { + const auto table = twolink_table(); + const xt::xarray q = {std::numbers::pi / 2.0, 0.0}; + const Eigen::Matrix4d T = forward_kinematics(table, q); + BOOST_CHECK_SMALL(std::abs(T(0, 3)), 1e-9); + BOOST_CHECK_CLOSE(T(1, 3), 2.0, 1e-9); + BOOST_CHECK_SMALL(std::abs(T(2, 3)), 1e-12); +} -BOOST_AUTO_TEST_CASE(twolink_zero) { - check_matches_numerical(twolink(), xt::zeros({std::size_t{2}})); +BOOST_AUTO_TEST_CASE(twolink_q2_pi_over_2_ee_at_1_1_0) { + const auto table = twolink_table(); + const xt::xarray q = {0.0, std::numbers::pi / 2.0}; + const Eigen::Matrix4d T = forward_kinematics(table, q); + BOOST_CHECK_CLOSE(T(0, 3), 1.0, 1e-9); + BOOST_CHECK_CLOSE(T(1, 3), 1.0, 1e-9); + BOOST_CHECK_SMALL(std::abs(T(2, 3)), 1e-12); } -BOOST_AUTO_TEST_CASE(twolink_q1_ninety) { - const xt::xarray q = {std::numbers::pi / 2.0, 0.0}; - check_matches_numerical(twolink(), q); +BOOST_AUTO_TEST_SUITE_END() + + +// ============================================================================ +// Numerical consistency: analytical Jacobian == central-difference Jacobian. +// ============================================================================ + +BOOST_AUTO_TEST_SUITE(jacobian_numerical_consistency_tests) + +BOOST_AUTO_TEST_CASE(twolink_zero) { + check_matches_numerical(twolink_table(), xt::zeros({std::size_t{2}})); } BOOST_AUTO_TEST_CASE(twolink_q1_q2_forty_five) { const xt::xarray q = {std::numbers::pi / 4.0, std::numbers::pi / 4.0}; - check_matches_numerical(twolink(), q); + check_matches_numerical(twolink_table(), q); } BOOST_AUTO_TEST_CASE(twolink_folded_back) { const xt::xarray q = {0.0, std::numbers::pi}; - check_matches_numerical(twolink(), q); + check_matches_numerical(twolink_table(), q); } -BOOST_AUTO_TEST_CASE(ur20_zero) { - check_matches_numerical(ur20(), xt::zeros({std::size_t{6}})); +BOOST_AUTO_TEST_CASE(threelink_with_spacers_zero) { + check_matches_numerical(threelink_with_spacers_table(), + xt::zeros({std::size_t{3}})); } -BOOST_AUTO_TEST_CASE(ur20_small_angles) { - const xt::xarray q = {0.1, 0.2, -0.1, 0.15, -0.05, 0.1}; - check_matches_numerical(ur20(), q); +BOOST_AUTO_TEST_CASE(threelink_with_spacers_typical) { + const xt::xarray q = {0.3, -0.5, 0.8}; + check_matches_numerical(threelink_with_spacers_table(), q); } -BOOST_AUTO_TEST_CASE(ur20_typical) { - const xt::xarray q = { - 0.0, -std::numbers::pi / 2.0, std::numbers::pi / 2.0, - 0.0, std::numbers::pi / 2.0, 0.0}; - check_matches_numerical(ur20(), q); +BOOST_AUTO_TEST_CASE(sixdof_zero) { + check_matches_numerical(sixdof_arm_table(), xt::zeros({std::size_t{6}})); +} + +BOOST_AUTO_TEST_CASE(sixdof_typical) { + const xt::xarray q = {0.1, -0.5, 1.2, -0.3, 0.6, -0.1}; + check_matches_numerical(sixdof_arm_table(), q); } BOOST_AUTO_TEST_SUITE_END() +// ============================================================================ +// Error handling. +// ============================================================================ + BOOST_AUTO_TEST_SUITE(jacobian_error_tests) -BOOST_AUTO_TEST_CASE(fk_rejects_wrong_q_size) { - const model m = twolink(); - data d(m); +BOOST_AUTO_TEST_CASE(rejects_wrong_q_size) { + const auto table = twolink_table(); const xt::xarray q_bad = {0.0, 0.0, 0.0}; - BOOST_CHECK_THROW(compute_forward_kinematics(m, q_bad, d), std::invalid_argument); + BOOST_CHECK_THROW(compute_jacobian(table, q_bad), std::invalid_argument); +} + +BOOST_AUTO_TEST_CASE(rejects_continuous_joint) { + const auto table = make_table({{0, 0, 0, 0, 0, 0, 0, 0, 1, kCont}}); + const xt::xarray q = {0.0}; + BOOST_CHECK_THROW(compute_jacobian(table, q), std::invalid_argument); +} + +BOOST_AUTO_TEST_CASE(rejects_prismatic_joint) { + const auto table = make_table({{0, 0, 0, 0, 0, 0, 0, 0, 1, kPris}}); + const xt::xarray q = {0.0}; + BOOST_CHECK_THROW(compute_jacobian(table, q), std::invalid_argument); +} + +BOOST_AUTO_TEST_CASE(rejects_zero_axis_for_revolute) { + const auto table = make_table({{0, 0, 0, 0, 0, 0, 0, 0, 0, kRev}}); + const xt::xarray q = {0.0}; + BOOST_CHECK_THROW(compute_jacobian(table, q), std::invalid_argument); +} + +// The SDK throws viam::sdk::Exception on malformed tensor input. Verify +// we don't swallow or wrap it. +BOOST_AUTO_TEST_CASE(propagates_sdk_exception_on_bad_shape) { + xt::xarray bad = xt::zeros({std::size_t{1}, std::size_t{9}}); + const xt::xarray q = {0.0}; + BOOST_CHECK_THROW(compute_jacobian(bad, q), viam::sdk::Exception); } BOOST_AUTO_TEST_SUITE_END() diff --git a/src/viam/trajex/service/mlmodel.cpp b/src/viam/trajex/service/mlmodel.cpp index 857deff..e71dfa7 100644 --- a/src/viam/trajex/service/mlmodel.cpp +++ b/src/viam/trajex/service/mlmodel.cpp @@ -118,12 +118,11 @@ std::vector mlmodel::validate(const vsdk::ResourceConfig& cfg) { } // NOLINTNEXTLINE(performance-unnecessary-value-param): Signature fixed by ModelRegistration factory. -mlmodel::mlmodel(vsdk::Dependencies deps, vsdk::ResourceConfig config) : MLModelService(config.name()) { - reconfigure(deps, config); -} +mlmodel::mlmodel(vsdk::Dependencies /*deps*/, vsdk::ResourceConfig config) + : MLModelService(config.name()), config_(parse_config(config)) {} -void mlmodel::reconfigure(const vsdk::Dependencies&, const vsdk::ResourceConfig& cfg) { - config new_config; +mlmodel::config mlmodel::parse_config(const vsdk::ResourceConfig& cfg) { + config out; // Parse generator_sequence auto seq_attr = cfg.attributes().find("generator_sequence"); @@ -132,7 +131,7 @@ void mlmodel::reconfigure(const vsdk::Dependencies&, const vsdk::ResourceConfig& if (!arr || arr->empty()) { throw std::invalid_argument("generator_sequence must be a non-empty array of strings"); } - new_config.generator_sequence.clear(); + out.generator_sequence.clear(); for (const auto& elem : *arr) { const auto* str = elem.get(); if (!str) { @@ -141,7 +140,7 @@ void mlmodel::reconfigure(const vsdk::Dependencies&, const vsdk::ResourceConfig& if (*str != "totg" && *str != "legacy") { throw std::invalid_argument("generator_sequence: unknown algorithm '" + *str + "' (expected 'totg' or 'legacy')"); } - new_config.generator_sequence.push_back(*str); + out.generator_sequence.push_back(*str); } } @@ -152,21 +151,13 @@ void mlmodel::reconfigure(const vsdk::Dependencies&, const vsdk::ResourceConfig& if (!val) { throw std::invalid_argument("segment_for_trajex must be a boolean"); } - new_config.segment_for_trajex = *val; + out.segment_for_trajex = *val; } - const std::unique_lock lock{config_mutex_}; - config_ = std::move(new_config); + return out; } std::shared_ptr mlmodel::infer(const named_tensor_views& inputs, const vsdk::ProtoStruct&) { - // Snapshot config under the read lock, then release - config local_config; - { - const std::shared_lock lock{config_mutex_}; - local_config = config_; - } - // Parse inputs const auto& waypoints_view = get_double_tensor(inputs, "waypoints_rads"); if (waypoints_view.dimension() != 2) { @@ -209,7 +200,7 @@ std::shared_ptr mlmodel::infer(const named_tensor_v .acceleration_limits = std::move(acceleration_limits), .path_blend_tolerance = path_tolerance, .colinearization_ratio = colinearization_ratio, - .segment_totg = local_config.segment_for_trajex, + .segment_totg = config_.segment_for_trajex, }); planner @@ -225,7 +216,7 @@ std::shared_ptr mlmodel::infer(const named_tensor_v .with_segmenter([](auto&, totg::waypoint_accumulator accumulator) { return totg::segment_at_reversals(std::move(accumulator)); }); // Register algorithms based on configured sequence - for (const auto& algo : local_config.generator_sequence) { + for (const auto& algo : config_.generator_sequence) { if (algo == "totg") { planner.with_totg([&, dof](const auto&, service_result& acc, const totg::waypoint_accumulator&, totg::trajectory&& traj, auto) { acc.dof = dof; @@ -329,7 +320,6 @@ std::shared_ptr mlmodel::infer(const named_tensor_v } struct mlmodel::metadata mlmodel::metadata(const vsdk::ProtoStruct&) { - const std::shared_lock lock{config_mutex_}; return { .name = "trajex", .type = "other", diff --git a/src/viam/trajex/service/mlmodel.hpp b/src/viam/trajex/service/mlmodel.hpp index f13fef8..7b2946a 100644 --- a/src/viam/trajex/service/mlmodel.hpp +++ b/src/viam/trajex/service/mlmodel.hpp @@ -2,26 +2,29 @@ #include #include -#include #include #include #include -#include #include namespace viam::trajex::service { -class mlmodel final : public ::viam::sdk::MLModelService, public ::viam::sdk::Reconfigurable { +class mlmodel final : public ::viam::sdk::MLModelService { public: mlmodel(::viam::sdk::Dependencies deps, ::viam::sdk::ResourceConfig config); - void reconfigure(const ::viam::sdk::Dependencies&, const ::viam::sdk::ResourceConfig&) override; - std::shared_ptr infer(const named_tensor_views& inputs, const ::viam::sdk::ProtoStruct& extra) override; struct metadata metadata(const ::viam::sdk::ProtoStruct& extra) override; + // SDK lifecycle hook (added in viam-cpp-sdk after v0.31.0). We don't + // surface any service-level status today; an empty ProtoStruct + // matches the SDK's bundled examples. + ::viam::sdk::ProtoStruct get_status() override { + return {}; + } + static std::vector validate(const ::viam::sdk::ResourceConfig& cfg); private: @@ -30,8 +33,9 @@ class mlmodel final : public ::viam::sdk::MLModelService, public ::viam::sdk::Re bool segment_for_trajex = false; }; - mutable std::shared_mutex config_mutex_; - config config_; + static config parse_config(const ::viam::sdk::ResourceConfig& cfg); + + const config config_; }; } // namespace viam::trajex::service From 25b78ea15108caf3f8ed71ccc59384903c206b63 Mon Sep 17 00:00:00 2001 From: Nick Franczak Date: Mon, 1 Jun 2026 17:50:34 -0400 Subject: [PATCH 04/19] using the class now --- CMakeLists.txt | 14 +++++++------- src/viam/trajex/jacobian/jacobian.cpp | 27 ++++++++++++++------------- src/viam/trajex/jacobian/jacobian.hpp | 4 ++-- 3 files changed, 23 insertions(+), 22 deletions(-) diff --git a/CMakeLists.txt b/CMakeLists.txt index 0ee61ac..fbb08c1 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -104,18 +104,18 @@ set(CMAKE_EXPORT_COMPILE_COMMANDS ON) if(TRAJEX_STANDALONE_BUILD) include(FetchContent) - # TEMPORARY: pinned to commit 35be728 on the SDK branch + # TEMPORARY: pinned to commit 7f45e51 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. + # module consumes sdk::ModelTable, which is 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/nfranczak/viam-cpp-sdk - GIT_TAG 35be728 + GIT_TAG 7f45e51 GIT_SHALLOW FALSE SYSTEM # Delete conflicting protobuf headers diff --git a/src/viam/trajex/jacobian/jacobian.cpp b/src/viam/trajex/jacobian/jacobian.cpp index 50f3bd2..c5c050f 100644 --- a/src/viam/trajex/jacobian/jacobian.cpp +++ b/src/viam/trajex/jacobian/jacobian.cpp @@ -7,7 +7,7 @@ #include -#include +#include namespace viam::trajex::jacobian { @@ -39,10 +39,11 @@ inline Eigen::Matrix4d link_transform(const Eigen::Vector3d& xyz, // 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]; + const auto& rows = table.rows(); + for (std::size_t i = 0; i < rows.size(); ++i) { + const auto& r = rows[i]; switch (r.type) { - case viam::sdk::JointType::revolute: + case viam::sdk::ModelTable::JointType::k_revolute: if (to_eigen(r.axis).squaredNorm() < 1e-24) { throw std::invalid_argument( "viam::trajex::jacobian: row " + std::to_string(i) + @@ -50,10 +51,10 @@ std::size_t validate_and_count_actuated(const viam::sdk::ModelTable& table) { } ++n_actuated; break; - case viam::sdk::JointType::fixed: + case viam::sdk::ModelTable::JointType::k_fixed: break; - case viam::sdk::JointType::continuous: - case viam::sdk::JointType::prismatic: + case viam::sdk::ModelTable::JointType::k_continuous: + case viam::sdk::ModelTable::JointType::k_prismatic: throw std::invalid_argument( "viam::trajex::jacobian: row " + std::to_string(i) + " has unsupported joint type (only revolute and fixed are supported)"); @@ -75,7 +76,7 @@ void check_q_size(std::size_t n_actuated, std::size_t q_size) { xt::xarray compute_jacobian(const xt::xarray& model_table, const xt::xarray& q) { - const auto table = viam::sdk::tensor_to_model_table(model_table); + const auto table = viam::sdk::ModelTable::from(model_table); const std::size_t n_actuated = validate_and_count_actuated(table); check_q_size(n_actuated, q.size()); @@ -88,10 +89,10 @@ xt::xarray compute_jacobian(const xt::xarray& model_table, Eigen::Matrix4d T = Eigen::Matrix4d::Identity(); std::size_t qi = 0; - for (const auto& row : table) { + for (const auto& row : table.rows()) { T = T * link_transform(to_eigen(row.xyz), to_eigen(row.rpy)); - if (row.type == viam::sdk::JointType::revolute) { + if (row.type == viam::sdk::ModelTable::JointType::k_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); @@ -124,16 +125,16 @@ xt::xarray compute_jacobian(const xt::xarray& model_table, Eigen::Matrix4d forward_kinematics(const xt::xarray& model_table, const xt::xarray& q) { - const auto table = viam::sdk::tensor_to_model_table(model_table); + const auto table = viam::sdk::ModelTable::from(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) { + for (const auto& row : table.rows()) { T = T * link_transform(to_eigen(row.xyz), to_eigen(row.rpy)); - if (row.type == viam::sdk::JointType::revolute) { + if (row.type == viam::sdk::ModelTable::JointType::k_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) = diff --git a/src/viam/trajex/jacobian/jacobian.hpp b/src/viam/trajex/jacobian/jacobian.hpp index 1804255..b0a7b15 100644 --- a/src/viam/trajex/jacobian/jacobian.hpp +++ b/src/viam/trajex/jacobian/jacobian.hpp @@ -14,7 +14,7 @@ namespace viam::trajex::jacobian { // positions q. // // model_table: (n, 10) tensor in the viam::sdk::ModelTable format -// (see viam/sdk/referenceframe/urdf_model_table.hpp). +// (see viam/sdk/referenceframe/kinematics_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. // @@ -28,7 +28,7 @@ namespace viam::trajex::jacobian { // - 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). +// encoding (propagated from sdk::ModelTable::from). xt::xarray compute_jacobian(const xt::xarray& model_table, const xt::xarray& q); From b58e73b94a3c3a005c80984b0d6013f918be5654 Mon Sep 17 00:00:00 2001 From: Nick Franczak Date: Wed, 3 Jun 2026 10:11:26 -0400 Subject: [PATCH 05/19] urdf to model table is done, net yet in a release --- CMakeLists.txt | 17 ++++++++--------- 1 file changed, 8 insertions(+), 9 deletions(-) diff --git a/CMakeLists.txt b/CMakeLists.txt index fbb08c1..514a670 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -104,18 +104,17 @@ set(CMAKE_EXPORT_COMPILE_COMMANDS ON) if(TRAJEX_STANDALONE_BUILD) include(FetchContent) - # TEMPORARY: pinned to commit 7f45e51 on the SDK branch - # 20250514-RSDK-13982-construct-model-table because trajex's jacobian - # module consumes sdk::ModelTable, which is 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. + # Pinned to commit 1460e457 on viamrobotics/viam-cpp-sdk main, the tip that + # merged RSDK-13982 (construct model table from urdf, #645). trajex's jacobian + # module consumes sdk::ModelTable, which landed on main just after the v0.37.1 + # release tag but is not yet in any tagged release. GIT_SHALLOW is FALSE so the + # non-tip commit resolves. Move to a release tag once a release containing #645 + # is cut. # See RSDK-13546. FetchContent_Declare( viam-cpp-sdk - GIT_REPOSITORY https://github.com/nfranczak/viam-cpp-sdk - GIT_TAG 7f45e51 + GIT_REPOSITORY https://github.com/viamrobotics/viam-cpp-sdk + GIT_TAG 1460e457 GIT_SHALLOW FALSE SYSTEM # Delete conflicting protobuf headers From 3ee76842daa082990c810088fa94d589e9bad0af Mon Sep 17 00:00:00 2001 From: Nick Franczak Date: Mon, 8 Jun 2026 17:31:02 -0400 Subject: [PATCH 06/19] wip --- src/viam/trajex/service/mlmodel.hpp | 3 --- 1 file changed, 3 deletions(-) diff --git a/src/viam/trajex/service/mlmodel.hpp b/src/viam/trajex/service/mlmodel.hpp index d120a1c..992d561 100644 --- a/src/viam/trajex/service/mlmodel.hpp +++ b/src/viam/trajex/service/mlmodel.hpp @@ -18,9 +18,6 @@ class mlmodel final : public ::viam::sdk::MLModelService { struct metadata metadata(const ::viam::sdk::ProtoStruct& extra) override; - // SDK lifecycle hook (added in viam-cpp-sdk after v0.31.0). We don't - // surface any service-level status today; an empty ProtoStruct - // matches the SDK's bundled examples. ::viam::sdk::ProtoStruct get_status() override { return {}; } From 737f5a8a807988ba2f9a737c4f963aa3957ad73f Mon Sep 17 00:00:00 2001 From: Nick Franczak Date: Tue, 9 Jun 2026 14:28:00 -0400 Subject: [PATCH 07/19] better --- CMakeLists.txt | 5 +- src/viam/trajex/jacobian/jacobian.cpp | 209 +++++++++++++-------- src/viam/trajex/jacobian/jacobian.hpp | 11 +- src/viam/trajex/jacobian/test/jacobian.cpp | 107 +++++++++-- 4 files changed, 228 insertions(+), 104 deletions(-) diff --git a/CMakeLists.txt b/CMakeLists.txt index d02af79..d3bbdcd 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -277,10 +277,10 @@ target_include_directories(viam-trajex-totg target_link_libraries(viam-trajex-totg PUBLIC - Eigen3::Eigen - viam-cpp-sdk::viamsdk xtensor xtl + PRIVATE + viam-cpp-sdk::viamsdk ) apply_wall_werror(viam-trajex-totg) @@ -503,6 +503,7 @@ if (VIAM_TRAJEX_BUILD_TESTS) target_link_libraries(viam-trajex-totg-test PRIVATE viam::trajex::totg::tools + viam-cpp-sdk::viamsdk Boost::boost jsoncpp_lib ) diff --git a/src/viam/trajex/jacobian/jacobian.cpp b/src/viam/trajex/jacobian/jacobian.cpp index c5c050f..2756da7 100644 --- a/src/viam/trajex/jacobian/jacobian.cpp +++ b/src/viam/trajex/jacobian/jacobian.cpp @@ -1,38 +1,103 @@ #include +#include +#include +#include #include #include -#include #include -#include - +#include #include namespace viam::trajex::jacobian { namespace { -// The SDK's ModelTable carries xyz/rpy/axis as viam::sdk::Vector3 (a thin -// wrapper around std::array, 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()); +using vec3 = std::array; + +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 to_vec3(const viam::sdk::Vector3& v) { + return {v.x(), v.y(), v.z()}; +} + +xt::xarray identity4() { + xt::xarray t = xt::zeros({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 matmul4(const xt::xarray& a, const xt::xarray& b) { + xt::xarray c = xt::zeros({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 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 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 link_transform(const viam::sdk::Vector3& xyz, const viam::sdk::Vector3& rpy) { + xt::xarray rx = axis_rotation4({1.0, 0.0, 0.0}, rpy.x()); + xt::xarray ry = axis_rotation4({0.0, 1.0, 0.0}, rpy.y()); + xt::xarray rz = axis_rotation4({0.0, 0.0, 1.0}, rpy.z()); + xt::xarray out = matmul4(matmul4(rz, ry), rx); + out(0, 3) = xyz.x(); + out(1, 3) = xyz.y(); + out(2, 3) = xyz.z(); + return out; } -// 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; +// Rotate a 3-vector by the rotation part of a 4x4 transform. +vec3 rotate(const xt::xarray& 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& transform) { + return {transform(0, 3), transform(1, 3), transform(2, 3)}; } // Validate types and return revolute count. Throws std::invalid_argument @@ -43,14 +108,16 @@ std::size_t validate_and_count_actuated(const viam::sdk::ModelTable& table) { for (std::size_t i = 0; i < rows.size(); ++i) { const auto& r = rows[i]; switch (r.type) { - case viam::sdk::ModelTable::JointType::k_revolute: - if (to_eigen(r.axis).squaredNorm() < 1e-24) { + case viam::sdk::ModelTable::JointType::k_revolute: { + const vec3 axis = to_vec3(r.axis); + if (dot(axis, axis) < 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::ModelTable::JointType::k_fixed: break; case viam::sdk::ModelTable::JointType::k_continuous: @@ -72,78 +139,66 @@ void check_q_size(std::size_t n_actuated, std::size_t q_size) { } } -} // namespace - -xt::xarray compute_jacobian(const xt::xarray& model_table, - const xt::xarray& q) { - const auto table = viam::sdk::ModelTable::from(model_table); +// Accumulated end-effector transform plus, for each revolute joint, its +// world-frame axis and origin captured before its motion is applied. +struct chain_state { + xt::xarray transform; + std::vector axes; + std::vector positions; +}; + +// 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). +chain_state run_chain(const viam::sdk::ModelTable& table, const xt::xarray& q) { 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> per_joint; - per_joint.reserve(n_actuated); + chain_state state{identity4(), {}, {}}; + state.axes.reserve(n_actuated); + state.positions.reserve(n_actuated); - Eigen::Matrix4d T = Eigen::Matrix4d::Identity(); std::size_t qi = 0; for (const auto& row : table.rows()) { - T = T * link_transform(to_eigen(row.xyz), to_eigen(row.rpy)); + state.transform = matmul4(state.transform, link_transform(row.xyz, row.rpy)); if (row.type == viam::sdk::ModelTable::JointType::k_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; + const vec3 a = to_vec3(row.axis); + const double n = norm(a); + const vec3 axis_local{a[0] / n, a[1] / n, a[2] / n}; + state.axes.push_back(rotate(state.transform, axis_local)); + state.positions.push_back(translation(state.transform)); + + state.transform = matmul4(state.transform, axis_rotation4(axis_local, q(qi))); ++qi; } // fixed: no motion to apply. } + return state; +} - const Eigen::Vector3d p_e = T.block<3, 1>(0, 3); +} // namespace + +xt::xarray compute_jacobian(const xt::xarray& model_table, + const xt::xarray& q) { + const auto table = viam::sdk::ModelTable::from(model_table); + const chain_state state = run_chain(table, q); + const std::size_t n_actuated = state.axes.size(); + const vec3 p_e = translation(state.transform); xt::xarray J = xt::zeros({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(); + const vec3& w = state.axes[i]; + const vec3& p = state.positions[i]; + const vec3 jv = cross(w, {p_e[0] - p[0], p_e[1] - p[1], 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; } -Eigen::Matrix4d forward_kinematics(const xt::xarray& model_table, - const xt::xarray& q) { - const auto table = viam::sdk::ModelTable::from(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.rows()) { - T = T * link_transform(to_eigen(row.xyz), to_eigen(row.rpy)); - - if (row.type == viam::sdk::ModelTable::JointType::k_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 diff --git a/src/viam/trajex/jacobian/jacobian.hpp b/src/viam/trajex/jacobian/jacobian.hpp index b0a7b15..1e81751 100644 --- a/src/viam/trajex/jacobian/jacobian.hpp +++ b/src/viam/trajex/jacobian/jacobian.hpp @@ -1,7 +1,5 @@ #pragma once -#include - #if __has_include() #include #else @@ -13,8 +11,7 @@ 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/kinematics_model_table.hpp). +// model_table: (n, 10) tensor in the viam::sdk::ModelTable format. // q: (N_actuated,) vector. One element per revolute row in the // table, in chain order. Fixed rows do not consume a q entry. // @@ -32,10 +29,4 @@ namespace viam::trajex::jacobian { xt::xarray compute_jacobian(const xt::xarray& model_table, const xt::xarray& 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& model_table, - const xt::xarray& q); - } // namespace viam::trajex::jacobian diff --git a/src/viam/trajex/jacobian/test/jacobian.cpp b/src/viam/trajex/jacobian/test/jacobian.cpp index 79c2bb9..16459e1 100644 --- a/src/viam/trajex/jacobian/test/jacobian.cpp +++ b/src/viam/trajex/jacobian/test/jacobian.cpp @@ -10,8 +10,6 @@ #include #include -#include - #if __has_include() #include #else @@ -25,7 +23,6 @@ namespace { using viam::trajex::jacobian::compute_jacobian; -using viam::trajex::jacobian::forward_kinematics; // Joint-type encodings for column 9 of the model-table tensor. // These match viam::sdk::JointType: revolute=0, continuous=1, @@ -115,7 +112,78 @@ xt::xarray sixdof_arm_table() { }; } -// Numerical geometric Jacobian via central differences on forward_kinematics. +// Reference forward kinematics (test oracle). Independent of compute_jacobian: +// builds the end-effector 4x4 transform straight from the (n, 10) model-table +// tensor, used to numerically validate the Jacobian. + +xt::xarray oracle_identity4() { + xt::xarray t = xt::zeros({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 oracle_matmul(const xt::xarray& a, const xt::xarray& b) { + xt::xarray c = xt::zeros({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 angle radians (Rodrigues). +xt::xarray oracle_axis_rotation(double x, double y, double z, double angle) { + const double c = std::cos(angle); + const double s = std::sin(angle); + const double t = 1.0 - c; + xt::xarray r = oracle_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; +} + +// End-effector 4x4 transform read straight off the model-table tensor: columns +// 0..2 xyz, 3..5 rpy (fixed-axis XYZ), 6..8 axis, 9 joint type. +xt::xarray forward_transform(const xt::xarray& table, const xt::xarray& q) { + xt::xarray T = oracle_identity4(); + std::size_t qi = 0; + const std::size_t n = table.shape()[0]; + for (std::size_t r = 0; r < n; ++r) { + xt::xarray link = + oracle_matmul(oracle_matmul(oracle_axis_rotation(0.0, 0.0, 1.0, table(r, 5)), + oracle_axis_rotation(0.0, 1.0, 0.0, table(r, 4))), + oracle_axis_rotation(1.0, 0.0, 0.0, table(r, 3))); + link(0, 3) = table(r, 0); + link(1, 3) = table(r, 1); + link(2, 3) = table(r, 2); + T = oracle_matmul(T, link); + + if (table(r, 9) == kRev) { + const double ax = table(r, 6), ay = table(r, 7), az = table(r, 8); + const double an = std::sqrt(ax * ax + ay * ay + az * az); + T = oracle_matmul(T, oracle_axis_rotation(ax / an, ay / an, az / an, q(qi))); + ++qi; + } + } + return T; +} + +// Numerical geometric Jacobian via central differences on the reference +// forward_transform, which returns a 4x4 homogeneous transform. xt::xarray numerical_jacobian(const xt::xarray& table, const xt::xarray& q, double delta = 1e-7) { @@ -128,20 +196,29 @@ xt::xarray numerical_jacobian(const xt::xarray& table, q_plus(i) += delta; q_minus(i) -= delta; - const Eigen::Matrix4d Tp = forward_kinematics(table, q_plus); - const Eigen::Matrix4d Tm = forward_kinematics(table, q_minus); + const xt::xarray Tp = forward_transform(table, q_plus); + const xt::xarray Tm = forward_transform(table, q_minus); for (std::size_t r = 0; r < 3; ++r) { J_num(r, i) = (Tp(r, 3) - Tm(r, 3)) / (2.0 * delta); } - // dR = R_plus * R_minus^T, extract omega from skew-symmetric part. - const Eigen::Matrix3d dR = - Tp.block<3, 3>(0, 0) * Tm.block<3, 3>(0, 0).transpose(); + // dR = R_plus * R_minus^T (top-left 3x3 blocks); extract omega from the + // skew-symmetric part. + std::array, 3> dR{}; + for (std::size_t a = 0; a < 3; ++a) { + for (std::size_t b = 0; b < 3; ++b) { + double s = 0.0; + for (std::size_t k = 0; k < 3; ++k) { + s += Tp(a, k) * Tm(b, k); + } + dR[a][b] = s; + } + } const double scale = 1.0 / (2.0 * delta); - J_num(3, i) = 0.5 * (dR(2, 1) - dR(1, 2)) * scale; - J_num(4, i) = 0.5 * (dR(0, 2) - dR(2, 0)) * scale; - J_num(5, i) = 0.5 * (dR(1, 0) - dR(0, 1)) * scale; + J_num(3, i) = 0.5 * (dR[2][1] - dR[1][2]) * scale; + J_num(4, i) = 0.5 * (dR[0][2] - dR[2][0]) * scale; + J_num(5, i) = 0.5 * (dR[1][0] - dR[0][1]) * scale; } return J_num; } @@ -267,7 +344,7 @@ BOOST_AUTO_TEST_SUITE(fk_tests) BOOST_AUTO_TEST_CASE(twolink_zero_ee_at_2_0_0) { const auto table = twolink_table(); - const Eigen::Matrix4d T = forward_kinematics(table, xt::zeros({std::size_t{2}})); + const xt::xarray T = forward_transform(table, xt::zeros({std::size_t{2}})); BOOST_CHECK_CLOSE(T(0, 3), 2.0, 1e-9); BOOST_CHECK_SMALL(std::abs(T(1, 3)), 1e-12); BOOST_CHECK_SMALL(std::abs(T(2, 3)), 1e-12); @@ -276,7 +353,7 @@ BOOST_AUTO_TEST_CASE(twolink_zero_ee_at_2_0_0) { BOOST_AUTO_TEST_CASE(twolink_q1_pi_over_2_ee_at_0_2_0) { const auto table = twolink_table(); const xt::xarray q = {std::numbers::pi / 2.0, 0.0}; - const Eigen::Matrix4d T = forward_kinematics(table, q); + const xt::xarray T = forward_transform(table, q); BOOST_CHECK_SMALL(std::abs(T(0, 3)), 1e-9); BOOST_CHECK_CLOSE(T(1, 3), 2.0, 1e-9); BOOST_CHECK_SMALL(std::abs(T(2, 3)), 1e-12); @@ -285,7 +362,7 @@ BOOST_AUTO_TEST_CASE(twolink_q1_pi_over_2_ee_at_0_2_0) { BOOST_AUTO_TEST_CASE(twolink_q2_pi_over_2_ee_at_1_1_0) { const auto table = twolink_table(); const xt::xarray q = {0.0, std::numbers::pi / 2.0}; - const Eigen::Matrix4d T = forward_kinematics(table, q); + const xt::xarray T = forward_transform(table, q); BOOST_CHECK_CLOSE(T(0, 3), 1.0, 1e-9); BOOST_CHECK_CLOSE(T(1, 3), 1.0, 1e-9); BOOST_CHECK_SMALL(std::abs(T(2, 3)), 1e-12); From fe2406f67c7d342129212e4a422c565208275156 Mon Sep 17 00:00:00 2001 From: Nick Franczak Date: Tue, 9 Jun 2026 14:38:43 -0400 Subject: [PATCH 08/19] format --- src/viam/trajex/jacobian/jacobian.cpp | 19 +++---- src/viam/trajex/jacobian/jacobian.hpp | 3 +- src/viam/trajex/jacobian/test/jacobian.cpp | 58 ++++++++++------------ 3 files changed, 33 insertions(+), 47 deletions(-) diff --git a/src/viam/trajex/jacobian/jacobian.cpp b/src/viam/trajex/jacobian/jacobian.cpp index 2756da7..725b48d 100644 --- a/src/viam/trajex/jacobian/jacobian.cpp +++ b/src/viam/trajex/jacobian/jacobian.cpp @@ -111,9 +111,8 @@ std::size_t validate_and_count_actuated(const viam::sdk::ModelTable& table) { case viam::sdk::ModelTable::JointType::k_revolute: { const vec3 axis = to_vec3(r.axis); if (dot(axis, axis) < 1e-24) { - throw std::invalid_argument( - "viam::trajex::jacobian: row " + std::to_string(i) + - " is a revolute joint with zero-magnitude axis"); + throw std::invalid_argument("viam::trajex::jacobian: row " + std::to_string(i) + + " is a revolute joint with zero-magnitude axis"); } ++n_actuated; break; @@ -122,9 +121,8 @@ std::size_t validate_and_count_actuated(const viam::sdk::ModelTable& table) { break; case viam::sdk::ModelTable::JointType::k_continuous: case viam::sdk::ModelTable::JointType::k_prismatic: - throw std::invalid_argument( - "viam::trajex::jacobian: row " + std::to_string(i) + - " has unsupported joint type (only revolute and fixed are supported)"); + 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; @@ -132,10 +130,8 @@ std::size_t validate_and_count_actuated(const viam::sdk::ModelTable& table) { 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)); + throw std::invalid_argument("viam::trajex::jacobian: q size mismatch: expected " + std::to_string(n_actuated) + + " (actuated joints), got " + std::to_string(q_size)); } } @@ -179,8 +175,7 @@ chain_state run_chain(const viam::sdk::ModelTable& table, const xt::xarray compute_jacobian(const xt::xarray& model_table, - const xt::xarray& q) { +xt::xarray compute_jacobian(const xt::xarray& model_table, const xt::xarray& q) { const auto table = viam::sdk::ModelTable::from(model_table); const chain_state state = run_chain(table, q); const std::size_t n_actuated = state.axes.size(); diff --git a/src/viam/trajex/jacobian/jacobian.hpp b/src/viam/trajex/jacobian/jacobian.hpp index 1e81751..cd57dc6 100644 --- a/src/viam/trajex/jacobian/jacobian.hpp +++ b/src/viam/trajex/jacobian/jacobian.hpp @@ -26,7 +26,6 @@ namespace viam::trajex::jacobian { // (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::ModelTable::from). -xt::xarray compute_jacobian(const xt::xarray& model_table, - const xt::xarray& q); +xt::xarray compute_jacobian(const xt::xarray& model_table, const xt::xarray& q); } // namespace viam::trajex::jacobian diff --git a/src/viam/trajex/jacobian/test/jacobian.cpp b/src/viam/trajex/jacobian/test/jacobian.cpp index 16459e1..a553d3a 100644 --- a/src/viam/trajex/jacobian/test/jacobian.cpp +++ b/src/viam/trajex/jacobian/test/jacobian.cpp @@ -72,8 +72,10 @@ twist J_times_qdot(const xt::xarray& J, const xt::xarray& q_dot) twist t{{0, 0, 0}, {0, 0, 0}}; const std::size_t n = q_dot.size(); for (std::size_t j = 0; j < n; ++j) { - for (std::size_t i = 0; i < 3; ++i) t.v[i] += J(i, j) * q_dot(j); - for (std::size_t i = 0; i < 3; ++i) t.w[i] += J(3 + i, j) * q_dot(j); + for (std::size_t i = 0; i < 3; ++i) + t.v[i] += J(i, j) * q_dot(j); + for (std::size_t i = 0; i < 3; ++i) + t.w[i] += J(3 + i, j) * q_dot(j); } return t; } @@ -102,13 +104,13 @@ xt::xarray threelink_with_spacers_table() { // 6-revolute spatial chain mimicking a UR-like structure. xt::xarray sixdof_arm_table() { return xt::xarray{ - {0, 0, 0.10, 0, 0, 0, 0, 0, 1, kRev}, - {0, 0, 0.15, 0, 0, 0, 0, 1, 0, kRev}, - {0.4, 0, 0, 0, 0, 0, 0, 1, 0, kRev}, - {0.4, 0, 0, 0, 0, 0, 0, 1, 0, kRev}, - {0, 0, 0.10, 0, 0, 0, 1, 0, 0, kRev}, - {0, 0, 0.10, 0, 0, 0, 0, 0, 1, kRev}, - {0, 0, 0.05, 0, 0, 0, 0, 0, 0, kFix}, + {0, 0, 0.10, 0, 0, 0, 0, 0, 1, kRev}, + {0, 0, 0.15, 0, 0, 0, 0, 1, 0, kRev}, + {0.4, 0, 0, 0, 0, 0, 0, 1, 0, kRev}, + {0.4, 0, 0, 0, 0, 0, 0, 1, 0, kRev}, + {0, 0, 0.10, 0, 0, 0, 1, 0, 0, kRev}, + {0, 0, 0.10, 0, 0, 0, 0, 0, 1, kRev}, + {0, 0, 0.05, 0, 0, 0, 0, 0, 0, kFix}, }; } @@ -164,8 +166,7 @@ xt::xarray forward_transform(const xt::xarray& table, const xt:: const std::size_t n = table.shape()[0]; for (std::size_t r = 0; r < n; ++r) { xt::xarray link = - oracle_matmul(oracle_matmul(oracle_axis_rotation(0.0, 0.0, 1.0, table(r, 5)), - oracle_axis_rotation(0.0, 1.0, 0.0, table(r, 4))), + oracle_matmul(oracle_matmul(oracle_axis_rotation(0.0, 0.0, 1.0, table(r, 5)), oracle_axis_rotation(0.0, 1.0, 0.0, table(r, 4))), oracle_axis_rotation(1.0, 0.0, 0.0, table(r, 3))); link(0, 3) = table(r, 0); link(1, 3) = table(r, 1); @@ -184,9 +185,7 @@ xt::xarray forward_transform(const xt::xarray& table, const xt:: // Numerical geometric Jacobian via central differences on the reference // forward_transform, which returns a 4x4 homogeneous transform. -xt::xarray numerical_jacobian(const xt::xarray& table, - const xt::xarray& q, - double delta = 1e-7) { +xt::xarray numerical_jacobian(const xt::xarray& table, const xt::xarray& q, double delta = 1e-7) { const std::size_t n_actuated = q.size(); xt::xarray J_num = xt::zeros({std::size_t{6}, n_actuated}); @@ -223,9 +222,7 @@ xt::xarray numerical_jacobian(const xt::xarray& table, return J_num; } -void check_matches_numerical(const xt::xarray& table, - const xt::xarray& q, - double tol = 1e-6) { +void check_matches_numerical(const xt::xarray& table, const xt::xarray& q, double tol = 1e-6) { const auto J = compute_jacobian(table, q); const auto J_num = numerical_jacobian(table, q); BOOST_CHECK_SMALL(matrix_diff_norm(J, J_num), tol); @@ -277,7 +274,6 @@ BOOST_AUTO_TEST_CASE(twolink_base_spin_bent) { BOOST_AUTO_TEST_SUITE_END() - // ============================================================================ // Ground-truth tests: hand-computed Jacobian values. // ============================================================================ @@ -307,11 +303,11 @@ BOOST_AUTO_TEST_CASE(twolink_q1_ninety) { const xt::xarray J_expected = { {-2.0, -1.0}, - { 0.0, 0.0}, - { 0.0, 0.0}, - { 0.0, 0.0}, - { 0.0, 0.0}, - { 1.0, 1.0}, + {0.0, 0.0}, + {0.0, 0.0}, + {0.0, 0.0}, + {0.0, 0.0}, + {1.0, 1.0}, }; BOOST_CHECK_SMALL(matrix_diff_norm(J, J_expected), 1e-10); } @@ -323,18 +319,17 @@ BOOST_AUTO_TEST_CASE(twolink_q2_ninety) { const xt::xarray J_expected = { {-1.0, -1.0}, - { 1.0, 0.0}, - { 0.0, 0.0}, - { 0.0, 0.0}, - { 0.0, 0.0}, - { 1.0, 1.0}, + {1.0, 0.0}, + {0.0, 0.0}, + {0.0, 0.0}, + {0.0, 0.0}, + {1.0, 1.0}, }; BOOST_CHECK_SMALL(matrix_diff_norm(J, J_expected), 1e-10); } BOOST_AUTO_TEST_SUITE_END() - // ============================================================================ // FK basic checks. The numerical-consistency suite depends on FK being // correct, so verify a few easy cases directly. @@ -370,7 +365,6 @@ BOOST_AUTO_TEST_CASE(twolink_q2_pi_over_2_ee_at_1_1_0) { BOOST_AUTO_TEST_SUITE_END() - // ============================================================================ // Numerical consistency: analytical Jacobian == central-difference Jacobian. // ============================================================================ @@ -392,8 +386,7 @@ BOOST_AUTO_TEST_CASE(twolink_folded_back) { } BOOST_AUTO_TEST_CASE(threelink_with_spacers_zero) { - check_matches_numerical(threelink_with_spacers_table(), - xt::zeros({std::size_t{3}})); + check_matches_numerical(threelink_with_spacers_table(), xt::zeros({std::size_t{3}})); } BOOST_AUTO_TEST_CASE(threelink_with_spacers_typical) { @@ -412,7 +405,6 @@ BOOST_AUTO_TEST_CASE(sixdof_typical) { BOOST_AUTO_TEST_SUITE_END() - // ============================================================================ // Error handling. // ============================================================================ From b4ea2ee51c0f5ad939ab00a3efaaca487627c60c Mon Sep 17 00:00:00 2001 From: Nick Franczak Date: Tue, 9 Jun 2026 16:28:19 -0400 Subject: [PATCH 09/19] appeace ci --- src/viam/trajex/jacobian/jacobian.cpp | 34 +++++++++++----------- src/viam/trajex/jacobian/test/jacobian.cpp | 34 ++++++++++++---------- 2 files changed, 36 insertions(+), 32 deletions(-) diff --git a/src/viam/trajex/jacobian/jacobian.cpp b/src/viam/trajex/jacobian/jacobian.cpp index 725b48d..02ef2f9 100644 --- a/src/viam/trajex/jacobian/jacobian.cpp +++ b/src/viam/trajex/jacobian/jacobian.cpp @@ -17,10 +17,10 @@ namespace { using vec3 = std::array; double dot(const vec3& a, const vec3& b) { - return a[0] * b[0] + a[1] * b[1] + a[2] * b[2]; + 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]}; + 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)); @@ -62,24 +62,24 @@ xt::xarray axis_rotation4(const vec3& axis, double theta) { const double z = axis[2]; xt::xarray 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; + 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 link_transform(const viam::sdk::Vector3& xyz, const viam::sdk::Vector3& rpy) { - xt::xarray rx = axis_rotation4({1.0, 0.0, 0.0}, rpy.x()); - xt::xarray ry = axis_rotation4({0.0, 1.0, 0.0}, rpy.y()); - xt::xarray rz = axis_rotation4({0.0, 0.0, 1.0}, rpy.z()); + const xt::xarray rx = axis_rotation4({1.0, 0.0, 0.0}, rpy.x()); + const xt::xarray ry = axis_rotation4({0.0, 1.0, 0.0}, rpy.y()); + const xt::xarray rz = axis_rotation4({0.0, 0.0, 1.0}, rpy.z()); xt::xarray out = matmul4(matmul4(rz, ry), rx); out(0, 3) = xyz.x(); out(1, 3) = xyz.y(); @@ -90,9 +90,9 @@ xt::xarray link_transform(const viam::sdk::Vector3& xyz, const viam::sdk // Rotate a 3-vector by the rotation part of a 4x4 transform. vec3 rotate(const xt::xarray& 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], + (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]), }; } diff --git a/src/viam/trajex/jacobian/test/jacobian.cpp b/src/viam/trajex/jacobian/test/jacobian.cpp index a553d3a..c613e14 100644 --- a/src/viam/trajex/jacobian/test/jacobian.cpp +++ b/src/viam/trajex/jacobian/test/jacobian.cpp @@ -61,7 +61,7 @@ double vec3_diff_norm(const std::array& a, const std::array& J, const xt::xarray& q_dot) twist t{{0, 0, 0}, {0, 0, 0}}; const std::size_t n = q_dot.size(); for (std::size_t j = 0; j < n; ++j) { - for (std::size_t i = 0; i < 3; ++i) + for (std::size_t i = 0; i < 3; ++i) { t.v[i] += J(i, j) * q_dot(j); - for (std::size_t i = 0; i < 3; ++i) + } + for (std::size_t i = 0; i < 3; ++i) { t.w[i] += J(3 + i, j) * q_dot(j); + } } return t; } @@ -146,15 +148,15 @@ xt::xarray oracle_axis_rotation(double x, double y, double z, double ang const double s = std::sin(angle); const double t = 1.0 - c; xt::xarray r = oracle_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; + 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; } @@ -174,8 +176,10 @@ xt::xarray forward_transform(const xt::xarray& table, const xt:: T = oracle_matmul(T, link); if (table(r, 9) == kRev) { - const double ax = table(r, 6), ay = table(r, 7), az = table(r, 8); - const double an = std::sqrt(ax * ax + ay * ay + az * az); + const double ax = table(r, 6); + const double ay = table(r, 7); + const double az = table(r, 8); + const double an = std::sqrt((ax * ax) + (ay * ay) + (az * az)); T = oracle_matmul(T, oracle_axis_rotation(ax / an, ay / an, az / an, q(qi))); ++qi; } @@ -438,7 +442,7 @@ BOOST_AUTO_TEST_CASE(rejects_zero_axis_for_revolute) { // The SDK throws viam::sdk::Exception on malformed tensor input. Verify // we don't swallow or wrap it. BOOST_AUTO_TEST_CASE(propagates_sdk_exception_on_bad_shape) { - xt::xarray bad = xt::zeros({std::size_t{1}, std::size_t{9}}); + const xt::xarray bad = xt::zeros({std::size_t{1}, std::size_t{9}}); const xt::xarray q = {0.0}; BOOST_CHECK_THROW(compute_jacobian(bad, q), viam::sdk::Exception); } From 8046071f8d21202093f16cf3d4231aae5f4e9d77 Mon Sep 17 00:00:00 2001 From: Nick Franczak Date: Tue, 9 Jun 2026 16:54:32 -0400 Subject: [PATCH 10/19] appeace ci2 --- src/viam/trajex/jacobian/test/jacobian.cpp | 6 +++--- 1 file changed, 3 insertions(+), 3 deletions(-) diff --git a/src/viam/trajex/jacobian/test/jacobian.cpp b/src/viam/trajex/jacobian/test/jacobian.cpp index c613e14..cfb7f28 100644 --- a/src/viam/trajex/jacobian/test/jacobian.cpp +++ b/src/viam/trajex/jacobian/test/jacobian.cpp @@ -38,7 +38,7 @@ xt::xarray make_table(std::initializer_list make_table(std::initializer_list& A, const xt::xarray& B) { double s = 0.0; - auto a = A.begin(); - auto b = B.begin(); + const auto* a = A.begin(); + const auto* b = B.begin(); for (; a != A.end(); ++a, ++b) { const double d = *a - *b; s += d * d; From 35a5d73ed2b6fa3d3e24d9f48edd71d6da4dcb52 Mon Sep 17 00:00:00 2001 From: Nick Franczak Date: Wed, 10 Jun 2026 15:43:42 -0400 Subject: [PATCH 11/19] fix nits --- src/viam/trajex/jacobian/jacobian.hpp | 37 +++++----- src/viam/trajex/jacobian/test/jacobian.cpp | 83 +++++++++------------- 2 files changed, 54 insertions(+), 66 deletions(-) diff --git a/src/viam/trajex/jacobian/jacobian.hpp b/src/viam/trajex/jacobian/jacobian.hpp index cd57dc6..3de66fe 100644 --- a/src/viam/trajex/jacobian/jacobian.hpp +++ b/src/viam/trajex/jacobian/jacobian.hpp @@ -8,24 +8,25 @@ 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. -// 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::ModelTable::from). +/// +/// Computes the geometric Jacobian for a URDF-style model table at joint +/// positions q. +/// +/// Supports only revolute and fixed joints. +/// +/// @param model_table (n, 10) tensor in the viam::sdk::ModelTable format +/// @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, unsupported joint type +/// (continuous or prismatic), or revolute row with zero-magnitude axis +/// @throws viam::sdk::Exception on malformed tensor shape or invalid +/// joint-type encoding (propagated from sdk::ModelTable::from) +/// xt::xarray compute_jacobian(const xt::xarray& model_table, const xt::xarray& q); } // namespace viam::trajex::jacobian diff --git a/src/viam/trajex/jacobian/test/jacobian.cpp b/src/viam/trajex/jacobian/test/jacobian.cpp index cfb7f28..892c77e 100644 --- a/src/viam/trajex/jacobian/test/jacobian.cpp +++ b/src/viam/trajex/jacobian/test/jacobian.cpp @@ -1,6 +1,5 @@ -// Tests for the simplified jacobian module (Option B: plain-return API, -// model table consumed as an xt::xarray tensor in the -// viam::sdk::ModelTable format). +// Tests for the jacobian module. compute_jacobian consumes the model table as +// an xt::xarray tensor in the viam::sdk::ModelTable format. #include @@ -27,10 +26,10 @@ using viam::trajex::jacobian::compute_jacobian; // Joint-type encodings for column 9 of the model-table tensor. // These match viam::sdk::JointType: revolute=0, continuous=1, // prismatic=2, fixed=3. -constexpr double kRev = 0.0; -constexpr double kCont = 1.0; -constexpr double kPris = 2.0; -constexpr double kFix = 3.0; +constexpr double k_rev = 0.0; +constexpr double k_cont = 1.0; +constexpr double k_pris = 2.0; +constexpr double k_fix = 3.0; xt::xarray make_table(std::initializer_list> rows) { const std::size_t n = rows.size(); @@ -85,34 +84,34 @@ twist J_times_qdot(const xt::xarray& J, const xt::xarray& q_dot) // Two 1m planar links rotating about z, ending in a 1m fixed flange. xt::xarray twolink_table() { return xt::xarray{ - {0, 0, 0, 0, 0, 0, 0, 0, 1, kRev}, - {1, 0, 0, 0, 0, 0, 0, 0, 1, kRev}, - {1, 0, 0, 0, 0, 0, 0, 0, 0, kFix}, + {0, 0, 0, 0, 0, 0, 0, 0, 1, k_rev}, + {1, 0, 0, 0, 0, 0, 0, 0, 1, k_rev}, + {1, 0, 0, 0, 0, 0, 0, 0, 0, k_fix}, }; } // 3 revolute joints separated by fixed 1m spacers, ending in a 1m flange. xt::xarray threelink_with_spacers_table() { return xt::xarray{ - {0, 0, 0, 0, 0, 0, 0, 0, 1, kRev}, - {1, 0, 0, 0, 0, 0, 0, 0, 0, kFix}, - {0, 0, 0, 0, 0, 0, 0, 0, 1, kRev}, - {1, 0, 0, 0, 0, 0, 0, 0, 0, kFix}, - {0, 0, 0, 0, 0, 0, 0, 0, 1, kRev}, - {1, 0, 0, 0, 0, 0, 0, 0, 0, kFix}, + {0, 0, 0, 0, 0, 0, 0, 0, 1, k_rev}, + {1, 0, 0, 0, 0, 0, 0, 0, 0, k_fix}, + {0, 0, 0, 0, 0, 0, 0, 0, 1, k_rev}, + {1, 0, 0, 0, 0, 0, 0, 0, 0, k_fix}, + {0, 0, 0, 0, 0, 0, 0, 0, 1, k_rev}, + {1, 0, 0, 0, 0, 0, 0, 0, 0, k_fix}, }; } // 6-revolute spatial chain mimicking a UR-like structure. xt::xarray sixdof_arm_table() { return xt::xarray{ - {0, 0, 0.10, 0, 0, 0, 0, 0, 1, kRev}, - {0, 0, 0.15, 0, 0, 0, 0, 1, 0, kRev}, - {0.4, 0, 0, 0, 0, 0, 0, 1, 0, kRev}, - {0.4, 0, 0, 0, 0, 0, 0, 1, 0, kRev}, - {0, 0, 0.10, 0, 0, 0, 1, 0, 0, kRev}, - {0, 0, 0.10, 0, 0, 0, 0, 0, 1, kRev}, - {0, 0, 0.05, 0, 0, 0, 0, 0, 0, kFix}, + {0, 0, 0.10, 0, 0, 0, 0, 0, 1, k_rev}, + {0, 0, 0.15, 0, 0, 0, 0, 1, 0, k_rev}, + {0.4, 0, 0, 0, 0, 0, 0, 1, 0, k_rev}, + {0.4, 0, 0, 0, 0, 0, 0, 1, 0, k_rev}, + {0, 0, 0.10, 0, 0, 0, 1, 0, 0, k_rev}, + {0, 0, 0.10, 0, 0, 0, 0, 0, 1, k_rev}, + {0, 0, 0.05, 0, 0, 0, 0, 0, 0, k_fix}, }; } @@ -175,7 +174,7 @@ xt::xarray forward_transform(const xt::xarray& table, const xt:: link(2, 3) = table(r, 2); T = oracle_matmul(T, link); - if (table(r, 9) == kRev) { + if (table(r, 9) == k_rev) { const double ax = table(r, 6); const double ay = table(r, 7); const double az = table(r, 8); @@ -234,10 +233,8 @@ void check_matches_numerical(const xt::xarray& table, const xt::xarray q = {0.0}; BOOST_CHECK_THROW(compute_jacobian(table, q), std::invalid_argument); } BOOST_AUTO_TEST_CASE(rejects_prismatic_joint) { - const auto table = make_table({{0, 0, 0, 0, 0, 0, 0, 0, 1, kPris}}); + const auto table = make_table({{0, 0, 0, 0, 0, 0, 0, 0, 1, k_pris}}); const xt::xarray q = {0.0}; BOOST_CHECK_THROW(compute_jacobian(table, q), std::invalid_argument); } BOOST_AUTO_TEST_CASE(rejects_zero_axis_for_revolute) { - const auto table = make_table({{0, 0, 0, 0, 0, 0, 0, 0, 0, kRev}}); + const auto table = make_table({{0, 0, 0, 0, 0, 0, 0, 0, 0, k_rev}}); const xt::xarray q = {0.0}; BOOST_CHECK_THROW(compute_jacobian(table, q), std::invalid_argument); } From cee4137a584d9a322d1f993e7698be6d0cc2a3ce Mon Sep 17 00:00:00 2001 From: Nick Franczak Date: Wed, 10 Jun 2026 16:22:51 -0400 Subject: [PATCH 12/19] stop using viam-cpp-sdk --- CMakeLists.txt | 3 - src/viam/trajex/jacobian/jacobian.cpp | 136 +++++++++++++-------- src/viam/trajex/jacobian/jacobian.hpp | 7 +- src/viam/trajex/jacobian/test/jacobian.cpp | 34 ++++-- 4 files changed, 117 insertions(+), 63 deletions(-) diff --git a/CMakeLists.txt b/CMakeLists.txt index d3bbdcd..f77310d 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -279,8 +279,6 @@ target_link_libraries(viam-trajex-totg PUBLIC xtensor xtl - PRIVATE - viam-cpp-sdk::viamsdk ) apply_wall_werror(viam-trajex-totg) @@ -503,7 +501,6 @@ if (VIAM_TRAJEX_BUILD_TESTS) target_link_libraries(viam-trajex-totg-test PRIVATE viam::trajex::totg::tools - viam-cpp-sdk::viamsdk Boost::boost jsoncpp_lib ) diff --git a/src/viam/trajex/jacobian/jacobian.cpp b/src/viam/trajex/jacobian/jacobian.cpp index 02ef2f9..998f127 100644 --- a/src/viam/trajex/jacobian/jacobian.cpp +++ b/src/viam/trajex/jacobian/jacobian.cpp @@ -7,15 +7,26 @@ #include #include -#include -#include - namespace viam::trajex::jacobian { namespace { using vec3 = std::array; +// Joint-type encodings for column 9 of the model-table tensor. These match +// viam::sdk::ModelTable::JointType: revolute=0, continuous=1, prismatic=2, +// fixed=3. +enum class joint_type : int { k_revolute = 0, k_continuous = 1, k_prismatic = 2, k_fixed = 3 }; + +// One unpacked row of the model-table tensor: columns 0..2 xyz, 3..5 rpy +// (fixed-axis XYZ), 6..8 axis, 9 joint type. +struct joint_row { + vec3 xyz; + vec3 rpy; + vec3 axis; + joint_type type; +}; + double dot(const vec3& a, const vec3& b) { return (a[0] * b[0]) + (a[1] * b[1]) + (a[2] * b[2]); } @@ -26,8 +37,59 @@ double norm(const vec3& a) { return std::sqrt(dot(a, a)); } -vec3 to_vec3(const viam::sdk::Vector3& v) { - return {v.x(), v.y(), v.z()}; +// Validate the model-table tensor and unpack it into rows. Throws +// std::invalid_argument on malformed shape, invalid joint-type encoding, +// unsupported joint type (continuous or prismatic), or revolute row with +// zero-magnitude axis. +std::vector parse_model_table(const xt::xarray& 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]) + ")"); + } + if (tensor.shape()[0] == 0) { + throw std::invalid_argument("viam::trajex::jacobian: empty model table"); + } + + std::vector 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(type_value); + if (static_cast(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(type_int); + + 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"); + } + 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 value " + + std::to_string(type_int) + " does not match any joint type"); + } + rows.push_back(row); + } + return rows; } xt::xarray identity4() { @@ -76,14 +138,14 @@ xt::xarray axis_rotation4(const vec3& axis, double theta) { // 4x4 transform for a URDF link: rotation Rz(yaw) * Ry(pitch) * Rx(roll) with // translation xyz. -xt::xarray link_transform(const viam::sdk::Vector3& xyz, const viam::sdk::Vector3& rpy) { - const xt::xarray rx = axis_rotation4({1.0, 0.0, 0.0}, rpy.x()); - const xt::xarray ry = axis_rotation4({0.0, 1.0, 0.0}, rpy.y()); - const xt::xarray rz = axis_rotation4({0.0, 0.0, 1.0}, rpy.z()); +xt::xarray link_transform(const vec3& xyz, const vec3& rpy) { + const xt::xarray rx = axis_rotation4({1.0, 0.0, 0.0}, rpy[0]); + const xt::xarray ry = axis_rotation4({0.0, 1.0, 0.0}, rpy[1]); + const xt::xarray rz = axis_rotation4({0.0, 0.0, 1.0}, rpy[2]); xt::xarray out = matmul4(matmul4(rz, ry), rx); - out(0, 3) = xyz.x(); - out(1, 3) = xyz.y(); - out(2, 3) = xyz.z(); + out(0, 3) = xyz[0]; + out(1, 3) = xyz[1]; + out(2, 3) = xyz[2]; return out; } @@ -100,34 +162,6 @@ vec3 translation(const xt::xarray& transform) { return {transform(0, 3), transform(1, 3), transform(2, 3)}; } -// 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; - const auto& rows = table.rows(); - for (std::size_t i = 0; i < rows.size(); ++i) { - const auto& r = rows[i]; - switch (r.type) { - case viam::sdk::ModelTable::JointType::k_revolute: { - const vec3 axis = to_vec3(r.axis); - if (dot(axis, axis) < 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::ModelTable::JointType::k_fixed: - break; - case viam::sdk::ModelTable::JointType::k_continuous: - case viam::sdk::ModelTable::JointType::k_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) + @@ -146,8 +180,13 @@ struct chain_state { // 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). -chain_state run_chain(const viam::sdk::ModelTable& table, const xt::xarray& q) { - const std::size_t n_actuated = validate_and_count_actuated(table); +chain_state run_chain(const std::vector& rows, const xt::xarray& q) { + std::size_t n_actuated = 0; + for (const auto& row : rows) { + if (row.type == joint_type::k_revolute) { + ++n_actuated; + } + } check_q_size(n_actuated, q.size()); chain_state state{identity4(), {}, {}}; @@ -155,13 +194,12 @@ chain_state run_chain(const viam::sdk::ModelTable& table, const xt::xarray compute_jacobian(const xt::xarray& model_table, const xt::xarray& q) { - const auto table = viam::sdk::ModelTable::from(model_table); - const chain_state state = run_chain(table, q); + const std::vector rows = parse_model_table(model_table); + const chain_state state = run_chain(rows, q); const std::size_t n_actuated = state.axes.size(); const vec3 p_e = translation(state.transform); diff --git a/src/viam/trajex/jacobian/jacobian.hpp b/src/viam/trajex/jacobian/jacobian.hpp index 3de66fe..937a277 100644 --- a/src/viam/trajex/jacobian/jacobian.hpp +++ b/src/viam/trajex/jacobian/jacobian.hpp @@ -22,10 +22,9 @@ namespace viam::trajex::jacobian { /// 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, unsupported joint type -/// (continuous or prismatic), or revolute row with zero-magnitude axis -/// @throws viam::sdk::Exception on malformed tensor shape or invalid -/// joint-type encoding (propagated from sdk::ModelTable::from) +/// @throws std::invalid_argument on malformed tensor shape, invalid joint-type +/// encoding, q-size mismatch, unsupported joint type (continuous or +/// prismatic), or revolute row with zero-magnitude axis /// xt::xarray compute_jacobian(const xt::xarray& model_table, const xt::xarray& q); diff --git a/src/viam/trajex/jacobian/test/jacobian.cpp b/src/viam/trajex/jacobian/test/jacobian.cpp index 892c77e..f4f3cd2 100644 --- a/src/viam/trajex/jacobian/test/jacobian.cpp +++ b/src/viam/trajex/jacobian/test/jacobian.cpp @@ -17,14 +17,12 @@ #include -#include - namespace { using viam::trajex::jacobian::compute_jacobian; // Joint-type encodings for column 9 of the model-table tensor. -// These match viam::sdk::JointType: revolute=0, continuous=1, +// These match viam::sdk::ModelTable::JointType: revolute=0, continuous=1, // prismatic=2, fixed=3. constexpr double k_rev = 0.0; constexpr double k_cont = 1.0; @@ -426,12 +424,34 @@ BOOST_AUTO_TEST_CASE(rejects_zero_axis_for_revolute) { BOOST_CHECK_THROW(compute_jacobian(table, q), std::invalid_argument); } -// The SDK throws viam::sdk::Exception on malformed tensor input. Verify -// we don't swallow or wrap it. -BOOST_AUTO_TEST_CASE(propagates_sdk_exception_on_bad_shape) { +BOOST_AUTO_TEST_CASE(rejects_wrong_column_count) { const xt::xarray bad = xt::zeros({std::size_t{1}, std::size_t{9}}); const xt::xarray q = {0.0}; - BOOST_CHECK_THROW(compute_jacobian(bad, q), viam::sdk::Exception); + BOOST_CHECK_THROW(compute_jacobian(bad, q), std::invalid_argument); +} + +BOOST_AUTO_TEST_CASE(rejects_non_2d_tensor) { + const xt::xarray bad = xt::zeros({std::size_t{10}}); + const xt::xarray q = {0.0}; + BOOST_CHECK_THROW(compute_jacobian(bad, q), std::invalid_argument); +} + +BOOST_AUTO_TEST_CASE(rejects_empty_table) { + const xt::xarray bad = xt::zeros({std::size_t{0}, std::size_t{10}}); + const xt::xarray q = xt::zeros({std::size_t{0}}); + BOOST_CHECK_THROW(compute_jacobian(bad, q), std::invalid_argument); +} + +BOOST_AUTO_TEST_CASE(rejects_non_integer_joint_type) { + const auto table = make_table({{0, 0, 0, 0, 0, 0, 0, 0, 1, 0.5}}); + const xt::xarray q = {0.0}; + BOOST_CHECK_THROW(compute_jacobian(table, q), std::invalid_argument); +} + +BOOST_AUTO_TEST_CASE(rejects_unknown_joint_type) { + const auto table = make_table({{0, 0, 0, 0, 0, 0, 0, 0, 1, 7.0}}); + const xt::xarray q = {0.0}; + BOOST_CHECK_THROW(compute_jacobian(table, q), std::invalid_argument); } BOOST_AUTO_TEST_SUITE_END() From 35ff0948d101e54f5d4fd12fe6cc27fc92f5932b Mon Sep 17 00:00:00 2001 From: Nick Franczak Date: Thu, 11 Jun 2026 15:09:29 -0400 Subject: [PATCH 13/19] add todo --- src/viam/trajex/jacobian/jacobian.cpp | 1 + 1 file changed, 1 insertion(+) diff --git a/src/viam/trajex/jacobian/jacobian.cpp b/src/viam/trajex/jacobian/jacobian.cpp index 998f127..fb411a9 100644 --- a/src/viam/trajex/jacobian/jacobian.cpp +++ b/src/viam/trajex/jacobian/jacobian.cpp @@ -11,6 +11,7 @@ namespace viam::trajex::jacobian { namespace { +// TODO(RSDK-14104): Remove duplicated code that exists in the viam-cpp-sdk using vec3 = std::array; // Joint-type encodings for column 9 of the model-table tensor. These match From 88dbd24df722c159027ae2cd2eef9f3af2f52a03 Mon Sep 17 00:00:00 2001 From: Nick Franczak Date: Thu, 11 Jun 2026 16:44:15 -0400 Subject: [PATCH 14/19] better design --- src/viam/trajex/jacobian/jacobian.cpp | 207 ++++++++++----------- src/viam/trajex/jacobian/jacobian.hpp | 119 ++++++++++-- src/viam/trajex/jacobian/test/jacobian.cpp | 57 +++++- 3 files changed, 258 insertions(+), 125 deletions(-) diff --git a/src/viam/trajex/jacobian/jacobian.cpp b/src/viam/trajex/jacobian/jacobian.cpp index fb411a9..1cde5cd 100644 --- a/src/viam/trajex/jacobian/jacobian.cpp +++ b/src/viam/trajex/jacobian/jacobian.cpp @@ -5,29 +5,15 @@ #include #include #include +#include #include namespace viam::trajex::jacobian { namespace { -// TODO(RSDK-14104): Remove duplicated code that exists in the viam-cpp-sdk using vec3 = std::array; -// Joint-type encodings for column 9 of the model-table tensor. These match -// viam::sdk::ModelTable::JointType: revolute=0, continuous=1, prismatic=2, -// fixed=3. -enum class joint_type : int { k_revolute = 0, k_continuous = 1, k_prismatic = 2, k_fixed = 3 }; - -// One unpacked row of the model-table tensor: columns 0..2 xyz, 3..5 rpy -// (fixed-axis XYZ), 6..8 axis, 9 joint type. -struct joint_row { - vec3 xyz; - vec3 rpy; - vec3 axis; - joint_type type; -}; - double dot(const vec3& a, const vec3& b) { return (a[0] * b[0]) + (a[1] * b[1]) + (a[2] * b[2]); } @@ -37,60 +23,9 @@ vec3 cross(const vec3& a, const vec3& b) { double norm(const vec3& a) { return std::sqrt(dot(a, a)); } - -// Validate the model-table tensor and unpack it into rows. Throws -// std::invalid_argument on malformed shape, invalid joint-type encoding, -// unsupported joint type (continuous or prismatic), or revolute row with -// zero-magnitude axis. -std::vector parse_model_table(const xt::xarray& 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]) + ")"); - } - if (tensor.shape()[0] == 0) { - throw std::invalid_argument("viam::trajex::jacobian: empty model table"); - } - - std::vector 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(type_value); - if (static_cast(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(type_int); - - 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"); - } - 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 value " + - std::to_string(type_int) + " does not match any joint type"); - } - rows.push_back(row); - } - return rows; +vec3 normalized(const vec3& a) { + const double n = norm(a); + return {a[0] / n, a[1] / n, a[2] / n}; } xt::xarray identity4() { @@ -163,68 +98,115 @@ vec3 translation(const xt::xarray& transform) { return {transform(0, 3), transform(1, 3), transform(2, 3)}; } -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 -// Accumulated end-effector transform plus, for each revolute joint, its -// world-frame axis and origin captured before its motion is applied. -struct chain_state { - xt::xarray transform; +// Accumulated chain-walk results: per-revolute-joint world-frame axis and +// origin, plus the end-effector position after the full walk. 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 axes; std::vector positions; + vec3 p_e; }; // 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). -chain_state run_chain(const std::vector& rows, const xt::xarray& q) { - std::size_t n_actuated = 0; - for (const auto& row : rows) { - if (row.type == joint_type::k_revolute) { - ++n_actuated; - } +kinematic_chain::chain_state kinematic_chain::walk_chain(const xt::xarray& 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())); } - check_q_size(n_actuated, q.size()); - chain_state state{identity4(), {}, {}}; - state.axes.reserve(n_actuated); - state.positions.reserve(n_actuated); + chain_state state; + state.axes.reserve(actuated_count_); + state.positions.reserve(actuated_count_); + xt::xarray T = identity4(); std::size_t qi = 0; - for (const auto& row : rows) { - state.transform = matmul4(state.transform, link_transform(row.xyz, row.rpy)); + for (const joint_row& row : rows_) { + T = matmul4(T, link_transform(row.xyz, row.rpy)); if (row.type == joint_type::k_revolute) { - const double n = norm(row.axis); - const vec3 axis_local{row.axis[0] / n, row.axis[1] / n, row.axis[2] / n}; - state.axes.push_back(rotate(state.transform, axis_local)); - state.positions.push_back(translation(state.transform)); + const vec3 axis_local = normalized(row.axis); + state.axes.push_back(rotate(T, axis_local)); + state.positions.push_back(translation(T)); - state.transform = matmul4(state.transform, axis_rotation4(axis_local, q(qi))); + T = matmul4(T, axis_rotation4(axis_local, q(qi))); ++qi; } // fixed: no motion to apply. } + state.p_e = translation(T); return state; } -} // namespace +kinematic_chain::kinematic_chain(std::vector 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"); + } + } +} -xt::xarray compute_jacobian(const xt::xarray& model_table, const xt::xarray& q) { - const std::vector rows = parse_model_table(model_table); - const chain_state state = run_chain(rows, q); - const std::size_t n_actuated = state.axes.size(); - const vec3 p_e = translation(state.transform); +kinematic_chain kinematic_chain::from(const xt::xarray& 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]) + ")"); + } - xt::xarray J = xt::zeros({std::size_t{6}, n_actuated}); - for (std::size_t i = 0; i < n_actuated; ++i) { + std::vector 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(type_value); + if (static_cast(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(type_int); + rows.push_back(row); + } + return kinematic_chain(std::move(rows)); +} + +xt::xarray kinematic_chain::jacobian(const xt::xarray& q) const { + const chain_state state = walk_chain(q); + + xt::xarray J = xt::zeros({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, {p_e[0] - p[0], p_e[1] - p[1], p_e[2] - p[2]}); + 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]; @@ -235,4 +217,19 @@ xt::xarray compute_jacobian(const xt::xarray& model_table, const return J; } +xt::xarray kinematic_chain::linear_jacobian(const xt::xarray& q) const { + const chain_state state = walk_chain(q); + + xt::xarray J = xt::zeros({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 diff --git a/src/viam/trajex/jacobian/jacobian.hpp b/src/viam/trajex/jacobian/jacobian.hpp index 937a277..99994cc 100644 --- a/src/viam/trajex/jacobian/jacobian.hpp +++ b/src/viam/trajex/jacobian/jacobian.hpp @@ -1,5 +1,9 @@ #pragma once +#include +#include +#include + #if __has_include() #include #else @@ -8,24 +12,109 @@ 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. /// -/// Computes the geometric Jacobian for a URDF-style model table at joint -/// positions q. +/// Build one with the `from` factory. Construction validates the chain, so a +/// constructed chain always satisfies the class invariant: every row is +/// revolute or fixed, and every revolute row has a non-zero axis. Parse once +/// and reuse across `jacobian()` calls to avoid re-validating the tensor on +/// every evaluation. /// -/// Supports only revolute and fixed joints. +/// **Thread safety**: All const methods are thread-safe for concurrent access. /// -/// @param model_table (n, 10) tensor in the viam::sdk::ModelTable format -/// @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 malformed tensor shape, invalid joint-type -/// encoding, q-size mismatch, unsupported joint type (continuous or -/// prismatic), or revolute row with zero-magnitude axis +/// Example usage: +/// @code +/// const auto chain = kinematic_chain::from(tensor); // (n, 10) tensor +/// xt::xarray J = chain.jacobian(q); // (6, N_actuated) +/// @endcode /// -xt::xarray compute_jacobian(const xt::xarray& model_table, const xt::xarray& q); +class kinematic_chain { + public: + /// + /// Parses an (n, 10) tensor in the viam::sdk::ModelTable format into a + /// kinematic chain. + /// + /// Columns: 0..2 xyz, 3..5 rpy, 6..8 axis, 9 joint type encoded as + /// revolute=0, continuous=1, prismatic=2, fixed=3 (matching + /// viam::sdk::ModelTable::JointType). + /// + /// @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& 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 jacobian(const xt::xarray& q) const; + + /// + /// Computes the linear-velocity block of the geometric Jacobian at joint + /// positions q. + /// + /// Equivalent to rows 0..2 of `jacobian(q)`: column i is + /// J_v_i = w_i x (p_e - p_i). Use when only the Cartesian linear velocity + /// is needed. + /// + /// @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 linear_jacobian(const xt::xarray& 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 { + 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 xyz{}; + std::array rpy{}; + std::array axis{}; + joint_type type = joint_type::k_fixed; + }; + + // Accumulated chain-walk results shared by both Jacobian assemblies; + // defined in jacobian.cpp. + 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 rows); + + // Walks the chain at joint positions q. Throws std::invalid_argument on + // q-size mismatch. + chain_state walk_chain(const xt::xarray& q) const; + + std::vector rows_; + std::size_t actuated_count_ = 0; +}; } // namespace viam::trajex::jacobian diff --git a/src/viam/trajex/jacobian/test/jacobian.cpp b/src/viam/trajex/jacobian/test/jacobian.cpp index f4f3cd2..80b7d95 100644 --- a/src/viam/trajex/jacobian/test/jacobian.cpp +++ b/src/viam/trajex/jacobian/test/jacobian.cpp @@ -1,8 +1,10 @@ -// Tests for the jacobian module. compute_jacobian consumes the model table as -// an xt::xarray tensor in the viam::sdk::ModelTable format. +// Tests for the jacobian module. kinematic_chain parses an xt::xarray +// tensor in the viam::sdk::ModelTable format and computes the geometric +// Jacobian at given joint positions. #include +#include #include #include #include @@ -19,7 +21,12 @@ namespace { -using viam::trajex::jacobian::compute_jacobian; +using viam::trajex::jacobian::kinematic_chain; + +// One-shot convenience for tests: parse the tensor and evaluate at q. +xt::xarray compute_jacobian(const xt::xarray& table, const xt::xarray& q) { + return kinematic_chain::from(table).jacobian(q); +} // Joint-type encodings for column 9 of the model-table tensor. // These match viam::sdk::ModelTable::JointType: revolute=0, continuous=1, @@ -113,7 +120,7 @@ xt::xarray sixdof_arm_table() { }; } -// Reference forward kinematics (test oracle). Independent of compute_jacobian: +// Reference forward kinematics (test oracle). Independent of kinematic_chain: // builds the end-effector 4x4 transform straight from the (n, 10) model-table // tensor, used to numerically validate the Jacobian. @@ -273,7 +280,7 @@ BOOST_AUTO_TEST_CASE(twolink_base_spin_bent) { BOOST_AUTO_TEST_SUITE_END() -// The tests in this suite check compute_jacobian against hand-computed +// The tests in this suite check kinematic_chain::jacobian against hand-computed // Jacobian values. BOOST_AUTO_TEST_SUITE(jacobian_ground_truth_tests) @@ -397,6 +404,40 @@ BOOST_AUTO_TEST_CASE(sixdof_typical) { BOOST_AUTO_TEST_SUITE_END() +// The tests in this suite cover kinematic_chain itself: reuse of one parsed +// chain across evaluations and agreement between the two Jacobian entry +// points. +BOOST_AUTO_TEST_SUITE(kinematic_chain_tests) + +BOOST_AUTO_TEST_CASE(parsed_chain_is_reusable_across_evaluations) { + const auto chain = kinematic_chain::from(twolink_table()); + const xt::xarray q1 = {0.3, -0.7}; + const xt::xarray q2 = {-1.1, 0.4}; + + BOOST_CHECK_SMALL(matrix_diff_norm(chain.jacobian(q1), compute_jacobian(twolink_table(), q1)), 1e-15); + BOOST_CHECK_SMALL(matrix_diff_norm(chain.jacobian(q2), compute_jacobian(twolink_table(), q2)), 1e-15); +} + +BOOST_AUTO_TEST_CASE(linear_jacobian_matches_jacobian_linear_block) { + const auto chain = kinematic_chain::from(sixdof_arm_table()); + const xt::xarray q = {0.1, -0.5, 1.2, -0.3, 0.6, -0.1}; + + const auto J = chain.jacobian(q); + const auto J_lin = chain.linear_jacobian(q); + BOOST_REQUIRE_EQUAL(J_lin.shape()[0], 3u); + BOOST_REQUIRE_EQUAL(J_lin.shape()[1], 6u); + + double max_abs_diff = 0.0; + for (std::size_t i = 0; i < 3; ++i) { + for (std::size_t j = 0; j < 6; ++j) { + max_abs_diff = std::max(max_abs_diff, std::abs(J_lin(i, j) - J(i, j))); + } + } + BOOST_CHECK_SMALL(max_abs_diff, 1e-15); +} + +BOOST_AUTO_TEST_SUITE_END() + // The tests in this suite verify error handling for invalid inputs. BOOST_AUTO_TEST_SUITE(jacobian_error_tests) @@ -406,6 +447,12 @@ BOOST_AUTO_TEST_CASE(rejects_wrong_q_size) { BOOST_CHECK_THROW(compute_jacobian(table, q_bad), std::invalid_argument); } +BOOST_AUTO_TEST_CASE(linear_jacobian_rejects_wrong_q_size) { + const auto chain = kinematic_chain::from(twolink_table()); + const xt::xarray q_bad = {0.0, 0.0, 0.0}; + BOOST_CHECK_THROW(chain.linear_jacobian(q_bad), std::invalid_argument); +} + BOOST_AUTO_TEST_CASE(rejects_continuous_joint) { const auto table = make_table({{0, 0, 0, 0, 0, 0, 0, 0, 1, k_cont}}); const xt::xarray q = {0.0}; From 9ea53bb04a414e20ed11364bad85d15288988227 Mon Sep 17 00:00:00 2001 From: Nick Franczak Date: Thu, 11 Jun 2026 16:45:37 -0400 Subject: [PATCH 15/19] no need for that comment --- src/viam/trajex/jacobian/test/jacobian.cpp | 4 ---- 1 file changed, 4 deletions(-) diff --git a/src/viam/trajex/jacobian/test/jacobian.cpp b/src/viam/trajex/jacobian/test/jacobian.cpp index 80b7d95..193315b 100644 --- a/src/viam/trajex/jacobian/test/jacobian.cpp +++ b/src/viam/trajex/jacobian/test/jacobian.cpp @@ -1,7 +1,3 @@ -// Tests for the jacobian module. kinematic_chain parses an xt::xarray -// tensor in the viam::sdk::ModelTable format and computes the geometric -// Jacobian at given joint positions. - #include #include From 18c3249d14e0bd1e32d9be12d1a624969013db4a Mon Sep 17 00:00:00 2001 From: Nick Franczak Date: Thu, 11 Jun 2026 16:55:35 -0400 Subject: [PATCH 16/19] ready once again --- src/viam/trajex/jacobian/jacobian.cpp | 16 +++++------ src/viam/trajex/jacobian/jacobian.hpp | 33 ++++------------------ src/viam/trajex/jacobian/test/jacobian.cpp | 2 +- 3 files changed, 15 insertions(+), 36 deletions(-) diff --git a/src/viam/trajex/jacobian/jacobian.cpp b/src/viam/trajex/jacobian/jacobian.cpp index 1cde5cd..c7ac226 100644 --- a/src/viam/trajex/jacobian/jacobian.cpp +++ b/src/viam/trajex/jacobian/jacobian.cpp @@ -100,8 +100,8 @@ vec3 translation(const xt::xarray& transform) { } // namespace -// Accumulated chain-walk results: per-revolute-joint world-frame axis and -// origin, plus the end-effector position after the full walk. With these in +// 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 { @@ -110,10 +110,10 @@ struct kinematic_chain::chain_state { vec3 p_e; }; -// 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). -kinematic_chain::chain_state kinematic_chain::walk_chain(const xt::xarray& q) const { +// 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& 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())); @@ -200,7 +200,7 @@ kinematic_chain kinematic_chain::from(const xt::xarray& tensor) { } xt::xarray kinematic_chain::jacobian(const xt::xarray& q) const { - const chain_state state = walk_chain(q); + const chain_state state = compute_chain_state(q); xt::xarray J = xt::zeros({std::size_t{6}, actuated_count_}); for (std::size_t i = 0; i < actuated_count_; ++i) { @@ -218,7 +218,7 @@ xt::xarray kinematic_chain::jacobian(const xt::xarray& q) const } xt::xarray kinematic_chain::linear_jacobian(const xt::xarray& q) const { - const chain_state state = walk_chain(q); + const chain_state state = compute_chain_state(q); xt::xarray J = xt::zeros({std::size_t{3}, actuated_count_}); for (std::size_t i = 0; i < actuated_count_; ++i) { diff --git a/src/viam/trajex/jacobian/jacobian.hpp b/src/viam/trajex/jacobian/jacobian.hpp index 99994cc..001e2c9 100644 --- a/src/viam/trajex/jacobian/jacobian.hpp +++ b/src/viam/trajex/jacobian/jacobian.hpp @@ -18,30 +18,12 @@ namespace viam::trajex::jacobian { /// A validated URDF-style serial kinematic chain, parsed from an (n, 10) /// model-table tensor and held in chain order. /// -/// Build one with the `from` factory. Construction validates the chain, so a -/// constructed chain always satisfies the class invariant: every row is -/// revolute or fixed, and every revolute row has a non-zero axis. Parse once -/// and reuse across `jacobian()` calls to avoid re-validating the tensor on -/// every evaluation. -/// -/// **Thread safety**: All const methods are thread-safe for concurrent access. -/// -/// Example usage: -/// @code -/// const auto chain = kinematic_chain::from(tensor); // (n, 10) tensor -/// xt::xarray J = chain.jacobian(q); // (6, N_actuated) -/// @endcode -/// class kinematic_chain { public: /// /// Parses an (n, 10) tensor in the viam::sdk::ModelTable format into a /// kinematic chain. /// - /// Columns: 0..2 xyz, 3..5 rpy, 6..8 axis, 9 joint type encoded as - /// revolute=0, continuous=1, prismatic=2, fixed=3 (matching - /// viam::sdk::ModelTable::JointType). - /// /// @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 @@ -69,10 +51,6 @@ class kinematic_chain { /// Computes the linear-velocity block of the geometric Jacobian at joint /// positions q. /// - /// Equivalent to rows 0..2 of `jacobian(q)`: column i is - /// J_v_i = w_i x (p_e - p_i). Use when only the Cartesian linear velocity - /// is needed. - /// /// @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. @@ -101,17 +79,18 @@ class kinematic_chain { joint_type type = joint_type::k_fixed; }; - // Accumulated chain-walk results shared by both Jacobian assemblies; - // defined in jacobian.cpp. + // 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 rows); - // Walks the chain at joint positions q. Throws std::invalid_argument on - // q-size mismatch. - chain_state walk_chain(const xt::xarray& q) const; + // 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& q) const; std::vector rows_; std::size_t actuated_count_ = 0; diff --git a/src/viam/trajex/jacobian/test/jacobian.cpp b/src/viam/trajex/jacobian/test/jacobian.cpp index 193315b..e7e9465 100644 --- a/src/viam/trajex/jacobian/test/jacobian.cpp +++ b/src/viam/trajex/jacobian/test/jacobian.cpp @@ -446,7 +446,7 @@ BOOST_AUTO_TEST_CASE(rejects_wrong_q_size) { BOOST_AUTO_TEST_CASE(linear_jacobian_rejects_wrong_q_size) { const auto chain = kinematic_chain::from(twolink_table()); const xt::xarray q_bad = {0.0, 0.0, 0.0}; - BOOST_CHECK_THROW(chain.linear_jacobian(q_bad), std::invalid_argument); + BOOST_CHECK_THROW(static_cast(chain.linear_jacobian(q_bad)), std::invalid_argument); } BOOST_AUTO_TEST_CASE(rejects_continuous_joint) { From 26a19927d44ac69e97bd4ea30a764e6439011f85 Mon Sep 17 00:00:00 2001 From: Nick Franczak Date: Thu, 11 Jun 2026 17:00:54 -0400 Subject: [PATCH 17/19] better comments --- src/viam/trajex/jacobian/test/jacobian.cpp | 20 ++++++++++---------- 1 file changed, 10 insertions(+), 10 deletions(-) diff --git a/src/viam/trajex/jacobian/test/jacobian.cpp b/src/viam/trajex/jacobian/test/jacobian.cpp index e7e9465..2aa5fa6 100644 --- a/src/viam/trajex/jacobian/test/jacobian.cpp +++ b/src/viam/trajex/jacobian/test/jacobian.cpp @@ -24,9 +24,8 @@ xt::xarray compute_jacobian(const xt::xarray& table, const xt::x return kinematic_chain::from(table).jacobian(q); } -// Joint-type encodings for column 9 of the model-table tensor. -// These match viam::sdk::ModelTable::JointType: revolute=0, continuous=1, -// prismatic=2, fixed=3. +// Joint-type encodings for column 9 of the model-table tensor; values match +// viam::sdk::ModelTable::JointType. constexpr double k_rev = 0.0; constexpr double k_cont = 1.0; constexpr double k_pris = 2.0; @@ -116,9 +115,10 @@ xt::xarray sixdof_arm_table() { }; } -// Reference forward kinematics (test oracle). Independent of kinematic_chain: -// builds the end-effector 4x4 transform straight from the (n, 10) model-table -// tensor, used to numerically validate the Jacobian. +// Reference forward kinematics used as the test oracle. It rebuilds the +// end-effector transform straight from the tensor, deliberately independent +// of kinematic_chain, so checking the analytical Jacobian against numerical +// derivatives of this oracle is not circular. xt::xarray oracle_identity4() { xt::xarray t = xt::zeros({std::size_t{4}, std::size_t{4}}); @@ -160,8 +160,8 @@ xt::xarray oracle_axis_rotation(double x, double y, double z, double ang return r; } -// End-effector 4x4 transform read straight off the model-table tensor: columns -// 0..2 xyz, 3..5 rpy (fixed-axis XYZ), 6..8 axis, 9 joint type. +// Tensor columns: 0..2 xyz, 3..5 rpy (fixed-axis XYZ), 6..8 axis, 9 joint +// type. xt::xarray forward_transform(const xt::xarray& table, const xt::xarray& q) { xt::xarray T = oracle_identity4(); std::size_t qi = 0; @@ -187,8 +187,8 @@ xt::xarray forward_transform(const xt::xarray& table, const xt:: return T; } -// Numerical geometric Jacobian via central differences on the reference -// forward_transform, which returns a 4x4 homogeneous transform. +// Numerical geometric Jacobian via central differences on the oracle's +// forward_transform. xt::xarray numerical_jacobian(const xt::xarray& table, const xt::xarray& q, double delta = 1e-7) { const std::size_t n_actuated = q.size(); xt::xarray J_num = xt::zeros({std::size_t{6}, n_actuated}); From b86f9f5ac11a84cdf778b5ccdec97f5aa085c4d8 Mon Sep 17 00:00:00 2001 From: Nick Franczak Date: Thu, 11 Jun 2026 17:04:39 -0400 Subject: [PATCH 18/19] appease ci --- src/viam/trajex/jacobian/jacobian.hpp | 3 ++- src/viam/trajex/jacobian/test/jacobian.cpp | 4 ++-- 2 files changed, 4 insertions(+), 3 deletions(-) diff --git a/src/viam/trajex/jacobian/jacobian.hpp b/src/viam/trajex/jacobian/jacobian.hpp index 001e2c9..2ed52c3 100644 --- a/src/viam/trajex/jacobian/jacobian.hpp +++ b/src/viam/trajex/jacobian/jacobian.hpp @@ -2,6 +2,7 @@ #include #include +#include #include #if __has_include() @@ -62,7 +63,7 @@ class kinematic_chain { // 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 { + enum class joint_type : std::uint8_t { k_revolute = 0, k_continuous = 1, k_prismatic = 2, diff --git a/src/viam/trajex/jacobian/test/jacobian.cpp b/src/viam/trajex/jacobian/test/jacobian.cpp index 2aa5fa6..7cf1ee3 100644 --- a/src/viam/trajex/jacobian/test/jacobian.cpp +++ b/src/viam/trajex/jacobian/test/jacobian.cpp @@ -420,8 +420,8 @@ BOOST_AUTO_TEST_CASE(linear_jacobian_matches_jacobian_linear_block) { const auto J = chain.jacobian(q); const auto J_lin = chain.linear_jacobian(q); - BOOST_REQUIRE_EQUAL(J_lin.shape()[0], 3u); - BOOST_REQUIRE_EQUAL(J_lin.shape()[1], 6u); + BOOST_REQUIRE_EQUAL(J_lin.shape()[0], 3U); + BOOST_REQUIRE_EQUAL(J_lin.shape()[1], 6U); double max_abs_diff = 0.0; for (std::size_t i = 0; i < 3; ++i) { From 63951ab0ec351bee919b1d5e60974e84fddedc94 Mon Sep 17 00:00:00 2001 From: Nick Franczak Date: Thu, 11 Jun 2026 17:11:34 -0400 Subject: [PATCH 19/19] better comments --- src/viam/trajex/jacobian/test/jacobian.cpp | 33 +++++++++++----------- 1 file changed, 16 insertions(+), 17 deletions(-) diff --git a/src/viam/trajex/jacobian/test/jacobian.cpp b/src/viam/trajex/jacobian/test/jacobian.cpp index 7cf1ee3..af4411f 100644 --- a/src/viam/trajex/jacobian/test/jacobian.cpp +++ b/src/viam/trajex/jacobian/test/jacobian.cpp @@ -115,12 +115,13 @@ xt::xarray sixdof_arm_table() { }; } -// Reference forward kinematics used as the test oracle. It rebuilds the -// end-effector transform straight from the tensor, deliberately independent -// of kinematic_chain, so checking the analytical Jacobian against numerical -// derivatives of this oracle is not circular. +// The analytic Jacobian in kinematic_chain is the being tested, so it +// cannot be its own evidence. The Jacobian's defining property is that it is +// the derivative of the forward kinematics; the numerical-consistency suite +// verifies exactly that property by finite-differencing the reference forward +// kinematics below, which is written independently of kinematic_chain. -xt::xarray oracle_identity4() { +xt::xarray reference_identity4() { xt::xarray t = xt::zeros({std::size_t{4}, std::size_t{4}}); for (std::size_t i = 0; i < 4; ++i) { t(i, i) = 1.0; @@ -128,7 +129,7 @@ xt::xarray oracle_identity4() { return t; } -xt::xarray oracle_matmul(const xt::xarray& a, const xt::xarray& b) { +xt::xarray reference_matmul(const xt::xarray& a, const xt::xarray& b) { xt::xarray c = xt::zeros({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) { @@ -143,11 +144,11 @@ xt::xarray oracle_matmul(const xt::xarray& a, const xt::xarray oracle_axis_rotation(double x, double y, double z, double angle) { +xt::xarray reference_axis_rotation(double x, double y, double z, double angle) { const double c = std::cos(angle); const double s = std::sin(angle); const double t = 1.0 - c; - xt::xarray r = oracle_identity4(); + xt::xarray r = reference_identity4(); r(0, 0) = (t * x * x) + c; r(0, 1) = (t * x * y) - (s * z); r(0, 2) = (t * x * z) + (s * y); @@ -163,31 +164,31 @@ xt::xarray oracle_axis_rotation(double x, double y, double z, double ang // Tensor columns: 0..2 xyz, 3..5 rpy (fixed-axis XYZ), 6..8 axis, 9 joint // type. xt::xarray forward_transform(const xt::xarray& table, const xt::xarray& q) { - xt::xarray T = oracle_identity4(); + xt::xarray T = reference_identity4(); std::size_t qi = 0; const std::size_t n = table.shape()[0]; for (std::size_t r = 0; r < n; ++r) { - xt::xarray link = - oracle_matmul(oracle_matmul(oracle_axis_rotation(0.0, 0.0, 1.0, table(r, 5)), oracle_axis_rotation(0.0, 1.0, 0.0, table(r, 4))), - oracle_axis_rotation(1.0, 0.0, 0.0, table(r, 3))); + xt::xarray link = reference_matmul( + reference_matmul(reference_axis_rotation(0.0, 0.0, 1.0, table(r, 5)), reference_axis_rotation(0.0, 1.0, 0.0, table(r, 4))), + reference_axis_rotation(1.0, 0.0, 0.0, table(r, 3))); link(0, 3) = table(r, 0); link(1, 3) = table(r, 1); link(2, 3) = table(r, 2); - T = oracle_matmul(T, link); + T = reference_matmul(T, link); if (table(r, 9) == k_rev) { const double ax = table(r, 6); const double ay = table(r, 7); const double az = table(r, 8); const double an = std::sqrt((ax * ax) + (ay * ay) + (az * az)); - T = oracle_matmul(T, oracle_axis_rotation(ax / an, ay / an, az / an, q(qi))); + T = reference_matmul(T, reference_axis_rotation(ax / an, ay / an, az / an, q(qi))); ++qi; } } return T; } -// Numerical geometric Jacobian via central differences on the oracle's +// Numerical geometric Jacobian via central differences on the reference // forward_transform. xt::xarray numerical_jacobian(const xt::xarray& table, const xt::xarray& q, double delta = 1e-7) { const std::size_t n_actuated = q.size(); @@ -206,8 +207,6 @@ xt::xarray numerical_jacobian(const xt::xarray& table, const xt: J_num(r, i) = (Tp(r, 3) - Tm(r, 3)) / (2.0 * delta); } - // dR = R_plus * R_minus^T (top-left 3x3 blocks); extract omega from the - // skew-symmetric part. std::array, 3> dR{}; for (std::size_t a = 0; a < 3; ++a) { for (std::size_t b = 0; b < 3; ++b) {