It's often convienent to represent an mobile robo trajectory as a spline, or for our purposes here, as a continuous piecwise polynomial. Here, I'm thinking about a robot that moves in the plane. Its configuration is described by position \(x\) and \(y\) and a yaw angle \(\theta\). Thus, our spline has three dimensions,
\begin{equation*} \begin{bmatrix} x(t) \\ y(t) \\ \theta(t) \end{bmatrix} = \begin{bmatrix} a_{x} + b_{x}t + c_{x}t^{2} + d_{x}t^{3} \\ a_{y} + b_{y}t + c_{y}t^{2} + d_{y}t^{3} \\ a_{\theta} + b_{\theta}t + c_{\theta}t^{2} + d_{\theta}t^{3} & \end{bmatrix} \end{equation*}Including \(\theta\) here allows for a holonomic robot that can, e.g., crab sideways. The queston we're interested in is:
Suppose we are given polynomials \([x(t), y(t) \theta(t)]\) in (e.g.) the ego frame of the robot. How do we transform them to (e.g.) the world frame?
I'm going to follow the notation used in these lecture notes on Rigid Body Kinematics (Slides). For example, if the pose of our robot in the world frame \(w\) is \(^{w}T_{ego}\) and we we have a goal pose \(p\) in the ego frame, \(^{ego}T_{p}\), then the goal pose in the world frame is given by \(^{w}T_{p}=\) \(^{w}T_{\cancel{ego}}\) \(^\cancel{ego}T_{p}\).
Each of these homogenous transforms has a rotation and translation and has a matrix representation
\begin{equation*} T =\begin{bmatrix} R(\theta) & d \\ 0 & 1 \end{bmatrix} \end{equation*}where \(R(\theta) \in SO(2)\) is a 2D rotation matrix and \(d \in \mathbb{R}^2\) is a vector representing the displacement. These transforms compose together through matrix multiplication.
To transform the polynomial \(^{ego}T(t)_{p}\) to the world frame means we need to apply the fixed transform \(^{w}T_{ego}\) to the time-varying transform \(^{ego}T(t)_{p}\) on the left. For the sake of brevity, I'm going to call these \(T_{1}\) (the fixed transform) and \(T_{2}(t)\), the time-varying transform. Thus, we want to work out
\begin{equation} T_{1}T_{2}(t) =\begin{bmatrix} R(\theta_1) & d_1 \\ 0 & 1 \end{bmatrix} \begin{bmatrix} R(\theta_2(t)) & d_2(t) \\ 0 & 1 \end{bmatrix} =\begin{bmatrix} R(\theta_1+\theta_2(t)) & R(\theta_1)d_2(t) + d_1 \\ 0 & 1 \end{bmatrix} \label{eqn:tv_transform} \end{equation}There are two cases of interest here. First, adding a constant to a polynomial means we simply add it to the \(a_{th}\) element, e.g.,
\begin{equation*} x(t) + d_{x} = (d_{1,x} + a_x) + b_x t + c_x t^2 + d_{x}t^{3}. \end{equation*}Similarly, the rotation matrix composition \(R(\theta_{1})R(\theta_{2}(t))\) is just
\begin{equation*} R(\theta_{1})R(\theta_{2}(t)) = (\theta_{1} + a_{\theta}) + b_{\theta}t + c_{\theta}t^{2} + d_{\theta}t^{3}. \end{equation*}Thus, the main complexity in \eqref{eqn:tv_transform} above is rotatating the x and y components,
\begin{aligned} R(\theta_{1}) d_{2}(t) &= R(\theta_{1}) \begin{bmatrix} x(t) \\ y(t) \end{bmatrix} \\ &= R(\theta_1) \begin{bmatrix} a_x + b_x t + c_x t^2 + d_{x}t^{3} \\ a_y + b_y t + c_y t^2 + d_{y}t^{3} \end{bmatrix}\\ &= R(\theta_1) \begin{bmatrix} a_x & b_x & c_x & d_{x} \\ a_y & b_y & c_y & d_{y} \end{bmatrix} \begin{bmatrix} 1 \\ t \\t^2 \\ t^3\end{bmatrix} \end{aligned}It follows that the trajectory spline components in the new frame are given by
\begin{aligned} \begin{bmatrix}\bar{x}(t)\\\bar{y}(t)\end{bmatrix} &= R(\theta_1) \begin{bmatrix} a_x & b_x & c_x & d_{x} \\ a_y & b_y & c_y & d_{y} \end{bmatrix} \begin{bmatrix} 1 \\ t \\t^2 \\ t^3\end{bmatrix} + \begin{bmatrix}d_{1,x}\\d_{1,y}\end{bmatrix}\\ %% %% \bar\theta(t) &= (\theta_1 + a_{\theta}) + b_{\theta}t + c_{\theta} t^2 + d_{\theta}t^3 \end{aligned}This is all relatively simple for basic polynomials (ie. in the Taylor or monomial basis). I've not worked it out, but it looks much more complicated for something like a BSpline.