Merge pull request #280 from suddrey-qut/patch-1

jhavl · web-flow · commit 9efe462c7899 · 2021-12-22T08:12:00.000+10:00
Fixing jacob0 for axis="1,0,0"
diff --git a/roboticstoolbox/core/fknm.c b/roboticstoolbox/core/fknm.c
@@ -669,9 +669,9 @@ void _jacob0(PyObject *links, int m, int n, npy_float64 *q, npy_float64 *etool,
                 J[0 * n + j] = U[0 * 4 + 2] * temp[1 * 4 + 3] - U[0 * 4 + 1] * temp[2 * 4 + 3];
                 J[1 * n + j] = U[1 * 4 + 2] * temp[1 * 4 + 3] - U[1 * 4 + 1] * temp[2 * 4 + 3];
                 J[2 * n + j] = U[2 * 4 + 2] * temp[1 * 4 + 3] - U[2 * 4 + 1] * temp[2 * 4 + 3];
-                J[3 * n + j] = U[0 * 4 + 2];
-                J[4 * n + j] = U[1 * 4 + 2];
-                J[5 * n + j] = U[2 * 4 + 2];
+                J[3 * n + j] = U[0 * 4 + 0];
+                J[4 * n + j] = U[1 * 4 + 0];
+                J[5 * n + j] = U[2 * 4 + 0];
             }
             else if (link->axis == 1)
             {

Original file line number	Diff line number	Diff line change
`@@ -669,9 +669,9 @@ void _jacob0(PyObject links, int m, int n, npy_float64 q, npy_float64 *etool,`
`669`	`669`	`J[0 * n + j] = U[0 * 4 + 2] * temp[1 * 4 + 3] - U[0 * 4 + 1] * temp[2 * 4 + 3];`
`670`	`670`	`J[1 * n + j] = U[1 * 4 + 2] * temp[1 * 4 + 3] - U[1 * 4 + 1] * temp[2 * 4 + 3];`
`671`	`671`	`J[2 * n + j] = U[2 * 4 + 2] * temp[1 * 4 + 3] - U[2 * 4 + 1] * temp[2 * 4 + 3];`
`672`		`- J[3 * n + j] = U[0 * 4 + 2];`
`673`		`- J[4 * n + j] = U[1 * 4 + 2];`
`674`		`- J[5 * n + j] = U[2 * 4 + 2];`
	`672`	`+ J[3 * n + j] = U[0 * 4 + 0];`
	`673`	`+ J[4 * n + j] = U[1 * 4 + 0];`
	`674`	`+ J[5 * n + j] = U[2 * 4 + 0];`
`675`	`675`	`}`
`676`	`676`	`else if (link->axis == 1)`
`677`	`677`	`{`