MPC controller · Hold UR Pla7e

§ 1Inputs and reference frames

Two right-handed Cartesian frames are used: the world frame $\{W\}$ at the UR7e base and the plate frame $\{P\}$ at the geometric centre of the plate. The ball state $X = (x, y, \dot x, \dot y)$ and the reference trajectory $r(t) = (x_r, y_r, \dot x_r, \dot y_r)$ are both expressed in $\{P\}$, in metres and metres per second. Two errors are derived from them and consumed by the controller:

e_p = \begin{pmatrix} x - x_r \\ y - y_r \end{pmatrix}, \qquad e_v = \begin{pmatrix} \dot x - \dot x_r \\ \dot y - \dot y_r \end{pmatrix}.

The relative orientation of the two frames is encoded in a rotation matrix $R_{WP}(q) \in SO(3)$, with components of a vector $v$ transforming as $v^{W} = R_{WP}(q)\, v^{P}$. The inverse mapping is $R_{PW}(q) = R_{WP}(q)^{\top}$.

In general, the pose of the plate frame in the world frame is a homogeneous transform

g_{WP}(q) \;=\; \begin{pmatrix} R_{WP}(q) & t(q) \\ 0 & 1 \end{pmatrix} \;\in\; SE(3), \qquad R_{WP}(q) \in SO(3), \; t(q) \in \mathbb{R}^{3},

and a full plate-pose controller would have to regulate both the rotation $R_{WP}(q)$ and the translation $t(q)$ as functions of the six joints $q \in \mathbb{R}^6$.

In the deployed setup (see § 5) the upper-arm joints $q_1, q_2, q_3$ are held at their home values, so the plate centre is fixed in $\{W\}$ throughout operation and the translation

t(q) \;\equiv\; t(q_{\text{home}}) \;=\; \text{const}

drops out of the control problem. The task reduces to tracking only the rotation $R_{WP}(q) \in SO(3)$. Its first two columns are the world-frame coordinates of the plate's local axes $\hat x_P, \hat y_P$, and feed the wiring stage below.

Figure 1. Plate frame $\{P\}$ seen from the overhead camera. Origin at the plate centre, $\hat x_P$ to the right, $\hat y_P$ up. The four corners carry the ArUco fiducials. The ball position is $(x, y)$; the target is $(x_r, y_r)$. The arrow from ball to target is the negative position error $-e_p$.

§ 2MPC law

The MPC block takes the tracking error and emits a pair of scalar tilt angles in radians,

(u_x, u_y) \;\in\; \mathbb{R}^2,

where $u_x$ is the desired tilt of the plate about the local $\hat y_P$ axis and $u_y$ is the desired tilt about $\hat x_P$. The sign convention is the same as for the PID page: $u_x > 0$ corresponds to a tilt that drives the ball toward $-\hat x_P$, as fixed by the rolling-without-slipping equation on the physics page.

2.1 Per-axis plant model

At small tilts each plate axis is a continuous-time double integrator,

\ddot x \;=\; -\beta\, u, \qquad \beta = \alpha\, g \;\approx\; \tfrac{5}{7}\, g.

Discretising the per-axis state $s = (x, \dot x)^\top$ with a zero-order hold on the input over the controller step $\Delta t$ gives a linear time-invariant system $s_{k+1} = A\, s_k + B\, u_k$ with

A \;=\; \begin{pmatrix} 1 & \Delta t \\ 0 & 1 \end{pmatrix}, \qquad B \;=\; \begin{pmatrix} -\tfrac{1}{2}\, \beta\, \Delta t^{2} \\ -\beta\, \Delta t \end{pmatrix}.

2.2 Quadratic cost over a horizon $N$

At each control tick the MPC plans the next $N$ inputs $u_0, \dots, u_{N-1}$ to minimise a quadratic cost on tracking error and control effort, with a terminal cost on the predicted state at step $N$:

\begin{aligned} \min_{u_0, \ldots, u_{N-1}} \;\; & \sum_{k=1}^{N-1} \Big[ q_p\, (x_k - x_{r,k})^2 + q_v\, (\dot x_k - \dot x_{r,k})^2 + r_u\, u_{k-1}^2 \Big] \\ & \;+\; q_{f,p}\, (x_N - x_{r,N})^2 \;+\; q_{f,v}\, (\dot x_N - \dot x_{r,N})^2. \end{aligned}

