Wrench Sensor and Tool Dynamics Calibration

Mar 20, 2023 | Tech Robotics

Preface

Many robotic applications, such as compliant assembly, require the robot end-effector to interact with the environment with certain force and torque. For these applications, a wrench sensor and a tool is often needed. However, wrench sensors typically contain measurement offset; tool dynamics properties are subject to uncertainty. To get acceptable force interaction performance, calibration of such quantities are necessary.

This post summarizes steps involved to calibrate or optimally estimate these quantities and some reminders in practice. For reference, see 1997@Voyles and 2019@Carlson .

Illustration and Notations

Below is a visual illustration of the robot with wrench sensor configuration and a list of notations used in this post.

‎Check network for missing images.
  • $\{b\}, \{s\}$ - sensor body frame and robot space frame, respectively;
  • $\sideset{^b}{}{\mathcal{X}}, \sideset{^s}{}{\mathcal{X}}$ - a quantity in the wrench sensor body frame and robot space frame, respectively;
  • $f, m$ - wrench sensor force measurement offset and moment measurement offset, respectively;
  • $w, r$ - tool weight vector and tool center of mass vector from sensor body frame origin, respectively;
  • $\begin{bmatrix} \mathcal{X} \end{bmatrix}_{\times}$ - the skew-symmetric matrix of $\mathcal{X} \in \mathbb{R}^3$;
  • $F, M$ - wrench sensor force and moment measurement, respectively;
  • $\sideset{^b}{_s}{R}, \sideset{^s}{_b}{R}$ - rotation from space frame to body frame, and body to space frame, respectively;
  • $J, \alpha, \tau$ - tool inertia tensor, angular acceleration, effective torque applied to the tool, respectively;
  • $\mathcal{X}^{\text{LS}}$ - the least square inverse of $\mathcal{X}$, as in Least Squares, Minimum Norm, Pseudo-Inverse Solutions.

Force Measurement Offset and Tool Weight

For the sensor force measurement offset $\sideset{^b}{}{f}$, tool weight $\sideset{^s}{}{w}$, and sensor force measurement $\sideset{^b}{}{F}$, the following equation holds. $$ I \begin{bmatrix} \sideset{^b}{_x}{f} \\ \sideset{^b}{_y}{f} \\ \sideset{^b}{_z}{f} \end{bmatrix} + \sideset{^b}{_s}{R} \begin{bmatrix} \sideset{^s}{_x}{w} \\ \sideset{^s}{_y}{w} \\ \sideset{^s}{_z}{w} \end{bmatrix} = \begin{bmatrix} \sideset{^b}{_x}{F} \\ \sideset{^b}{_y}{F} \\ \sideset{^b}{_z}{F} \end{bmatrix} $$

Rearranging the above into the standard $Ax=b$ form, one gets, $$ \begin{bmatrix} I & \sideset{^b}{_s}{R} \end{bmatrix} \begin{bmatrix} \sideset{^b}{}{f} \\ \sideset{^s}{}{w} \end{bmatrix} = \sideset{^b}{}{F} $$ or with simplified notations, $$ \mathcal{A}^R \begin{bmatrix} f \\ w\end{bmatrix} = F $$

To compute the optimal $f$ and $w$, it is simply $$ \begin{bmatrix} \mathcal{A}^R_1 \\ \vdots \\ \mathcal{A}^R_n \end{bmatrix} \begin{bmatrix} f \\ w \end{bmatrix} \backsimeq \begin{bmatrix} F_1 \\ \vdots \\ F_n \end{bmatrix} \label{eq:force-sample} $$ with the least square solution $$ \begin{bmatrix} f \\ w \end{bmatrix} = \begin{bmatrix} \mathcal{A}^R_1 \\ \vdots \\ \mathcal{A}^R_n \end{bmatrix}^{\text{LS}} \begin{bmatrix} F_1 \\ \vdots \\ F_n \end{bmatrix} $$

Moment Measurement Offset and Tool Center of Mass

