[PyKDL] add vector version of JntToCart for fk pos and vel

Author: MatthijsBurgh

python_orocos_kdl/PyKDL/kinfam.cpp

@@ -335,9 +335,19 @@ void init_kinfam(pybind11::module &m)
     py::class_<ChainFkSolverPos, SolverI> chain_fk_solver_pos(m, "ChainFkSolverPos");
     chain_fk_solver_pos.def("JntToCart", (int (ChainFkSolverPos::*)(const JntArray&, Frame&, int)) &ChainFkSolverPos::JntToCart,
                             py::arg("q_in"), py::arg("p_out"), py::arg("segmentNr")=-1);
-//    Argument by reference doesn't work for container types
-//    chain_fk_solver_pos.def("JntToCart", (int (ChainFkSolverPos::*)(const JntArray&, std::vector<Frame>&, int)) &ChainFkSolverPos::JntToCart,
-//                            py::arg("q_in"), py::arg("p_out"), py::arg("segmentNr")=-1);
+    chain_fk_solver_pos.def("JntToCart", [](ChainFkSolverPos& self, const JntArray& q_in, py::list& p_out, int segmentNr)
+    {
+        std::vector<Frame> temp;
+        temp.resize(p_out.size());
+        int result = self.JntToCart(q_in, temp, segmentNr);
+        p_out.clear();
+        for (const Frame& f : temp)
+        {
+            p_out.append(f);
+        }
+        return result;
+    },
+    py::arg("q_in"), py::arg("p_out"), py::arg("segmentNr")=-1);
 
 
     // --------------------
@@ -346,9 +356,19 @@ void init_kinfam(pybind11::module &m)
     py::class_<ChainFkSolverVel, SolverI> chain_fk_solver_vel(m, "ChainFkSolverVel");
     chain_fk_solver_vel.def("JntToCart", (int (ChainFkSolverVel::*)(const JntArrayVel&, FrameVel&, int)) &ChainFkSolverVel::JntToCart,
                             py::arg("q_in"), py::arg("p_out"), py::arg("segmentNr")=-1);
-//    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);
+    chain_fk_solver_vel.def("JntToCart", [](ChainFkSolverVel& self, const JntArrayVel& q_in, py::list& p_out, int segmentNr)
+    {
+        std::vector<FrameVel> temp;
+        temp.resize(p_out.size());
+        int result = self.JntToCart(q_in, temp, segmentNr);
+        p_out.clear();
+        for (const FrameVel& f : temp)
+        {
+            p_out.append(f);
+        }
+        return result;
+    },
+    py::arg("q_in"), py::arg("p_out"), py::arg("segmentNr")=-1);
 
 
     // ------------------------------

python_orocos_kdl/tests/kinfamtest.py

@@ -257,6 +257,41 @@ def testFkPosAndIkPosGivens(self):
         epsJ = 1e-3
         self.testFkPosAndIkPosImpl(self.fksolverpos, self.iksolverpos_givens, epsJ)
 
+    def testFkPosVect(self):
+        epsC = 1e-5
+    
+        q = JntArray(self.chain.getNrOfJoints())
+
+        for i in range(q.rows()):
+            q[i] = random.uniform(-0.99, 0.99)
+
+        v_out: list[Frame | None] = [None] * self.chain.getNrOfSegments()  # Cheaper than Frames
+        f_out = Frame()
+        self.assertEqual(self.fksolverpos.JntToCart(q, f_out), 0)
+        self.assertEqual(self.fksolverpos.JntToCart(q, v_out), 0)
+
+        self.assertEqual(len(v_out), self.chain.getNrOfSegments())
+        self.assertTrue(Equal(v_out[self.chain.getNrOfSegments() - 1], f_out, epsC))
+
+    def testFkVelVect(self):
+        epsC = 1e-5
+    
+        q = JntArray(self.chain.getNrOfJoints())
+        qdot = JntArray(self.chain.getNrOfJoints())
+
+        for i in range(q.rows()):
+            q[i] = random.uniform(-0.99, 0.99)
+            qdot[i] = random.uniform(-0.99, 0.99)
+
+        v_out: list[FrameVel | None] = [None] * self.chain.getNrOfSegments()  # Cheaper than FrameVels
+        f_out = FrameVel()
+        q_vel = JntArrayVel(q, qdot)
+        self.assertEqual(self.fksolvervel.JntToCart(q_vel, f_out), 0)
+        self.assertEqual(self.fksolvervel.JntToCart(q_vel, v_out), 0)
+
+        self.assertEqual(len(v_out), self.chain.getNrOfSegments())
+        self.assertTrue(Equal(v_out[self.chain.getNrOfSegments() - 1], f_out, epsC))
+
     def compare_Jdot_Diff_vs_Solver(self, dt, representation):
         NrOfJoints = self.chain.getNrOfJoints()
         q = JntArray(NrOfJoints)
@@ -357,6 +392,8 @@ def suite():
     suite.addTest(KinfamTestFunctions('testFkVelAndIkVelGivens'))
     suite.addTest(KinfamTestFunctions('testFkPosAndIkPos'))
     suite.addTest(KinfamTestFunctions('testFkPosAndIkPosGivens'))
+    suite.addTest(KinfamTestFunctions('testFkPosVect'))
+    suite.addTest(KinfamTestFunctions('testFkVelVect'))
     suite.addTest(KinfamTestFunctions('testJacDot'))
     suite.addTest(KinfamTestTree('testTreeGetChainMemLeak'))
     return suite