c++ - ツールの x、y、z のみを提供する逆運動学ソリューションのプログラミング

Question

私は現在、ツールの位置が x、y、z である場合に、ロボットが可能なすべての Q 状態を見つけることができる逆運動学ソリューションを実装しようとしています。

私は最小二乗アプローチを使用してそれを行うことを選択しましたが、すべての可能なソリューションを提供するのではなく、エラーが最小のソリューションのみを提供することがわかりました。この場合、位置を満たすすべての可能な Q 状態に関心があります。ツールの。

私の実装はそのように見えます。

Eigen::MatrixXd jq(device_.get()->baseJend(state).e().cols(),device_.get()->baseJend(state).e().rows());
      jq = device_.get()->baseJend(state).e(); //Extract J(q) directly from robot


      //Least square solver - [AtA]⁻1AtB

      Eigen::MatrixXd A (6,6);
      A = jq.transpose()*(jq*jq.transpose()).inverse();



      Eigen::VectorXd du(6);
      du(0) = 0.1 - t_tool_base.P().e()[0];
      du(1) = 0 - t_tool_base.P().e()[1];
      du(2) = 0 - t_tool_base.P().e()[2];
      du(3) = 0;  // Should these be set to something if i don't want the tool position to rotate?
      du(4) = 0;
      du(5) = 0;

      ROS_ERROR("What you want!");
      Eigen::VectorXd q(6);
      q = A*du;


      cout << q << endl; // Least square solution - want a vector of solutions. 

まず第一に、Q-state がロボットを目的の位置に動かさないため、逆親族は正しくないようです。実装のどこが間違っているのかわかりませんか?