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:
Firstly, we successfully developed a new state planner given unrestricted motion in high degrees of freedom,
SLITHERS is generalizable and can be easily adjusted for mobile manipulators of multiple degrees of freedom,
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.
The lie_theory directory contains the main files and functions for solving inverse kinematics. The main functionality of the repository is located here.
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.
The coppeliasim directory contains the robot model files (urdf, dae, stl, etc).
The kinematics directory contains the orginal code and inspiration for shifting to the Lie Theory based optimization.
The export_fig directory contains a copy of export_fig for generating figures.
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
Lie theory is useful to model manipulator kinematics and dynamics due to is generic format and wide applications.
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}$$
