2021

[1]Xinming Lu

[1]College of Computer Science and Engineering, Shandong University of Science
and Technology, 579 Qianwangang Road, Qingdao, 266590, Shandong Province,
China

2]Graduate School of Advanced Science and Engineering, Hiroshima University,
1-3-2 Kagamiyama, Higashi-Hiroshima City, 739-8527, Hiroshima, Japan

# Interactive Physically-Based Simulation of Roadheader Robot

Shengzhe Hou<EMAIL_ADDRESS><EMAIL_ADDRESS>Wenli Gao
<EMAIL_ADDRESS>Shuai Jiang<EMAIL_ADDRESS>Xingli Zhang
<EMAIL_ADDRESS>*  [

###### Abstract

Roadheader is an engineering robot widely used in underground engineering and
mining industry. Interactive dynamics simulation of roadheader is a
fundamental problem in unmanned excavation and virtual reality training.
However, current research is only based on traditional animation techniques or
commercial game engines. There are few studies that apply real-time physical
simulation of computer graphics to the field of roadheader robot. This paper
aims to present an interactive physically-based simulation system of
roadheader robot. To this end, an improved multibody simulation method based
on generalized coordinates is proposed. First, our simulation method describes
robot dynamics based on generalized coordinates. Compared to state-of-the-art
methods, our method is more stable and accurate. Numerical simulation results
showed that our method has significantly less error than the game engine in
the same number of iterations. Second, we adopt the symplectic Euler
integrator instead of the conventional fourth-order Runge-Kutta (RK4) method
for dynamics iteration. Compared with other integrators, our method is more
stable in energy drift during long-term simulation. The test results showed
that our system achieved real-time interaction performance of 60 frames per
second (fps). Furthermore, we propose a model format for geometric and
robotics modeling of roadheaders to implement the system. Our interactive
simulation system of roadheader meets the requirements of interactivity,
accuracy and stability.

###### keywords:

Physically-based simulation, Robotics simulation, Roadheader, Interactive
simulation, Numerical integration

##  1 Introduction

Roadheader is a kind of tunneling robot, one of the most important machinery
in the mining industry and underground engineering deshmukh2020roadheader .
Generally, underground engineering is very dangerous for people. To improve
safety, unmanned underground tunneling li2018intelligent  and Virtual Reality
(VR) based worker training grabowski2015virtual  have become a trend in recent
years. The development of interactive computer graphics technology, especially
physically-based robotics simulation, provides theoretical support for
achieving these targets. The dynamics-based three-dimensional (3D)
visualization system can not only simulate the working state of the roadheader
robot most physically but also provide a realistic user experience. Therefore,
physically-based robot simulation has become one of the core technologies of
digital twin bilberg2019digital . However, most of the current solutions are
based on kinematics animation grabowski2015virtual  or directly using game-
oriented commercial engines choi118use . This paper presents a dynamics-based
interactive roadheader simulation system that is accurate and stable.

Graphical robot simulation belongs to the interdisciplinary field of computer
graphics and robotics liu2021role . Interactive robot simulation aims to
compute and show the robot’s motion state in real-time based on articulated
rigid body dynamics (usually called multibody dynamics in applied mathematics
and mechanics) bender2014interactive . Since the 1980s, rigid body simulation
has been an important topic in computer graphics. Researchers from computer
science, robotics, and mechanics have made this field flourish, and then the
results have been applied to various industries luckcuck2019formal ;
inproceedings . Although single rigid body dynamics are well understood in
computer graphics, articulated rigid body simulation is still a very
challenging research field. Unlike keyframe animation, physical simulation
must conform to the laws of physics. To achieve interactivity, the computing
speed of the simulator must be fast enough. Therefore, the realization of
physically-based roadheader simulation has two main challenges: accuracy and
real-time. The metrics for the former generally include position error, energy
drift, etc. The latter usually requires robot simulators to reach 60 frames
per second (fps) or higher bender2014interactive . The difference in
preference for these two factors has led to different research fields. In
video games or other entertainment products, accuracy is sacrificed in pursuit
of real-time performance. In contrast, mechanical engineering simulation
systems usually take a long time to compute for accuracy. The balance of
accuracy and performance is an eternal topic in the field of interactive
simulation. Our goal is to achieve a sufficiently accurate robot simulation
system under the premise of satisfying interactivity.

A typical robot, like the roadheader, can be modeled as an articulated rigid
body system, which is composed of rigid bodies, called links, connected by
joints lynch2017modern . Although Newton’s second law laid the foundation of
dynamics, modern simulators generally do not directly use it to describe the
effect of force on bodies. As the complexity of the system increases,
equations based on Newtonian mechanics will become difficult to handle.
Inspired by analytical mechanics (including Lagrangian and Hamiltonian
mechanics), computer simulators generally adopt constraint-based dynamics.
Briefly, the task of the simulator is to make the system meet the established
constraints at each time step iteration bargteil2019introduction .

The researches on constraint-based rigid body dynamics can be divided into two
categories: the maximal coordinates formulation and the generalized (also
named reduced or minimal) coordinates formulation. The former uses the
original coordinates to represent rigid body motion and adds auxiliary
conditions to define constraints. In contrast, the generalized coordinates
formulation uses a minimal set of coordinates to describe constrained
dynamics. The maximal coordinates methods are widely used in game engines,
such as Bullet coumans2013bullet , ODE smith2005open , etc. However, methods
of maximal coordinates are all approximations to dynamics and are difficult to
meet the requirements of robotics for accuracy and stability. In the field of
industrial research and application, methods of generalized coordinates have
many advantages: not only accuracy is higher, but computation speed is also
sufficient. The theoretical basis of generalized coordinates methods,
analytical mechanics, can more essentially describe the nature of dynamics.
Therefore, algorithms based on generalized coordinates are adopted by our
roadheader simulation system.

Figure 1: The framework of roadheader robot simulation system.

Rigid multibody dynamics algorithms based on generalized coordinates is a key
part of our roadheader simulation system. We use space algebra based on 6-D
vectors to describe related dynamic theories and algorithms. Traditional
graphics simulation processes the linear and rotation movement of rigid bodies
separately. That will cause the simulation algorithm and implementation to be
very cumbersome. The language of space algebra describes dynamics from a more
essential perspective and is more concise. After introducing the basic theory
of space algebra, we derived the dynamic expression of a single rigid body.
Then, the equation of motion of multibody dynamics is introduced. The dynamic
algorithms of the simulation system aim to solve the unknown variables in the
equation of motion. Recursive Newton-Euler Algorithm and Composite Rigid Body
Algorithm are adopted by our system featherstone2014rigid . In recent years,
the theoretical research of these algorithms is developing rapidly
agarwal2014dynamics ; korayem2014systematic . Related applications have also
been studied extensively korayem2015motion . We apply these algorithms to
real-time simulation of roadheader robot. Numerical experiment results show
that the simulation accuracy of our system is better than the solution based
on a game engine (using maximal coordinates).

The type of numerical integrator is an important factor affecting the
simulation stability. In each iteration of the simulation process, the
integrator will integrate the current acceleration of the robot obtained by
the dynamic algorithm to obtain the current position. An ideal simulator
guarantees the conservation of energy. However, energy drift caused by
numerical error is inevitable in every integration. In most interactive
graphics applications, the fourth-order Runge-Kutta (RK4) method can make a
good trade-off between performance and accuracy. Therefore, RK4 integrator has
been widely used in video games and other fields. However, RK4 is not an
energy preserving method. The energy drift generated by the RK4 method will
gradually accumulate, which makes it unsuitable for long-term robot
simulation. We propose to use the symplectic Euler (also called semi-implicit
Euler), an energy preserving method, as the integrator of our roadheader
simulation system. In the energy drift test, the symplectic Euler integrator
is more stable than other methods.

As shown in Figure 1, the roadheader robot simulation system is divided into
three parts: roadheader modelling, simulator, and user interface. We propose
robotics model and geometric model to model physical information and graphics
information of roadheader, respectively. Robotics model will be encoded by the
simulator as the equation of motion for multibody system. Our goal is to get
the unknown acceleration $\ddot{\bm{q}}$ . User interaction will change the
right-hand term $\bm{\tau}$ . Algorithms 1 and 2 are used to solve part of
this equation. The numerical integrator will get the current position
${\bm{q}}$ based on $\ddot{\bm{q}}$. The renderer will visualize the
simulation results based on the current position ${\bm{q}}$ and the geometric
model.

Our contribution is mainly in three aspects:

  * •

We propose to use the rigid multibody dynamics algorithm based on generalized
coordinates to simulate the roadheader robot.

  * •

A symplectic Euler integrator instead of traditional RK4 is adopted to achieve
better simulation stability.

  * •

A roadheader simulator is implemented to meet the requirements of
interactivity, accuracy and stability.

Numerical tests show that our system meets the interactive performance
requirements, and has better accuracy and stability in long-term simulations.

The paper is structured as follows. In Section 2, we briefly review the
research on roadheader simulation and robot interactive algorithms. In Section
3, we describe the theoretical basis and algorithms of rigid multibody
dynamics and the principle of numerical integrators. In Section 4, the
geometry and robotics modeling methods of the roadheader are introduced. In
Section 5, the architecture and implementation of our roadheader simulation
system are described. In Section 6, we test the system’s performance, accuracy
(compared with a game engine), and energy stability (compared with other
numerical integrators) through numerical experiments. Finally, conclusions of
this work and future research are summarized in Section 8.

##  2 Related Work

###  2.1 Roadheader robot simulation

In recent years, information technology, especially virtual reality
technology, has been widely used in traditional mining engineering
bellanca2019developing ; van2009virtual . Roadheader is an indispensable robot
equipment in mining engineering. Improving the safety of roadheaders,
especially unmanned operation and training, has been a research focus in
recent years yan2019multi . However, the existing research only focuses on the
basic kinematics tian2018kinematic ; yan2019multi  or the use of remote video
to manually control the roadheader wang2017recent , and the dynamics have not
been considered. According to the idea of digital twin, the key to efficient
and safe human-robot interaction is virtual simulation of robots
malik2018digital . Robot simulation technology based on interactive computer
graphics makes it possible to create a virtual robot environment.

Although robot simulation technology has been widely used in video games and
movies, there is little research in traditional industrial fields, especially
mining engineering. Some studies only used simple animations without
considering robot dynamics marshall2016robotics , or simple secondary
development of commercial platforms zhang2017head ; andersen2020mets . The
system proposed in this paper is based on robot dynamics and graphical
visualization techniques. Our system is more realistic and reliable than non-
physical visualization methods. Compared to commercial physics engine based
solutions, our method is more stable and accurate.

###  2.2 Dynamics modeling methods

Robot simulation theory is derived from classic rigid body mechanics and is
still active in computer graphics. The current robot simulation algorithms are
generally divided into two categories: maximal coordinates methods and
generalized coordinates methods. In addition, some non-physical simulation
methods, such as Position Based Dynamics muller2020detailed ; deul2016position
, have also been applied in recent years. But the accuracy of these methods is
difficult to meet the requirements of robot simulation. Currently, the most
widely used methods in the maximal coordinates can be roughly divided into two
categories: penalty methods and Lagrange multipliers bender2014interactive .
Penalty methods satisfy constraints by adding spring-like penalty forces. This
type of method is easy to implement but has poor accuracy. The method of
Lagrange multipliers explicitly computes constraint forces to satisfy system
constraints. The impulse-based method is similar to the Lagrangian multiplier
method and is used in many game engines, such as Bullet coumans2013bullet . In
short, all current methods based on maximal coordinates are approximately
accurate. The maximal coordinate method works well in video games, but it is
difficult to meet the accuracy requirements in industrial simulators.

To achieve better accuracy and stability, our system is based on generalized
coordinates algorithms instead of these maximal coordinates methods. The idea
of generalized coordinates originated in analytical mechanics
goldstein2002classical . Robotics and graphics have developed a variety of
algorithms based on the idea of generalized coordinates featherstone2014rigid
; wang2019redmax . Accordingly, the mathematics of rigid body dynamics also
vary murray2017mathematical . Our approach is rigid multibody dynamics based
on spatial algebra using 6-D vectors featherstone2010beginner1 . Compared with
traditional coordinate systems, our method is more concise. The dynamics
algorithms are applied to the simulation of roadheader robot. In the same
simulation process, our system is more accurate and stable than commercial
game engine.

###  2.3 Time integrators

A system of nonlinear differential equations (ODEs) was established by the
robot dynamics simulation algorithms. These differential equations are
difficult to solve analytically, so numerical integrators must be used to
solve them step by step. The explicit Euler method (also known as forward
Euler method) is the simplest ODE solver. Each iteration of explicit Euler
integrator uses only the gradient of the system and the state of the previous
step. Although explicit Euler is highly efficient and easy to implement, it
introduces additional capabilities (called numerical explosions) at each
iteration hairer2006geometric . Implicit methods (e.g. implicit Euler methods)
have higher stability, but lead to energy decay. Most importantly, implicit
methods take a lot of time to compute the implicit equation, so it is not
suitable for interactive simulation which requires high real-time performance
house2016foundations ; hairer2006geometric . To improve the accuracy and
stability of explicit Euler method, many higher order methods have been
adopted by physically-based animation field. Currently, the fourth-order
Runge-Kutta (RK4) method is widely used in virtual simulation and game
engines. While RK4 performs well in most situations, balancing performance
with accuracy, it is still an explicit approach butcher2008numerical . That
is, RK4 will inevitably introduce additional energy and lead to simulation
errors. This is not to be ignored in the long-term simulation process. In
addition, higher order methods have been applied to graphical simulation
loschner2020higher .

The rigid multibody system of robotics is a typical Hamiltonian system. In
recent years, symplectic integrators based on Hamiltonian mechanics have
attracted the attention of many researchers in robotics sharma2020review  and
graphics xu2014implicit ; kobilarov2009lie . The semi-implicit Euler (also
called symplectic Euler) method we adopt is a first-order symplectic method,
which can have both performance and stability. Various order integrators
(including explicit Euler, explicit midpoint, RK4, and RK6) are used to
compare our method. Experiments show that our method performs better in the
energy conservation of long-term simulation.

##  3 Methods

In this section, the theoretical background and core algorithms of interactive
roadheader robot simulation are described. The space algebra using 6-D vectors
is introduced as the theoretical foundation of rigid body dynamics. Then, the
dynamics of a single rigid body based on 6-D vector language are described.
After describing the multibody dynamics, two key dynamics algorithms for our
simulation system are introduced. Finally, the related numerical integration
methods and their properties are introduced.

###  3.1 Rigid Multibody Dynamics

####  3.1.1 Spatial Algebra using 6-D Vectors

Spatial algebra based on 6-D vectors is a concise and efficient tool for
describing dynamics. In traditional computer dynamics simulation, the linear
motion and rotational motion of a rigid body are represented separately. This
will cause the dynamic equations to be very cumbersome and fragmented.
Considering that a rigid body in three-dimensional space has six degrees of
freedom (DOF), it is more appropriate to directly use six variables to
represent the state of a rigid body. Some physicists and roboticists have
since proposed various 6-D representation tools, such as screw theory, Lie
algebra, and motor algebra. These methods have different perspectives, but
they are essentially equivalent. In this paper, we use 6-D vectors based on
Plücker coordinates for easier programming. A more complete introduction is in
featherstone2010beginner1 .

The 6-D vectors does not belong to Euclidean space. There are two vector
spaces as the basis of space algebra: spatial motion vector space $\bm{M}^{6}$
and spatial force vector space $\bm{F}^{6}$. The elements of the two spaces
are: spatial motion vectors $\hat{\bm{v}}\in\mathrm{M}^{6}$ and space force
vectors $\hat{\bm{f}}\in\bm{F}^{6}$. The spaces $\bm{M}^{6}$ and $\bm{F}^{6}$
are equivalent to Lie algebras $\mathfrak{se}(3)$ and $\mathfrak{se}^{*}(3)$.
We introduce an arbitrary but fixed reference point $O$. The Plücker
coordinate bases of these two vector spaces are
$\mathcal{D}_{O}:\mathbb{R}^{6}\rightarrow\mathrm{M}^{6}$ and
$\rm{E}_{O}:\mathbb{R}^{6}\rightarrow\rm{F}^{6}$, respectively. The
$\hat{\bm{v}}$ can be written in this form:

$\displaystyle\hat{\bm{v}}=\left[\begin{array}[]{c}\omega\\\
v_{O}\end{array}\right]=\left[\omega_{x},\omega_{y},\omega_{z},v_{Ox},v_{Oy},v_{Oz}\right]^{T},$
(3)

where $\omega\in\mathbb{R}^{3}$ and $v_{O}\in\mathbb{R}^{3}$ represent the
angular and linear velocity of a rigid body, respectively. Similarly, the
$\hat{f}$ can be written in this form:

$\displaystyle\hat{\bm{f}}=\left[\begin{array}[]{c}n_{O}\\\
f\end{array}\right]=\left[n_{Ox},n_{Oy},n_{Oz},f_{x},f_{y},f_{z}\right]^{T}$
(6)

where $n_{O}\in\mathbb{R}^{3}$ and $f\in\mathbb{R}^{3}$ represent the total
moment and linear force of a rigid body, respectively. All these variables are
defined according to the reference point $O$. The essence of the spatial
algebra is that a pair of $\mathbb{R}^{3}$ vectors simultaneously describe the
linear and angular configuration of a rigid body.

The transformation between different coordinate frames is the key to rigid
body dynamics. $\bm{M}^{6}$ and $\bm{F}^{6}$ are dual space of each other.
Correspondingly, the Plücker bases $\mathcal{D}_{O}$ and $\mathcal{E}_{O}$
define a dual coordinate system on $\bm{M}^{6}$ and $\bm{F}^{6}$. Let $A$ and
$B$ be two Plücker coordinate system. Let ${}^{A}\hat{\bm{f}}\in\bm{F}^{6}$,
${}^{A}\hat{\bm{m}}\in\bm{M}^{6}$ in $A$ and
${}^{B}\hat{\bm{f}}\in\bm{F}^{6}$, ${}^{B}\hat{\bm{m}}\in\bm{M}^{6}$ in $B$.
The coordinate transformation for motion vectors is as follows:

$\displaystyle{}^{B}\hat{\bm{m}}={}^{B}\bm{X}_{A}{}^{A}\hat{\bm{m}}$ (7)

where ${}^{B}\bm{X}_{A}{}$ is the transformation matrix from $A$ to $B$. Due
to the duality between $\bm{M}^{6}$ and $\bm{F}^{6}$, the corresponding
transformation for force vectors is called ${}^{B}\bm{X}_{A}^{*}{}$. The
coordinate transformation for force vectors is as follows:

$\displaystyle{}^{B}\hat{\bm{f}}={}^{B}\bm{X}_{A}^{*}{}^{A}\hat{\bm{f}}.$ (8)

Let $r\in\mathbb{R}^{3}$ and $E\in\mathbb{R}^{3\times 3}$ are position and
rotation transform from $A$ to $B$, respectively. ${}^{B}\bm{X}_{A}{}$ and
${}^{B}\bm{X}_{A}^{*}{}$ are given by

$\displaystyle{}^{B}\bm{X}_{A}{}=\left[\begin{array}[]{cc}E&0\\\
-Er\times&E\end{array}\right],\quad^{B}\bm{X}_{A}^{*}=\left[\begin{array}[]{cc}E&-Er\times\\\
0&E\end{array}\right].$ (13)

####  3.1.2 Single Rigid Body Dynamics

A rigid body is an idealized solid model without deformation. In other words,
the relative position of any pair of points of a rigid body will never change.
Unlike a mass point, a rigid body has not only a linear configuration but also
a rotational configuration. Therefore, at least 6 parameters are required to
determine the configuration of a rigid body in $\mathbb{R}^{3}$ space. That
is, the degrees of freedom of a spatial rigid body is 6. The mass of a rigid
body is distributed over its volume. In physics, the inertia tensor is usually
used to describe the mass distribution of rigid bodies. In the framework of
spatial algebra using 6-D vectors, spatial inertia is used to replace the
inertia tensor. The spatial inertia
$\hat{\bm{I}}:\bm{M}^{6}\rightarrow\bm{F}^{6}$ for a rigid body is given by

$\displaystyle\hat{\bm{I}}=\left[\begin{array}[]{cc}\hat{\bm{I}}_{C}+mc\times
c\times^{T}&mc\times\\\ mc\times^{T}&m1\end{array}\right]$ (16)

where $m$ is the mass of the body, $c$ is the coordinate of the body’s center
of mass, and $\hat{\bm{I}}_{C}$ is the body’s inertia about its center of
mass. The spatial inertia is a positive-definite, symmetric $6\times 6$
matrix. Analogous to transformations between different coordinate frame for
motions and forces, we can define transformation for spatial inertia by:

$\displaystyle{}^{B}\hat{\bm{I}}={}^{B}\bm{X}_{A}^{*A}\hat{\bm{I}}^{A}\bm{X}_{B}$
(17)

where ${}^{A}\hat{\bm{I}}$ in coordinate frame $A$ and ${}^{B}\hat{\bm{I}}$ in
coordinate frame $B$.

Given spatial inertia $\hat{\bm{I}}$ and spatial velocity $\hat{\bm{v}}$, we
can define the momentum $\hat{\bm{h}}\in\bm{F}^{6}$ of a rigid body, using the
formula

$\displaystyle\hat{\bm{h}}=\hat{\bm{I}}\hat{\bm{v}}.$ (18)

Analogous to classical mechanics, acceleration is defined by the time
derivative of velocity, we can define the spatial acceleration of a rigid body
by the time derivative of its spatial velocity. Given the spatial velocity
$\hat{\bm{v}}$, then the spatial acceleration is the time derivative of
$\hat{\bm{v}}$:

$\displaystyle\hat{\bm{a}}=\frac{\mathrm{d}}{\mathrm{d}t}\hat{\bm{v}}=\frac{\mathrm{d}}{\mathrm{d}t}\left[\begin{array}[]{c}\omega\\\
v_{O}\end{array}\right]=\left[\begin{array}[]{c}\dot{{\omega}}\\\
\ddot{{r}}-{\omega}\times\dot{{r}}\end{array}\right].$ (23)

The time derivative of the spatial inertia $\hat{\bm{I}}$ is:

$\displaystyle\frac{\mathrm{d}}{\mathrm{d}t}\hat{\bm{I}}=\hat{\bm{v}}\times^{*}\hat{\bm{I}}-\hat{\bm{I}}\hat{\bm{v}}\times$
(24)

With these basic concepts of space algebra mentioned above, we can use an
equation, called equation of motion, to describe the state of a rigid body in
three-dimensional space. Let $\hat{\bm{v}}$, $\hat{\bm{a}}$, and
$\hat{\bm{I}}$ are spatial velocity, spatial acceleration, and spatial inertia
of a rigid body, respectively. The equation of motion of the rigid body is
given by the formula

$\displaystyle\hat{\bm{f}}$
$\displaystyle=\frac{\mathrm{d}}{\mathrm{d}t}(\hat{\bm{h}})$
$\displaystyle=\frac{\mathrm{d}}{\mathrm{d}t}(\hat{\bm{I}}\hat{\bm{v}})$
$\displaystyle=\hat{\bm{I}}\hat{\bm{a}}+(\hat{\bm{v}}\times^{*}\hat{\bm{I}}-\hat{\bm{I}}\hat{\bm{v}}\times)\hat{\bm{v}}$
$\displaystyle=\hat{\bm{I}}\hat{\bm{a}}+\hat{\bm{v}}\times^{*}\hat{\bm{I}}\hat{\bm{v}}.$
(25)

where $\hat{\bm{f}}$ is the spatial force on the rigid body.

####  3.1.3 Multibody Dynamics Algorithms

A multibody system consists of rigid bodies connected by joints. In this
paper, we assume that each joint connects two different rigid bodies. Each
joint is essentially a holonomic kinematic constraint, which reduces the
degree of freedom of the connected rigid body. A free rigid body has a degree
of freedom of 6 in three-dimensional space, so it can move in any direction.
The degree of freedom of a rigid body connected by a joint is reduced, which
usually means that it is forbidden to move in certain directions. In fact, the
effect on rigid bodies is caused by the constraint forces.

A joint is usually designed to allow 0 to 6 degrees of freedom. In our
roadheader simulation system, two joints, hinge and slider, are mainly
considered. The degrees of freedom of these two joints are both 1. The motion
subspace matrix $\bm{S}\subset\bm{M}^{6}$ is used to define a joint
constraint. Let $n_{f}$ be the degrees of freedom, that is, the number of
constraints imposed by it is $6-n_{f}$, then $\bm{S}$ is $6\times n_{f}$. The
rotate (revolute) joint and slider (prismatic) joint are applied to our
roadheader robot simulation system. The detailed matrices for the joints are
listed in Section 4.

The motion of a multibody system can be described by an equation of motion
like the single body above. Let $\bm{q}$, $\dot{\bm{q}}$, and $\ddot{\bm{q}}$
are generalized positions, generalized velocities, and generalized
accelerations of a multibody system, the equation of motion for the system in
canonical form is as follows:

$\displaystyle\bm{H}(\bm{q})\ddot{\bm{q}}+\bm{C}(\bm{q},\dot{\bm{q}})=\bm{\tau}$
(26)

where $\bm{H}(\bm{q})$ is the generalized inertia matrix,
$\bm{C}(\bm{q},\dot{\bm{q}})$ is the generalized bias force (Coriolis and
centrifugal forces) matrix, and $\bm{\tau}$ is the generalized external forces
(e.g. gravity, user-defined joint forces, etc.). Our main goal is to solve 26
to get the acceleration $\ddot{\bm{q}}$. We will use the Recursive Newton-
Euler Algorithm (RNEA) and Composite Rigid Body Algorithm (CRBA) to get
$\bm{H}$ and $\bm{C}$ respectively. Given the known variables $\bm{H}$,
$\bm{C}$ and $\bm{\tau}$, we have a simple linear equation
$\bm{H}\ddot{\bm{q}}=\bm{\tau}-\bm{C}$. The acceleration $\ddot{\bm{q}}$ can
be obtained directly by using any linear equation solution (such as Cholesky
decomposition press2007numerical ). Then the numerical integrator will use
$\ddot{\bm{q}}$ to get $\dot{\bm{q}}$ and $\bm{q}$ to complete one step of the
simulation.

Algorithm 1 Recursive Newton-Euler Algorithm

1:Robot state $\bm{q},\dot{\bm{q}}$

2:Generalized bias term $\bm{C}$

3:$\bm{v}_{0}=\mathbf{0}$

4:$\bm{a}_{0}=-\bm{a}_{g}$

5:for $i=1,2,\ldots n_{B}$ do

6:
$\left[\bm{X}_{\mathbf{J}i},\bm{S}_{i},\bm{c}_{\mathbf{J}i}\right]=\operatorname{jcalc}\left(\operatorname{jtype}(i),\bm{q}_{i},\dot{\bm{q}}_{i}\right)$

7: ${}^{i}\bm{X}_{\lambda_{i}}=\bm{X}_{\mathbf{J}i}\bm{X}_{\mathbf{T}i}$

8: if $\lambda_{i}\neq 0$ then

9: ${}^{i}\bm{X}_{0}={}^{i}\bm{X}_{\lambda_{i}}{}^{\lambda_{i}}\bm{X}_{0}$

10: end if

11: $\bm{v}_{\mathbf{J}i}=\bm{S}_{i}\dot{\bm{q}}_{i}$

12: $\bm{a}_{\mathbf{J}i}=\bm{c}_{\mathbf{J}i}$

13:
$\bm{v}_{i}={}^{i}\bm{X}_{\lambda_{i}}\bm{v}_{\lambda_{i}}+\bm{v}_{\mathbf{J}i}$

14:
$\bm{a}_{i}={}^{i}\bm{X}_{\lambda_{i}}\bm{a}_{\lambda_{i}}+\bm{a}_{\mathbf{J}i}$

15:
$\bm{f}_{i}=\bm{I}_{i}\bm{a}_{i}+\bm{v}_{i}\times^{*}\bm{I}_{i}\bm{v}_{i}-{}^{i}\bm{X}_{0}^{*}\bm{f}_{i}^{x}$

16:end for

17:for $i=n_{B},n_{B}-1,\ldots 1$ do

18: $\bm{C}_{i}=\bm{S}_{i}^{T}\bm{f}_{i}$

19: if $\lambda_{i}\neq 0$ then

20:
$\bm{f}_{\lambda_{i}}=\bm{f}_{\lambda_{i}}+^{\lambda_{i}}\bm{X}_{i}^{*}\bm{f}_{i}$

21: end if

22:end for

First, The Recursive Newton-Euler Algorithm (RNEA) is used to compute
generalized bias term $\bm{C}$ according to the current state of the robot.
The RNEA is one of the most effective inverse dynamics algorithms in robotics.
It has three steps: First, compute the $\bm{q}$, $\dot{\bm{q}}$, and
$\ddot{\bm{q}}$ of each rigid body. Then, compute the net force required to
satisfy $\ddot{\bm{q}}$ according to 3.1.2 for each rigid body. Finally,
compute the force transmitted across each joint. Let
$\bm{a}_{g}\in\mathrm{M}^{6}$ be the acceleration of gravity in 6-D vector
form. The pseudocode of Recursive Newton-Euler Algorithm is shown in Algorithm
1. In this pseudocode, jtype($i$) is a function for obtaining the type of
joint $i$, and jcalc() is a function for calculating joint transformation
based on joint information featherstone2010beginner2 . Suppose the number of
joints in the system is $n$. Let $n_{B}$ be the number of bodies in the
system. The time complexity of RNEA algorithm is $O(n_{B})$.

Then, the Composite Rigid-Body Algorithm (CRBA) is used to compute the joint
generalized inertia matrix $\bm{H}$. The key process of this algorithm is to
compute the non-zero part of $\bm{H}$ recursively. Specifically, the algorithm
recursively calculates the matrix blocks $\bm{H}_{ij}$ and $\bm{H}_{ji}$
generated by joints $i$ and $j$. The pseudocode of CRBA is shown in Algorithm
2. Refer to Section 3 for symbol definitions. Let $n_{B}$ be the number of
bodies in the system. The complexity of CRBA is $O(n_{B}^{2})$.

Finally, we have the bias force $\bm{C}$ obtained by the RNEA, the given
applied forces $\bm{\tau}$, and the $\bm{H}$ obtained by the CRBA. According
to 26, we obtain a simple system of linear equations
$\bm{H}\ddot{\bm{q}}=\bm{\tau}-\bm{C}$. Cholesky decomposition method is used
to solve it directly to get the unknown acceleration $\ddot{\bm{q}}$. So far
the responsibility of the dynamic algorithms has been completed. After that,
the simulator will use the numerical integrator to obtain the current position
$\bm{q}$ of the multibody system to complete a simulation iteration.

Algorithm 2 Composite Rigid Body Algorithm

1:Robot physical information

2:Generalized inertia matrix $\bm{H}$

3:$\bm{H}=0$

4:for $i=1,2,\ldots n_{B}$ do

5: $\bm{I}_{i}^{c}=\bm{I}_{i}$

6:end for

7:for $i=n_{B},n_{B}-1,\ldots 1$ do

8: if $\lambda_{i}\neq 0$ then

9:
$\bm{I}_{\lambda_{i}}^{c}=\bm{I}_{\lambda_{i}}^{c}+{}^{\lambda_{i}}\bm{X}_{i}^{*}\bm{I}_{i}^{ci}\bm{X}_{\lambda_{i}}$

10: end if

11: $\bm{F}=\bm{I}_{i}^{c}\bm{S}_{i}$

12: $\bm{H}_{ii}=\bm{S}_{i}^{T}\bm{F}$

13: $j=i$

14: while $\lambda_{i}\neq 0$ do

15: $\bm{F}={}^{\lambda_{i}}\bm{X}_{i}^{*}\bm{F}$

16: $j=\lambda_{i}$

17: $\bm{H}_{ij}=\bm{F}^{T}\bm{S}_{j}$

18: $\bm{H}_{ji}=\bm{H}_{ij}^{T}$

19: end while

20:end for

In this subsection, we introduced the rigid multibody dynamics framework based
on generalized coordinates. Based on the language of space algebra, we
established the dynamics mathematical models of single rigid body and
multibody. Subsequently, numerical algorithms RNEA and CRBA were used to
numerically solve the dynamics mathematical model. In summary, our algorithm
describes multibody dynamics with generalized coordinates, which inherently
does not violate constraints. Current state-of-the-art physics engines, such
as Bullet, are based on impulse-based methods coumans2013bullet . impulse-
based methods belong to the maximum coordinate method, and its basic idea is
to approximate the constraints with a series of impulses bender2014interactive
. Although such methods are widely used in video games, it is difficult to
meet the accuracy requirements of industrial robot simulation choi118use ;
liu2021role . In recent years, Position based dynamics (PBD) method has become
a new paradigm of robot simulation muller2020detailed . However, the accuracy
and stability of PBD are still inadequate. We’ll show experimental comparisons
in Section 6.

###  3.2 Numerical Integrators

The system of ordinary differential equations constructed by rigid multibody
dynamics algorithms is so complex that it has no analytical solution and must
be solved iteratively by numerical integrator. The numerical integration
method is always the key problem of physically-based simulation. For a given
application scenario, a good numerical integrator should not only have
accuracy and stability, but also have high computational performance.

The explicit Euler integrator is the most naive numerical integration method,
which only utilizes the first order Taylor approximation of ordinary
differential equations. Numerical instability makes explicit Euler integrator
difficult to simulate complex systems. As the extensions of explicit Euler
method, integrators with higher order Taylor approximations were developed,
such as the explicit midpoint method and the Runge-Kutta method. In
particular, the fourth-order Runge-Kutta method is widely used in the field of
physical simulation. Although these methods have improved stability, as
explicit methods, they still introduce numerical dissipation. We propose to
use symplectic Euler integrator, which has better stability in long-term
simulation.

Explicit Euler integrator Explicit Euler (forward Euler) integrator is the
most intuitive and easy to implement method. However, the extreme instability
of the explicit Euler method has been observed in the early work of graphic
simulation terzopoulos1988deformable ; terzopoulos1987elastically . As above,
we denote $\bm{q}$, $\dot{\bm{q}}$, and $\ddot{\bm{q}}$ as generalized
positions, generalized velocities, and generalized accelerations,
respectively. The explicit Euler integrator has the following form:

$\left\\{\begin{array}[]{lr}\dot{\bm{q}}_{n+1}=\dot{\bm{q}}_{n}+h\ddot{\bm{q}}_{n}\\\
\bm{q}_{n+1}=\bm{q}_{n}+h\dot{\bm{q}}_{n}\par\end{array}\right.$ (27)

where $h$ is the is the fixed time step length, and the subscript $n$
represents the iteration step number. Explicit Euler’s method uses the first-
order approximation of the current iteration to obtain the velocity and
position of the next step directly. Obviously, each iteration introduces
additional energy and results in a numerical explosion. Although reducing the
time step $h$ can delay the occurrence of numerical explosion, the amount of
calculation will be greatly increased. In contrast, the implicit Euler
(backward Euler) method takes the first-order approximation of the next step
to update the next step.That is, each iteration must solve the implicit
equation. The implicit Euler integrator is expressed as follows:

$\left\\{\begin{array}[]{lr}\dot{\bm{q}}_{n+1}=\dot{\bm{q}}_{n}+h\ddot{\bm{q}}_{n+1}\\\
\bm{q}_{n+1}=\bm{q}_{n}+h\dot{\bm{q}}_{n+1}\par\end{array}.\right.$ (28)

Each iteration of the implicit Euler method introduces an energy loss, which
is called artificial damping in the physics-based animation community.
Although the implicit Euler method can also cause energy inaccuracy, it can
guarantee that there will be no numerical explosion. Therefore, some
simulation applications that require stability but do not require real time
will adopt this method baraff1998large ; press2007numerical . In interactive
robotics simulation, the implicit method is difficult to meet the performance
requirements.

Fourth-order Runge-Kutta method In order to improve the accuracy of explicit
Euler, various high-order explicit integration methods have been proposed. One
of the most widely used high-order methods is the fourth-order Runge-Kutta
method (RK4). The time differential of generalized position and generalized
velocity is expressed as $\frac{dq}{dt}=\dot{q}(t,q)$ and
$\frac{d\dot{q}}{dt}=\ddot{q}(t,\dot{q})$, respectively. The form of the
fourth-order Runge-Kutta integrator is as follows:

$\left\\{\begin{array}[]{lr}\dot{\bm{q}}_{n+1}=\dot{\bm{q}}_{n}+\frac{1}{6}h\left(\ddot{k}_{1}+2\ddot{k}_{2}+2\ddot{k}_{3}+\ddot{k}_{4}\right)\\\
\bm{q}_{n+1}=\bm{q}_{n}+\frac{1}{6}h\left(\dot{k}_{1}+2\dot{k}_{2}+2\dot{k}_{3}+\dot{k}_{4}\right)\\\
\end{array}.\right.$ (29)

Detailed definitions are given in Appendix B.

The RK4 method is much more accurate than the first order explicit Euler
method. Due to RK4’s efficiency and accuracy, many game engines use it for
numerical integration. However, RK4 is still an explicit integrator, so the
errors will gradually accumulate in the long-term dynamics simulation. Our
experimental result (Section 6) shows the numerical error accumulation of RK4
method. We also choose the second-order explicit midpoint method and the
sixth-order Runge-Kutta hairer1993solving  as comparison, and the numerical
results are shown in Section 6.

Symplectic Euler integrator In recent years, symplectic integration methods
have attracted much attention from the physics-based simulation community.
Theoretically and experimentally, symplectic integrators have good energy
conservation properties for multibody systems hairer2006geometric ;
stern2006discrete . The Symplectic Euler integrator has the following form:

$\left\\{\begin{array}[]{lr}\dot{\bm{q}}_{n+1}=\dot{\bm{q}}_{n}+h\ddot{\bm{q}}_{n}\\\
\bm{q}_{n+1}=\bm{q}_{n}+h\dot{\bm{q}}_{n+1}\par\end{array}.\right.$ (30)

The Symplectic Euler integrator is most notable for explicitly updating
velocity but implicitly updating position. So Symplectic Euler method is also
called semi-implicit Euler or semi-explicit Euler method. Although the second
step of the Symplectic Euler method is implicit, it is based on the result of
the explicit compute of the first step. That is, Symplectic Euler method is
generally first-order explicit and naturally have high computational
performance. Explicit or implicit method cause an incremental increase or loss
of system energy. But the Symplectic Euler method makes the numerical result
"oscillate" around the real value hairer2006geometric ; kuhl1999energy . In
this way, the energy conservation (stability) of the system can be guaranteed
in the long-term simulation. Our numerical results (Section 6) show that the
Symplectic Euler method is significantly more stable than other methods.

##  4 Roadheader Modeling

In this section, the method of modeling the roadheader robot is introduced. We
divide the model of the roadheader into two parts: the geometric model that
records static information and the robotics model that records physical
information. For geometric modeling, we propose a hierarchical 3D model
format. The components of the roadheader robot, the rigid bodies of
components, and the geometric information of rigid bodies are recorded in this
format. Correspondingly, the physical information of the robot, including the
parameters of each component and joint, is modeled by the robotics model.
These two models will be input into the simulation system as initial
information.

###  4.1 Geometric Modeling

Figure 2: Roadheader Model Rendered by OpenGL.

To visualize the roadheader and working environment, we propose a universal
hierarchical 3D model format called .3DT. The .3DT is geometric information in
plain text, which can be parsed and processed quickly and directly. Unlike the
.obj format, the .3DT format can organize the robot’s component models
hierarchically in a single file. In our design, a roadheader robot is divided
into several components like the physical world, and each component is
composed of multiple rigid bodies. The .3DT file stores the hierarchical
structure of a robot and the geometric information, color, and other
attributes of each rigid body. The model file structure of a roadheader robot
is shown in Figure 4.

In this paper, we will use an EBZ230 roadheader as a prototype to implement
the simulation system. The .3DT format 3D model of this roadheader is rendered
with OpenGL as shown in Figure 2. The entire robot model consists of 162 rigid
body models with a total of 24582 points and 27233 surface elements. The key
components of the roadheader model are also marked in Figure 2. This research
only focuses on the movement of important functional components (such as
cutting parts and shovel) and ignore other details (such as crawlers) of the
robot.

Figure 3: The data structure of Components and Joints. Figure 4: Hierarchical
Structure of .3DT Model File.

###  4.2 Robotics Modeling

The geometric modeling of the roadheader robot only has static graphics
information. To describe the physical information of the robot, the robotics
modeling is introduced into our system. Robotics modeling consists of two
important parts: the physical information of each robot component and the
constraints between the two components. The former includes physical
information such as the mass and inertia of each component, which corresponds
to the components in the geometric model. The latter includes the constraint
information between two robot components, that is, the description of a joint.

The description of the data structure of the robot components and joints is
shown in Figure 3. A rigid body is the basic element in a robot system, and
its data structure consists of Name (the ID corresponding to the .3DT file),
Type, Mass and Inertia, Global Configuration (global position and
orientation), etc. In our design, complex components (such as the Cutting
Head) of the robot are composed of various rigid bodies connected by fixed
joints. Therefore, the entire simulation system only contains a few basic
rigid body types, such as Box, Sphere, Cylinder, Capsule, etc. The data
structure of the joint constraint is consists of Type (including Hinge,
Slider, Universal, Fixed, etc.), Predecessor and Successor Rigid Body, Bias
(the relative position of the joint on Predecessor Rigid Body), Axis (the
rotation axis or sliding direction of the joint), etc. Both the robotics model
file and the geometric model file will be parsed by the simulator as the
initial data of the dynamics algorithm.

Figure 5: The predecessor and successor rigid body are connected by a joint.
The motion space matrix for joint $i$ is $\bm{S}_{i}$. The spatial
transformation matrices of the bodies and joint are shown in the figure. The
spatial force $\bm{f}_{i}$ from body $\lambda_{i}$ is applied to body $i$ via
joint $i$.

Each joint connects two rigid bodies: the predecessor and successor. The
relationship between joint and rigid bodies is shown in Figure 5. The
predecessor and successor of any joint $i$ are called body $\lambda_{i}$ and
body $i$, respectively. The spatial transformations from body $\lambda_{i}$ to
joint $i$ and from joint $i$ to body $i$ are denoted as $\bm{X}_{\mathbf{T}i}$
and $\bm{X}_{\mathbf{J}i}$, respectively. The spatial velocity, spatial
acceleration, spatial inertia and composite inertia of body $i$ are
$\bm{v}_{i}$, $\bm{a}_{i}$, $\bm{I}_{i}$ and $\bm{I}_{i}^{c}$ respectively.
The corresponding notations for body $\lambda_{i}$ is similar. The spatial
force $\bm{f}_{i}$ from body $\lambda_{i}$ is applied to body $i$ via joint
$i$. The motion subspace matrix $\bm{S}_{i}$ is used to describe the
constraint action of joint $i$ on the configuration space of the rigid bodies.
For the roadheader robot simulation, we use two kinds of joints: rotate
(revolute) joint and slider (prismatic) joint. The motion subspace matrix of
the rotate joint with the x-axis, y-axis and z-axis as the rotation axis are
defined as

$\bm{S}_{Rx}=\left[\begin{array}[]{l}1\\\ 0\\\ 0\\\ 0\\\ 0\\\
0\end{array}\right],\bm{S}_{Ry}=\left[\begin{array}[]{l}0\\\ 1\\\ 0\\\ 0\\\
0\\\ 0\end{array}\right],\bm{S}_{Rz}=\left[\begin{array}[]{c}0\\\ 0\\\ 1\\\
0\\\ 0\\\ 0\end{array}\right],$

respectively. The motion subspace matrix of slider joint with the x-axis,
y-axis and z-axis as the motion direction are defined as

$\bm{S}_{Tx}=\left[\begin{array}[]{l}0\\\ 0\\\ 0\\\ 1\\\ 0\\\
0\end{array}\right],\bm{S}_{Ty}=\left[\begin{array}[]{l}0\\\ 0\\\ 0\\\ 0\\\
1\\\ 0\end{array}\right],\bm{S}_{Tz}=\left[\begin{array}[]{l}0\\\ 0\\\ 0\\\
0\\\ 0\\\ 1\end{array}\right],$

respectively.

All joints and components information constitute the topology of the entire
Roadheader robot. The topology graph of the key structure the Roadheader is
shown in Figure 6. In this graph, nodes represent robot components, and arcs
represent joints. The simulation system will compute the real-time state of
each joint and component frame by frame according to the dynamic algorithm.
Just like manipulating a robot in reality, the user sends instructions (via
control panel) to change the state of the joints so that the corresponding
components move.

Figure 6: Topology of the Roadheader Robot. Description of nodes: M (Main
Body) CH (Cutting Head), TA (Telescopic Arm), SA (Supporting Arm), AB (Arm
Base), PR (Piston Rod), CB (Cylinder Barrel), RF (Rear Frame), SP (Shovel
Plate), LG (Loader Gathering Arm). Description of arcs: H (Hinge Joint), S
(Slider Joint), F (Fixed Joint), U (Universal Joint, equivalent to the cascade
of two hinge joints).

##  5 System Architecture and Implementation

The architecture of our roadheader simulation system is shown in Figure 1. The
whole system consists of two main parts: the Graphical User Interfaces (GUI)
on the front end and the simulator on the back end. The GUI includes a
renderer (implemented by OpenGL) to display the computation results from the
simulator in real time. The rendering result of a roadheader working in an
underground space is shown in Figure 7. Another major part of the GUI is the
control panel, which accepts user instructions (via keyboard and mouse). The
control panel will send user control signals (such as raising the supporting
arm) to the simulator, or sending rendering commands (such as changing the
viewport) directly to the renderer. A demo of the control panel is shown in
Figure 8.

Algorithm 3 Interactive Roadheader Simulation Loop

1:Geometric model, Robotics model

2:Simulation visualization results

3:$h\leftarrow\Delta t/numSubsteps$;

4:$\bm{q}\leftarrow
initialposition,\dot{\bm{q}}\leftarrow\bm{0},\ddot{\bm{q}}\leftarrow\bm{0}$;

5:$currentState\leftarrow(\bm{q},\dot{\bm{q}},\ddot{\bm{q}})$;

6:while simulating do

7: if userAction == quitSim then

8: Quit the simulation loop;

9: end if

10: for $numSubsteps$ do

11: if $userAction\neq null$ then

12: $\ddot{\bm{q}}\leftarrow$

13: $Solver(currentState,userAction)$;

14: else

15: $\ddot{\bm{q}}\leftarrow Solver(currentState)$;

16: end if

17: $\dot{\bm{q}}\leftarrow\dot{\bm{q}}+h\ddot{\bm{q}}$;

18: $\bm{q}\leftarrow\bm{q}+h\dot{\bm{q}}$;

19: $currentState\leftarrow(\bm{q},\dot{\bm{q}},\ddot{\bm{q}})$;

20: end for

21: Renderer($\bm{q}$);

22:end while

Figure 7: Roadheader Simulation Visualization Effect. Figure 8: Control Panel.

The process of the interactive simulation loop is shown in Algorithm 3. The
model files that are input into the simulator consists of the two parts,
geometric model and robotics model, introduced in Section 4. After
initializing the current state (step 2 and 3), the main simulation loop
begins. Each time step $\Delta t$ is divided into several substeps $h$. In
each substep, the simulator will compute (denoted as $Solver$ in step 11 and
13) the current acceleration $\ddot{\bm{q}}$ based on the last state and the
user’s command using dynamics algorithms (introduced in Section 3). Then the
numerical integrator will compute the current velocity $\dot{\bm{q}}$ and
position $\bm{q}$ based on the current acceleration (step 15, 16, and 17). The
simulation state will be rendered at each time step.

##  6 Numerical Results

In this section, we designed a series of experiments to test the numerical
results of our roadheader simulation system. We tested three main areas:
performance, accuracy, and energy conservation. A personal computer with a
Core-i5 CPU at 3.4 GHz and a GeForce GTX 950M GPU was used to test our
roadheader simulation system. The whole system is implemented in C++ with
Visual Studio 2019 and runs on Windows 10. The rendering effect of the system
is based on OpenGL and is embedded in a Qt5 window. In all tests, the
acceleration of gravity was set to $9.8m/s^{2}$.

Figure 9: The Process of Raising the Supporting Arm of Roadheader.

###  6.1 Accuracy

Compared with video games, industry-oriented simulation systems require higher
simulation accuracy. The position error (or called position drift) during the
simulation is used to measure the accuracy of the simulator. However, complex
robot models usually have no analytical solution, so the specific position
error of the simulator is difficult to obtain. Therefore, we test the position
drift through a simple arm raising motion. The process of raising the
supporting arm of roadheader is shown in Figure 9. This movement process is a
simple circular movement with a closed-form solution, that is, the distance
between the cutting head and the main body is constant. Let $d$ be the initial
distance, and $d^{\prime}$ be the current distance computed frame by frame.
Then we get the percentage of position error for each iteration:

$\frac{\|d-d^{\prime}\|}{d^{\prime}}\times 100\%$

We use the same robot model to run in our simulation system (using generalized
coordinate algorithms), Bullet physics engine (using maximal coordinate
algorithms) and Position based dynamics (PBD) method muller2020detailed  to
compare position errors. The contrast results of position error and the number
of iterations between our method and Bullet engine is shown in Figure 10. The
Bullet engine using the maximal coordinate algorithm has a significantly
faster position error growth than our simulator using the generalized
coordinate algorithm. The PBD method has significant fluctuation of position
error, which means its stability and accuracy are not reliable. It turns out
that our method has more advantages in terms of accuracy.

Figure 10: Relationship between iterations number and position error in
different simulators.

###  6.2 Energy conservation

We explored the influence of different numerical integrators on the simulation
results. In our simulation system, symplectic Euler method is used instead of
conventional Runge–Kutta method. In most graphics applications, the higher the
order of the integrator, the higher the accuracy of the simulation (and the
higher the computational cost). However, this rule is not necessarily correct
for industrial-oriented machine simulation. In theory, the total mechanical
energy (the sum of the potential and kinetic energies) of a multibody system
that is only acted on by conservative forces remains constant. But in the
process of numerical simulation, every iteration will inevitably lead to
truncation errors, that is, cause energy drift of the simulation system.
Therefore, energy conservation is very important for long-term simulation.
Although the RK4 method has higher accuracy in short-term simulation, it will
cause energy drift accumulation like explicit Euler method. The symplectic
Euler method used in our system is an energy preserving method, which performs
well in long-term simulations.

We designed an experiment to compare the energy drift of symplectic Euler
method, explicit Euler method (first order), explicit midpoint method (second
order), RK4 method (fourth-order,), and RK6 method (sixth order) in the same
environment. Let $E$ and $E^{\prime}$ be the current and initial total energy
(the Hamiltonian), respectively. The percentage of energy drift

$\frac{\|E-E^{\prime}\|}{E^{\prime}}\times 100\%$

is computed frame by frame. The relationship between the number of iterations
and energy drift under different time integrators is shown in Figure 11. As
shown in Figure 11, low order methods (such as explicit Euler and explicit
midpoint) are easy to make the system energy drift significantly. So the lower
order methods is obviously unstable and cannot be used for long-term
simulation. The medium order method (RK4) performs well in short time but has
poor energy conservation in long-term simulation. The higher order method
(RK6) performs better, but higher order means more computation. Our
experiments show that its efficiency is too low to meet the needs of
interactivity. As shown in Figure 11, in the long-term simulation, the energy
drift of the symplectic Euler will stabilize within a small range.

Experiments show that the symplectic Euler method adopted by our system is
more accurate and stable for multibody simulation than other non-preserving
numerical integrators.

Figure 11: Relationship between iterations number and energy drift under
different time integrators.

###  6.3 Performance

We tested the performance of our simulation system. A model of an EBZ230
roadheader with 4582 points and 27233 surface elements was used in our test
system. The rendering of the system is based on OpenGL 4.5 with a resolution
of $1920\times 1080$. The simulator should be fast enough to support real-time
interaction between the user and the simulation system. Usually, a user-
friendly simulator requires the system to reach at least 60 fps, which means
that each frame takes at most 16.67 milliseconds (ms). We use the mean
computation time per frame as the performance metric. The test results of our
method and others are shown in Figure 12. According to multiple tests, the
simulation system takes about 8.9 ms per frame, which meets the need for Real-
time interaction. Compared with other methods, our method has better
computational performance.

Figure 12: Comparison of computational performance of different methods.

##  7 Limitations and Future Work

Compared with other types of methods (maximal coordinates methods, PBD),
although the numerical error of our method is smaller, its mathematical theory
is more difficult to understand and code. In future work, we will try to use
more intuitive mathematical description forms. The integrator we use is more
stable than the other non-preserving numerical integrators , but there is
still the inevitable energy drift phenomenon. In the future we will
investigate more advanced forms of integration (such as implicit midpoint and
Newmark integrator) or use variable time steps to further reduce energy drift.

In this paper, only multibody dynamics algorithms are used to model the
roadheader robot. The scenario in which the robot works is much more complex
in reality. In future work, we will study the interaction between the
roadheader robot and the environment, such as impact, contact and friction.
And we will improve the usability of the system from the perspective of human-
computer interaction. Our method can also be extended to other types of
robots. We hope our work will inspire further work that combines graphics and
industry.

##  8 Conclusion

This paper presents an accurate and stable interactive simulation system for
roadheader robot. The roadheader robot is modeled in two parts: the robotics
model for physical information and the geometric model for visual information.
The simulator constructs the equation of motion of roadheader model based on
generalized coordinates. In each iteration, the simulator computes the new
state of the robot based on previous state and user interaction, and renders
it in the user interface.

First, existing works in the field of roadheader simulation are based on
commercial game engines, and the accuracy cannot meet industrial requirements.
We adopted the generalized coordinates based algorithms to simulate the
roadheader robot, which is more accurate than other state-of-the-art methods.
We compared the proposed system to the conventional Bullet game engine. The
simulation results show that the position error of our method is lower for the
same simulation process. Second, the time integrator has a significant effect
on simulation stability. We adopted the symplectic Euler integrator instead of
the most common RK4 method. Our approach is a better trade-off between
efficiency and energy accuracy. Common integrators of different orders are
used for comparison. Numerical tests show that our method has less energy
drift in long-term simulations. That is, our method is more stable. In the
end, our system reached 60 fps, which meets the requirements of an interactive
simulation system.

Acknowledgments This work was supported in part by the National Key Research
and Development Projects of China under Grant 2017YFC0804406, and in part by
the National Natural Science Foundation of China under Grant 51904173.

##  Appendix A Symbols

The symbol $\times$ and $\times^{*}$ are spatial cross product operators. For
any vector $v=\left[x,y,z\right]^{\mathsf{T}}$:

$\displaystyle v\times=\left[\begin{array}[]{c}x\\\ y\\\
z\end{array}\right]\times=\left[\begin{array}[]{ccc}0&-z&y\\\ z&0&-x\\\
-y&x&0\end{array}\right],$ $\displaystyle v\times^{*}=-v\times^{\mathsf{T}}.$

Other important symbols and descriptions are listed in Table 1.

Table 1: List of Symbols Symbol | Description
---|---
$\hat{\bm{v}}_{i}$ | Spatial velocity of body $i$
$\hat{\bm{a}}_{i}$ | Spatial acceleration of body $i$
$\hat{\bm{f}}_{i}$ | Spatial force acting on body $i$
$\hat{\bm{I}}_{i}$ | Spatial inertia of body $i$
$\hat{\bm{h}}_{i}$ | Spatial momentum of body $i$
${}^{j}\bm{X}_{i}$ | Spatial transformation from frame $i$ to frame $j$
$\lambda_{i}$ | Predecessor of joint $i$
$\bm{q},\dot{\bm{q}},\ddot{\bm{q}}$ | Generalized position, velocity and acceleration
$\bm{H}$ | Generalized inertia
$\bm{C}$ | Generalized bias force
$\bm{\tau}$ | Generalized external force
$\bm{S}_{i}$ | Motion subspace matrix
$h$ | Time step of integration

##  Appendix B Details of RK4 method

The form of the fourth-order Runge-Kutta integrator is as follows:

$\left\\{\begin{array}[]{lr}\dot{\bm{q}}_{n+1}=\dot{\bm{q}}_{n}+\frac{1}{6}h\left(\ddot{k}_{1}+2\ddot{k}_{2}+2\ddot{k}_{3}+\ddot{k}_{4}\right)\\\
\bm{q}_{n+1}=\bm{q}_{n}+\frac{1}{6}h\left(\dot{k}_{1}+2\dot{k}_{2}+2\dot{k}_{3}+\dot{k}_{4}\right)\\\
\end{array}.\right.$

where

$\left\\{\begin{array}[]{lr}\ddot{k}_{1}=\ddot{\bm{q}}\left(t_{n},\dot{\bm{q}}_{n}\right)\\\
\ddot{k}_{2}=\ddot{\bm{q}}\left(t_{n}+\frac{h}{2},\dot{\bm{q}}_{n}+h\frac{\ddot{k}_{1}}{2}\right)\\\
\ddot{k}_{3}=\ddot{\bm{q}}\left(t_{n}+\frac{h}{2},\dot{\bm{q}}_{n}+h\frac{\ddot{k}_{2}}{2}\right)\\\
\ddot{k}_{4}=\ddot{\bm{q}}\left(t_{n}+h,\dot{\bm{q}}_{n}+h\ddot{k}_{3}\right)\\\
\end{array}\right.$

and

$\left\\{\begin{array}[]{lr}\dot{k}_{1}=\dot{\bm{q}}\left(t_{n},{\bm{q}}_{n}\right)\\\
\dot{k}_{2}=\dot{\bm{q}}\left(t_{n}+\frac{h}{2},{\bm{q}}_{n}+h\frac{\dot{k}_{1}}{2}\right)\\\
\dot{k}_{3}=\dot{\bm{q}}\left(t_{n}+\frac{h}{2},{\bm{q}}_{n}+h\frac{\dot{k}_{2}}{2}\right)\\\
\dot{k}_{4}=\dot{\bm{q}}\left(t_{n}+h,{\bm{q}}_{n}+h\dot{k}_{3}\right)\\\
\end{array}.\right.$

## References

  *   * (1) Deshmukh, S., Raina, A., Murthy, V., Trivedi, R., Vajre, R.: Roadheader–a comprehensive review. Tunnelling and Underground Space Technology 95, 103148 (2020)
  * (2) Li, J.-g., Zhan, K.: Intelligent mining technology for an underground metal mine based on unmanned equipment. Engineering 4(3), 381–391 (2018)
  * (3) Grabowski, A., Jankowski, J.: Virtual reality-based pilot training for underground coal miners. Safety science 72, 310–314 (2015)
  * (4) Bilberg, A., Malik, A.A.: Digital twin driven human–robot collaborative assembly. CIRP Annals 68(1), 499–502 (2019)
  * (5) Choi, H., Crump, C., Duriez, C., Elmquist, A., Hager, G., Han, D., Hearl, F., Hodgins, J., Jain, A., Leve, F., et al.: On the use of simulation in robotics: Opportunities, challenges, and suggestions for moving forward. Proceedings of the National Academy of Sciences 118(1) (2020)
  * (6) Liu, C.K., Negrut, D.: The role of physics-based simulators in robotics. Annual Review of Control, Robotics, and Autonomous Systems 4, 35–58 (2021)
  * (7) Bender, J., Erleben, K., Trinkle, J.: Interactive simulation of rigid body dynamics in computer graphics. In: Computer Graphics Forum, vol. 33, pp. 246–270 (2014). Wiley Online Library
  * (8) Luckcuck, M., Farrell, M., Dennis, L.A., Dixon, C., Fisher, M.: Formal specification and verification of autonomous robotic systems: A survey. ACM Computing Surveys (CSUR) 52(5), 1–41 (2019)
  * (9) Tasora, A., Serban, R., Mazhar, H., Pazouki, A., Melanz, D., Fleischmann, J., Taylor, M., Sugiyama, H., Negrut, D.: Chrono: An open source multi-physics dynamics engine, pp. 19–49 (2016)
  * (10) Lynch, K.M., Park, F.C.: Modern Robotics, pp. 11–38. Cambridge University Press, London (2017)
  * (11) Bargteil, A.W., Shinar, T., Kry, P.G.: An introduction to physics-based animation. In: SIGGRAPH Asia 2020 Courses, pp. 1–57. acm, New York (2019)
  * (12) Coumans, E., Bai, Y.: Pybullet, a python module for physics simulation for games, robotics and machine learning (2016)
  * (13) Smith, R., et al.: Open dynamics engine (2005)
  * (14) Featherstone, R.: Rigid Body Dynamics Algorithms. Springer, New York (2014)
  * (15) Agarwal, A., Shah, S., Bandyopadhyay, S., Saha, S.: Dynamics of serial kinematic chains with large number of degrees-of-freedom. Multibody System Dynamics 32(3), 273–298 (2014)
  * (16) Korayem, M., Shafei, A., Dehkordi, S.: Systematic modeling of a chain of n-flexible link manipulators connected by revolute–prismatic joints using recursive gibbs-appell formulation. Archive of applied mechanics 84(2), 187–206 (2014)
  * (17) Korayem, M., Shafei, A.: Motion equation of nonholonomic wheeled mobile robotic manipulator with revolute–prismatic joints using recursive gibbs–appell formulation. Applied Mathematical Modelling 39(5-6), 1701–1716 (2015)
  * (18) Bellanca, J.L., Orr, T.J., Helfrich, W.J., Macdonald, B., Navoyski, J., Demich, B.: Developing a virtual reality environment for mining research. Mining, metallurgy & exploration 36(4), 597–606 (2019)
  * (19) Van Wyk, E., De Villiers, R.: Virtual reality training applications for the mining industry. In: Proceedings of the 6th International Conference on Computer Graphics, Virtual Reality, Visualisation and Interaction in Africa, pp. 53–63 (2009)
  * (20) Yan, C., Zhao, W., Lu, X.: A multi-sensor based roadheader positioning model and arbitrary tunnel cross section automatic cutting. Sensors 19(22), 4955 (2019)
  * (21) Tian, J., Wang, S., Wu, M.: Kinematic models and simulations for trajectory planning in the cutting of spatially-arbitrary crosssections by a robotic roadheader. Tunnelling and Underground Space Technology 78, 115–123 (2018)
  * (22) Wang, J., Huang, Z.: The recent technological development of intelligent mining in china. Engineering 3(4), 439–444 (2017)
  * (23) Malik, A.A., Bilberg, A.: Digital twins of human robot collaboration in a production setting. Procedia manufacturing 17, 278–285 (2018)
  * (24) Marshall, J.A., Bonchis, A., Nebot, E., Scheding, S.: Robotics in mining. In: Springer Handbook of Robotics, pp. 1549–1576. Springer, New York (2016)
  * (25) Zhang, H.: Head-mounted display-based intuitive virtual reality training system for the mining industry. International Journal of Mining Science and Technology 27(4), 717–722 (2017)
  * (26) Andersen, K., Gaab, S.J., Sattarvand, J., Harris, F.C.: Mets vr: Mining evacuation training simulator in virtual reality for underground mines. In: 17th International Conference on Information Technology–New Generations (ITNG 2020), pp. 325–332 (2020). Springer
  * (27) Müller, M., Macklin, M., Chentanez, N., Jeschke, S., Kim, T.-Y.: Detailed rigid body simulation with extended position based dynamics. In: Computer Graphics Forum, vol. 39, pp. 101–112 (2020). Wiley Online Library
  * (28) Deul, C., Charrier, P., Bender, J.: Position-based rigid-body dynamics. Computer Animation and Virtual Worlds 27(2), 103–112 (2016)
  * (29) Goldstein, H., Poole, C., Safko, J.: Classical mechanics. American Association of Physics Teachers (2002)
  * (30) Wang, Y., Weidner, N.J., Baxter, M.A., Hwang, Y., Kaufman, D.M., Sueda, S.: Redmax: Efficient & flexible approach for articulated dynamics. ACM Transactions on Graphics (TOG) 38(4), 1–10 (2019)
  * (31) Murray, R.M., Li, Z., Sastry, S.S.: A Mathematical Introduction to Robotic Manipulation. CRC press, Boca Raton (2017)
  * (32) Featherstone, R.: A beginner’s guide to 6-d vectors (part 1). IEEE robotics & automation magazine 17(3), 83–94 (2010)
  * (33) Hairer, E., Lubich, C., Wanner, G.: Geometric Numerical Integration: Structure-preserving Algorithms for Ordinary Differential Equations vol. 31, pp. 179–233. Springer, New York (2006)
  * (34) House, D., Keyser, J.C.: Foundations of Physically Based Modeling and Animation, pp. 113–147. CRC Press, Boca Raton (2016)
  * (35) Butcher, J.C., Goodwin, N.: Numerical Methods for Ordinary Differential Equations vol. 2, pp. 97–111. Wiley, Hoboken (2008)
  * (36) Löschner, F., Longva, A., Jeske, S., Kugelstadt, T., Bender, J.: Higher-order time integration for deformable solids. In: Computer Graphics Forum, vol. 39, pp. 157–169 (2020). Wiley Online Library
  * (37) Sharma, H., Patil, M., Woolsey, C.: A review of structure-preserving numerical methods for engineering applications. Computer Methods in Applied Mechanics and Engineering 366, 113067 (2020)
  * (38) Xu, H., Zhao, Y., Barbič, J.: Implicit multibody penalty-baseddistributed contact. IEEE transactions on visualization and computer graphics 20(9), 1266–1279 (2014)
  * (39) Kobilarov, M., Crane, K., Desbrun, M.: Lie group integrators for animation and control of vehicles. ACM transactions on Graphics (TOG) 28(2), 1–14 (2009)
  * (40) Press, W.H., William, H., Teukolsky, S.A., Vetterling, W.T., Saul, A., Flannery, B.P.: Numerical Recipes 3rd Edition: The Art of Scientific Computing, pp. 155–196. Cambridge university press, London (2007)
  * (41) Featherstone, R.: A beginner’s guide to 6-d vectors (part 2)[tutorial]. IEEE robotics & automation magazine 17(4), 88–99 (2010)
  * (42) Terzopoulos, D., Fleischer, K.: Deformable models. The visual computer 4(6), 306–331 (1988)
  * (43) Terzopoulos, D., Platt, J., Barr, A., Fleischer, K.: Elastically deformable models. In: Proceedings of the 14th Annual Conference on Computer Graphics and Interactive Techniques, pp. 205–214 (1987)
  * (44) Baraff, D., Witkin, A.: Large steps in cloth simulation. In: Proceedings of the 25th Annual Conference on Computer Graphics and Interactive Techniques, pp. 43–54 (1998)
  * (45) Hairer, E., Nørsett, S.P., Wanner, G.: Solving Ordinary Differential Equations. 1, Nonstiff Problems, pp. 173–188. Springer, New York (1993)
  * (46) Stern, A., Desbrun, M.: Discrete geometric mechanics for variational time integrators. In: ACM SIGGRAPH 2006 Courses, pp. 75–80 (2006)
  * (47) Kuhl, D., Crisfield, M.: Energy-conserving and decaying algorithms in non-linear structural dynamics. International journal for numerical methods in engineering 45(5), 569–599 (1999)