RSDK-13546- compute jacobian from model table by nfranczak · Pull Request #17 · viam-modules/trajex

Nick Franczak (nfranczak) · 2026-05-27T20:17:23Z

tcp-trajex will access jacobian code through a callback

…SDK-13546-Jacobian

Andrew C. Morrow (acmorrow)

I left some general comments, but this needs to be reworked to avoid a dependency on the C++ SDK inside the totg library. We can't do that per our goal of using trajex by way of cgo inside the RDK.

Andrew C. Morrow (acmorrow) · 2026-06-10T18:24:53Z

    xtensor
    xtl
+  PRIVATE
+    viam-cpp-sdk::viamsdk


Unfortunately, creating this dependency edge is pretty much not permissible for viam-trajex-totg. It would mean dragging the C++ SDK and all of its transitive dependencies in when we link trajex into the RDK. Worse, for a static build, it would require producing a link line which explicitly referenced all of those libraries.

Andrew C. Morrow (acmorrow) · 2026-06-10T18:26:01Z

@@ -0,0 +1,450 @@
+// Tests for the simplified jacobian module (Option B: plain-return API,


Try to teach your claude that it shouldn't reference ephemeral state of local information in code it produces. It is hard: it really likes to do this, but Option B doesn't mean anything.

Andrew C. Morrow (acmorrow) · 2026-06-10T18:26:23Z

+// 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;


Style in our C++ is k_rev not kRev.

Andrew C. Morrow (acmorrow) · 2026-06-10T18:28:41Z

+
+}  // namespace
+
+// ============================================================================


You don't need these block comments. You can just comment on the BOOST_AUTO_TEST_SUITE itself, since that naturally forms a scope:

// The tests in this suite validate that `J * q_dot` produces the expected Cartesian velocity BOOST_AUTO_TEST_SUITE(jacobian_velocity_tests)

Andrew C. Morrow (acmorrow)

I'm approving this to unblock things, but I have some high-level reservations, and I'd like us to agree to revisit it in the future:

The duplicated understanding of the model table format between this repo and the C++ SDK is a problem. It is better than the alternative, but it isn't great.
I find the mixing of std::array / vec3 and xtensor a little troubling. It looks to me like there could be a much simpler implementation living entirely in xtensor space.
I feel like there is a lot of "manual work", where the capabilities of xtensor aren't really being leveraged.

Please take a pass through on the comments I left, they are mostly small, and then lets get this merged, but agree to revisit it later.

Andrew C. Morrow (acmorrow) · 2026-06-18T02:11:23Z

+    // Evaluates the forward kinematics at joint positions q, capturing the
+    // per-joint quantities the Jacobian assemblies need. Throws
+    // std::invalid_argument on q-size mismatch.
+    chain_state compute_chain_state(const xt::xarray<double>& q) const;


The private function should have a trailing _ as well.

Andrew C. Morrow (acmorrow) · 2026-06-22T16:49:37Z

+    // 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 {


It is private, so joint_type_ with a trailing _

Andrew C. Morrow (acmorrow) · 2026-06-22T16:52:48Z

+    // Evaluates the forward kinematics at joint positions q, capturing the
+    // per-joint quantities the Jacobian assemblies need. Throws
+    // std::invalid_argument on q-size mismatch.
+    chain_state compute_chain_state(const xt::xarray<double>& q) const;


Ditto trailing _: compute_chain_state_

Andrew C. Morrow (acmorrow) · 2026-06-22T16:58:43Z

