SLITHERS (State planner using LIe THEory for RoboticS)

Pictorial representation of using Lie theory to move across the Lie group using the corresponding Lie algebra. The motion in between each point on the Lie group (manifold) can be represented as a motion on the corresponding Lie algebra (tangent surface).

We present SLITHERS (State planner using LIe THEory for RoboticS), a Lie theory based optimization approach for state planning of mobile manipulators. Given the sequence of end effector poses, SLITHERS solves for the joint values incrementally using kinematic constraints found through Lie theory. The joint state values are estimated by solving a constrained optimization problem which yields the sequence of states given the current pose and desired pose. In this study, our contribution is three-folds:

1. Firstly, we successfully developed a new state planner given unrestricted motion in high degrees of freedom,
2. SLITHERS is generalizable and can be easily adjusted for mobile manipulators of multiple degrees of freedom,
3. and lastly SLITHERS guarantees smoothness in the joint states with unified motion between the base and manipulator.

The framework of SLITHERS which takes in a set of desired end effector poses, computes the desired state based off of an objective function, and executes the motion to achieve the desired states.

Directories

lie_theory

The lie_theory directory contains the main files and functions for solving inverse kinematics. The main functionality of the repository is located here.

coppeliasim

The coppeliasim directory contains the files and scenes to run simulations with the tested robot models. The code for simulations is set up to run open loop by precomputing the states and playing them back.

robot_models

The coppeliasim directory contains the robot model files (urdf, dae, stl, etc).

kinematic

The kinematics directory contains the orginal code and inspiration for shifting to the Lie Theory based optimization.

export_fig

The export_fig directory contains a copy of export_fig for generating figures.

Experimental Validation

Test paths used for the simulated experiment with the colored axes representing the desired pose of the end effector. The red, blue, and green axes represent the x, y, and z axes, respectively.

The error of the end effector on each of the different paths for both the position and orientation when using a non-holonomic mobile platform and computed by the proposed method.

The computed angular and linear velocity for the mobile manipulator with a non-holonomic base using the proposed method.

The computed joint states for the robotic manipulator mounted on the non-holonomic base using the proposed method.

The error of the end effector on each of the different paths for both the position and orientation when using a holonomic mobile platform and computed by the proposed method.

The computed angular and linear velocities for the mobile manipulator with a holonomic base using the proposed method. The forward and translational velocities represent the velocities in the x and y direction with respect to the base frame, respectively.

The computed joint states for the robotic manipulator mounted on the holonomic base using the proposed method.

Mobile Base Configuration	Non-holonomic			Holonomic
Trajectories	Vertical Helix	Sine Wave	Horizontal Helix	Vertical Helix	Sine Wave	Horizontal Helix
Position RMSE (m)	0.0415	0.0299	0.0031	0.0472	0.0147	0.0094
Rotation RMSE (rad)	0.0214	0.0159	0.0016	0.0199	0.2841	0.0011
Computation Time (s)	0.1862	0.1092	0.1216	0.1572	0.1075	0.1312
Max Forward Velocity (m/s)	0.8317	0.3004	3.6881	2.0000	1.4923	0.5514
Max Translation Velocity (m/s)	-	-	-	0.9933	0.9028	0.9650
Max Angular Velocity (rad/s)	2.5299	1.7195	0.6449	3.1416	2.4128	0.4185
Max Forward Acceleration (m/s$^2$)	1.1774	0.5491	1.1238	3.4888	3.1030	1.2341
Max Translation Acceleration (m/s$^2$)	-	-	-	0.6746	0.5749	0.4107
Max Ang Accel (rad/s$^2$)	5.8892	4.2186	1.2413	6.1535	5.1969	1.0681
Max Forward Jerk (m/s$^3$)	5.8309	1.5568	3.4703	24.6442	15.3703	11.4400
Max Translation Jerk (m/s$^3$)	-	-	-	1.1581	0.6296	0.3900
Max Angular Jerk (rad/s$^3$)	23.7865	19.4592	6.7439	36.1791	25.6212	3.4932
Max Joint Velocity (rad/s)	1.7848	1.8732	1.0609	6.1663	1.2688	2.6576
Max Joint Acceleration (rad/s$^2$)	1.9905	2.5079	1.1399	5.4608	1.4042	1.8988
Max Joint Jerk (rad/s$^3$)	10.0259	7.9320	2.6727	9.6929	2.7737	3.4553

Manipulator Kinematics

Lie theory is useful to model manipulator kinematics and dynamics due to is generic format and wide applications.

Screw Coordinates

A twist, $\mathbf{S} \in \mathbb{R}^6$, is a screw composed of two three dimensional vectors, the rotational and translational motion about an axis, as given by

$$\begin{equation} \mathbf{S} = \begin{Bmatrix} \boldsymbol{\omega} \\\ \mathbf{v} + \mathbf{d} \times \boldsymbol{\omega} \end{Bmatrix} \end{equation}$$

where $\boldsymbol{\omega} \in \mathbb{R}^3$ is the angular motion, $\mathbf{v} \in \mathbb{R}^3$ is the linear motion, and $\mathbf{d} \in \mathbb{R}^3$ is a translation from the reference frame to the joint. In a revolute joint ($\mathbf{v}=\mathbf{0}$), the twist element, $\mathbf{S}_{\omega}$, is defined by rotational motion and translational vectors as given by

$$\begin{equation} \mathbf{S}_{\omega} = \begin{Bmatrix} \boldsymbol{\omega} \\\ \mathbf{d} \times \boldsymbol{\omega} \end{Bmatrix} \end{equation}$$

A prismatic joint ($\boldsymbol{\omega}=\mathbf{0}$) has a twist element, $\mathbf{S}_{v}$, defined by the linear motion vector as given by

$$\begin{equation} \mathbf{S}_{v} = \begin{Bmatrix} \mathbf{0} \\\ \mathbf{v} \end{Bmatrix} \end{equation}$$

The defined screw coordinates can then be used to compute the forward and inverse kinematics from the fundamentals of Lie theory using the product of exponentials.

Lie Groups and Lie Algebra

In the case of a three dimensional rigid body transformation, $[\mathbf{P}] \in SE(3)$, any element in the Lie group can be defined by a rotation matrix, $[\mathbf{R}]$, and translation vector, $\mathbf{t}$, as given by

