# End effector velocity control of a 6-DOF robotic manipulator using MPC

I've been trying to utilize Model Predictive Control (MPC) scheme that I have for end-effector position reference $$x(t)$$ tracking control to build end-effector velocity reference $$\dot{x}(t)$$ tracking. Hopefully, I can design a smooth velocity reference profile from the positions reference profile so that I CAN ACHIEVE BOTH POSITION AND VELOCITY REFERENCE TRACKING.

Let me first describe how I think the MPC should be formulated to achieve what I want:

Assuming that we have a common 6-DOF robot manipulator without any redundancy (e.g., 6-joints cobot), and there is a way to calculate inverse of its Jacobian matrix $$J^{-1}(q_t)$$. Then, from end-effector velocity reference profile $$\dot{x}(t)$$, we can calculate desired joint velocities $$\dot{q_t} = J^{-1}(q_t)\dot{x_t}$$.

A system of equation is as following:

$$\dot{q}_{t+1} = \dot{q_t} + \Delta_t a_t$$ = $$A\dot{q_t} + B\Delta_t a_t$$

From here, we can formulate the MPC problem as:

$$min_{a_0,...a_{N-1}} {J = \sum_{t=1}^{N}||\dot{q_t} - \dot{q_g}||^2_{Q} + ||\dot{q_N} - \dot{q_g}||^2_{P}}_{} + ||a_t||^2_R$$

$$s.t. \dot{q}_{t+1} = A\dot{q_t} + B\Delta_t a_t$$

$$\dot{q}_{min} \leq \dot{q}_t \leq \dot{q}_{max}$$, $$\dot{q}_0$$ given

Now, the question is: how am I supposed to put joint angle limits instead joint acceleration limits ($$a_{lim}$$) in the optimization problem above (OEM provides only the joint angle and velocity limits)?

I was thinking of using Quadratic Programming (QP), as the problem looks like a strictly convex optimization. But there is no place to put the angle limits as parameters in QP (may be these limits cannot be expressed as linear constraints). I appreciate it if anyone could provide me some advices and/or references.

$$\ddot q$$ equals $$a$$? Then augment your states with the joint positions, change the equality constraints to a double-integrating dynamic system from $$\ddot q$$->$$\dot q$$->$$q$$ and add simple linear inequality constraints on q. Wouldn't that solve your problem?

• Yeah, that's what I ended up doing it. I was being lazy and wondered whether there is a way to put that constraint without providing extra states. Of course, it doesn't make sense. Oct 23, 2023 at 14:28

The question defines $$x(t)$$ as a "position reference". Given the remainder of the question, e.g. $$\dot{q_t} = J^{-1}(q_t)\dot{x_t}$$, I conclude that rather a full 6-DOF reference trajectory is meant (i.e. a specification for positions and orientations for the end effector).

For a 6D reference trajectory, a 6-joint robot is non-redundant. As long as the robot does not travel through singularities thoughout the trajectory $$x(t)$$, then the trajectory is feasible. But that purely depends on the start configuration of your robot (if multiple are possible) and the specified 6D trajectory. There is a one-to-one mapping between the trajectory (i.e. sequence of end effector poses) and the robot motion (i.e. sequence of joint positions).

In other words: for a given 6D motion trajectory and a 6 joint robot, you cannot optimize joint motions towards some criteria (such as avoiding joint angle limits or reducing max joint velocities, etc).

If you want to formulate the robot task as an optimization problem, you need extra degrees of freedom so one trajectory in task space (in your example: cartesian space) has multiple possible trajectories in joint space. The optimization problem is then to identify, from all possible trajectories in joint space, that one which yields minimum values for your optimization criterium.

E.g.:

• Consider a 6DOF trajectory with a 7 joint robot: due to the 7th joint, the robot is redundant and this redundancy can be used to minimize some optimization criterium (such as remaining clear of joint limits, or minimizing kinetic energy, etc).

• Consider a 6 joint robot, but a 5 DOF kinematic task. E.g. a MIG welding operation: the weld seam geometry defines position references $$x(t)$$, $$y(t)$$, $$z(t)$$ and two angles $$\phi(t)$$ and $$\theta(t)$$ but the rotation of the welding gun around the axis defined by the welding wire is not relevant. Null motion around that axis can be used, e.g. to avoid singular robot positions throughout the motion along the seam.

An alternative optimization problem is if the task function is only partially specified, and you need to calculate an optimum path through the task space to realize that partial specification. E.g. instead of a full trajectory $$x(t)$$, you specify only a start pose $$x(0)$$ and end pose $$x(T)$$ (and maybe some intermediate poses) and you calculate an optimal trajectory to realize that motion.

• Oh yeah, I totally forgot to say that I fixed the orientation of the end-effector. Thank you for the thorough explanation. Oct 23, 2023 at 14:31