Controlling a robot with NMPC using end effector positions and not joint angles

Hello everyone,

I am attempting to modify the example provided at

https://www.mathworks.com/help/mpc/ug/control-of-robot-manipulator-using-passivity-based-nonlinear-mpc.html.

Currently, we are able to set the desired values of q (joint angles) and q_dot (joint velocities). The state vector appears as follows:

To enable the controller to track an end effector position, I aim to edit the example and the manipulatorStateFcn function. My desired state vector would be structured as:

Essentially, I wish to instruct the robot to reach, for instance, coordinates (1, 0.5) on the x and y axes, respectively. I intend to assign weights to the end effector position for optimization purposes.

We possess the equations necessary to compute both forward and inverse kinematics. However, I'm encountering difficulties integrating this modification into the system.

Any assistance on this matter would be greatly appreciated.

Best regards,

Szymon