Calculate end effector force from torque vector of joints
I am trying to estimate the force apply to PR2 arm using kdl Library, and i saw some code one the internet that show
for (unsigned int i = 0 ; i < 6 ; i++)
{
F_measured_(i) = 0;
for (unsigned int j = 0 ; j < kdl_chain_.getNrOfJoints() ; j++)
F_measured_ += J_(i,j) * tau_measured_(j);
}
should works, which means F = Jacob_transpose * tau.......however, according to what I know, shouldn't the equation be F=(Jacob_transpose)_inverse * tau ......because jacobian is not an orthogonal matrix as i know.