MPC for 7 DOF Robot Manipulator

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 :slight_smile:

Hi,

that sounds like an interesting project!

It might need some tuning and surely depends on the initialization.
Did you try the following?

  • initialize the solver
  • try some regularization

Also: how big are the residuals? How close are you to a solution.
You could also try globalization.

Cheers!

Thanks for the help. I manged to make it work by setting p_des close to f(q) such that singularities are avoided. I have some more troubles with that whole setup but I will try around first and then ask more helpful questions.
Thanks :slight_smile:

1 Like