$$\begin{equation} [\mathbf{P}] = \begin{bmatrix} [\mathbf{R}] & \mathbf{t} \\\ \mathbf{0} & 1 \end{bmatrix} \end{equation}$$

The Lie algebra, $[\mathbf{S}]^{\wedge} \in \mathfrak{se}(3)$, for a homogeneous transformation is defined by the twist governing the motion and given by

$$\begin{equation} [\mathbf{S}]^{\wedge} = \begin{bmatrix} [\boldsymbol{\omega}]_{\times} & \mathbf{v} + \mathbf{d} \times \boldsymbol{\omega} \\\ \mathbf{0} & 0 \end{bmatrix} \end{equation}$$

where $[\cdot]_{\times}$ indicates a skew symmetric matrix.

The matrix exponential is used to map $[\mathbf{S}(q)]^{\wedge}$ onto the $SE(3)$ manifold changing the pose from $[\mathbf{P}_{0}]$ to $[\mathbf{P}]$ as given by

$$\begin{equation} [\mathbf{P}] = e^{[\mathbf{S(q)}]^{\wedge}} [\mathbf{P}_{0}] \end{equation}$$

where $[\mathbf{P}_{0}]$ is the configuration state at the identity and $S$ is a function of $q$, the joint state. For a manipulator, the product of exponentials is used to solve for the forward kinematics as given by

$$\begin{equation} [\textbf{P}_{n}^{0}] = \prod_{i \in n} \left( e^{[\mathbf{S}_{i}(q_{i})]^{\wedge}} \right) [\mathbf{P}_{0}] \end{equation}$$

where $n$ is the number of links in the robotic system.

Derivations of Forward Kinematics

Differential Drive Mobile Robot

Diagram of differential drive robot frame, $B$, located in the world frame, $W$, with states, $x,\ y,\ \mathrm{and}\ \phi$, and control inputs, $v\ \mathrm{and}\ \omega$. $$\begin{equation} \boldsymbol{\omega} = \begin{Bmatrix} 0 \\\ 0 \\\ \omega \end{Bmatrix} , \qquad \mathbf{v} = \begin{Bmatrix} v \\\ 0 \\\ 0 \end{Bmatrix} , \qquad \mathbf{d} = \begin{Bmatrix} 0 \\\ 0 \\\ 0 \end{Bmatrix} \\\ \end{equation}$$ $$\begin{equation} \mathbf{S} = \begin{Bmatrix} 0 \\\ 0 \\\ \omega \\\ v \\\ 0 \\\ 0 \end{Bmatrix} \end{equation}$$ $$\begin{equation} [\mathbf{S}]^{\wedge} = \begin{bmatrix} 0 & -\omega & 0 & v \\\ \omega & 0 & 0 & 0 \\\ 0 & 0 & 0 & 0 \\\ 0 & 0 & 0 & 0 \end{bmatrix} \end{equation}$$ $$\begin{equation} [\Delta\mathbf{P}^{B}_{B,k}] = \begin{bmatrix} \cos{\omega_k} & -\sin{\omega_k} & 0 & \frac{v_k}{\omega_k}\sin{\omega_k} \\\ \sin{\omega_k} & \cos{\omega_k} & 0 & \frac{v_k}{\omega_k}\left(1-\cos{\omega_k}\right) \\\ 0 & 0 & 1 & 0 \\\ 0 & 0 & 0 & 1 \end{bmatrix} dt \end{equation}$$ $$\begin{equation} [\mathbf{P}^{B}_{0}] = \begin{bmatrix} 1 & 0 & 0 & 0 \\\ 0 & 1 & 0 & 0 \\\ 0 & 0 & 1 & 0 \\\ 0 & 0 & 0 & 1 \end{bmatrix} \end{equation}$$ $$\begin{equation} [\mathbf{P}^{W}_{B,k+1}] = [\mathbf{P}^{W}_{B,k}][\Delta\mathbf{P}^{B}_{B,k}][\mathbf{P}^{B}_{0}] \end{equation}$$

Mecanum Drive Mobile Robot

Diagram of mecanum drive robot frame, $B$, located in the world frame, $W$, with states, $x,\ y,\ \mathrm{and}\ \phi$, and control inputs, $v_{x},\ v_{y},\ \mathrm{and}\ \omega$. $$\begin{equation} \boldsymbol{\omega} = \begin{Bmatrix} 0 \\\ 0 \\\ \omega \end{Bmatrix} , \qquad \mathbf{v} = \begin{Bmatrix} v_{x} \\\ v_{y} \\\ 0 \end{Bmatrix} , \qquad \mathbf{d} = \begin{Bmatrix} 0 \\\ 0 \\\ 0 \end{Bmatrix} \\\ \end{equation}$$ $$\begin{equation} \mathbf{S} = \begin{Bmatrix} 0 \\\ 0 \\\ \omega \\\ v_{x} \\\ v_{y} \\\ 0 \end{Bmatrix} \end{equation}$$ $$\begin{equation} [\mathbf{S}]^{\wedge} = \begin{bmatrix} 0 & -\omega & 0 & v_{x} \\\ \omega & 0 & 0 & v_{y} \\\ 0 & 0 & 0 & 0 \\\ 0 & 0 & 0 & 0 \end{bmatrix} \end{equation}$$ $$\begin{equation} [\Delta\mathbf{P}^{B}_{B,k}] = \begin{bmatrix} \cos{\omega_k} & -\sin{\omega_k} & 0 & \frac{v_{x,k}\sin{\omega_k}+v_{y,k}\left(\cos{\omega_k}-1\right)}{\omega_k} \\\ \sin{\omega_k} & \cos{\omega_k} & 0 & \frac{v_{y,k}\sin{\omega_k}+v_{x,k}\left(1-\cos{\omega_k}\right)}{\omega_k} \\\ 0 & 0 & 1 & 0 \\\ 0 & 0 & 0 & 1 \end{bmatrix} dt \end{equation}$$ $$\begin{equation} [\mathbf{P}^{B}_{0}] = \begin{bmatrix} 1 & 0 & 0 & 0 \\\ 0 & 1 & 0 & 0 \\\ 0 & 0 & 1 & 0 \\\ 0 & 0 & 0 & 1 \end{bmatrix} \end{equation}$$ $$\begin{equation} [\mathbf{P}^{W}_{B,k+1}] = [\mathbf{P}^{W}_{B,k}][\Delta\mathbf{P}^{B}_{B,k}][\mathbf{P}^{B}_{0}] \end{equation}$$

