TermTaskspaceGeometricJacobian

[awa152]

I have a few questions regarding the Taskspace costfunction term you provide in the rbd module:
https://github.com/ethz-adrl/control-toolbox/blob/v3.0.2/ct_rbd/include/ct/rbd/robot/costfunction/TermTaskspaceGeometricJacobian.hpp
especially concerning the analytical derivatives.

1. In the stateDerivative method you use the geometric Jacobian to get the gradient. For the position part it seems clear, but is it valid for the orientation if the geometric Jacobian is used instead of an analytical one?
2. You define the orientation error as

 Eigen::Matrix<double, 3, 1> qDiff = error_quaternion_angle_axis.axis() * error_quaternion_angle_axis.angle(); 

In the book Robotics: Modelling, planning and control (Siciliano et. al 2009) I found the orientation error in terms of angle and axis as . Is this the same?

3. In the stateSecondDerivative method the Jacobian is also used in the Hessian. Does that mean that a Newton step with this costfunction term would fail if the Jacobian is singular?

Sorry if my questions are somehow naive and thank you for your help!

[markusgft]

Hi @awa152, yes, this term is going to fail if the Jacobian is singular, therefore it is not optimal to use it. You could consider adding some Tikhonov style regularization scheme to this term, if you wanted.
In general I would recommend you building your own, proper task-space cost function term, since the one here is only experimental. You will need to parameterize poses appropriately, a good starting point would be to include manif for computing local errors. Hope this helps.