+        const joint_row& row = rows_[i];
+        switch (row.type) {
+            case joint_type::k_revolute:
+                if (dot(row.axis, row.axis) < 1e-24) {


Why this number and not some other?

changed it to zero since if any row axis is zero that'll be the result

Andrew C. Morrow (acmorrow) · 2026-06-22T17:09:37Z

+}
+
+xt::xarray<double> identity4() {
+    xt::xarray<double> t = xt::zeros<double>({std::size_t{4}, std::size_t{4}});


This is just xt::eye<double>(4) I think.

Andrew C. Morrow (acmorrow) · 2026-06-22T17:14:48Z

+    return t;
+}
+
+xt::xarray<double> matmul4(const xt::xarray<double>& a, const xt::xarray<double>& b) {


Disappointing that xtensor pushed the linalg stuff into xtensor-blas.

Andrew C. Morrow (acmorrow) · 2026-06-22T17:20:26Z

+    return t;
+}
+
+xt::xarray<double> matmul4(const xt::xarray<double>& a, const xt::xarray<double>& b) {


Are you doing this as explicitly 4 so that a and b can be larger than 4 and you just do 4x4 sub-view? If so, probably better to just write this as a general matrix multiplier, and then pass in views into the things you want to multiply.

Are you doing this as explicitly 4 so that a and b can be larger than 4 and you just do 4x4 sub-view?

With respect to jacobians, a and b will always be 4x4.
Although I see where you are coming from.
Technically the inputs to this function do not have to be a 4 by 4 matrix.

I think cleaning up this function should be deferred to future work when we can pull in an actual math library. That way we can get rid of the duplicated model table code and refactor the linear algebra all in one go.

I will add a comment for clarity.

Andrew C. Morrow (acmorrow) · 2026-06-22T17:21:16Z

+using vec3 = std::array<double, 3>;
+
+double dot(const vec3& a, const vec3& b) {
+    return (a[0] * b[0]) + (a[1] * b[1]) + (a[2] * b[2]);
+}
+vec3 cross(const vec3& a, const vec3& b) {
+    return {(a[1] * b[2]) - (a[2] * b[1]), (a[2] * b[0]) - (a[0] * b[2]), (a[0] * b[1]) - (a[1] * b[0])};
+}
+double norm(const vec3& a) {
+    return std::sqrt(dot(a, a));
+}
+vec3 normalized(const vec3& a) {
+    const double n = norm(a);
+    return {a[0] / n, a[1] / n, a[2] / n};
+}


I guess I'm OK with it to start, but I guess I'd have hoped to see this whole thing work in xtensor space.

Andrew C. Morrow (acmorrow) · 2026-06-22T17:22:37Z

+    std::vector<vec3> axes;
+    std::vector<vec3> positions;


This is sort of what makes me wonder about why these aren't xtensor arrays., since a vector of vec3 sure looks like a 2d array to me

while yes this is true, not enitrely feasible until we have said math library.

Andrew C. Morrow (acmorrow) · 2026-06-22T17:23:11Z

+    state.axes.reserve(actuated_count_);
+    state.positions.reserve(actuated_count_);
+
+    xt::xarray<double> T = identity4();


Is there a better name than T?

Nick Franczak (nfranczak)

.

Nick Franczak (nfranczak) added 3 commits April 20, 2026 15:07

jacobian from DH params

de61c1a

use eigen

05980a2

jacobian from model table, includes SDK bump

98c534d

Nick Franczak (nfranczak) changed the title ~~20260420 rsdk 13546 jacobian~~ RSDK-13546- compute jacobian from model table May 27, 2026

Nick Franczak (nfranczak) added 5 commits June 1, 2026 17:50

using the class now

25b78ea

urdf to model table is done, net yet in a release

b58e73b

Merge branch 'main' of github.com:viam-modules/trajex into 20260420-R…

d236e4f

…SDK-13546-Jacobian

wip

3ee7684

better

737f5a8

Nick Franczak (nfranczak) commented Jun 9, 2026

View reviewed changes

Comment thread src/viam/trajex/jacobian/jacobian.cpp

Comment thread src/viam/trajex/jacobian/jacobian.cpp

format

fe2406f

Nick Franczak (nfranczak) marked this pull request as ready for review June 9, 2026 18:45

appeace ci

b4ea2ee

Nick Franczak (nfranczak) requested a review from Andrew C. Morrow (acmorrow) June 9, 2026 20:32

appeace ci2

8046071

Andrew C. Morrow (acmorrow) requested changes Jun 10, 2026

View reviewed changes

Nick Franczak (nfranczak) added 9 commits June 10, 2026 15:43

fix nits

35a5d73

stop using viam-cpp-sdk

cee4137

add todo

35ff094

better design

88dbd24

no need for that comment

9ea53bb

ready once again

18c3249

better comments

26a1992

appease ci

b86f9f5

better comments

63951ab

Nick Franczak (nfranczak) requested a review from Andrew C. Morrow (acmorrow) June 11, 2026 21:12

Andrew C. Morrow (acmorrow) approved these changes Jun 22, 2026

View reviewed changes

Nick Franczak (nfranczak) added 2 commits June 25, 2026 15:34

wip

56964de

also this

7c4b597

Nick Franczak (nfranczak) commented Jun 26, 2026

View reviewed changes

Nick Franczak (nfranczak) merged commit ebaa7d5 into main Jun 26, 2026
10 checks passed

Nick Franczak (nfranczak) deleted the 20260420-RSDK-13546-Jacobian branch June 29, 2026 14:04

		@@ -0,0 +1,450 @@
		// Tests for the simplified jacobian module (Option B: plain-return API,


		} // namespace

		// ============================================================================

Uh oh!

Conversation

Nick Franczak (nfranczak) commented May 27, 2026 • edited Loading Uh oh! There was an error while loading. Please reload this page.

Uh oh!

Uh oh!

Uh oh!

Uh oh!

Andrew C. Morrow (acmorrow) left a comment

Choose a reason for hiding this comment

Uh oh!

Choose a reason for hiding this comment

Uh oh!

Choose a reason for hiding this comment

Uh oh!

Choose a reason for hiding this comment

Uh oh!

Choose a reason for hiding this comment

Uh oh!

Uh oh!

Andrew C. Morrow (acmorrow) left a comment

Choose a reason for hiding this comment

Uh oh!

Choose a reason for hiding this comment

Uh oh!

Choose a reason for hiding this comment

Uh oh!

Choose a reason for hiding this comment

Uh oh!

Choose a reason for hiding this comment

Uh oh!

Choose a reason for hiding this comment

Uh oh!

Choose a reason for hiding this comment

Uh oh!

Choose a reason for hiding this comment

Uh oh!

Choose a reason for hiding this comment

Uh oh!

Choose a reason for hiding this comment

Uh oh!

Choose a reason for hiding this comment

Uh oh!

Choose a reason for hiding this comment

Uh oh!

Choose a reason for hiding this comment

Uh oh!

Choose a reason for hiding this comment

Uh oh!

Nick Franczak (nfranczak) left a comment

Choose a reason for hiding this comment

Uh oh!

Uh oh!

Reviewers

Assignees

Labels

Projects

Milestone

Development

Uh oh!

2 participants

Nick Franczak (nfranczak) commented May 27, 2026 •

edited

Loading