From bf9b26b5d38b2244da0c9e14669a5ae68b52f9b6 Mon Sep 17 00:00:00 2001 From: Craig Carignan Date: Wed, 25 Sep 2024 13:45:51 -0400 Subject: [PATCH 1/6] chainjnttojacsolver: added jnttojac function that returns all segment jacobians JntToJac(const JntArray& q_in, Jacobian& jac, int seg_nr) in ChainJntToJacSolver Class only returns the Jacobian for the specified input segment. Because this function internally calculates all of the Jacobians leading up to seg_nr, the modified JntToJac() returns all of the intermediate Jacobians by encapsulating them in a std vector . Note that this is analogous to overloaded function JntToCart() in the ChainFkSolverPos_recursive Class which returns Cartesian frames for all of the intermediate segments in std::vector. --- orocos_kdl/src/chainjnttojacsolver.cpp | 60 ++++++++++++++++++++++++++ orocos_kdl/src/chainjnttojacsolver.hpp | 1 + 2 files changed, 61 insertions(+) diff --git a/orocos_kdl/src/chainjnttojacsolver.cpp b/orocos_kdl/src/chainjnttojacsolver.cpp index c24a9a312..688528b88 100644 --- a/orocos_kdl/src/chainjnttojacsolver.cpp +++ b/orocos_kdl/src/chainjnttojacsolver.cpp @@ -97,5 +97,65 @@ namespace KDL } return (error = E_NOERROR); } + + int ChainJntToJacSolver::JntToJac(const JntArray& q_in, std::vector& jac, int seg_nr) + { + if(locked_joints_.size() != chain.getNrOfJoints()) + return (error = E_NOT_UP_TO_DATE); + unsigned int segmentNr; + if(seg_nr<0) + segmentNr=chain.getNrOfSegments(); + else + segmentNr = seg_nr; + + //Initialize Jacobian to zero since only segmentNr columns are computed + SetToZero(jac[0]); + + if( q_in.rows()!=chain.getNrOfJoints() || jac[0].columns() != chain.getNrOfJoints()) + return (error = E_SIZE_MISMATCH); + else if(segmentNr>chain.getNrOfSegments()) + return (error = E_OUT_OF_RANGE); + else if(jac.size() < segmentNr) + return (error = E_SIZE_MISMATCH); + + T_tmp = Frame::Identity(); + SetToZero(t_tmp); + int j=0; + int k=0; + Frame total; + + // Loop through segments + for (unsigned int i=0;i& jac, int seg_nr=-1); /** * From 1c3a79f0242e9ebea474abb4ba6cb8dabc307b4a Mon Sep 17 00:00:00 2001 From: Craig Carignan Date: Fri, 27 Sep 2024 17:20:09 -0400 Subject: [PATCH 2/6] orocos_kdl/tests: added jac solver test for jnttojac that returns all jacs Unit test 'JacAllSegments()' was added to solvertest.cpp for the new jnttojac() function that returns the Jacobians for all of the segments. It uses the (current) JntToJac function to test the return value of the Jacobian for each individual segment. Note that the index starts at 0 so that segment N has the index N-1 in the Jacobian vector. --- orocos_kdl/tests/solvertest.cpp | 37 +++++++++++++++++++++++++++++++++ orocos_kdl/tests/solvertest.hpp | 2 ++ 2 files changed, 39 insertions(+) diff --git a/orocos_kdl/tests/solvertest.cpp b/orocos_kdl/tests/solvertest.cpp index c42c8b696..eda0afd55 100644 --- a/orocos_kdl/tests/solvertest.cpp +++ b/orocos_kdl/tests/solvertest.cpp @@ -815,6 +815,43 @@ void SolverTest::FkPosAndIkPosLocal(Chain& chain,ChainFkSolverPos& fksolverpos, } +void SolverTest::JacAllSegments() +{ + std::cout << "KDL Jac Solver Test for returning all segment Jacobians" << std::endl; + + double eps = 1e-6; + + unsigned int nj = motomansia10.getNrOfJoints(); + unsigned int ns = motomansia10.getNrOfSegments(); + + JntArray q(nj); + Jacobian jac(nj); + std::vector jac_all(ns, Jacobian(nj)); + + ChainJntToJacSolver jacsolver(motomansia10); + + // random + q(0) = 0.2; + q(1) = 0.6; + q(2) = 1.; + q(3) = 0.5; + q(4) = -1.4; + q(5) = 0.3; + q(6) = -0.8; + + CPPUNIT_ASSERT_EQUAL((int)SolverI::E_NOERROR, jacsolver.JntToJac(q, jac_all, ns)); + for (unsigned int seg=0; seg Date: Fri, 28 Mar 2025 13:08:36 -0400 Subject: [PATCH 3/6] chainjnttojacsolver: Add PyKDL definitions for new JntToJac function --- python_orocos_kdl/PyKDL/kinfam.cpp | 4 +++- 1 file changed, 3 insertions(+), 1 deletion(-) diff --git a/python_orocos_kdl/PyKDL/kinfam.cpp b/python_orocos_kdl/PyKDL/kinfam.cpp index fec7eda76..371cf1318 100644 --- a/python_orocos_kdl/PyKDL/kinfam.cpp +++ b/python_orocos_kdl/PyKDL/kinfam.cpp @@ -469,7 +469,9 @@ void init_kinfam(pybind11::module &m) // ------------------------------ py::class_ chain_jnt_to_jac_solver(m, "ChainJntToJacSolver"); chain_jnt_to_jac_solver.def(py::init(), py::arg("chain")); - chain_jnt_to_jac_solver.def("JntToJac", &ChainJntToJacSolver::JntToJac, + chain_jnt_to_jac_solver.def("JntToJac", (int (ChainJntToJacSolver::*)(const JntArray&, Jacobian&, int)) &ChainJntToJacSolver::JntToJac, + py::arg("q_in"), py::arg("jac"), py::arg("seg_nr")=-1); + chain_jnt_to_jac_solver.def("JntToJac", (int (ChainJntToJacSolver::*)(const JntArray&, std::vector&, int)) &ChainJntToJacSolver::JntToJac, py::arg("q_in"), py::arg("jac"), py::arg("seg_nr")=-1); chain_jnt_to_jac_solver.def("setLockedJoints", &ChainJntToJacSolver::setLockedJoints, py::arg("locked_joints")); From 6ecfa261874eb09a3e0f6d5a45bbc6950947e963 Mon Sep 17 00:00:00 2001 From: craigirobot Date: Thu, 31 Jul 2025 10:34:27 -0400 Subject: [PATCH 4/6] Update chainjnttojacsolver.hpp with comment for Jacobin output This adds the description for the new array of Jacobian outputs. --- orocos_kdl/src/chainjnttojacsolver.hpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/orocos_kdl/src/chainjnttojacsolver.hpp b/orocos_kdl/src/chainjnttojacsolver.hpp index 0af01da13..4007e4593 100644 --- a/orocos_kdl/src/chainjnttojacsolver.hpp +++ b/orocos_kdl/src/chainjnttojacsolver.hpp @@ -48,7 +48,7 @@ namespace KDL * KDL::ChainFkSolverVel_recursive * * @param q_in input joint positions - * @param jac output jacobian + * @param jac output jacobian of last segment or array of jacobians for each segment * @param seg_nr The final segment to compute * @return success/error code */ From b09c9060dfc3ee96c08c3df1004a85334e4c2ac7 Mon Sep 17 00:00:00 2001 From: craigirobot Date: Thu, 31 Jul 2025 10:37:29 -0400 Subject: [PATCH 5/6] Update orocos_kdl/tests/solvertest.cpp removed white space Co-authored-by: Matthijs van der Burgh --- orocos_kdl/tests/solvertest.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/orocos_kdl/tests/solvertest.cpp b/orocos_kdl/tests/solvertest.cpp index eda0afd55..fa18a1aff 100644 --- a/orocos_kdl/tests/solvertest.cpp +++ b/orocos_kdl/tests/solvertest.cpp @@ -819,7 +819,7 @@ void SolverTest::JacAllSegments() { std::cout << "KDL Jac Solver Test for returning all segment Jacobians" << std::endl; - double eps = 1e-6; + double eps = 1e-6; unsigned int nj = motomansia10.getNrOfJoints(); unsigned int ns = motomansia10.getNrOfSegments(); From 45e8083287176f95e21b5ffbce25a141426f866f Mon Sep 17 00:00:00 2001 From: craigirobot Date: Thu, 31 Jul 2025 10:53:21 -0400 Subject: [PATCH 6/6] Update solvertest.cpp with Jacobian row dimension Jacobians generally have 6 rows corresponding to the 6 Cartesian degrees of freedom, but this makes it explicit that the indexing is based on the number of rows of the Jacobian. --- orocos_kdl/tests/solvertest.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/orocos_kdl/tests/solvertest.cpp b/orocos_kdl/tests/solvertest.cpp index fa18a1aff..3a88d0606 100644 --- a/orocos_kdl/tests/solvertest.cpp +++ b/orocos_kdl/tests/solvertest.cpp @@ -843,7 +843,7 @@ void SolverTest::JacAllSegments() for (unsigned int seg=0; seg