PID 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$.

§ 2PID law

The PID 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 fixed by the rolling-without-slipping equation derived on the physics page: $u_x > 0$ corresponds to a tilt that drives the ball toward $-\hat x_P$.

One scalar PID controller is instantiated per plate axis. The two run independently with shared gains. Each consumes the tracking error along its axis and emits the corresponding tilt angle:

u_x \;=\; K_p\, e_x \;+\; K_i \int_0^t e_x(\tau)\, d\tau \;+\; K_d\, \dot e_x.

The integrator state is anti-windup-clamped to a symmetric bound and the output is clipped to a maximum tilt amplitude. The deployed gains were tuned by hand on the live arm.

Figure 2. PID block diagram for the $x$-axis. The setpoint $x_r$ and the measurement $x$ enter a subtractor that forms the error $e = x_r - x$. The error feeds three parallel branches: proportional ($K_p$), integral ($K_i \int e\,dt$) and derivative ($K_d \, \dot e$). The three branch outputs are summed (Σ) to produce the tilt command $u_x$. The $y$-axis controller is identical.

§ 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 = (1,0,0)^{\top}, \hat e_y = (0,1,0)^{\top}$, 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 vector $\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$ acting on the full joint increment $\Delta q$ to produce the plate rotation increment $\Delta\theta$. Locked-joint entries of $\Delta q$ are zero by construction; only the wrist entries contribute, so the effective system is $3 \times 3$ on the wrist columns of $J_r$.

§ 6Smoothing and dispatch

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.