I am at the moment trying to implement an inverse kinematics function which function is to take a desired transformation matrix, and the current transformation matrix, and compute the Q states that is needed to move my robot arm from current state to end state.
I have already written the code, but since my simulation isn't showing the right path, or what I would expect it to be, this makes me unsure as to whether my implementation is correct. Could someone comment on my implementation and maybe spot an error?
std::vector<Q> pathPlanning::invKin_largeDisplacement(std::vector<Transform3D<>> t_tool_base_desired_i)
{
for(unsigned int i = 0; i<t_tool_base_desired_i.size(); ++i)
{
Transform3D<> T_tool_base_current_i = device_backup->baseTframe(this->toolFrame,state_backup);
Eigen::MatrixXd jq(device_backup->baseJframe(this->toolFrame,state_backup).e().cols(), this->device.get()->baseJframe(this->toolFrame,state_backup).e().rows());
jq = this->device.get()->baseJframe(this->toolFrame,state_backup).e();
//Least square solver - dq = [j(q)]T (j(q)[j(q)]T)⁻1 du <=> dq = A*du
Eigen::MatrixXd A (6,6);
//A = jq.transpose()*(jq*jq.transpose()).inverse();
A = (jq*jq.transpose()).inverse()*jq.transpose();
Vector3D<> dif_p = t_tool_base_desired_i[i].P()-T_tool_base_current_i.P(); // Difference in position between current_i and desired_i
Eigen::Matrix3d dif = t_tool_base_desired_i[i].R().e()- T_tool_base_current_i.R().e(); // Difference in rotation between current_i and desired_i
Rotation3D<> dif_r(dif); //Construct rotation matrix
RPY<> dif_rot(dif_r); // compute RPY from rotation matrix
//Jq*dq = du
Eigen::VectorXd du(6);
du(0) = dif_p[0];
du(1) = dif_p[1];
du(2) = dif_p[2];
du(3) = dif_rot[0];
du(4) = dif_rot[1];
du(5) = dif_rot[2];
Eigen::VectorXd q(6);
q = A*du; // Compute change dq
Q q_current;
q_current = this->device->getQ(this->state); // Get Current Q
Q dq(q);
Q q_new = q_current+ dq; // compute new Q by adding dq
output.push_back(q_new); // Pushback to output vector
device_backup->setQ(q_new,state_backup); //set current state to newly calculated Q.
}
return output;
}
Example of output:
Q{-1.994910, -94.421754, -123.448429, 15.218864, 6.602184, -13.742988}
Q{2627.867315, -2048.863588, -51.340574, 287.654959, 270.187026, 258.581800}
Q{12941.812459, -536.870516, -294.362593, -2145.963577, -31133.660814, -4742.343433}
Q{32.044799, -14.220020, -14.312226, -12.444921, 12.269179, -24.393637}
Q{125.537278, 28.626924, -55.646716, -20.945348, 17.536762, -2.656717}
Q{9.514525, -107.455064, -17.009190, -15.245588, -0.960273, -2.010570}
Q{8.255582, -3.010934, -4.882207, -1.369533, 0.848644, 1.175172}
Q{208.655993, -28.443465, -64.413952, -3.129896, 13.063806, -6.042187}
Q{-73.706483, -20.381540, -5.306434, -1.204419, -4.035149, 21.806934}
Q{10.003481, 10.867394, 13.256192, -6.491445, -1.711469, 2.896646}
Q{24.890626, -72.265307, -94.886507, 12.327304, -4.425786, 4.188531}
Q{7.111258, 31.500732, -0.111033, -20.434697, 5.302118, 1.781690}
Q{477.993581, 659.221820, 19.819916, -88.627757, 65.850191, -77.267367}
Q{-30.672145, -53.496243, -18.170871, 83.648574, 48.311796, -28.015005}
Q{-36.677982, -15.908633, 17.751008, 0.995766, -0.500259, 9.409435}
Q{114246.358249, -10664.813432, -75.904830, 462.907904, 7992.514723, -18484.319327}
Q{83.827086, -75.899321, -38.576446, 37.266068, 47.843725, 39.096061}
Q{-119.682661, -774.773093, -251.969174, 23.212110, -42.662580, 53.247454}
Q{98.608881, -28.013383, 132.896921, 17.121488, 36.916894, -14.627180}
Q{-11519.051453, 5761.564318, -364.916044, -1188.567128, -2582.813750, -462.784007}
Q{54802.605226, 40971.776641, 10204.739981, -654.963987, -244.277958, -8618.970216}
Q{-21.334047, -14.314134, 17.714174, 2.463993, 0.963385, 5.304530}
