As you can see in the screenshot from #48 , the reference and inverse kinematics trajectory for the endeffector do not align. Looking into this, it turns out the reference velocity for the endeffector was not computed properly: The polynominal gets differentiated but the dt spacing is not taken into account. This PR fixes this.
This seems to be a regression from the recent kindyn alignment change in #47. There, the polynominal time was changed from time t to time index it, causing the missing dt in the differentiation.
With this change, the reference and solution from the second order inverse kinematics align very much - see screenshot below.
As you can see in the screenshot from #48 , the reference and inverse kinematics trajectory for the endeffector do not align. Looking into this, it turns out the reference velocity for the endeffector was not computed properly: The polynominal gets differentiated but the dt spacing is not taken into account. This PR fixes this.
This seems to be a regression from the recent kindyn alignment change in #47. There, the polynominal time was changed from time
tto time indexit, causing the missingdtin the differentiation.With this change, the reference and solution from the second order inverse kinematics align very much - see screenshot below.