From af242f6d9266af1688c9cfd5fad8193191de3439 Mon Sep 17 00:00:00 2001 From: jfdupuis Date: Tue, 3 Mar 2020 14:49:51 -0800 Subject: [PATCH 1/2] Fix reference to local variable KDL::ChainFkSolverPos_recursive is holding a reference to the KDL::Chain and should be passed a chain that is owned by SNSPositionIK --- sns_ik_lib/src/sns_position_ik.cpp | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/sns_ik_lib/src/sns_position_ik.cpp b/sns_ik_lib/src/sns_position_ik.cpp index 525b250..a2430a4 100644 --- a/sns_ik_lib/src/sns_position_ik.cpp +++ b/sns_ik_lib/src/sns_position_ik.cpp @@ -28,8 +28,8 @@ namespace sns_ik { SNSPositionIK::SNSPositionIK(KDL::Chain chain, std::shared_ptr velocity_ik, double eps) : m_chain(chain), m_ikVelSolver(velocity_ik), - m_positionFK(chain), - m_jacobianSolver(chain), + m_positionFK(m_chain), + m_jacobianSolver(m_chain), m_linearMaxStepSize(0.2), m_angularMaxStepSize(0.2), m_maxIterations(150), From dd1b8a58c6d2fef05982ce0243b394d571809ad7 Mon Sep 17 00:00:00 2001 From: jfdupuis Date: Wed, 4 Mar 2020 15:13:47 -0800 Subject: [PATCH 2/2] Initialize the length of qSoln This avoid ChainIkSolverPos_NR_JL::CartToJnt to complain about mismatch size. --- sns_ik_lib/test/sns_ik_pos_test.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/sns_ik_lib/test/sns_ik_pos_test.cpp b/sns_ik_lib/test/sns_ik_pos_test.cpp index c2283ea..c31b3b1 100644 --- a/sns_ik_lib/test/sns_ik_pos_test.cpp +++ b/sns_ik_lib/test/sns_ik_pos_test.cpp @@ -117,7 +117,7 @@ PosTestResult runPosIkSingleTest(int seed, const KDL::JntArray& qLow, const KDL: } // loop over each solver type - KDL::JntArray qSoln; + KDL::JntArray qSoln(qLow.rows()); PosTestResult result; result.solveTime = ros::Duration(0); result.nPass = 0;