Self-Collision Avoidance Implementation

Hey there again!

my mpc for a robotic manipulator indeed works and produces acceptable results. One part of my implementation however includes self-collision avoidance, thus integrating them as hard constraints.

I now invested a decent amount of time in returning locations of each joint frame and calculating symbolic expressions of the lumelsky algorithm to calculate distances between each link-segment. I now included these expressions as nonlinear inequality constraints with each joint frame position as parameter for those equations.

But now I face the following problem:
Since my mpc predicts a N-long horizon of optimal controls and resulting states, updating the current position of each joint frame and assigning them as parameters for distance calculation at every new mpc step (as constant for all N shooting nodes) is not really helpful, since the optimization / solver of the QP assumes a static posture then.

What do you think would be my best bet? Rethinking this “define joint frame positions as parameters”-approach and assign them as states? In such a way that my nonlinear constraints calculate the distance between specific link segments for each Predictionstep N based on an always updating state-vector which yields those points?

Thanks again for your input! Have some nice holidays and a happy new years eve’!

I think I found a solution myself after thinking a bit more about my problem. For those interested:

assigning the current position of joint frames as parameters and using those in inequality constraints is obviously a bad idea, since the optimizer does not know about the dependency of system states (in this case configuration) of those parameters!

What should work would be constructing an inequality function for each collision pair (eg. link 1 and 4) which calculates Lumelskys Algorithm based on the system states. No need to adjust or extend the system state vector x, just using it as input to those inequality constraints.

Sorry for causing some confusion!

Best regards

1 Like