c++roboticsinverse-kinematics

Programming inverse kinematic solution provided only x,y,z of tool


I at moment trying to implement a inverse kinematic solution capable of finding all possible Q-states a robot can have given the position of the tool is x,y,z..

I am choosen to do it using the least square approach, but something tells me that it won't provide all possible solution but just the one with smallest error, which in this case i am interested in all possible Q-states that fulfill the position of the tool.

My implementation looks as such.

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. 

first of all the inverse kin doesn't seem right as the Q-state doesn't move the robot to the desired position. I can't seem to see where my implementation is wrong?


Solution

  • Solving the inverse kinematics problem with numerical solution is not the best option. You should go for that option just in case you could not find the analytical solution. BTW, well designed robots should have a clear inverse kinematics model.

    For your robot, here is the inverse kinematics model: https://smartech.gatech.edu/bitstream/handle/1853/50782/ur_kin_tech_report_1.pdf