Dear Acados Team,
thanks for this nice library. I am trying to build an MPC using the SQP solver to control the cartesian position of a robot manipulator with 7 joints. To do this I made a simple test case where the integration function just integrates the joint angles and the cost function gives the difference between a desired cartesian position and the current position as
||f(q) - p_des||, where
f(q) is the forward kinematic using the current joint config and
p_des is the desired pose. This however does not work. The residual never converges and the solver always runs out of iterations (even if I put them higher).
I understand that the forwards kinematics are a complex nonlinear function but a SQP solver should still be able to solve this right?
Do you have any experience with that ?
Any help is appreciated. Thank you