For the sensor moment measurement offset $\sideset{^b}{}{m}$, tool center of mass vector $\sideset{^b}{}{r}$, tool weight $\sideset{^s}{}{w}$ , and sensor's moment measurement $\sideset{^b}{}{M}$, the following equation holds. $$ I \begin{bmatrix} \sideset{^b}{_x}{m} \\ \sideset{^b}{_y}{m} \\ \sideset{^b}{_z}{m} \end{bmatrix} + \begin{bmatrix} \sideset{^b}{_x}{r} \\ \sideset{^b}{_y}{r} \\ \sideset{^b}{_z}{r} \end{bmatrix} \times \Big( \sideset{^b}{_s}{R} \begin{bmatrix} \sideset{^s}{_x}{w} \\ \sideset{^s}{_y}{w} \\ \sideset{^s}{_z}{w} \end{bmatrix} \Big) = \begin{bmatrix} \sideset{^b}{_x}{M} \\ \sideset{^b}{_y}{M} \\ \sideset{^b}{_z}{M} \end{bmatrix} $$ where $\sideset{^s}{}{w}$ is known, and ote the property of cross product, the above is equivalent to, $$ I \begin{bmatrix} \sideset{^b}{_x}{m} \\ \sideset{^b}{_y}{m} \\ \sideset{^b}{_z}{m} \end{bmatrix} - \sideset{^b}{_s}{R} \begin{bmatrix} \sideset{^s}{}{w} \end{bmatrix}_{\times} \begin{bmatrix} \sideset{^b}{_x}{r} \\ \sideset{^b}{_y}{r} \\ \sideset{^b}{_z}{r} \end{bmatrix} = \begin{bmatrix} \sideset{^b}{_x}{M} \\ \sideset{^b}{_y}{M} \\ \sideset{^b}{_z}{M} \end{bmatrix} $$

Rearranging the above into the standard $Ax=b$ form, one gets, $$ \begin{bmatrix} I & -\sideset{^b}{_s}{R} \begin{bmatrix} \sideset{^s}{}{w} \end{bmatrix}_{\times} \end{bmatrix} \begin{bmatrix} \sideset{^b}{}{m} \\ \sideset{^b}{}{r} \end{bmatrix} = \sideset{^b}{}{M} $$ or with simplified notations, $$ \mathcal{A}^{W} \begin{bmatrix} m \\ r \end{bmatrix} = M $$

To compute the optimal $m$ and $r$, it is simply $$ \begin{bmatrix} \mathcal{A}^W_1 \\ \vdots \\ \mathcal{A}^W_n \end{bmatrix} \begin{bmatrix} m \\ r \end{bmatrix} \backsimeq \begin{bmatrix} M_1 \\ \vdots \\ M_n \end{bmatrix} \label{eq:moment-sample} $$ with the least square solution $$ \begin{bmatrix} m \\ r \end{bmatrix} = \begin{bmatrix} \mathcal{A}^W_1 \\ \vdots \\ \mathcal{A}^W_n \end{bmatrix}^{\text{LS}} \begin{bmatrix} M_1 \\ \vdots \\ M_n \end{bmatrix} $$

Tool Inertia Tensor

For the tool inertia tensor $\sideset{^b}{}{J}$ in the sensor body frame $\{b\}$, an angular acceleration vector $\sideset{^b}{}{\alpha}$ in the sensor body frame $\{b\}$, and the actual torque causing tool rotation $\sideset{^b}{}{\tau}$ in the sensor body frame $\{b\}$, the following equation holds. $$ \sideset{^b}{}{J} \sideset{^b}{}{\alpha} = \sideset{^b}{}{\tau} \label{eq:inertia-tensor} $$ Expanding the above, one gets, $$ \begin{bmatrix} \sideset{^b}{_{11}}{J} & \sideset{^b}{_{12}}{J} & \sideset{^b}{_{13}}{J} \\ \sideset{^b}{_{21}}{J} & \sideset{^b}{_{22}}{J} & \sideset{^b}{_{23}}{J} \\ \sideset{^b}{_{31}}{J} & \sideset{^b}{_{32}}{J} & \sideset{^b}{_{33}}{J} \end{bmatrix} \begin{bmatrix} \sideset{^b}{_x}{\alpha} \\ \sideset{^b}{_y}{\alpha} \\ \sideset{^b}{_z}{\alpha} \end{bmatrix} = \begin{bmatrix} \sideset{^b}{_x}{\tau} \\ \sideset{^b}{_y}{\tau} \\ \sideset{^b}{_z}{\tau} \end{bmatrix} $$ where $$ \sideset{^b}{}{\tau} = \sideset{^b}{}{M} - \sideset{^b}{}{m} $$ and $\sideset{^b}{}{J}$ is symmetric positive definite. Note that in the above, the excitation signal angular acceleration $\sideset{^b}{}{\alpha}$ is designed and known, the tool inertia tensor $\sideset{^b}{}{J}$ is unknown and to be estimated.

