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