diff --git a/CMakeLists.txt b/CMakeLists.txt index 1cf77ff..f77310d 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -260,6 +260,7 @@ add_library(viam-trajex-totg) target_sources(viam-trajex-totg PRIVATE src/viam/trajex/trajex.cpp + src/viam/trajex/jacobian/jacobian.cpp src/viam/trajex/totg/observers.cpp src/viam/trajex/totg/path.cpp src/viam/trajex/totg/trajectory.cpp @@ -494,6 +495,7 @@ if (VIAM_TRAJEX_BUILD_TESTS) src/viam/trajex/totg/test/uniform_sampler.cpp src/viam/trajex/totg/test/integration.cpp src/viam/trajex/totg/test/observers.cpp + src/viam/trajex/jacobian/test/jacobian.cpp ) target_link_libraries(viam-trajex-totg-test diff --git a/src/viam/trajex/jacobian/jacobian.cpp b/src/viam/trajex/jacobian/jacobian.cpp new file mode 100644 index 0000000..c7ac226 --- /dev/null +++ b/src/viam/trajex/jacobian/jacobian.cpp @@ -0,0 +1,235 @@ +#include + +#include +#include +#include +#include +#include +#include +#include + +namespace viam::trajex::jacobian { + +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]); +} +vec3 cross(const vec3& a, const vec3& b) { + return {(a[1] * b[2]) - (a[2] * b[1]), (a[2] * b[0]) - (a[0] * b[2]), (a[0] * b[1]) - (a[1] * b[0])}; +} +double norm(const vec3& a) { + return std::sqrt(dot(a, a)); +} +vec3 normalized(const vec3& a) { + const double n = norm(a); + return {a[0] / n, a[1] / n, a[2] / n}; +} + +xt::xarray 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 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[0]; + out(1, 3) = xyz[1]; + out(2, 3) = xyz[2]; + return out; +} + +// Rotate a 3-vector by the rotation part of a 4x4 transform. +vec3 rotate(const xt::xarray& 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)}; +} + +} // namespace + +// Per-revolute-joint world-frame axis and origin, plus the end-effector +// position, captured while evaluating the forward kinematics. With these in +// hand, Jacobian column i is J_v_i = axes[i] x (p_e - positions[i]) (linear) +// stacked on J_w_i = axes[i] (angular). +struct kinematic_chain::chain_state { + std::vector axes; + std::vector positions; + vec3 p_e; +}; + +// Evaluate the forward kinematics row by row (base to end-effector), +// accumulating the running link transform. For each revolute joint, capture +// its world-frame axis and origin before applying joint motion. +kinematic_chain::chain_state kinematic_chain::compute_chain_state(const xt::xarray& q) const { + if (q.size() != actuated_count_) { + throw std::invalid_argument("viam::trajex::jacobian: q size mismatch: expected " + std::to_string(actuated_count_) + + " (actuated joints), got " + std::to_string(q.size())); + } + + chain_state state; + state.axes.reserve(actuated_count_); + state.positions.reserve(actuated_count_); + + xt::xarray T = identity4(); + std::size_t qi = 0; + for (const joint_row& row : rows_) { + T = matmul4(T, link_transform(row.xyz, row.rpy)); + + if (row.type == joint_type::k_revolute) { + const vec3 axis_local = normalized(row.axis); + state.axes.push_back(rotate(T, axis_local)); + state.positions.push_back(translation(T)); + + T = matmul4(T, axis_rotation4(axis_local, q(qi))); + ++qi; + } + // fixed: no motion to apply. + } + state.p_e = translation(T); + return state; +} + +kinematic_chain::kinematic_chain(std::vector rows) : rows_(std::move(rows)) { + if (rows_.empty()) { + throw std::invalid_argument("viam::trajex::jacobian: empty model table"); + } + for (std::size_t i = 0; i < rows_.size(); ++i) { + const joint_row& row = rows_[i]; + switch (row.type) { + case joint_type::k_revolute: + if (dot(row.axis, row.axis) < 1e-24) { + throw std::invalid_argument("viam::trajex::jacobian: row " + std::to_string(i) + + " is a revolute joint with zero-magnitude axis"); + } + ++actuated_count_; + break; + case joint_type::k_fixed: + break; + case joint_type::k_continuous: + case joint_type::k_prismatic: + throw std::invalid_argument("viam::trajex::jacobian: row " + std::to_string(i) + + " has unsupported joint type (only revolute and fixed are supported)"); + default: + throw std::invalid_argument("viam::trajex::jacobian: row " + std::to_string(i) + + " joint type does not match any joint_type value"); + } + } +} + +kinematic_chain kinematic_chain::from(const xt::xarray& tensor) { + if (tensor.dimension() != 2) { + throw std::invalid_argument("viam::trajex::jacobian: expected 2D model-table tensor, got " + std::to_string(tensor.dimension()) + + "D"); + } + if (tensor.shape()[1] != 10) { + throw std::invalid_argument("viam::trajex::jacobian: expected model-table shape (n, 10), got (n, " + + std::to_string(tensor.shape()[1]) + ")"); + } + + std::vector 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 = 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) { + const vec3& w = state.axes[i]; + const vec3& p = state.positions[i]; + const vec3 jv = cross(w, {state.p_e[0] - p[0], state.p_e[1] - p[1], state.p_e[2] - p[2]}); + J(0, i) = jv[0]; + J(1, i) = jv[1]; + J(2, i) = jv[2]; + J(3, i) = w[0]; + J(4, i) = w[1]; + J(5, i) = w[2]; + } + return J; +} + +xt::xarray kinematic_chain::linear_jacobian(const xt::xarray& q) const { + 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) { + 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 new file mode 100644 index 0000000..2ed52c3 --- /dev/null +++ b/src/viam/trajex/jacobian/jacobian.hpp @@ -0,0 +1,100 @@ +#pragma once + +#include +#include +#include +#include + +#if __has_include() +#include +#else +#include +#endif + +namespace viam::trajex::jacobian { + +// TODO(RSDK-14104): Remove duplicated code that exists in the viam-cpp-sdk + +/// +/// A validated URDF-style serial kinematic chain, parsed from an (n, 10) +/// model-table tensor and held in chain order. +/// +class kinematic_chain { + public: + /// + /// Parses an (n, 10) tensor in the viam::sdk::ModelTable format into a + /// kinematic chain. + /// + /// @param tensor (n, 10) tensor in the viam::sdk::ModelTable format + /// @return Validated kinematic chain + /// @throws std::invalid_argument on non-2D input, wrong column count, a + /// non-integer joint-type encoding, an empty table, an unsupported + /// joint type (continuous or prismatic), or a revolute row with + /// zero-magnitude axis + /// + [[nodiscard]] static kinematic_chain from(const xt::xarray& 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. + /// + /// @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 : std::uint8_t { + k_revolute = 0, + k_continuous = 1, + k_prismatic = 2, + k_fixed = 3, + }; + + // One row of the model table: the per-joint URDF fields. xyz/rpy are the + // joint origin relative to the parent link (rpy is fixed-axis XYZ); axis + // is the joint axis in the local frame. + struct joint_row { + std::array xyz{}; + std::array rpy{}; + std::array axis{}; + joint_type type = joint_type::k_fixed; + }; + + // Per-revolute-joint world-frame axes and origins plus the end-effector + // position. + struct chain_state; + + // Validates the rows (joint types, axes) and counts actuated joints; all + // public construction funnels through here via `from`. + explicit kinematic_chain(std::vector rows); + + // Evaluates the forward kinematics at joint positions q, capturing the + // per-joint quantities the Jacobian assemblies need. Throws + // std::invalid_argument on q-size mismatch. + chain_state compute_chain_state(const xt::xarray& 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 new file mode 100644 index 0000000..af4411f --- /dev/null +++ b/src/viam/trajex/jacobian/test/jacobian.cpp @@ -0,0 +1,499 @@ +#include + +#include +#include +#include +#include +#include +#include + +#if __has_include() +#include +#else +#include +#endif + +#include + +namespace { + +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; values match +// viam::sdk::ModelTable::JointType. +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(); + 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 (const double v : r) { + t(i, j++) = v; + } + ++i; + } + return t; +} + +double matrix_diff_norm(const xt::xarray& A, const xt::xarray& B) { + double s = 0.0; + const auto* a = A.begin(); + const 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)); +} + +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; +} + +// 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, 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, 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, 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}, + }; +} + +// 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 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; + } + return t; +} + +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) { + 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 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 = 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); + 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; +} + +// 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 = 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 = 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 = 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 = 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 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(); + xt::xarray J_num = xt::zeros({std::size_t{6}, n_actuated}); + + 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; + + 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); + } + + 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; + } + return J_num; +} + +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 + +// The tests in this suite validate that `J * q_dot` produces the expected +// Cartesian velocity. +BOOST_AUTO_TEST_SUITE(jacobian_velocity_tests) + +BOOST_AUTO_TEST_CASE(twolink_base_spin_extended) { + const auto table = twolink_table(); + const xt::xarray q = xt::zeros({std::size_t{2}}); + const xt::xarray q_dot = {1.0, 0.0}; + + 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); +} + +BOOST_AUTO_TEST_CASE(twolink_base_spin_rotated) { + 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}; + + 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); +} + +BOOST_AUTO_TEST_CASE(twolink_base_spin_bent) { + 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}; + + 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); +} + +BOOST_AUTO_TEST_SUITE_END() + +// The tests in this suite check kinematic_chain::jacobian against hand-computed +// Jacobian values. +BOOST_AUTO_TEST_SUITE(jacobian_ground_truth_tests) + +BOOST_AUTO_TEST_CASE(twolink_zero) { + const auto table = twolink_table(); + const xt::xarray q = xt::zeros({std::size_t{2}}); + const auto J = compute_jacobian(table, q); + + 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(matrix_diff_norm(J, J_expected), 1e-10); +} + +BOOST_AUTO_TEST_CASE(twolink_q1_ninety) { + const auto table = twolink_table(); + const xt::xarray q = {std::numbers::pi / 2.0, 0.0}; + const auto J = compute_jacobian(table, q); + + 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(matrix_diff_norm(J, J_expected), 1e-10); +} + +BOOST_AUTO_TEST_CASE(twolink_q2_ninety) { + const auto table = twolink_table(); + const xt::xarray q = {0.0, std::numbers::pi / 2.0}; + const auto J = compute_jacobian(table, q); + + 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(matrix_diff_norm(J, J_expected), 1e-10); +} + +BOOST_AUTO_TEST_SUITE_END() + +// The numerical-consistency suite depends on the reference forward kinematics +// being correct, so the tests in this suite verify a few easy cases directly. +BOOST_AUTO_TEST_SUITE(fk_tests) + +BOOST_AUTO_TEST_CASE(twolink_zero_ee_at_2_0_0) { + const auto table = twolink_table(); + 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); +} + +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 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); +} + +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 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); +} + +BOOST_AUTO_TEST_SUITE_END() + +// The tests in this suite check the analytical Jacobian against a +// central-difference numerical 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_table(), q); +} + +BOOST_AUTO_TEST_CASE(twolink_folded_back) { + const xt::xarray q = {0.0, std::numbers::pi}; + check_matches_numerical(twolink_table(), q); +} + +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(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(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() + +// 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) + +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_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(static_cast(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}; + 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, 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, k_rev}}); + const xt::xarray q = {0.0}; + BOOST_CHECK_THROW(compute_jacobian(table, q), std::invalid_argument); +} + +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), 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()