Rearranging \eqref{eq:inertia-tensor} into the standard $Ax=b$ form, parameterize $J$ into vector form in this case, one gets, $$ \begin{bmatrix} \sideset{^b}{_x}{\alpha} & \sideset{^b}{_y}{\alpha} & \sideset{^b}{_z}{\alpha} & 0 & 0 & 0 & 0 & 0 & 0 \\ 0 & 0 & 0 & \sideset{^b}{_x}{\alpha} & \sideset{^b}{_y}{\alpha} & \sideset{^b}{_z}{\alpha} & 0 & 0 & 0 \\ 0 & 0 & 0 & 0 & 0 & 0 & \sideset{^b}{_x}{\alpha} & \sideset{^b}{_y}{\alpha} & \sideset{^b}{_z}{\alpha} \end{bmatrix} \begin{bmatrix} \sideset{^b}{_{11}}{J} \\ \sideset{^b}{_{12}}{J} \\ \sideset{^b}{_{13}}{J} \\ \sideset{^b}{_{21}}{J} \\ \sideset{^b}{_{22}}{J} \\ \sideset{^b}{_{23}}{J} \\ \sideset{^b}{_{31}}{J} \\ \sideset{^b}{_{32}}{J} \\ \sideset{^b}{_{33}}{J} \end{bmatrix} = \begin{bmatrix} \sideset{^b}{_x}{\tau} \\ \sideset{^b}{_y}{\tau} \\ \sideset{^b}{_z}{\tau} \end{bmatrix} $$ or, with simplified notations, $$ \mathcal{A} \mathcal{J} = \tau $$

To compute the optimal $\mathcal{J}$, it is simply $$ \begin{bmatrix} \mathcal{A}_1 \\ \vdots \\ \mathcal{A}_n \end{bmatrix} \mathcal{J} \backsimeq \begin{bmatrix} \tau_1 \\ \vdots \\ \tau_n \end{bmatrix} \label{eq:inertial-sample} $$ or $$ \mathcal{A}^{J} \mathcal{J} = \Gamma $$ One can then formulate and solve for the following QP-problem. $$ \begin{split} \min\,\,\, &|| \mathcal{A}^J \mathcal{J} - \Gamma || \\ \text{s.t.} \,\,\, &J_{ij} > 0 \\ &J_{12} - J_{21} = 0 \\ &J_{13} - J_{31} = 0 \\ &J_{23} - J_{32} = 0 \end{split} $$ where the equality and inequality constraints ensure that $\mathcal{J}$ is symmetric and positive definite.

Practical Considerations

When collecting samples for force in \eqref{eq:force-sample} and moment in \eqref{eq:moment-sample}, at the $i$-th robot configuration, one should stop the robot at each configuration and take the mean of multiple samples as $F_i$ and $M_i$ to reduce the impact of noise for better data quality. One should also choose robot configurations such that matrices $A^R$ and $A^W$ are full rank. When collecting samples for inertia tensor in \eqref{eq:inertial-sample}, angular acceleration is needed for the $\mathcal{A}$ matrix, it's not possible to stop the robot and take the mean of multiple samples to reduce the impact of noise.

To get the Cartesian angular acceleration for $A^J$, in additional to keeping it full-rank, one can choose to compute it via robot Jacobian $J_r$ and $$ \ddot{x} = \dot{J}_r\dot{q} + J_r \ddot{q} $$ or, one can choose trajectories with known Cartesian angular acceleration such as using 3D Lissajous Curves, where the acceleration can be easily computed analytically.

One should also note that due to control performance and measurement limitations, it is difficult to get perfect Cartesian angular accelerations, hence the estimation of $\mathcal{J}$ will not be as good as $f$, $w$, $m$, or $r$.