I am trying to calculate the Jacobian between the marker and the coordinate of my model.
I used CalculateStationJacobian to calculate the jacobian like that for each marker:
Code: Select all
const SimTK::MobilizedBodyIndex moBodyIndex = marker.getParentFrame().getMobilizedBodyIndex();
SimTK::Matrix Jm(3, mNbrU); Jm = 0.0;
mModel->getMatterSubsystem().calcStationJacobian(mModel->updWorkingState(), moBodyIndex, marker.get_location(), Jm);
I used this Jacobian as part of my own IK solver.
Unfortunately I saw that the output of the IK was very noisy/shaky/wobbly (the segment is moving back and forth around the markers).
I decided to then compare the Jacobian output to an implementation of the Jacobian that was symbolically made wih Maple. I tested this one in Matlab using the same input (coordinate and marker values were the same as the one used in the OpenSim implementation). I was surprised to see significant differences in the Jacobian ouput between the OpenSim and Maple versions.
More specifically, the differences seem to happen on the 2nd and 3rd coordinate of the body. In other words, the first column of J is identical while the second and first column are different.
After that test, I used the Jacobian output from my Maple implementation and feed it to OpenSim to replace the values calculated by CalcStationJacobian. It solved the wobling.
So the calculation of the Jacobian seem to be the issue.
Do you have any idea what would be the reason for this issue with calcStationJacobian? I guess you have thoroughly checked your implementation of the Jacobian so I am not saying that you have done something wrong! Might be coming from my side for sure
Cheers