1
$\begingroup$

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}
$\endgroup$
1
$\begingroup$

Shouldn't A be

jq.transpose() * (jq*jq.transpose()).inverse() 

instead of

(jq*jq.transpose()).inverse()*jq.transpose()

?

$\endgroup$
  • $\begingroup$ Nope... it didn't work :( $\endgroup$ – test May 6 '16 at 15:39

Your Answer

By clicking “Post Your Answer”, you agree to our terms of service, privacy policy and cookie policy

Not the answer you're looking for? Browse other questions tagged or ask your own question.