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

orocos_kdl/src/chainjnttojacsolver.cpp

@@ -97,5 +97,68 @@ namespace KDL
         }
         return (error = E_NOERROR);
     }
+
+    int ChainJntToJacSolver::JntToJac(const JntArray& q_in, std::vector<Jacobian>& 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<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);
+            }
+
+            // 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);
+    }
 }
 

orocos_kdl/src/chainjnttojacsolver.hpp

@@ -48,13 +48,27 @@ namespace KDL
          * KDL::ChainFkSolverVel_recursive
          *
          * @param q_in input joint positions
-         * @param jac output jacobian
+         * @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 ~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<Jacobian>& jac, int seg_nr=-1);
+
+/**
          *
          * @param locked_joints new values for locked joints
          * @return success/error code

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<Jacobian> 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<ns; seg++)
+    {
+        jacsolver.JntToJac(q, jac, seg+1);
+        for ( unsigned int i=0; i<jac.rows(); i++ ) {
+            for ( unsigned int j=0; j<nj; j++ ) {
+                CPPUNIT_ASSERT(Equal(jac(i,j), jac_all[seg](i,j), eps));
+            }
+        }
+    }
+}
+
+
 void SolverTest::VereshchaginTest()
 {
     std::cout << "KDL Vereshchagin Hybrid Dynamics Tests" <<std::endl;

orocos_kdl/tests/solvertest.hpp

@@ -33,6 +33,7 @@ class SolverTest : public CppUnit::TestFixture
     CPPUNIT_TEST(FkVelAndJacTest );
     CPPUNIT_TEST(FkVelAndIkVelTest );
     CPPUNIT_TEST(FkPosAndIkPosTest );
+    CPPUNIT_TEST(JacAllSegments );
     CPPUNIT_TEST(VereshchaginTest );
     CPPUNIT_TEST(ExternalWrenchEstimatorTest );
     CPPUNIT_TEST(IkSingularValueTest );
@@ -54,6 +55,7 @@ class SolverTest : public CppUnit::TestFixture
     void FkVelAndJacTest();
     void FkVelAndIkVelTest();
     void FkPosAndIkPosTest();
+    void JacAllSegments();
     void VereshchaginTest();
     void ExternalWrenchEstimatorTest();
     void IkSingularValueTest() ;

python_orocos_kdl/PyKDL/kinfam.cpp

@@ -469,7 +469,9 @@ void init_kinfam(pybind11::module &m)
     // ------------------------------
     py::class_<ChainJntToJacSolver, SolverI> chain_jnt_to_jac_solver(m, "ChainJntToJacSolver");
     chain_jnt_to_jac_solver.def(py::init<const Chain&>(), 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<Jacobian>&, 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"));