Industrial Robotic Manipulator

Diagram of Universal Robot UR5 with base frame, $B$, and end effector frame, $E$, in the zero position. Screw coordinates, $S_{i}\ \forall i \in n$, and joint angles, $\theta_{i}\ \forall i \in n$, are used to define kinematic model of the six link manipulator, $n=6$. $$\begin{equation} \boldsymbol{\omega}_{1} = \begin{Bmatrix} 0 \\\ 0 \\\ \theta_{1} \end{Bmatrix} , \mathbf{v}_{1} = \begin{Bmatrix} 0 \\\ 0 \\\ 0 \end{Bmatrix} , \mathbf{d}_{1} = \begin{Bmatrix} 0 \\\ 0 \\\ 0 \end{Bmatrix} \end{equation}$$ $$\begin{equation} \boldsymbol{\omega}_{2} = \begin{Bmatrix} 0 \\\ -\theta_{2} \\\ 0 \end{Bmatrix} , \mathbf{v}_{2} = \begin{Bmatrix} 0 \\\ 0 \\\ 0 \end{Bmatrix} , \mathbf{d}_{2} = \begin{Bmatrix} 0 \\\ 0 \\\ H_{1} \end{Bmatrix} \end{equation}$$ $$\begin{equation} \boldsymbol{\omega}_{3} = \begin{Bmatrix} 0 \\\ -\theta_{3} \\\ 0 \end{Bmatrix} , \mathbf{v}_{3} = \begin{Bmatrix} 0 \\\ 0 \\\ 0 \end{Bmatrix} , \mathbf{d}_{3} = \begin{Bmatrix} -L_{1} \\\ 0 \\\ H_{1} \end{Bmatrix} \end{equation}$$ $$\begin{equation} \boldsymbol{\omega}_{4} = \begin{Bmatrix} 0 \\\ -\theta_{4} \\\ 0 \end{Bmatrix} , \mathbf{v}_{4} = \begin{Bmatrix} 0 \\\ 0 \\\ 0 \end{Bmatrix} , \mathbf{d}_{4} = \begin{Bmatrix} -L_{1}-L_{2} \\\ 0 \\\ H_{1} \end{Bmatrix} \end{equation}$$ $$\begin{equation} \boldsymbol{\omega}_{5} = \begin{Bmatrix} 0 \\\ 0 \\\ -\theta_{5} \end{Bmatrix} , \mathbf{v}_{5} = \begin{Bmatrix} 0 \\\ 0 \\\ 0 \end{Bmatrix} , \mathbf{d}_{5} = \begin{Bmatrix} -L_{1}-L_{2} \\\ -W_{1} \\\ 0 \end{Bmatrix} \end{equation}$$ $$\begin{equation} \boldsymbol{\omega}_{6} = \begin{Bmatrix} 0 \\\ -\theta_{6} \\\ 0 \end{Bmatrix} , \mathbf{v}_{6} = \begin{Bmatrix} 0 \\\ 0 \\\ 0 \end{Bmatrix} , \mathbf{d}_{6} = \begin{Bmatrix} -L_{1}-L_{2} \\\ 0 \\\ H_{1}-H_{2} \end{Bmatrix} \end{equation}$$ $$\begin{equation} \mathbf{S}_{1} = \begin{Bmatrix} 0 & 0 & 1 & 0 & 0 & 0 \end{Bmatrix}^{\top} \theta_{1} \end{equation}$$ $$\begin{equation} \mathbf{S}_{2} = \begin{Bmatrix} 0 & -1 & 0 & H_{1} & 0 & 0 \end{Bmatrix}^{\top} \theta_{2} \end{equation}$$ $$\begin{equation} \mathbf{S}_{3} = \begin{Bmatrix} 0 & -1 & 0 & H_{1} & 0 & L_{1} \end{Bmatrix}^{\top} \theta_{3} \end{equation}$$ $$\begin{equation} \mathbf{S}_{4} = \begin{Bmatrix} 0 & -1 & 0 & H_{1} & 0 & L_{1}+L_{2} \end{Bmatrix}^{\top} \theta_{4} \end{equation}$$ $$\begin{equation} \mathbf{S}_{5} = \begin{Bmatrix} 0 & 0 & -1 & W_{1} & -L_{1}-L_{2} & 0 \end{Bmatrix}^{\top} \theta_{5} \end{equation}$$ $$\begin{equation} \mathbf{S}_{6} = \begin{Bmatrix} 0 & -1 & 0 & H_{1}-H_{2} & 0 & L_{1}-L_{2} \end{Bmatrix}^{\top} \theta_{6} \end{equation}$$ $$\begin{equation} [\mathbf{S}_{{1}}]^{\wedge} = \begin{bmatrix} 0 & -1 & 0 & 0 \\\ 1 & 0 & 0 & 0 \\\ 0 & 0 & 0 & 0 \\\ 0 & 0 & 0 & 0 \end{bmatrix} \theta_{1} \end{equation}$$ $$\begin{equation} [\mathbf{S}_{{2}}]^{\wedge} = \begin{bmatrix} 0 & 0 & 1 & H_{1} \\\ 0 & 0 & 0 & 0 \\\ -1 & 0 & 0 & 0 \\\ 0 & 0 & 0 & 0 \end{bmatrix} \theta_{2} \end{equation}$$ $$\begin{equation} [\mathbf{S}_{{3}}]^{\wedge} = \begin{bmatrix} 0 & 0 & 1 & H_{1} \\\ 0 & 0 & 0 & 0 \\\ -1 & 0 & 0 & L_{1} \\\ 0 & 0 & 0 & 0 \end{bmatrix} \theta_{3} \end{equation}$$ $$\begin{equation} [\mathbf{S}_{{4}}]^{\wedge} = \begin{bmatrix} 0 & 0 & 1 & H_{1} \\\ 0 & 0 & 0 & 0 \\\ -1 & 0 & 0 & L_{1}+L_{2} \\\ 0 & 0 & 0 & 0 \end{bmatrix} \theta_{4} \end{equation}$$ $$\begin{equation} [\mathbf{S}_{{5}}]^{\wedge} = \begin{bmatrix} 0 & -1 & 0 & W_{1} \\\ 1 & 0 & 0 & -L_{1}-L_{2} \\\ 0 & 0 & 0 & 0 \\\ 0 & 0 & 0 & 0 \end{bmatrix} \theta_{5} \end{equation}$$ $$\begin{equation} [\mathbf{S}_{{6}}]^{\wedge} = \begin{bmatrix} 0 & 0 & 1 & H_{1}-H_{2} \\\ 0 & 0 & 0 & 0 \\\ -1 & 0 & 0 & L_{1}+L_{2} \\\ 0 & 0 & 0 & 0 \end{bmatrix} \theta_{6} \end{equation}$$ $$\begin{equation} e^{[\mathbf{S}_{{1}}]^{\wedge}} = \begin{bmatrix} \cos{\theta_{1}} & -\sin{\theta_{1}} & 0 & 0 \\\ \sin{\theta_{1}} & \cos{\theta_{1}} & 0 & 0 \\\ 0 & 0 & 1 & 0 \\\ 0 & 0 & 0 & 1 \end{bmatrix} \end{equation}$$ $$\begin{equation} e^{[\mathbf{S}_{{2}}]^{\wedge}} = \begin{bmatrix} \cos{\theta_{2}} & 0 & \sin{\theta_{2}} & H_{1}\sin{\theta_{2}} \\\ 0 & 1 & 0 & 0 \\\ -\sin{\theta_{2}} & 0 & \cos{\theta_{2}} & H_{1}\left(\cos{\theta_{2}}-1\right) \\\ 0 & 0 & 0 & 1 \end{bmatrix} \end{equation}$$ $$\begin{equation} e^{[\mathbf{S}_{{3}}]^{\wedge}} = \begin{bmatrix} \cos{\theta_{3}} & 0 & \sin{\theta_{3}} & H_{1}\sin{\theta_{3}} + L_{1}\left(1-\cos{\theta_{3}}\right) \\\ 0 & 1 & 0 & 0 \\\ -\sin{\theta_{3}} & 0 & \cos{\theta_{3}} & H_{1}\left(\cos{\theta_{3}}-1\right) + L_{1}\sin{\theta_{3}} \\\ 0 & 0 & 0 & 1 \end{bmatrix} \end{equation}$$ $$\begin{equation} e^{[\mathbf{S}_{{4}}]^{\wedge}} = \begin{bmatrix} \cos{\theta_{4}} & 0 & \sin{\theta_{4}} & H_{1}\sin{\theta_{4}} + \left(L_{1}+L_{2}\right)\left(1-\cos{\theta_{4}}\right) \\\ 0 & 1 & 0 & 0 \\\ -\sin{\theta_{4}} & 0 & \cos{\theta_{4}} & H_{1}\left(\cos{\theta_{4}}-1\right) + \left(L_{1}+L_{2}\right)\sin{\theta_{4}} \\\ 0 & 0 & 0 & 1 \end{bmatrix} \end{equation}$$ $$\begin{equation} e^{[\mathbf{S}_{{5}}]^{\wedge}} = \begin{bmatrix} \cos{\theta_{5}} & -\sin{\theta_{5}} & 0 & W_{1}\sin{\theta_{5}}-\left(L_{1}+L_{2}\right)\left(\cos{\theta_{5}}-1\right) \\\ \sin{\theta_{5}} & \cos{\theta_{5}} & 0 & W_{1}\left(1-\cos{\theta_{5}}\right)-\left(L_{1}+L_{2}\right)\sin{\theta_{5}} \\\ 0 & 0 & 1 & 0 \\\ 0 & 0 & 0 & 1 \end{bmatrix} \end{equation}$$ $$\begin{equation} e^{[\mathbf{S}_{{6}}]^{\wedge}} = \begin{bmatrix} \cos{\theta_{6}} & 0 & \sin{\theta_{6}} & \left(H_{1}-H_{2}\right)\sin{\theta_{6}} + \left(L_{1}+L_{2}\right)\left(1-\cos{\theta_{6}}\right) \\\ 0 & 1 & 0 & 0 \\\ -\sin{\theta_{6}} & 0 & \cos{\theta_{6}} & \left(H_{1}-H_{2}\right)\left(\cos{\theta_{6}}-1\right) + \left(L_{1}+L_{2}\right)\sin{\theta_{6}} \\\ 0 & 0 & 0 & 1 \end{bmatrix} \end{equation}$$ $$\begin{equation} [\mathbf{P}^{B}_{0}] = \begin{bmatrix} 1 & 0 & 0 & -L_{1}-L_{2} \\\ 0 & 0 & -1 & -W_{1}-W_{2} \\\ 0 & 1 & 0 & H_{1}-H_{2} \\\ 0 & 0 & 0 & 1 \end{bmatrix} \end{equation}$$ $$\begin{equation} [\mathbf{P}^{B}_{6}] = e^{[\mathbf{S}_{{1}}]^{\wedge}}e^{[\mathbf{S}_{{2}}]^{\wedge}}e^{[\mathbf{S}_{{3}}]^{\wedge}}e^{[\mathbf{S}_{{4}}]^{\wedge}}e^{[\mathbf{S}_{{5}}]^{\wedge}}e^{[\mathbf{S}_{{6}}]^{\wedge}}[\mathbf{P}^{B}_{0}] \end{equation}$$

