Generate overload JntToJac function in ChainJntToJacSolver Class to return Jacobians for all segments #492

craigirobot · 2025-05-07T13:41:45Z

JntToJac() in the ChainJntToJacSolver Class only returns the Jacobian for the specified input segment seg_nr. Some inverse kinematics algorithms require Jacobians for multiple segments in a chain such as those used for commanding self-motion. Because JntToJac already calculates all of the Jacobians leading up to seg_nr, it does not cost anything computationally to return the intermediate Jacobians. The modified JntToJac() returns Jacobians for all of the segments 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. JntToJac() follows JntToCart() as a model to generate the same functionality.

… 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 <Jacobian>. 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<Frame>.

… 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.

MatthijsBurgh · 2025-07-24T05:44:31Z

orocos_kdl/src/chainjnttojacsolver.hpp

@@ -53,6 +53,7 @@ namespace KDL
         * @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<Jacobian>& jac, int seg_nr=-1);


Add missing docstring

Added the missing comment on the Jacobian string output.

I would copy the entire docstring and make it specific for each set of arguments

orocos_kdl/tests/solvertest.cpp

MatthijsBurgh · 2025-07-24T05:49:24Z

orocos_kdl/tests/solvertest.cpp

+    for (unsigned int seg=0; seg<ns; seg++)
+    {
+        jacsolver.JntToJac(q, jac, seg+1);
+        for ( unsigned int i=0; i<6; i++ ) {


Where is this magic number 6 coming from?

The Jacobian has 6 rows corresponding to the 6 Cartesian degrees of freedom (3 translation, 3 rotation), but this is changed to jac.rows() to be more explicit.

MatthijsBurgh · 2025-07-24T05:49:42Z

orocos_kdl/tests/solvertest.cpp

@@ -815,6 +815,43 @@ void SolverTest::FkPosAndIkPosLocal(Chain& chain,ChainFkSolverPos& fksolverpos,
 }


+void SolverTest::JacAllSegments()


Please also add this test in python

I can take a look at this

MatthijsBurgh · 2025-07-24T06:56:06Z

python_orocos_kdl/PyKDL/kinfam.cpp

-    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<Jacobian>&, int)) &ChainJntToJacSolver::JntToJac,


This doesn't work yet, see also

orocos_kinematics_dynamics/python_orocos_kdl/PyKDL/kinfam.cpp

Lines 349 to 351 in ec5d67e

// Argument by reference doesn't work for container types

// chain_fk_solver_vel.def("JntToCart", (int (ChainFkSolverVel::*)(const JntArrayVel&, std::vector<FrameVel>&, int)) &ChainFkSolverVel::JntToCart,

// py::arg("q_in"), py::arg("p_out"), py::arg("segmentNr")=-1);

There are multiple ways to work around this. docs I don't like making custom vectors. I would go for converting this to a lamda function, which accepts a py::list instead of the vector. Then you need to copy from the vector to the python list.

Take a look at #497

I made these changes for Craig. These were just a copy/paste of existing code.
Are you saying that the existing code doesn't work and you would like that fixed? Or is there an error in the copy/paste itself?

What I am saying is that pybind can't handle references of container arguments. So when a reference container argument is an output of the function, you don't get the update value of the argument. I will be same as when you called the function. So see #497 for how I fixed it for another function with a reference container argument.

This adds the description for the new array of Jacobian outputs.

removed white space Co-authored-by: Matthijs van der Burgh <[email protected]>

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.

MatthijsBurgh · 2025-08-05T12:25:44Z

orocos_kdl/src/chainjnttojacsolver.hpp

@@ -53,6 +53,7 @@ namespace KDL
         * @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<Jacobian>& jac, int seg_nr=-1);


I would copy the entire docstring and make it specific for each set of arguments

MatthijsBurgh · 2025-08-05T12:34:57Z

orocos_kdl/src/chainjnttojacsolver.cpp

+        for (unsigned int i=0;i<segmentNr;i++) {
+            if ( 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
+                //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);
+            }


Suggested change

for (unsigned int i=0;i<segmentNr;i++) {

if ( 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

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

}

for (unsigned int i=0;i<segmentNr;i++)

{

if (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);

}

else

{

total = T_tmp * chain.getSegment(i).pose(0.0);

}

MatthijsBurgh · 2025-08-05T12:37:11Z

orocos_kdl/src/chainjnttojacsolver.cpp

+            //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 put the twist inside if it is not locked
+                if(!locked_joints_[j])
+                    jac[i].setColumn(k++,t_tmp);
+                j++;
+            }
+
+            T_tmp = total;
+        }
+        return (error = E_NOERROR);
+    }
+


Suggested change

//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 put the twist inside if it is not locked

if(!locked_joints_[j])

jac[i].setColumn(k++,t_tmp);

j++;

}

T_tmp = total;

}

return (error = E_NOERROR);

}

// 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 put the twist inside if it is not locked

if (!locked_joints_[j])

jac[i].setColumn(k++, t_tmp);

j++;

}

T_tmp = total;

}

return (error = E_NOERROR);

}

MatthijsBurgh · 2025-08-05T12:37:45Z

orocos_kdl/src/chainjnttojacsolver.cpp

+        // Loop through segments
+        for (unsigned int i=0;i<segmentNr;i++) {
+            if ( i!=0 ) {
+                jac[i] = jac[i-1];


Please explain why you take the jacobian of the previous segment

MatthijsBurgh · 2025-08-05T12:40:56Z

python_orocos_kdl/PyKDL/kinfam.cpp

-    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<Jacobian>&, int)) &ChainJntToJacSolver::JntToJac,


What I am saying is that pybind can't handle references of container arguments. So when a reference container argument is an output of the function, you don't get the update value of the argument. I will be same as when you called the function. So see #497 for how I fixed it for another function with a reference container argument.

Craig Carignan and others added 3 commits May 7, 2025 09:27

chainjnttojacsolver: Add PyKDL definitions for new JntToJac function

d4bc889

MatthijsBurgh requested changes Jul 24, 2025

View reviewed changes

craigirobot and others added 3 commits July 31, 2025 10:34

Update chainjnttojacsolver.hpp with comment for Jacobin output

6ecfa26

This adds the description for the new array of Jacobian outputs.

Update orocos_kdl/tests/solvertest.cpp

b09c906

removed white space Co-authored-by: Matthijs van der Burgh <[email protected]>

Update solvertest.cpp with Jacobian row dimension

45e8083

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.

MatthijsBurgh requested changes Aug 5, 2025

View reviewed changes

		@@ -815,6 +815,43 @@ void SolverTest::FkPosAndIkPosLocal(Chain& chain,ChainFkSolverPos& fksolverpos,
		}


		void SolverTest::JacAllSegments()

	// Argument by reference doesn't work for container types
	// chain_fk_solver_vel.def("JntToCart", (int (ChainFkSolverVel::*)(const JntArrayVel&, std::vector<FrameVel>&, int)) &ChainFkSolverVel::JntToCart,
	// py::arg("q_in"), py::arg("p_out"), py::arg("segmentNr")=-1);

Generate overload JntToJac function in ChainJntToJacSolver Class to return Jacobians for all segments #492

Are you sure you want to change the base?

Generate overload JntToJac function in ChainJntToJacSolver Class to return Jacobians for all segments #492

Conversation

craigirobot commented May 7, 2025

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!

Uh oh!

Choose a reason for hiding this comment

Uh oh!

craigirobot Jul 31, 2025 • edited Loading Uh oh! There was an error while loading. Please reload this page.

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!

Uh oh!

craigirobot Jul 31, 2025 •

edited

Loading