Collecting the state weights into diagonal matrices $Q = \mathrm{diag}(q_p, q_v)$, $Q_f = \mathrm{diag}(q_{f,p}, q_{f,v})$ and writing $\tilde s_k = s_k - r_k$ for the state-tracking error, the cost takes the compact matrix form

\min_{u_0, \ldots, u_{N-1}} \;\; \sum_{k=1}^{N-1} \Big[ \tilde s_k^{\top}\, Q\, \tilde s_k \;+\; r_u\, u_{k-1}^{2} \Big] \;+\; \tilde s_N^{\top}\, Q_f\, \tilde s_N.

The weights $(q_p, q_v, r_u, q_{f,p}, q_{f,v})$ are scalars tuned offline.

2.3 Closed-form solve, only the first input applied

Substituting the state-evolution constraint $s_k = A^{k} s_0 + \sum_{j

u_0^\star \;=\; K_x(\Delta t, A, B, Q, R)\, s_0 \;+\; K_r(\Delta t, A, B, Q, R)\, r_{0:N},

with $K_x, K_r$ precomputed offline. The remaining inputs $u_1^\star, \dots, u_{N-1}^\star$ are discarded; at the next tick the plan is recomputed from the new state. This receding-horizon principle is what makes the MPC a feedback controller rather than an open-loop trajectory.

The MPC variant emits the same $(u_x, u_y)$ as the PID and feeds the same downstream pipeline. It was implemented and unit-tested against analytic tracking; its weights were not retuned to match the PID baseline on the perturbed-domain benchmark, so the PID remains the controller deployed on the live arm.

Figure 2. One MPC tick. The plant model is rolled forward over the next $N$ steps, the candidate input sequence is scored by the quadratic cost, and the closed-form QP returns the optimal sequence. Only the first input $u_0^\star$ is sent to the plate; the controller replans on the next tick.

§ 3Wiring: scalars to a world-frame rotation

The scalars $(u_x, u_y)$ are tilt angles about the plate's local axes $\hat x_P, \hat y_P$. The downstream inverse-kinematics step needs the corresponding rotation 3-vector expressed in the world frame $\{W\}$, because the manipulator Jacobian maps joint motion to world-frame plate rotations. The wiring stage performs that change of basis.

Writing $\hat x_P^{\,W}(q)$ for the plate's local $\hat x_P$ axis with its components expressed in $\{W\}$,

\hat x_P^{\,W}(q) \;=\; R_{WP}(q)\, \hat e_x, \qquad \hat y_P^{\,W}(q) \;=\; R_{WP}(q)\, \hat e_y,

where $\hat e_x, \hat e_y$ are the standard basis vectors, the two vectors $\hat x_P^{\,W}(q)$ and $\hat y_P^{\,W}(q)$ are the first two columns of $R_{WP}(q)$.

For a small angle $\alpha$, a rotation by $\alpha$ about a unit axis $\hat n$ is represented in axis-angle form by the 3-vector $\alpha \hat n$. Stacking the two scalar tilts, the target rotation in $\{W\}$ is

\theta_{\text{tar}} \;=\; u_y\, \hat x_P^{\,W}(q) \;-\; u_x\, \hat y_P^{\,W}(q) \;\in\; \mathbb{R}^{3}.

The minus sign on the $u_x$ term comes from the rolling-equation convention fixed on the physics page: $u_x > 0$ must drive the ball toward $-\hat x_P$, which corresponds to a rotation about $-\hat y_P$.

Figure 3. The plate is tilted by an angle $\theta_y$ about the world's $\hat y_W$ axis. The arrow $\hat x_P^{\,W}(q)$ is the plate's local $\hat x_P$ axis expressed in world coordinates at the current configuration $q$, i.e. the first column of $R_{WP}(q)$, pointing along the slope in $\{W\}$.

§ 4Rotation increment

The next stage is the inverse kinematics, where the rotational Jacobian $J_r(q) \in \mathbb{R}^{3 \times 6}$ relates joint displacements $\Delta q$ to plate rotations: in the small-displacement regime, $J_r(q)\, \Delta q \approx \Delta\theta$. This relation is a first-order Taylor expansion around $q$ and is accurate only when $\Delta\theta$ is genuinely small. Feeding the IK an absolute target rotation produces large residual joint motion and breaks the linearisation as soon as the plate is no longer near home.

To avoid this, the current plate rotation $\theta_{\text{cur}}$ is read from the transform tree (encoding the current $R_{WP}(q)$ in axis-angle form) and subtracted from the absolute target:

\Delta\theta \;=\; \theta_{\text{tar}} \;-\; \theta_{\text{cur}}.

The IK then solves for the joint displacement that produces $\Delta\theta$, not $\theta_{\text{tar}}$; the linearisation is exercised only on small residuals.

Figure 4. Rotation increment. The absolute target $\theta_{\text{tar}}$ produced by the wiring stage and the current plate rotation $\theta_{\text{cur}}$ read from the transform tree are subtracted; the IK consumes the residual $\Delta\theta$.

§ 5Inverse kinematics

The geometric Jacobian of the manipulator decomposes into a positional block and a rotational block:

J(q) \;=\; \begin{pmatrix} J_p(q) \\ J_r(q) \end{pmatrix} \;\in\; \mathbb{R}^{6 \times 6}, \qquad J_p, J_r \in \mathbb{R}^{3 \times 6}.

$J_p$ relates joint velocities to the linear velocity of the wrist and is unused here, because the position of the plate centre is fixed by the locked upper-arm joints. $J_r$ relates joint velocities to the angular velocity of the plate; in the small-displacement regime, $J_r(q)\, \Delta q \approx \Delta\theta$.

The system has three equations and six unknowns and is therefore underdetermined. With the upper-arm joints $q_1, q_2, q_3$ locked, the corresponding three columns of $J_r$ are dropped; only the wrist columns contribute. The remaining $3 \times 3$ subsystem is square and well-conditioned at the home configuration. The minimum-norm solution is computed via the Moore-Penrose pseudoinverse:

\Delta q \;=\; J_r(q)^{+} \, \Delta\theta.

In practice the pseudoinverse is computed via the SVD-based least-squares routine, which is numerically stable when the wrist subsystem approaches singularity (e.g. at gimbal-lock configurations).

Figure 5. The rotational Jacobian $J_r \in \mathbb{R}^{3 \times 6}$. The columns corresponding to locked joints are dropped; the remaining three wrist columns form a $3 \times 3$ subsystem.

§ 6Smoothing and publication

The raw $\Delta q$ is clipped per-joint to a safe maximum tilt $\Delta q_{\max}$ and then passed through a first-order low-pass filter to attenuate high-frequency content that would excite the structural modes of the arm:

\Delta q_f^{(k+1)} \;=\; \alpha\, \Delta q^{(k+1)} \;+\; (1 - \alpha)\, \Delta q_f^{(k)}, \qquad \alpha \in (0, 1].

The filtered correction is added to the current configuration to produce the target,

q_{\text{tgt}} \;=\; q_{\text{cur}} \;+\; \Delta q_f,

which is published as a joint trajectory message at the controller rate. The vendor's joint-space firmware takes over from there: it interpolates between successive waypoints and runs an internal joint-level loop with gravity compensation. This boundary is described in more detail on the ROS page.

§ 7Notation

$e_p, e_v$: position and velocity tracking errors (defined on the setup page).
$u_x, u_y$: scalar tilt angles, output of the position-level controller.
$K_p, K_i, K_d$: PID gains.
$\beta = \alpha g$: plate-tilt gain in the rolling equation, $\alpha = 5/7$.
$R_{WP}(q) \in SO(3)$: rotation matrix that takes components in $\{P\}$ to components in $\{W\}$: $v^W = R_{WP}(q)\, v^P$.
$\hat x_P^{\,W}(q), \hat y_P^{\,W}(q)$: plate's local $\hat x_P, \hat y_P$ axes expressed in $\{W\}$: the first two columns of $R_{WP}(q)$.
$\theta_{\text{tar}}, \theta_{\text{cur}}, \Delta\theta \in \mathbb{R}^3$: target, current, and incremental plate rotation in axis-angle form.
$J(q), J_p(q), J_r(q)$: full geometric Jacobian and its positional and rotational blocks.
$\Delta q, \Delta q_f$: raw and low-pass-filtered joint displacement.
$q_{\text{cur}}, q_{\text{tgt}}$: current and target joint configurations.