Mobile Manipulator with Non-Holonomic Mobile Base

Mobile manipulator consisting of a six degree of freedom industrial robotic manipulator (Universal Robots UR5e) mounted on a non-holonomic mobile platform (Clearpath Robotics Husky). $$\begin{equation} \boldsymbol{\omega}_{b} = \begin{Bmatrix} 0 \\\ 0 \\\ \omega \end{Bmatrix} , \mathbf{v}_{b} = \begin{Bmatrix} v \\\ 0 \\\ 0 \end{Bmatrix} , \mathbf{d}_{b} = \begin{Bmatrix} 0 \\\ 0 \\\ 0 \end{Bmatrix} \end{equation}$$ $$\begin{equation} \boldsymbol{\omega}_{1} = \begin{Bmatrix} 0 \\\ 0 \\\ \theta_{1} \end{Bmatrix} , \mathbf{v}_{1} = \begin{Bmatrix} 0 \\\ 0 \\\ 0 \end{Bmatrix} , \mathbf{d}_{1} = \begin{Bmatrix} 0 \\\ 0 \\\ 0 \end{Bmatrix} \end{equation}$$ $$\begin{equation} \boldsymbol{\omega}_{2} = \begin{Bmatrix} 0 \\\ -\theta_{2} \\\ 0 \end{Bmatrix} , \mathbf{v}_{2} = \begin{Bmatrix} 0 \\\ 0 \\\ 0 \end{Bmatrix} , \mathbf{d}_{2} = \begin{Bmatrix} 0 \\\ 0 \\\ H_{1} \end{Bmatrix} \end{equation}$$ $$\begin{equation} \boldsymbol{\omega}_{3} = \begin{Bmatrix} 0 \\\ -\theta_{3} \\\ 0 \end{Bmatrix} , \mathbf{v}_{3} = \begin{Bmatrix} 0 \\\ 0 \\\ 0 \end{Bmatrix} , \mathbf{d}_{3} = \begin{Bmatrix} -L_{1} \\\ 0 \\\ H_{1} \end{Bmatrix} \end{equation}$$ $$\begin{equation} \boldsymbol{\omega}_{4} = \begin{Bmatrix} 0 \\\ -\theta_{4} \\\ 0 \end{Bmatrix} , \mathbf{v}_{4} = \begin{Bmatrix} 0 \\\ 0 \\\ 0 \end{Bmatrix} , \mathbf{d}_{4} = \begin{Bmatrix} -L_{1}-L_{2} \\\ 0 \\\ H_{1} \end{Bmatrix} \end{equation}$$ $$\begin{equation} \boldsymbol{\omega}_{5} = \begin{Bmatrix} 0 \\\ 0 \\\ -\theta_{5} \end{Bmatrix} , \mathbf{v}_{5} = \begin{Bmatrix} 0 \\\ 0 \\\ 0 \end{Bmatrix} , \mathbf{d}_{5} = \begin{Bmatrix} -L_{1}-L_{2} \\\ -W_{1} \\\ 0 \end{Bmatrix} \end{equation}$$ $$\begin{equation} \boldsymbol{\omega}_{6} = \begin{Bmatrix} 0 \\\ -\theta_{6} \\\ 0 \end{Bmatrix} , \mathbf{v}_{6} = \begin{Bmatrix} 0 \\\ 0 \\\ 0 \end{Bmatrix} , \mathbf{d}_{6} = \begin{Bmatrix} -L_{1}-L_{2} \\\ 0 \\\ H_{1}-H_{2} \end{Bmatrix} \end{equation}$$ $$\begin{equation} \mathbf{S}_{b} = \begin{Bmatrix} 0 & 0 & \omega & v & 0 & 0 \end{Bmatrix}^{\top} \end{equation}$$ $$\begin{equation} \mathbf{S}_{1} = \begin{Bmatrix} 0 & 0 & 1 & 0 & 0 & 0 \end{Bmatrix}^{\top} \theta_{1} \end{equation}$$ $$\begin{equation} \mathbf{S}_{2} = \begin{Bmatrix} 0 & -1 & 0 & H_{1} & 0 & 0 \end{Bmatrix}^{\top} \theta_{2} \end{equation}$$ $$\begin{equation} \mathbf{S}_{3} = \begin{Bmatrix} 0 & -1 & 0 & H_{1} & 0 & L_{1} \end{Bmatrix}^{\top} \theta_{3} \end{equation}$$ $$\begin{equation} \mathbf{S}_{4} = \begin{Bmatrix} 0 & -1 & 0 & H_{1} & 0 & L_{1}+L_{2} \end{Bmatrix}^{\top} \theta_{4} \end{equation}$$ $$\begin{equation} \mathbf{S}_{5} = \begin{Bmatrix} 0 & 0 & -1 & W_{1} & -L_{1}-L_{2} & 0 \end{Bmatrix}^{\top} \theta_{5} \end{equation}$$ $$\begin{equation} \mathbf{S}_{6} = \begin{Bmatrix} 0 & -1 & 0 & H_{1}-H_{2} & 0 & L_{1}-L_{2} \end{Bmatrix}^{\top} \theta_{6} \end{equation}$$ $$\begin{equation} [\mathbf{S}_{{b}}]^{\wedge} = \begin{bmatrix} 0 & -\omega & 0 & v \\\ \omega & 0 & 0 & 0 \\\ 0 & 0 & 0 & 0 \\\ 0 & 0 & 0 & 0 \end{bmatrix} \end{equation}$$ $$\begin{equation} [\mathbf{S}_{{1}}]^{\wedge} = \begin{bmatrix} 0 & -1 & 0 & 0 \\\ 1 & 0 & 0 & 0 \\\ 0 & 0 & 0 & 0 \\\ 0 & 0 & 0 & 0 \end{bmatrix} \theta_{1} \end{equation}$$ $$\begin{equation} [\mathbf{S}_{{2}}]^{\wedge} = \begin{bmatrix} 0 & 0 & 1 & H_{1} \\\ 0 & 0 & 0 & 0 \\\ -1 & 0 & 0 & 0 \\\ 0 & 0 & 0 & 0 \end{bmatrix} \theta_{2} \end{equation}$$ $$\begin{equation} [\mathbf{S}_{{3}}]^{\wedge} = \begin{bmatrix} 0 & 0 & 1 & H_{1} \\\ 0 & 0 & 0 & 0 \\\ -1 & 0 & 0 & L_{1} \\\ 0 & 0 & 0 & 0 \end{bmatrix} \theta_{3} \end{equation}$$ $$\begin{equation} [\mathbf{S}_{{4}}]^{\wedge} = \begin{bmatrix} 0 & 0 & 1 & H_{1} \\\ 0 & 0 & 0 & 0 \\\ -1 & 0 & 0 & L_{1}+L_{2} \\\ 0 & 0 & 0 & 0 \end{bmatrix} \theta_{4} \end{equation}$$ $$\begin{equation} [\mathbf{S}_{{5}}]^{\wedge} = \begin{bmatrix} 0 & -1 & 0 & W_{1} \\\ 1 & 0 & 0 & -L_{1}-L_{2} \\\ 0 & 0 & 0 & 0 \\\ 0 & 0 & 0 & 0 \end{bmatrix} \theta_{5} \end{equation}$$ $$\begin{equation} [\mathbf{S}_{{6}}]^{\wedge} = \begin{bmatrix} 0 & 0 & 1 & H_{1}-H_{2} \\\ 0 & 0 & 0 & 0 \\\ -1 & 0 & 0 & L_{1}+L_{2} \\\ 0 & 0 & 0 & 0 \end{bmatrix} \theta_{6} \end{equation}$$ $$\begin{equation} [\Delta\mathbf{P}^{B}_{B,k}] = \begin{bmatrix} \cos{\omega_k} & -\sin{\omega_k} & 0 & \frac{v_k}{\omega_k}\sin{\omega_k} \\\ \sin{\omega_k} & \cos{\omega_k} & 0 & \frac{v_k}{\omega_k}\left(1-\cos{\omega_k}\right) \\\ 0 & 0 & 1 & 0 \\\ 0 & 0 & 0 & 1 \end{bmatrix} dt \end{equation}$$ $$\begin{equation} e^{[\mathbf{S}_{{1}}]^{\wedge}} = \begin{bmatrix} \cos{\theta_{1}} & -\sin{\theta_{1}} & 0 & 0 \\\ \sin{\theta_{1}} & \cos{\theta_{1}} & 0 & 0 \\\ 0 & 0 & 1 & 0 \\\ 0 & 0 & 0 & 1 \end{bmatrix} \end{equation}$$ $$\begin{equation} e^{[\mathbf{S}_{{2}}]^{\wedge}} = \begin{bmatrix} \cos{\theta_{2}} & 0 & \sin{\theta_{2}} & H_{1}\sin{\theta_{2}} \\\ 0 & 1 & 0 & 0 \\\ -\sin{\theta_{2}} & 0 & \cos{\theta_{2}} & H_{1}\left(\cos{\theta_{2}}-1\right) \\\ 0 & 0 & 0 & 1 \end{bmatrix} \end{equation}$$ $$\begin{equation} e^{[\mathbf{S}_{{3}}]^{\wedge}} = \begin{bmatrix} \cos{\theta_{3}} & 0 & \sin{\theta_{3}} & H_{1}\sin{\theta_{3}} + L_{1}\left(1-\cos{\theta_{3}}\right) \\\ 0 & 1 & 0 & 0 \\\ -\sin{\theta_{3}} & 0 & \cos{\theta_{3}} & H_{1}\left(\cos{\theta_{3}}-1\right) + L_{1}\sin{\theta_{3}} \\\ 0 & 0 & 0 & 1 \end{bmatrix} \end{equation}$$ $$\begin{equation} e^{[\mathbf{S}_{{4}}]^{\wedge}} = \begin{bmatrix} \cos{\theta_{4}} & 0 & \sin{\theta_{4}} & H_{1}\sin{\theta_{4}} + \left(L_{1}+L_{2}\right)\left(1-\cos{\theta_{4}}\right) \\\ 0 & 1 & 0 & 0 \\\ -\sin{\theta_{4}} & 0 & \cos{\theta_{4}} & H_{1}\left(\cos{\theta_{4}}-1\right) + \left(L_{1}+L_{2}\right)\sin{\theta_{4}} \\\ 0 & 0 & 0 & 1 \end{bmatrix} \end{equation}$$ $$\begin{equation} e^{[\mathbf{S}_{{5}}]^{\wedge}} = \begin{bmatrix} \cos{\theta_{5}} & -\sin{\theta_{5}} & 0 & W_{1}\sin{\theta_{5}}-\left(L_{1}+L_{2}\right)\left(\cos{\theta_{5}}-1\right) \\\ \sin{\theta_{5}} & \cos{\theta_{5}} & 0 & W_{1}\left(1-\cos{\theta_{5}}\right)-\left(L_{1}+L_{2}\right)\sin{\theta_{5}} \\\ 0 & 0 & 1 & 0 \\\ 0 & 0 & 0 & 1 \end{bmatrix} \end{equation}$$ $$\begin{equation} e^{[\mathbf{S}_{{6}}]^{\wedge}} = \begin{bmatrix} \cos{\theta_{6}} & 0 & \sin{\theta_{6}} & \left(H_{1}-H_{2}\right)\sin{\theta_{6}} + \left(L_{1}+L_{2}\right)\left(1-\cos{\theta_{6}}\right) \\\ 0 & 1 & 0 & 0 \\\ -\sin{\theta_{6}} & 0 & \cos{\theta_{6}} & \left(H_{1}-H_{2}\right)\left(\cos{\theta_{6}}-1\right) + \left(L_{1}+L_{2}\right)\sin{\theta_{6}} \\\ 0 & 0 & 0 & 1 \end{bmatrix} \end{equation}$$ $$\begin{equation} [\mathbf{P}^{B}_{0}] = \begin{bmatrix} 1 & 0 & 0 & -L_{1}-L_{2} \\\ 0 & 0 & -1 & -W_{1}-W_{2} \\\ 0 & 1 & 0 & H_{1}-H_{2} \\\ 0 & 0 & 0 & 1 \end{bmatrix} \end{equation}$$ $$\begin{equation} [\mathbf{P}^{W}_{E,k+1}] = [\mathbf{P}^{W}_{B,k}][\Delta\mathbf{P}^{B}_{B,k}]e^{[\mathbf{S}_{{1,k}}]^{\wedge}}e^{[\mathbf{S}_{{2,k}}]^{\wedge}}e^{[\mathbf{S}_{{3,k}}]^{\wedge}}e^{[\mathbf{S}_{{4,k}}]^{\wedge}}e^{[\mathbf{S}_{{5,k}}]^{\wedge}}e^{[\mathbf{S}_{{6,k}}]^{\wedge}}[\mathbf{P}^{B}_{0}] \end{equation}$$

