From bf9b26b5d38b2244da0c9e14669a5ae68b52f9b6 Mon Sep 17 00:00:00 2001 From: Craig Carignan Date: Wed, 25 Sep 2024 13:45:51 -0400 Subject: [PATCH 1/9] 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/9] 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/9] 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/9] 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/9] 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/9] 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 Date: Wed, 17 Dec 2025 10:29:35 -0500 Subject: [PATCH 7/9] Update orocos_kdl/src/chainjnttojacsolver.cpp formatting change only Co-authored-by: Matthijs van der Burgh --- orocos_kdl/src/chainjnttojacsolver.cpp | 27 ++++++++++++++------------ 1 file changed, 15 insertions(+), 12 deletions(-) diff --git a/orocos_kdl/src/chainjnttojacsolver.cpp b/orocos_kdl/src/chainjnttojacsolver.cpp index 688528b88..ed8a1bfac 100644 --- a/orocos_kdl/src/chainjnttojacsolver.cpp +++ b/orocos_kdl/src/chainjnttojacsolver.cpp @@ -125,20 +125,23 @@ namespace KDL Frame total; // Loop through segments - for (unsigned int i=0;i 0) jac[i] = jac[i-1]; + + // Calculate new Frame_base_ee + if (chain.getSegment(i).getJoint().getType() != Joint::Fixed) + { + // Pose of the new end-point expressed in the base + total = T_tmp * chain.getSegment(i).pose(q_in(j)); + // Changing base of new segment's twist to base frame if it is not locked + if (!locked_joints_[j]) + t_tmp = T_tmp.M * chain.getSegment(i).twist(q_in(j),1.0); } - //Calculate new Frame_base_ee - if(chain.getSegment(i).getJoint().getType()!=Joint::Fixed) { - //pose of the new end-point expressed in the base - total = T_tmp*chain.getSegment(i).pose(q_in(j)); - //changing base of new segment's twist to base frame if it is not locked - //t_tmp = T_tmp.M*chain.getSegment(i).twist(1.0); - if(!locked_joints_[j]) - t_tmp = T_tmp.M*chain.getSegment(i).twist(q_in(j),1.0); - }else{ - total = T_tmp*chain.getSegment(i).pose(0.0); + else + { + total = T_tmp * chain.getSegment(i).pose(0.0); } //Changing Refpoint of all columns to new ee From ad7e879acbb64381572147a8644946af06224ea3 Mon Sep 17 00:00:00 2001 From: craigirobot Date: Wed, 17 Dec 2025 10:30:41 -0500 Subject: [PATCH 8/9] Update orocos_kdl/src/chainjnttojacsolver.cpp formatting change only Co-authored-by: Matthijs van der Burgh --- orocos_kdl/src/chainjnttojacsolver.cpp | 14 +++++++------- 1 file changed, 7 insertions(+), 7 deletions(-) diff --git a/orocos_kdl/src/chainjnttojacsolver.cpp b/orocos_kdl/src/chainjnttojacsolver.cpp index ed8a1bfac..0564cef9f 100644 --- a/orocos_kdl/src/chainjnttojacsolver.cpp +++ b/orocos_kdl/src/chainjnttojacsolver.cpp @@ -144,14 +144,15 @@ namespace KDL total = T_tmp * chain.getSegment(i).pose(0.0); } - //Changing Refpoint of all columns to new ee - changeRefPoint(jac[i],total.p-T_tmp.p,jac[i]); + // Changing Refpoint of all columns to new ee + changeRefPoint(jac[i], total.p - T_tmp.p, jac[i]); - //Only increase jointnr if the segment has a joint - if(chain.getSegment(i).getJoint().getType()!=Joint::Fixed) { + // Only increase jointnr if the segment has a joint + if (chain.getSegment(i).getJoint().getType() != Joint::Fixed) + { //Only put the twist inside if it is not locked - if(!locked_joints_[j]) - jac[i].setColumn(k++,t_tmp); + if (!locked_joints_[j]) + jac[i].setColumn(k++, t_tmp); j++; } @@ -159,6 +160,5 @@ namespace KDL } return (error = E_NOERROR); } - } From 481f19175fc8bb8da7a1fa0ccf6bd4bc4cfb50f4 Mon Sep 17 00:00:00 2001 From: craigirobot Date: Wed, 17 Dec 2025 11:50:38 -0500 Subject: [PATCH 9/9] Update chainjnttojacsolver.hpp Added member function to output array of Jacobians --- orocos_kdl/src/chainjnttojacsolver.hpp | 17 +++++++++++++++-- 1 file changed, 15 insertions(+), 2 deletions(-) diff --git a/orocos_kdl/src/chainjnttojacsolver.hpp b/orocos_kdl/src/chainjnttojacsolver.hpp index 4007e4593..334a73bda 100644 --- a/orocos_kdl/src/chainjnttojacsolver.hpp +++ b/orocos_kdl/src/chainjnttojacsolver.hpp @@ -48,14 +48,27 @@ namespace KDL * KDL::ChainFkSolverVel_recursive * * @param q_in input joint positions - * @param jac output jacobian of last segment or array of jacobians for each segment + * @param jac output jacobian of last segment * @param seg_nr The final segment to compute * @return success/error code */ virtual int JntToJac(const JntArray& q_in, Jacobian& jac, int seg_nr=-1); - virtual int JntToJac(const JntArray& q_in, std::vector& jac, int seg_nr=-1); + virtual ~ChainJntToJacSolver(); /** + * Calculate the jacobian expressed in the base frame of the + * chain, with reference point at the end effector of the + * *chain. The algorithm is similar to the one used in + * KDL::ChainFkSolverVel_recursive + * + * @param q_in input joint positions + * @param jac output array of jacobians for each segment + * @param seg_nr The final segment to compute + * @return success/error code + */ + virtual int JntToJac(const JntArray& q_in, std::vector& jac, int seg_nr=-1); + +/** * * @param locked_joints new values for locked joints * @return success/error code