search

ethz-adrl / control-toolbox

The Control Toolbox - An Open-Source C++ Library for Robotics, Optimal and Model Predictive Control
BSD 2-Clause "Simplified" License
1.47k stars 314 forks source link

TermTaskspaceGeometricJacobian #190

Open awa152 opened 3 years ago

awa152 commented 3 years ago

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 <img src= "https://render.githubusercontent.com/render/math?math=%5Cdisplaystyle+e_O+%3D+r+sin%28%5Cvartheta%29%0A" alt="e_O = r sin(\vartheta) ">. 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 commented 3 years ago

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.