Mobile Manipulator with Holonomic Mobile Base

Mobile manipulator consisting of a six degree of freedom industrial robotic manipulator (Universal Robots UR5e) mounted on a holonomic mobile platform. $$\begin{equation} \boldsymbol{\omega}_{b} = \begin{Bmatrix} 0 \\\ 0 \\\ \omega \end{Bmatrix} , \mathbf{v}_{b} = \begin{Bmatrix} v_{x} \\\ v_{y} \\\ 0 \end{Bmatrix} , \mathbf{d}_{b} = \begin{Bmatrix} 0 \\\ 0 \\\ 0 \end{Bmatrix} \end{equation}$$ $$\begin{equation} \boldsymbol{\omega}_{1} = \begin{Bmatrix} 0 \\\ 0 \\\ \theta_{1} \end{Bmatrix} , \mathbf{v}_{1} = \begin{Bmatrix} 0 \\\ 0 \\\ 0 \end{Bmatrix} , \mathbf{d}_{1} = \begin{Bmatrix} 0 \\\ 0 \\\ 0 \end{Bmatrix} \end{equation}$$ $$\begin{equation} \boldsymbol{\omega}_{2} = \begin{Bmatrix} 0 \\\ -\theta_{2} \\\ 0 \end{Bmatrix} , \mathbf{v}_{2} = \begin{Bmatrix} 0 \\\ 0 \\\ 0 \end{Bmatrix} , \mathbf{d}_{2} = \begin{Bmatrix} 0 \\\ 0 \\\ H_{1} \end{Bmatrix} \end{equation}$$ $$\begin{equation} \boldsymbol{\omega}_{3} = \begin{Bmatrix} 0 \\\ -\theta_{3} \\\ 0 \end{Bmatrix} , \mathbf{v}_{3} = \begin{Bmatrix} 0 \\\ 0 \\\ 0 \end{Bmatrix} , \mathbf{d}_{3} = \begin{Bmatrix} -L_{1} \\\ 0 \\\ H_{1} \end{Bmatrix} \end{equation}$$ $$\begin{equation} \boldsymbol{\omega}_{4} = \begin{Bmatrix} 0 \\\ -\theta_{4} \\\ 0 \end{Bmatrix} , \mathbf{v}_{4} = \begin{Bmatrix} 0 \\\ 0 \\\ 0 \end{Bmatrix} , \mathbf{d}_{4} = \begin{Bmatrix} -L_{1}-L_{2} \\\ 0 \\\ H_{1} \end{Bmatrix} \end{equation}$$ $$\begin{equation} \boldsymbol{\omega}_{5} = \begin{Bmatrix} 0 \\\ 0 \\\ -\theta_{5} \end{Bmatrix} , \mathbf{v}_{5} = \begin{Bmatrix} 0 \\\ 0 \\\ 0 \end{Bmatrix} , \mathbf{d}_{5} = \begin{Bmatrix} -L_{1}-L_{2} \\\ -W_{1} \\\ 0 \end{Bmatrix} \end{equation}$$ $$\begin{equation} \boldsymbol{\omega}_{6} = \begin{Bmatrix} 0 \\\ -\theta_{6} \\\ 0 \end{Bmatrix} , \mathbf{v}_{6} = \begin{Bmatrix} 0 \\\ 0 \\\ 0 \end{Bmatrix} , \mathbf{d}_{6} = \begin{Bmatrix} -L_{1}-L_{2} \\\ 0 \\\ H_{1}-H_{2} \end{Bmatrix} \end{equation}$$ $$\begin{equation} \mathbf{S}_{b} = \begin{Bmatrix} 0 & 0 & \omega & v_{x} & v_{y} & 0 \end{Bmatrix}^{\top} \end{equation}$$ $$\begin{equation} \mathbf{S}_{1} = \begin{Bmatrix} 0 & 0 & 1 & 0 & 0 & 0 \end{Bmatrix}^{\top} \theta_{1} \end{equation}$$ $$\begin{equation} \mathbf{S}_{2} = \begin{Bmatrix} 0 & -1 & 0 & H_{1} & 0 & 0 \end{Bmatrix}^{\top} \theta_{2} \end{equation}$$ $$\begin{equation} \mathbf{S}_{3} = \begin{Bmatrix} 0 & -1 & 0 & H_{1} & 0 & L_{1} \end{Bmatrix}^{\top} \theta_{3} \end{equation}$$ $$\begin{equation} \mathbf{S}_{4} = \begin{Bmatrix} 0 & -1 & 0 & H_{1} & 0 & L_{1}+L_{2} \end{Bmatrix}^{\top} \theta_{4} \end{equation}$$ $$\begin{equation} \mathbf{S}_{5} = \begin{Bmatrix} 0 & 0 & -1 & W_{1} & -L_{1}-L_{2} & 0 \end{Bmatrix}^{\top} \theta_{5} \end{equation}$$ $$\begin{equation} \mathbf{S}_{6} = \begin{Bmatrix} 0 & -1 & 0 & H_{1}-H_{2} & 0 & L_{1}-L_{2} \end{Bmatrix}^{\top} \theta_{6} \end{equation}$$ $$\begin{equation} [\mathbf{S}_{{b}}]^{\wedge} = \begin{bmatrix} 0 & -\omega & 0 & v_{x} \\\ \omega & 0 & 0 & v_{y} \\\ 0 & 0 & 0 & 0 \\\ 0 & 0 & 0 & 0 \end{bmatrix} \end{equation}$$ $$\begin{equation} [\mathbf{S}_{{1}}]^{\wedge} = \begin{bmatrix} 0 & -1 & 0 & 0 \\\ 1 & 0 & 0 & 0 \\\ 0 & 0 & 0 & 0 \\\ 0 & 0 & 0 & 0 \end{bmatrix} \theta_{1} \end{equation}$$ $$\begin{equation} [\mathbf{S}_{{2}}]^{\wedge} = \begin{bmatrix} 0 & 0 & 1 & H_{1} \\\ 0 & 0 & 0 & 0 \\\ -1 & 0 & 0 & 0 \\\ 0 & 0 & 0 & 0 \end{bmatrix} \theta_{2} \end{equation}$$ $$\begin{equation} [\mathbf{S}_{{3}}]^{\wedge} = \begin{bmatrix} 0 & 0 & 1 & H_{1} \\\ 0 & 0 & 0 & 0 \\\ -1 & 0 & 0 & L_{1} \\\ 0 & 0 & 0 & 0 \end{bmatrix} \theta_{3} \end{equation}$$ $$\begin{equation} [\mathbf{S}_{{4}}]^{\wedge} = \begin{bmatrix} 0 & 0 & 1 & H_{1} \\\ 0 & 0 & 0 & 0 \\\ -1 & 0 & 0 & L_{1}+L_{2} \\\ 0 & 0 & 0 & 0 \end{bmatrix} \theta_{4} \end{equation}$$ $$\begin{equation} [\mathbf{S}_{{5}}]^{\wedge} = \begin{bmatrix} 0 & -1 & 0 & W_{1} \\\ 1 & 0 & 0 & -L_{1}-L_{2} \\\ 0 & 0 & 0 & 0 \\\ 0 & 0 & 0 & 0 \end{bmatrix} \theta_{5} \end{equation}$$ $$\begin{equation} [\mathbf{S}_{{6}}]^{\wedge} = \begin{bmatrix} 0 & 0 & 1 & H_{1}-H_{2} \\\ 0 & 0 & 0 & 0 \\\ -1 & 0 & 0 & L_{1}+L_{2} \\\ 0 & 0 & 0 & 0 \end{bmatrix} \theta_{6} \end{equation}$$ $$\begin{equation} [\Delta\mathbf{P}^{B}_{B,k}] = \begin{bmatrix} \cos{\omega_k} & -\sin{\omega_k} & 0 & \frac{v_{x,k}\sin{\omega_k}+v_{y,k}\left(\cos{\omega_k}-1\right)}{\omega_k} \\\ \sin{\omega_k} & \cos{\omega_k} & 0 & \frac{v_{y,k}\sin{\omega_k}+v_{x,k}\left(1-\cos{\omega_k}\right)}{\omega_k} \\\ 0 & 0 & 1 & 0 \\\ 0 & 0 & 0 & 1 \end{bmatrix} dt \end{equation}$$ $$\begin{equation} e^{[\mathbf{S}_{{1}}]^{\wedge}} = \begin{bmatrix} \cos{\theta_{1}} & -\sin{\theta_{1}} & 0 & 0 \\\ \sin{\theta_{1}} & \cos{\theta_{1}} & 0 & 0 \\\ 0 & 0 & 1 & 0 \\\ 0 & 0 & 0 & 1 \end{bmatrix} \end{equation}$$ $$\begin{equation} e^{[\mathbf{S}_{{2}}]^{\wedge}} = \begin{bmatrix} \cos{\theta_{2}} & 0 & \sin{\theta_{2}} & H_{1}\sin{\theta_{2}} \\\ 0 & 1 & 0 & 0 \\\ -\sin{\theta_{2}} & 0 & \cos{\theta_{2}} & H_{1}\left(\cos{\theta_{2}}-1\right) \\\ 0 & 0 & 0 & 1 \end{bmatrix} \end{equation}$$ $$\begin{equation} e^{[\mathbf{S}_{{3}}]^{\wedge}} = \begin{bmatrix} \cos{\theta_{3}} & 0 & \sin{\theta_{3}} & H_{1}\sin{\theta_{3}} + L_{1}\left(1-\cos{\theta_{3}}\right) \\\ 0 & 1 & 0 & 0 \\\ -\sin{\theta_{3}} & 0 & \cos{\theta_{3}} & H_{1}\left(\cos{\theta_{3}}-1\right) + L_{1}\sin{\theta_{3}} \\\ 0 & 0 & 0 & 1 \end{bmatrix} \end{equation}$$ $$\begin{equation} e^{[\mathbf{S}_{{4}}]^{\wedge}} = \begin{bmatrix} \cos{\theta_{4}} & 0 & \sin{\theta_{4}} & H_{1}\sin{\theta_{4}} + \left(L_{1}+L_{2}\right)\left(1-\cos{\theta_{4}}\right) \\\ 0 & 1 & 0 & 0 \\\ -\sin{\theta_{4}} & 0 & \cos{\theta_{4}} & H_{1}\left(\cos{\theta_{4}}-1\right) + \left(L_{1}+L_{2}\right)\sin{\theta_{4}} \\\ 0 & 0 & 0 & 1 \end{bmatrix} \end{equation}$$ $$\begin{equation} e^{[\mathbf{S}_{{5}}]^{\wedge}} = \begin{bmatrix} \cos{\theta_{5}} & -\sin{\theta_{5}} & 0 & W_{1}\sin{\theta_{5}}-\left(L_{1}+L_{2}\right)\left(\cos{\theta_{5}}-1\right) \\\ \sin{\theta_{5}} & \cos{\theta_{5}} & 0 & W_{1}\left(1-\cos{\theta_{5}}\right)-\left(L_{1}+L_{2}\right)\sin{\theta_{5}} \\\ 0 & 0 & 1 & 0 \\\ 0 & 0 & 0 & 1 \end{bmatrix} \end{equation}$$ $$\begin{equation} e^{[\mathbf{S}_{{6}}]^{\wedge}} = \begin{bmatrix} \cos{\theta_{6}} & 0 & \sin{\theta_{6}} & \left(H_{1}-H_{2}\right)\sin{\theta_{6}} + \left(L_{1}+L_{2}\right)\left(1-\cos{\theta_{6}}\right) \\\ 0 & 1 & 0 & 0 \\\ -\sin{\theta_{6}} & 0 & \cos{\theta_{6}} & \left(H_{1}-H_{2}\right)\left(\cos{\theta_{6}}-1\right) + \left(L_{1}+L_{2}\right)\sin{\theta_{6}} \\\ 0 & 0 & 0 & 1 \end{bmatrix} \end{equation}$$ $$\begin{equation} [\mathbf{P}^{B}_{0}] = \begin{bmatrix} 1 & 0 & 0 & -L_{1}-L_{2} \\\ 0 & 0 & -1 & -W_{1}-W_{2} \\\ 0 & 1 & 0 & H_{1}-H_{2} \\\ 0 & 0 & 0 & 1 \end{bmatrix} \end{equation}$$ $$\begin{equation} [\mathbf{P}^{W}_{E,k+1}] = [\mathbf{P}^{W}_{B,k}][\Delta\mathbf{P}^{B}_{B,k}]e^{[\mathbf{S}_{{1,k}}]^{\wedge}}e^{[\mathbf{S}_{{2,k}}]^{\wedge}}e^{[\mathbf{S}_{{3,k}}]^{\wedge}}e^{[\mathbf{S}_{{4,k}}]^{\wedge}}e^{[\mathbf{S}_{{5,k}}]^{\wedge}}e^{[\mathbf{S}_{{6,k}}]^{\wedge}}[\mathbf{P}^{B}_{0}] \end{equation}$$