Closed PepMS closed 2 years ago
@PepMS Thanks for reaching us with this concern. This is just to let you know that this code is working perfectly on the current pinocchio-3x devel version. This version is not yet made available but should be released soon.
If you need more details, contact me on Element for further discussions. Best,
Justin
Hello,
I was trying the new release of Pinocchio and its support for CasADI. Such a great and helpful tool, congrats and thanks! I'm writing this because I'm facing an issue when computing a Jacobian of a cost function and I'd appreciate a little bit of help.
Here's a little bit of context. I played with CasADI and Pinocchio and tried to solve an OCP using a quadrotor as the robot model. Since IPOPT does not support optimization in SE(3), I set the decision variables to lie in the tangent space of a nominal state, which I set to be the identity. When the optimization finishes, I retract the solution to the manifold.
Thus, a cost for the desired pose would have a form like $l(\mathbf{\Delta x}) = \mathbf{e}(\mathbf{\Delta x})^\top \mathbf{e}(\mathbf{\Delta x})$ , with $\mathbf{e}(\mathbf{\Delta x}) = \mathbf{x}_g \ominus (\mathbf{x}_n \oplus \mathbf{\Delta x}) $ and $\mathbf{x}_g$ the goal state and $\mathbf{x}_n$ the nominal state, which I normally set to identity.
When adding this cost to the OCP it generates an error due to
nan
values at its gradient. Then I computed this gradient separately and it does havenans
. However, when computing the gradient using regular Pinocchio and the chain rule, it outputs the correct solution.I might be doing something wrong with CasADI, since I'm not very familiar with its syntax. At the end of this issue, I'm attaching the code showing the problem. The CasADI Jacobian does not coincide with Pinocchio's output with the chain rule.
Thanks a lot!
The output of the script:
The script: