text:
\section{Introduction} \label{sec_introduction}

\subsection{Motivation and Methodology}
The first six films in the famous \textit{Star Wars} space saga starred the sidekick robot R2-D2, which looks like a dome-topped cylindrical trash can attached to a three-wheeled tripod.  A new generation of sidekick robots, the BB-series astromech droids 2BB-2, BB-4, BB-8, and BB-9E, were introduced in the seventh and eighth films in that saga, \textit{The Force Awakens} and \textit{The Last Jedi}. Unlike R2-D2, an astromech BB-unit consists of a rolling ball topped by a free-moving sensor and control unit as depicted in Figure~\ref{fig_bb9e}. While these rolling ball robots were mostly computer-generated in the films, the toy company Sphero sells working toy models of BB-8 and BB-9E which can be remotely controlled via a smartphone app.  However, rolling ball robots are not just gimmicks used by the entertainment and toy industries. The defense, security, energy, and agricultural industries are also interested in exploiting sensor-equipped rolling ball robots, such as Rosphere shown in Figure~\ref{fig_rosphere}, for such tasks as surveillance and environmental monitoring. \begin{figure}[h]
	\centering
	\subfloat[Sphero's toy incarnation of \textit{Star Wars'} BB-9E, the evil nemesis of BB-8  \cite{BB_9E}.]{\includegraphics[scale=.1025]{BB_9E}\label{fig_bb9e}}
	\hspace{5mm}
	\subfloat[Rosphere can be used in agriculture for monitoring crops, \copyright\ 2013 Emerald \cite{hernandez2013moisture}.]{\includegraphics[scale=.151]{rosphere}\label{fig_rosphere}}
	\caption{Examples of real rolling ball robots.}
\end{figure}
More specifically, suppose a rolling ball like an astromech BB-unit or Rosphere is actuated by some internal mechanism which may be controlled, for example, by spinning internal rotors, by swinging an internal pendulum, or by moving internal point masses along arbitrarily-shaped rails fixed within the ball. In addition, suppose initial and final conditions, like the ball's initial and final positions and velocities, algebraic (i.e. non-differential) path inequality constraints, like engineering limitations on the internal mechanism's acceleration, and a performance index, such as the mean error between the ball's actual and prescribed trajectory, are prescribed. How can the ball's internal mechanism be controlled to minimize the prescribed performance index while satisfying the prescribed initial and final conditions and the prescribed  algebraic  path inequality constraints?

This paper attempts to answer  this question using a theoretical and numerical approach  assuming that the ball is actuated by internal point masses traveling along fixed trajectories in the ball's coordinate frame, such as arbitrarily-shaped rails. This problem, as we discuss in the next subsection, has not received sufficient attention in the literature due to the relative complexity of the theoretical considerations. We believe that  internal control mechanisms of this type for rolling ball robots may be beneficial both for practical applications and  for theoretical studies of geometric control theory.  A recent paper \cite{putkaradze2018dynamics} by the authors deals exclusively with the derivation and analysis of the uncontrolled equations of motion  (a.k.a the dynamics) for rolling ball robots  actuated by internal point masses. The goal of this paper is to study the optimal control of  such rolling ball robots, to locomote over a prescribed trajectory, avoid obstacles, and/or perform some other maneuver by minimizing a prescribed  performance index.

Before optimal control can be applied to the rolling ball,  or, for that matter, to any other dynamical system, the equations of motion must be derived first.  Henceforth, the ordinary differential equations of motion of a dynamical system will be referred to as  the dynamics, the equations of motion, or the uncontrolled equations of motion to distinguish them from the controlled equations of motion which may be obtained by the indirect method of optimal control. The  dynamics of a rolling ball  actuated by internal  point  masses moving  along fixed, arbitrarily-shaped trajectories inside the ball is nontrivial, and, as far as we are aware, was only solved in this  generality in a recent paper by the authors \cite{putkaradze2018dynamics}. For  conciseness,  we shall not rederive  the uncontrolled  equations of motion  here,  but instead refer the  interested reader to that paper \cite{putkaradze2018dynamics}.

\subsection{Background}

Consider a ball rolling without slipping on a flat surface in the presence of a uniform gravitational field. Figure~\ref{fig:simple_rolling_ball} shows a ball of radius $r$ rolling without slipping on a flat surface in the presence of a uniform gravitational field of magnitude $g$.

\begin{figure}[h]
	\centering
	\includegraphics[width=0.5\linewidth]{simple_rolling_ball}
	\caption{A ball of radius $r$ rolls without slipping on a flat surface in the presence of a uniform gravitational field of magnitude $g$. The ball's geometric center, center of mass, and contact point with the flat surface are denoted by GC, CM, and CP, respectively.}
	\label{fig:simple_rolling_ball}
\end{figure}

There are several terminologies in the literature to describe a ball rolling without slipping on a flat surface in the presence of a uniform gravitational field, depending on its mass distribution and the location of its center of mass \footnote{ A historical note on the terminology is warranted here to avoid confusion.  A Chaplygin sphere is a ball with an inhomogeneous mass distribution, but with its center of mass located at the ball's geometric center \cite{shen2008controllability}. A Chaplygin top is a ball with an inhomogeneous mass distribution, but with its center of mass not located at the ball's geometric center \cite{shen2008controllability}. Reference \cite{Ho2011_pII} does not distinguish between these two cases, calling a Chaplygin ball a ball with an inhomogeneous mass distribution, regardless of the location of its center of mass; as a special case of a Chaplygin ball, \cite{Ho2011_pII} calls a Chaplygin concentric sphere a ball with an inhomogeneous mass distribution with its center of mass coinciding with the ball's geometric center. Thus, the Chaplygin concentric sphere (used by \cite{Ho2011_pII}) and the Chaplygin sphere (used by \cite{shen2008controllability}) are different terms for the same mechanical system. Note that a ball with a homogeneous mass distribution (in a uniform gravitational field) necessarily has its center of mass at the ball's geometric center, and is therefore not very interesting. In this paper, these terminologies are not used, rather the mechanical system is referred to simply as a ball or a rolling ball, regardless of its mass distribution (homogeneous vs inhomogeneous) and regardless of the location of its center of mass (at the ball's geometric center vs not at the ball's geometric center).
}.
In this paper, the motion of the rolling ball is investigated assuming dynamic internal structure. The dynamics of the rolling ball with static internal structure was first solved analytically by Chaplygin for the cylindrically symmetric rolling ball, i.e. a ball such that the line joining the ball's center of mass and geometric center forms an axis of symmetry, in 1897 \cite{chaplygin2002motion} and for the Chaplygin sphere in 1903 \cite{chaplygin2002ball}, though dynamical properties of the cylindrically symmetric rolling ball were previously investigated by Routh \cite{routh1884advanced} and Jellet \cite{jellett1872treatise}. The dynamics of the rolling ball with dynamic internal structure is still an active topic in the nonholonomic mechanics literature \cite{das2001design,mojabi2002introducing,shen2008controllability,borisov2012control,bolotin2012problem,gajbhiye2016geometric,GaBa:RNC3457,kilin2015spherical,burkhardt2016reduced}. \rem{For example, the recent paper \cite{GaBa:RNC3457} explores \emph{local} controllability of a rolling ball with dynamic internal structure, in which an internal pendulum and yoke serve as the control mechanisms.}

Many methods have been proposed (and some realized) to actuate a rolling ball, such as illustrated in Figure~\ref{fig_ball_actuation}. References \cite{borisov2012control,bolotin2012problem,gajbhiye2016geometric} actuate the rolling ball by internal rotors such as shown in Figure~\ref{fig_ball_rotors}, while \cite{burkhardt2014energy,davoodi2014moball,asama2015design,davoodi2015moball,bowkett2016combined,burkhardt2016reduced} actuate the rolling ball via $6$ internal magnets, each of which slides inside its own linear, solenoidal tube, i.e. a straight tube embedded within a solenoid that generates a magnetic field along the tube's longitudinal axis as illustrated in Figure~\ref{fig_ball_magnets}.
References  \cite{das2001design,mojabi2002introducing} study the locomotion and trajectory-tracking of a ball with masses moving along straight rails inside the ball, as well as practical realizations of such a device. In particular, \cite{mojabi2002introducing} actuates the rolling ball by internal masses which reciprocate along spokes. Reference \cite{shen2008controllability} actuates the rolling ball by a combination of internal rotors and sliders, \cite{kilin2015spherical} actuates the rolling ball by an internal gyroscopic pendulum as shown in Figure~\ref{fig_ball_gyro_pend}, \cite{bolotin2013motion} actuates the rolling ball by an internal spherical pendulum as shown in Figure~\ref{fig_ball_sph_pend}, and \cite{GaBa:RNC3457} actuates the rolling ball by an internal pendulum and yoke.
References \cite{karavaev2016nonholonomic,kilin2017experimental} study the theory of and experiment with a rolling ball actuated by an internal omniwheel platform, which is a more sophisticated version of Sphero's driving mechanism depicted in Figure~\ref{fig_ball_sphero_internal}. This paper considers a rolling ball actuated by internal point masses that move along arbitrarily-shaped rails fixed within the ball, such as depicted in Figure~\ref{fig_intro_bsim2_control_masses_rails}. Actuating the rolling ball by moving internal point masses along general rails has not been considered yet in the literature; references \cite{burkhardt2016reduced,das2001design,mojabi2002introducing,shen2008controllability} actuate the rolling ball by moving internal masses with inertias along linear trajectories (e.g. spokes or hollow tubes) in the ball's frame. Prior to this paper, controlling the motion of a nonholonomic mechanical system by moving internal point masses has been studied in \cite{osborne2005steering}, which investigates the controlled motion of the Chaplygin sleigh actuated by a single internal point mass.

More generally,  the optimal control of other nonholonomic mechanical systems has been investigated recently; the continuous variable transmission and the Chaplgyin sleigh are investigated in \cite{Bl-etal-2015}, while Suslov's problem is investigated in \cite{putkaradze2018constraint}.
In a comprehensive review of nonholonomic optimal control, \cite{Bloch2003} briefly discusses the controllability and optimal control (in the sense of the variational Pontryagin's minimum principle) of a rolling ball, where an external control force pushes the ball's geometric center. A recent survey \cite{borisov2017dynamical} provides an overview of the theory of mechanical systems having different types of nonintegrable velocity constraints, such as vakonomic, which is useful for control theory, and nonholonomic. Reference~\cite{shen2008controllability} investigates the controllability of a rolling ball actuated by internal rotors and sliders, while \cite{GaBa:RNC3457} investigates the controllability of a rolling ball actuated by an internal pendulum and yoke. Reference~\cite{borisov2012control} investigates the controllability and trajectory tracking control of a rolling ball actuated by internal rotors, \cite{bolotin2012problem} investigates the optimal control (in the sense of the variational Pontryagin's minimum principle) of a rolling ball actuated by internal rotors for some very specific  performance indexes, and \cite{gajbhiye2016geometric} investigates the orientation and contact point trajectory tracking control of a rolling ball actuated by internal rotors by constructing feedback control laws. Reference \cite{mojabi2002introducing} investigates the trajectory tracking control of a rolling ball, actuated by internal masses which reciprocate along spokes, by optimizing the ball's energy expense during the motion by discretizing the trajectory and finding a global minimum of a certain function, i.e. by using the direct method. Reference~\cite{kilin2015spherical} investigates the trajectory tracking control of a rolling ball actuated by an internal gyroscopic pendulum by piecing together special gaits, each of whose motion is determined analytically. To contrast with previous work, this paper investigates the optimal control (in the sense of the variational Pontryagin's minimum principle) of a rolling ball actuated by internal point masses for very general  performance indexes.

\rem{
\begin{figure}[h]
	\centering
	\subfloat[A ball actuated by $3$ rotors, studied in \cite{borisov2012control,bolotin2012problem,gajbhiye2016geometric}, \copyright\ 2016 IFAC   \cite{gajbhiye2016geometric}.]{\includegraphics[width=0.45\textwidth]{ball_rotors}\label{fig_ball_rotors}}
	\hspace{5mm}
	\subfloat[A ball actuated by $3$ point masses, each on its own circular control rail, studied in this paper.]{
		\includegraphics[width=0.45\textwidth]{bsim2_control_masses_rails_png}\label{fig_intro_bsim2_control_masses_rails}}
	\\
	\subfloat[A ball actuated by a gyroscopic pendulum, studied in \cite{kilin2015spherical}.]{
		\includegraphics[width=0.45\textwidth]{ball_gyro_pend_custom}\label{fig_ball_gyro_pend}}
	\hspace{5mm}
	\subfloat[Sphero has $4$ wheels wedged inside the spherical shell, but only the lower $2$ are spun by the motor \cite{Sphero_cut}.]{
		\includegraphics[width=0.45\textwidth]{sphero_cutaway}\label{fig_ball_sphero_internal}}
	\caption{Different methods to actuate a rolling ball.} \label{fig_ball_actuation}
\end{figure} }

\begin{figure}[h]
	\centering
	\subfloat[A ball actuated by $3$ rotors, studied in \cite{borisov2012control,bolotin2012problem,gajbhiye2016geometric}, \copyright\ 2016 IFAC   \cite{gajbhiye2016geometric}.]{\includegraphics[width=0.3\textwidth]{ball_rotors}\label{fig_ball_rotors}}
	\hspace{5mm}
	\subfloat[A ball actuated by $6$ magnets, each in its own linear, solenoidal tube, studied in \cite{burkhardt2014energy,davoodi2014moball,asama2015design,davoodi2015moball,bowkett2016combined,burkhardt2016reduced}.]{
		\includegraphics[width=0.3\textwidth]{ball_magnets}\label{fig_ball_magnets}}
	\hspace{5mm}
	\subfloat[A ball actuated by $3$ point masses, each on its own circular rail, studied in this paper and in \cite{putkaradze2018dynamics}.]{
		\includegraphics[width=0.3\textwidth]{bsim2_control_masses_rails_png}\label{fig_intro_bsim2_control_masses_rails}}
	\hspace{5mm}
	\\
	\subfloat[A ball actuated by a gyroscopic pendulum, studied in \cite{kilin2015spherical}.]{
		\includegraphics[width=0.3\textwidth]{ball_gyro_pend_custom}\label{fig_ball_gyro_pend}}
	\hspace{5mm}
	\subfloat[A ball actuated by a spherical pendulum, studied in \cite{bolotin2013motion}.]{
		\includegraphics[width=0.3\textwidth]{ball_sph_pend}\label{fig_ball_sph_pend}}
	\hspace{5mm}
	\subfloat[Sphero has $4$ wheels wedged inside the spherical shell, but only the lower $2$ are spun by the motor \cite{Sphero_cut}.]{
		\includegraphics[width=0.3\textwidth]{sphero_cutaway_resize3}\label{fig_ball_sphero_internal}}
	\caption{Different methods to actuate a rolling ball.} \label{fig_ball_actuation}
\end{figure}

\subsection{Contributions and Outline}
The key contributions of this paper are listed below.
\begin{itemize}
	\item The controlled equations of motion for a rolling ball actuated by internal point masses that move along arbitrarily-shaped rails fixed within the ball are constructed, a solution of which obeys the uncontrolled equations of motion, satisfies prescribed initial and final conditions, and minimizes a prescribed performance index. The controlled equations of motion are solved numerically by a predictor-corrector continuation method, starting from an initial solution provided by a direct method.
		\item Jacobians of the ordinary differential equations (ODEs) and boundary conditions (BCs) which constitute the controlled equations of motion (i.e. an ODE TPBVP) corresponding to the optimal control of a dynamical system are derived. These Jacobians are useful for numerically solving the controlled equations of motion.
	\item Algorithms for solving an ODE TPBVP by predictor-corrector continuation are developed and were implemented in \mcode{MATLAB} to numerically solve the controlled equations of motion for the rolling ball. There are not very many predictor-corrector continuation methods publicly available for solving dynamical systems. The idea of using a monotonic continuation ODE TPBVP solver in conjunction with a predictor-corrector continuation method to advance (or ``sweep'') as far along the tangent as possible is new,  and this novel technique was used to obtain all the numerical results in this paper.
\end{itemize}

The paper is organized as follows.  Section~\ref{sec_uncontrolled} discusses the specific type of rolling disk and ball considered, defines coordinate systems and notation used to describe this rolling disk and ball, and presents the uncontrolled equations of motion for this rolling disk and ball derived earlier in \cite{putkaradze2018dynamics}. Section~\ref{sec_ball_controlled} formulates the regular Hamiltonian and endpoint function required to construct the controlled equations of motion for the rolling disk and ball. In Section~\ref{sec_ball_numerics}, the controlled equations of motion for the rolling disk and ball are solved numerically via a predictor-corrector continuation method, starting from an initial solution provided by a direct method. Section~\ref{sec_conclusions} summarizes the results of the paper and discusses topics for future work.  The background material for this paper is contained in several Appendices. In particular, Appendix~\ref{sec_optimal_control} reviews the theory of optimal control needed to derive the controlled equations of motion for a generic dynamical system given initial and final conditions, given a performance index to be minimized, and in the absence of path inequality constraints. Appendix~\ref{app_imp_details} provides details for numerically solving the controlled equations of motion. Appendices~\ref{app_predictor_corrector} and \ref{app_sweep_predictor_corrector} develop  two predictor-corrector continuation algorithms which numerically solve an ODE TPBVP, the latter of which is utilized to numerically solve the controlled equations of motion for the rolling disk and ball. Appendix~\ref{app_ccem} derives formulas needed to construct the controlled equations of motion for the rolling disk and ball.

\section{Uncontrolled Equations of Motion \label{sec_uncontrolled} }
This section presents the notation, coordinate systems, and assumptions used to describe the rolling ball and its special case, the rolling disk. The uncontrolled equations of motion for these mechanical systems are recalled from \cite{putkaradze2018dynamics}. We refer  the interested reader to that paper for the  detailed derivations.

\subsection{Uncontrolled Equations of Motion for the Rolling Ball} \label{ssec_ball_uncontrolled}
\begin{figure}[h]
	\centering
	\includegraphics[width=0.5\linewidth]{detailed_1dparam_rboc_rolling_ball}
	\caption{A ball of radius $r$ and mass $m_0$ rolls without slipping on a flat surface in the presence of a uniform gravitational field of magnitude $g$. The ball's geometric center, center of mass, and contact point with the flat surface are denoted by GC, $m_0$, and CP, respectively. The spatial frame has origin located at height $r$ above the flat surface and orthonormal axes $\mathbf{e}_1$, $\mathbf{e}_2$, and $\mathbf{e}_3$. The body frame has origin located at the ball's center of mass (denoted by $m_0$) and orthonormal axes $\mathbf{E}_1$, $\mathbf{E}_2$, and $\mathbf{E}_3$. The ball's motion is actuated by $n$ point masses, each of mass $m_i$, $1 \le i \le n$, and each moving along its own rail fixed inside the ball. The $i^\mathrm{th}$ rail is depicted here by the dashed hoop. The position of the $i^\mathrm{th}$ rail, with respect to the body frame translated to the GC, is denoted by $\boldsymbol{\zeta}_i$ and is parameterized by $\theta_i$. All vectors inside the ball are expressed with respect to the body frame, while all vectors outside the ball are expressed with respect to the spatial frame. }
	\label{fig:detailed_1dparam_rolling_ball}
\end{figure}
Consider a rigid ball of radius $r$ containing some static internal structure as well as $n \in \mathbb{N}^0$ point masses. This ball rolls without slipping on a flat  surface in the presence of a uniform gravitational field of magnitude $g$, as illustrated in Figure~\ref{fig:detailed_1dparam_rolling_ball}. The ball with its static internal structure has mass $m_0$ and the $i^\mathrm{th}$ point mass has mass $m_i$ for $1 \le i \le n$. Let $M = \sum_{i=0}^n m_i$ denote the mass of the total system. The total mechanical system consisting of the ball with its static internal structure and the $n$ point masses is referred to as the ball or the rolling ball, the ball with its static internal structure but without the $n$  point masses may also be referred to as $m_0$, and the $i^\mathrm{th}$  point mass may also be referred to as $m_i$ for $1 \le i \le n$.

Two coordinate systems, or frames of reference, will be used to describe the motion of the rolling ball, an inertial spatial coordinate system and a body coordinate system in which each particle within the ball is always fixed. For brevity, the spatial coordinate system will be referred to as the spatial frame and the body coordinate system will be referred to as the body frame. These two frames are depicted in Figure~\ref{fig:detailed_1dparam_rolling_ball}. The spatial frame has orthonormal axes $\mathbf{e}_1$, $\mathbf{e}_2$, $\mathbf{e}_3$, such that the $\mathbf{e}_1$-$\mathbf{e}_2$ plane is parallel to the flat surface and passes through the ball's geometric center (i.e. the $\mathbf{e}_1$-$\mathbf{e}_2$ plane is a height $r$ above the flat surface), such that $\mathbf{e}_3$ is vertical (i.e. $\mathbf{e}_3$ is perpendicular to the flat surface) and points ``upward" and away from the flat surface, and such that $\left(\mathbf{e}_1, \mathbf{e}_2, \mathbf{e}_3 \right)$ forms a right-handed coordinate system. For simplicity, the spatial frame axes are chosen to be
\begin{equation}
\mathbf{e}_1 = \begin{bmatrix} 1 & 0 & 0 \end{bmatrix}^\mathsf{T}, \quad \mathbf{e}_2 = \begin{bmatrix} 0 & 1 & 0 \end{bmatrix}^\mathsf{T}, \quad \mathrm{and} \quad \mathbf{e}_3 = \begin{bmatrix} 0 & 0 & 1 \end{bmatrix}^\mathsf{T}.
\end{equation}
The acceleration due to gravity in the uniform gravitational field is $\mathbf{g} = -g \mathbf{e}_3  = \begin{bmatrix} 0 & 0 & -g  \end{bmatrix}^\mathsf{T}$ in the spatial frame.

The body frame's origin is chosen to coincide with the position of $m_0$'s center of mass. The body frame has orthonormal axes $\mathbf{E}_1$, $\mathbf{E}_2$, and $\mathbf{E}_3$, chosen to coincide with $m_0$'s principal axes, in which $m_0$'s inertia tensor $\inertia$ is diagonal, with corresponding principle moments of inertia $d_1$, $d_2$, and $d_3$. That is, in this body frame the inertia tensor is the diagonal matrix $\inertia = \mathrm{ \textbf{diag}} \left( \begin{bmatrix} d_1 & d_2 & d_3 \end{bmatrix} \right)$.
Moreover, $\mathbf{E}_1$, $\mathbf{E}_2$, and $\mathbf{E}_3$ are chosen so that $\left(\mathbf{E}_1, \mathbf{E}_2, \mathbf{E}_3 \right)$ forms a right-handed coordinate system. For simplicity, the body frame axes are chosen to be
\begin{equation}
\mathbf{E}_1 = \begin{bmatrix} 1 & 0 & 0 \end{bmatrix}^\mathsf{T}, \quad \mathbf{E}_2 = \begin{bmatrix} 0 & 1 & 0 \end{bmatrix}^\mathsf{T}, \quad \mathrm{and} \quad \mathbf{E}_3 = \begin{bmatrix} 0 & 0 & 1 \end{bmatrix}^\mathsf{T}.
\end{equation}
In the spatial frame, the body frame is the moving frame $\left(\Lambda \left(t\right) \mathbf{E}_1, \Lambda \left(t\right) \mathbf{E}_2, \Lambda \left(t\right) \mathbf{E}_3  \right)$, where $\Lambda \left(t\right) \in SO(3)$ defines the orientation (or attitude) of the ball at time $t$ relative to its reference configuration, for example at some initial time.

For $1 \le i \le n$, it is asssumed that $m_i$ moves along its own 1-d rail. It is further assumed that the $i^\mathrm{th}$  rail is parameterized by a 1-d parameter $\theta_i$, so that the position $\boldsymbol{\zeta}_i$ of the $i^\mathrm{th}$  rail, in the body frame translated to the ball's geometric center, as a function of $\theta_i$ is $\boldsymbol{\zeta}_i(\theta_i)$. Refer to Figure~\ref{fig:detailed_1dparam_rolling_ball} for an illustration. Therefore, the body frame vector from the ball's geometric center to $m_i$'s center of mass is denoted by $\boldsymbol{\zeta}_i(\theta_i (t))$. Since $m_0$ is stationary in the body frame and to be consistent with the positional notation for $m_i$ for $1 \le i \le n$, $\boldsymbol{\zeta}_0 \equiv \boldsymbol{\zeta}_0(\theta_0) \equiv \boldsymbol{\zeta}_0(\theta_0 (t))$ is the constant vector from the ball's geometric center to $m_0$'s center of mass for any scalar-valued, time-varying function $\theta_0(t)$. In addition, suppose a time-varying external force $\mathbf{F}_\mathrm{e}(t)$ acts at the ball's geometric center.

Let $\mathbf{z}_i(t)$ denote the position of $m_i$'s center of mass in the spatial frame so that the position of $m_i$'s center of mass in the spatial frame is $\mathbf{z}_i(t)=\mathbf{z}_0(t)+\Lambda(t) \left[\boldsymbol{\zeta}_i(\theta(t))-\boldsymbol{\zeta}_0\right]$. In general, a particle with position $\mathbf{w}(t)$ in the body frame has position $\mathbf{z}(t) = \mathbf{z}_0(t)+\Lambda(t) \mathbf{w}(t)$ in the spatial frame and has position $\mathbf{w}(t)+\boldsymbol{\zeta}_0$ in the body frame translated to the ball's geometric center.

For conciseness, the ball's geometric center is often denoted GC and $m_0$'s center of mass is often denoted CM. The GC is located at $\mathbf{z}_\mathrm{GC}(t) = \mathbf{z}_0(t)-\Lambda(t) \boldsymbol{\zeta}_0$ in the spatial frame, at $-\boldsymbol{\zeta}_0$ in the body frame, and at $\mathbf{0} = \begin{bmatrix} 0 & 0 & 0 \end{bmatrix}^\mathsf{T}$ in the body frame translated to the GC. The CM is located at $\mathbf{z}_0(t)$ in the spatial frame, at $\mathbf{0}$ in the body frame, and at $\boldsymbol{\zeta}_0$ in the body frame translated to the GC.

For succintness, the explicit time dependence of variables is often dropped. That is, the orientation of the ball at time $t$ is denoted simply $\Lambda$ rather than $\Lambda(t)$, the position of $m_i$'s center of mass in the spatial frame at time $t$ is denoted $\mathbf{z}_i$ rather than $\mathbf{z}_i(t)$,  the position of $m_i$'s center of mass in the body frame translated to the GC at time $t$ is denoted $\boldsymbol{\zeta}_i$ or $\boldsymbol{\zeta}_i(\theta_i)$ rather than $\boldsymbol{\zeta}_i(\theta_i(t))$, and the external force is denoted $\mathbf{F}_\mathrm{e}$ rather than $\mathbf{F}_\mathrm{e}(t)$.

As shown in \cite{putkaradze2018dynamics}, the uncontrolled equations of motion for this rolling ball are
\begin{equation}
\begin{split} \label{uncon_ball_eqns_explicit_1d}
\dot \boldsymbol{\Omega} &= \left[\sum_{i=0}^n m_i \widehat{\mathbf{s}_i}^2  -\inertia \right]^{-1}  \Bigg[\boldsymbol{\Omega} \times \inertia \boldsymbol{\Omega}+r \tilde \boldsymbol{\Gamma} \times \boldsymbol{\Gamma}
\\ &\hphantom{=} + \sum_{i=0}^n m_i \mathbf{s}_i \times  \left\{ g \boldsymbol{\Gamma}+ \boldsymbol{\Omega} \times \left(\boldsymbol{\Omega} \times \boldsymbol{\zeta}_i +2 \dot \theta_i \boldsymbol{\zeta}_i^{\prime} \right) + \dot \theta_i^2 \boldsymbol{\zeta}_i^{\prime \prime} + \ddot \theta_i  \boldsymbol{\zeta}_i^{\prime} \right\}  \Bigg], \\
\dot \boldsymbol{\Gamma} &= \boldsymbol{\Gamma} \times \boldsymbol{\Omega},
\end{split}
\end{equation}
where $\mathbf{s}_i \equiv r \boldsymbol{\Gamma} +\boldsymbol{\zeta}_i$ for $0\le i\le n$, $\boldsymbol{\Omega} \equiv \left( \Lambda^{-1} \dot \Lambda \right)^\vee$, $\boldsymbol{\Gamma} \equiv \Lambda^{-1} \mathbf{e}_3$, and $\tilde \boldsymbol{\Gamma} \equiv \Lambda^{-1} \mathbf{F}_\mathrm{e}$. For $\mathbf{v} = \begin{bmatrix} v_1 & v_2 & v_3  \end{bmatrix}^\mathsf{T} \in \mathbb{R}^3$, $\widehat{\mathbf{v}}^2=\widehat{\mathbf{v}}\widehat{\mathbf{v}}$ is the symmetric matrix given by
\begin{equation}
\widehat{\mathbf{v}}^2 = \begin{bmatrix}
-(v_2^2+v_3^2) & v_1 v_2 & v_1 v_3 \\
v_1 v_2 & -(v_1^2+v_3^2)  & v_2 v_3 \\
v_1 v_3 & v_2 v_3 & -(v_1^2+v_2^2)
\end{bmatrix}.
\end{equation}

\subsection{Uncontrolled Equation of Motion for the Rolling Disk} \label{ssec_disk_uncontrolled}
\begin{figure}[h]
	\centering
	\includegraphics[width=0.6\linewidth]{detailed_rboc_rolling_disk}
	\caption{A disk of radius $r$ and mass $m_0$ rolls without slipping in the $\mathbf{e}_1$-$\mathbf{e}_3$ plane. $\mathbf{e}_2$ and $\mathbf{E}_2$ are directed into the page and are omitted from the figure. The disk's center of mass is denoted by $m_0$. The disk's motion is actuated by $n$ point masses, each of mass $m_i$, $1 \le i \le n$, and each moving along its own rail fixed inside the disk. The point mass depicted here by $m_i$ moves along a circular hoop in the disk that is not centered on the disk's geometric center (GC). The disk's orientation is determined by $\phi$, the angle measured counterclockwise from $\mathbf{e}_1$ to $\mathbf{E}_1$.  }
	\label{fig:detailed_rolling_disk}
\end{figure}
Now suppose that $m_0$'s inertia is such that one of $m_0$'s principal axes, say the one labeled $\mathbf{E}_2$, is orthogonal to the plane containing the GC and CM. Also assume that all the  point masses move along 1-d  rails which lie in the plane containing the GC and CM. Moreover, suppose that the ball is oriented initially so that the plane containing the GC and CM coincides with the $\mathbf{e}_1$-$\mathbf{e}_3$ plane and that the external force $\mathbf{F}_\mathrm{e}$ acts in the $\mathbf{e}_1$-$\mathbf{e}_3$ plane. Then for all time, the ball will remain oriented so that the plane containing the GC and CM coincides with the $\mathbf{e}_1$-$\mathbf{e}_3$ plane and the ball will only move in the $\mathbf{e}_1$-$\mathbf{e}_3$ plane, with the ball's rotation axis always parallel to $\mathbf{e}_2$. Note that the dynamics of this system are equivalent to that of the Chaplygin disk \cite{Ho2011_pII}, equipped with point masses, rolling in the $\mathbf{e}_1$-$\mathbf{e}_3$ plane, and where the Chaplygin disk (minus the  point masses) has polar moment of inertia $d_2$. Therefore, henceforth, this particular ball with this special inertia, orientation,  and placement of the rails and point masses, may be referred to as the disk or the rolling disk. Figure~\ref{fig:detailed_rolling_disk} depicts the rolling disk. Let $\phi$ denote the angle between $\mathbf{e}_1$ and $\mathbf{E}_1$, measured counterclockwise from $\mathbf{e}_1$ to $\mathbf{E}_1$. Thus, if $\dot \phi > 0$, the disk rolls in the $-\mathbf{e}_1$ direction and $\boldsymbol{\Omega}$ has the same direction as $-\mathbf{e}_2$, and  if $\dot \phi < 0$, the disk rolls in the $\mathbf{e}_1$ direction and $\boldsymbol{\Omega}$ has the same direction as $\mathbf{e}_2$.

As shown in \cite{putkaradze2018dynamics}, the uncontrolled equation of motion for this rolling disk is
\begin{equation} \label{eqmo_chap_disk_4}
\ddot \phi = \frac{ -r F_{\mathrm{e},1}+ \sum_{i=0}^n m_i K_i }{d_2+\sum_{i=0}^n m_i \left[\left( r \sin \phi + \zeta_{i,1} \right)^2+\left( r \cos \phi+ \zeta_{i,3} \right)^2 \right]} \equiv \kappa\left(t,\boldsymbol{\theta},\dot \boldsymbol{\theta},\phi,\dot \phi,\ddot \boldsymbol{\theta}\right),
\end{equation}
where
\begin{equation} \label{eq_K_i}
\begin{split}
K_i &\equiv  \left(g+ r {\dot \phi}^2 \right) \left(\zeta_{i,3} \sin \phi - \zeta_{i,1} \cos \phi  \right)+
\left(r \cos \phi + \zeta_{i,3} \right) \left(- 2 \dot \phi {\dot \theta}_i \zeta_{i,3}^{\prime} + {\dot \theta}_i^2 \zeta_{i,1}^{\prime \prime} + {\ddot \theta}_i \zeta_{i,1}^{\prime} \right)\\
&\hphantom{\equiv}   - \left(r \sin \phi + \zeta_{i,1} \right) \left( 2 \dot \phi {\dot \theta}_i \zeta_{i,1}^{\prime}+ {\dot \theta}_i^2 \zeta_{i,3}^{\prime \prime} + {\ddot \theta}_i \zeta_{i,3}^{\prime} \right).
\end{split}
\end{equation}
In \eqref{eqmo_chap_disk_4}, $\kappa$ is a function that depends on time ($t$) through the possibly time-varying external force $F_{\mathrm{e},1}(t)$, on the  point mass parameterized positions ($\boldsymbol{\theta}$), velocities ($\dot \boldsymbol{\theta}$), and accelerations ($\ddot \boldsymbol{\theta}$), and on the disk's orientation angle ($\phi$) and its time-derivative ($\dot \phi$).

\section{Controlled Equations of Motion}
\label{sec_ball_controlled}

This section uses the results of the previous section to derive the controlled equations of motion for the rolling disk and ball.
Subsection~\ref{ssec_disk_controlled} exploits the uncontrolled equations of motion \eqref{eqmo_chap_disk_4} to derive the controlled equations of motion for a rolling disk actuated by internal point masses that move along arbitrarily-shaped rails fixed within the disk. Subsection~\ref{ssec_ball_controlled} exploits the uncontrolled equations of motion \eqref{uncon_ball_eqns_explicit_1d} to derive the controlled equations of motion for a rolling ball actuated by internal point masses that move along arbitrarily-shaped rails fixed within the ball.

\subsection{Controlled Equations of Motion for the Rolling Disk} \label{ssec_disk_controlled}
Before  treating the problem of  the rolling ball, the controlled equations of motion are first developed for the rolling disk, which is a simpler mechanical system  with substantially simpler equations.
Let $z$ and $\dot z$ denote the spatial $\mathbf{e}_1$ position and velocity, respectively, of the disk's GC, and recall that $\boldsymbol{\theta} = \begin{bmatrix} \theta_1 & \theta_2 & \dots & \theta_n \end{bmatrix}^\mathsf{T}$ denotes the vector of the control mass parameterizations. If the disk's GC is at initial spatial $\mathbf{e}_1$ position $z_a$ and if the disk's initial orientation is $\phi_a$ at initial time $t=a$, note that the spatial $\mathbf{e}_1$ position and velocity of the disk's GC are $z = z_a - r \left(\phi-\phi_a \right)$ and $\dot z = -r \dot \phi$, respectively, due to the sign convention adopted for $\phi$ in Subsection~\ref{ssec_disk_uncontrolled}. The rotation matrix that maps the body to spatial frame at time $t$ is a function of $\phi(t)$ and is given by
\begin{equation}
\tilde \Lambda (\phi(t)) = \Lambda(t) = \begin{bmatrix} \cos \phi(t) & 0 & - \sin \phi(t) \\ 0 & 1 & 0 \\ \sin \phi(t) & 0 & \cos \phi(t) \end{bmatrix}.
\end{equation}

Suppose it is desired to roll the disk from some initial configuration at a prescribed or free initial time $a$ to some final configuration at a prescribed or free final time $b$, without moving the control masses too rapidly along their control rails. In addition, in between the initial and final times, it may be desired that the disk's GC tracks a prescribed spatial $\mathbf{e}_1$ path $z_\mathrm{d}$ or traces out a minimum energy path. Finally, if the initial or final time is free, it may be desired to minimize the duration $b-a$ of the maneuver. How must the control masses be moved in order to accomplish these tasks? This problem can be solved by posing it as an optimal control problem.

Concretely, at the prescribed or free initial time $a$,  the positions of the control mass parameterizations are prescribed to be $\boldsymbol{\theta}(a) = \boldsymbol{\theta}_a$, the velocities of the control mass parameterizations are prescribed to be ${\dot \boldsymbol{\theta}}(a) = {\dot \boldsymbol{\theta}}_a$, the spatial $\mathbf{e}_1$ position of the disk's GC is prescribed to be $z(a) = z_a - r \left(\phi(a)-\phi_a\right)= z_a$ (which is equivalent to prescribing the disk's orientation to be $\phi(a)=\phi_a$), and the spatial $\mathbf{e}_1$ velocity of the disk's GC is prescribed to be ${\dot z}(a) = -r \dot \phi(a) = {\dot z}_a$ (which is equivalent to prescribing the rate of change of the disk's orientation to be $\dot \phi(a)=-\frac{{\dot z}_a}{r}$).

Furthermore, at the prescribed or free final time $b$, some components (determined by the projection operator $\boldsymbol{\Pi}$) of the disk's center of mass expressed in the spatial frame translated to the GC are prescribed to be
\begin{equation}
\boldsymbol{\Pi} \left(\tilde \Lambda\left(\phi(b)\right) \left[ \frac{1}{M} \sum_{i=0}^n m_i \boldsymbol{\zeta}_i\left(\theta_i(b)\right) \right] \right) = \boldsymbol{\Delta}_b,
\end{equation}
the velocities of the control mass parameterizations are prescribed to be ${\dot \boldsymbol{\theta}}(b) = {\dot \boldsymbol{\theta}}_b$, the spatial $\mathbf{e}_1$ position of the disk's GC is prescribed to be $z(b) =  z_a - r \left(\phi(b)-\phi_a\right) = z_b$ (which is equivalent to prescribing the disk's orientation to be $\phi(b)=\phi_a-\frac{z_b-z_a}{r}$), and the spatial $\mathbf{e}_1$ velocity of the disk's GC is prescribed to be ${\dot z}(b) = -r \dot \phi(b) = {\dot z}_b$ (which is equivalent to prescribing the rate of change of the disk's orientation to be $\dot \phi(b)=-\frac{{\dot z}_b}{r}$).

For example, if it is desired to start and stop the disk at rest, then $\boldsymbol{\Pi}$ is projection onto the first component, $\boldsymbol{\Delta}_b=\Delta_b = 0$, $\boldsymbol{\theta}_a$ and $\phi_a$ are such that
\begin{equation} \label{eq_disk_stab_init}
\boldsymbol{\Pi} \left(\tilde \Lambda\left(\phi_a\right) \left[ \frac{1}{M} \sum_{i=0}^n m_i \boldsymbol{\zeta}_i\left(\theta_{a,i}\right) \right] \right) = 0,
\end{equation}
$ {\dot \boldsymbol{\theta}}_a = \mathbf{0}$, ${\dot z}_a = 0$, $\boldsymbol{\theta}(b)$ and $\phi(b)$ are such that
\begin{equation} \label{eq_disk_stab_fin}
\boldsymbol{\Pi} \left(\tilde \Lambda\left(\phi(b)\right) \left[ \frac{1}{M} \sum_{i=0}^n m_i \boldsymbol{\zeta}_i\left(\theta_i(b)\right) \right] \right) = 0,
\end{equation}
${\dot \boldsymbol{\theta}}_b = \mathbf{0}$, and ${\dot z}_b = 0$. With this choice of $\boldsymbol{\Pi}$, \eqref{eq_disk_stab_init} and \eqref{eq_disk_stab_fin} mean that the CM in the spatial frame translated to the GC is above or below the GC at the initial and final times.

The system state ${\boldsymbol {x} }$ and control $\boldsymbol{u}$ are
\begin{equation}
{\boldsymbol {x} } = \begin{bmatrix} \boldsymbol{\theta} \\ \dot \boldsymbol{\theta} \\ \phi \\ \dot \phi  \end{bmatrix} \quad \mathrm{and} \quad \boldsymbol{u} = \ddot \boldsymbol{\theta}.
\end{equation}
The system dynamics defined for $a \le t \le b$ are
\begin{equation}
\dot {{\boldsymbol {x} }} = \begin{bmatrix} \dot \boldsymbol{\theta} \\ \ddot \boldsymbol{\theta} \\ \dot \phi \\ \ddot \phi  \end{bmatrix}  = \mathbf{f}\left(t,{\boldsymbol {x} },\boldsymbol{u},\mu\right) \equiv \begin{bmatrix} \dot \boldsymbol{\theta} \\ \boldsymbol{u}  \\ \dot \phi \\ \kappa\left(t,{\boldsymbol {x} },\boldsymbol{u}\right)  \end{bmatrix},
\end{equation}
where $\kappa\left(t,{\boldsymbol {x} },\boldsymbol{u} \right)$ is given by the right-hand side of \eqref{eqmo_chap_disk_4}, the prescribed initial conditions at time $t=a$ are
\begin{equation} \label{eq_disk_initial_conds}
\boldsymbol{\sigma}\left(a,{\boldsymbol {x} }(a),\mu\right) = \begin{bmatrix} \boldsymbol{\theta}(a) - \boldsymbol{\theta}_a \\ \dot \boldsymbol{\theta}(a) - {\dot \boldsymbol{\theta}}_a \\ \phi(a)-\phi_a \\ -r \dot \phi(a) - {\dot z}_a  \end{bmatrix} = \mathbf{0},
\end{equation}
and the prescribed final conditions at time $t=b$ are
\begin{equation} \label{eq_disk_final_conds}
\boldsymbol{\psi}\left(b,{\boldsymbol {x} }(b),\mu\right) = \begin{bmatrix} \boldsymbol{\Pi} \left(\tilde \Lambda\left(\phi(b)\right) \left[ \frac{1}{M} \sum_{i=0}^n m_i \boldsymbol{\zeta}_i\left(\theta_i(b)\right) \right] \right) - \boldsymbol{\Delta}_b \\ \dot \boldsymbol{\theta}(b) - {\dot \boldsymbol{\theta}}_b  \\ z_a - r \left(\phi(b)-\phi_a\right) - z_b \\ -r \dot \phi(b) - {\dot z}_b  \end{bmatrix} = \mathbf{0}.
\end{equation}

Consider the endpoint and integrand cost functions
\begin{equation} \label{eq_disk_endpoint_cost}
p\left(a,{\boldsymbol {x} }(a),b,{\boldsymbol {x} }(b),\mu\right)=\frac{\upsilon_a}{2}\left(a-a_\mathrm{e} \right)^2+\frac{\upsilon_b}{2}\left(b-b_\mathrm{e} \right)^2
\end{equation}
and
\begin{equation} \label{eq_disk_integrand_cost}
L\left(t,{\boldsymbol {x} },\boldsymbol{u},\mu\right) = \frac{\alpha}{2} \left(z_a - r \left(\phi-\phi_a\right) - z_\mathrm{d} \right)^2+ \frac{\beta}{2} \left(- r \dot \phi \right)^2 + \sum_{i=1}^n \frac{\gamma_i}{2} {\ddot \theta}_i^2+\delta,
\end{equation}
for constants $a_\mathrm{e}$ and $b_\mathrm{e}$ and for fixed nonnegative constants $\upsilon_a$, $\upsilon_b$, $\alpha$, $\beta$, $\gamma_i$, $1 \le i \le n$, and $\delta$ so that the performance index is
\begin{equation} \label{eq_disk_J}
\begin{split}
J &= p\left(a,{\boldsymbol {x} }(a),b,{\boldsymbol {x} }(b),\mu\right)+ \int_a^b L\left(t,{\boldsymbol {x} },\boldsymbol{u},\mu\right) \mathrm{d}t \\
&= \frac{\upsilon_a}{2}\left(a-a_\mathrm{e} \right)^2+\frac{\upsilon_b}{2}\left(b-b_\mathrm{e} \right)^2+\int_a^b \left[ \frac{\alpha}{2} \left(z_a - r \left(\phi-\phi_a \right) - z_\mathrm{d} \right)^2+ \frac{\beta}{2} \left(- r \dot \phi \right)^2  + \sum_{i=1}^n \frac{\gamma_i}{2} {\ddot \theta}_i^2 + \delta \right] \mathrm{d}t.
\end{split}
\end{equation}
The first summand $\frac{\upsilon_a}{2}\left(a-a_\mathrm{e} \right)^2$ in $p$ encourages the initial time $a$ to be near $a_\mathrm{e}$ if the initial time is free, while the second summand $\frac{\upsilon_b}{2}\left(b-b_\mathrm{e} \right)^2$ in $p$ encourages the final time $b$ to be near $b_\mathrm{e}$ if the final time is free. The first summand $\frac{\alpha}{2} \left(z_a - r (\phi-\phi_a) - z_\mathrm{d} \right)^2$ in $L$ encourages the disk's GC to track the desired spatial $\mathbf{e}_1$ path $z_\mathrm{d}$, the second summand $\frac{\beta}{2} \left(- r \dot \phi \right)^2 $ in $L$ encourages the disk's GC to track a minimum energy path, the next $n$ summands $\frac{\gamma_i}{2} {\ddot \theta}_i^2$, $1 \le i \le n$, in $L$ limit the magnitude of the acceleration of the $i^\mathrm{th}$ control mass parameterization, and the final summand $\delta$ in $L$ encourages a minimum time maneuver.
For example, the desired spatial $\mathbf{e}_1$ path might have the form
\begin{equation} \label{eq_z_d}
z_\mathrm{d}(t) = \left[z_a w(t)+\tilde{z}_\mathrm{d}(t) \left(1-w(t)\right) \right] \left(1-y(t)\right)+z_b y(t),
\end{equation}
where
\begin{equation} \label{eq_sigmoid}
S(t)=\frac{1}{2} \left[1+\tanh{\left( \frac{-t}{\epsilon}\right)} \right],
\end{equation}
\begin{equation} \label{eq_tran_sigmoidl}
w(t)=S\left(t-a_\mathrm{e}\right),
\end{equation}
and
\begin{equation} \label{eq_tran_sigmoidr}
y(t)=S\left(-t+b_\mathrm{e}\right).
\end{equation}
$z_\mathrm{d}$ \eqref{eq_z_d} holds steady at $z_a$ for $t < a_\mathrm{e}$, smoothly transitions between $z_a$ and $\tilde{z}_\mathrm{d}$ at $t = a_\mathrm{e}$, follows $\tilde{z}_\mathrm{d}$ for $a_\mathrm{e} < t < b_\mathrm{e}$, smoothly transitions between $\tilde{z}_\mathrm{d}$ and $z_b$ at $t = b_\mathrm{e}$, and holds steady at $z_b$ for $b_\mathrm{e} < t$.  $S$ \eqref{eq_sigmoid} is a time-reversed sigmoid function, i.e. a smooth approximation of the time-reversed unit step function; $\epsilon$ in \eqref{eq_sigmoid}  is a parameter such as $.01$ that determines how rapidly $S$ \eqref{eq_sigmoid} transitions from $1$ to $0$ at time 0. $w$ \eqref{eq_tran_sigmoidl} is the time-translation of $S$ \eqref{eq_sigmoid} to time $a_\mathrm{e}$ and $y$ \eqref{eq_tran_sigmoidr} is the time-translation of the time-reversal of $S$ \eqref{eq_sigmoid} to time $b_\mathrm{e}$. $w$ \eqref{eq_tran_sigmoidl} enables $z_\mathrm{d}$ \eqref{eq_z_d} to smoothly transition between $z_a$ and $\tilde{z}_\mathrm{d}$ at $t = a_\mathrm{e}$, while $y$ \eqref{eq_tran_sigmoidr} enables $z_\mathrm{d}$ \eqref{eq_z_d} to smoothly transition between $\tilde{z}_\mathrm{d}$ and $z_b$ at $t = b_\mathrm{e}$. $\tilde{z}_\mathrm{d}$, which appears in \eqref{eq_z_d}, might be the cubic polynomial
\begin{equation} \label{eq_tz_d}
\tilde{z}_\mathrm{d}(t) = k_1 \left[-\frac{1}{3}t^3+\frac{1}{2}\left(a_\mathrm{e}+b_\mathrm{e} \right)t-a_\mathrm{e} b_\mathrm{e} t+k_2 \right]= k_1 \left[q(t)+k_2 \right],
\end{equation}
where
\begin{equation} q(t) = -\frac{1}{3}t^3+\frac{1}{2}\left(a_\mathrm{e}+b_\mathrm{e} \right)t-a_\mathrm{e} b_\mathrm{e} t, \quad k_1 = \frac{z_b-z_a}{q(b_\mathrm{e})-q(a_\mathrm{e})}, \quad \mathrm{and} \quad k_2 = \frac{z_b}{k_1}-q(b_\mathrm{e}).  \end{equation}
$\tilde{z}_\mathrm{d}$ has the special properties $\tilde{z}_\mathrm{d}(a_\mathrm{e})=z_a$, $\dot {\tilde{z}}_\mathrm{d}(a_\mathrm{e})=0$, $\tilde{z}_\mathrm{d}(b_\mathrm{e})=z_b$, and $\dot {\tilde{z}}_\mathrm{d}(b_\mathrm{e})=0$, so that the disk's GC is encouraged to start with zero velocity at $\mathbf{e}_1$-coordinate $z_a$ at $t=a_\mathrm{e}$ and to stop with zero velocity at $\mathbf{e}_1$-coordinate $z_b$ at $t=b_\mathrm{e}$. Figure~\ref{fig:plot_rolling_disk_z_d} illustrates \eqref{eq_z_d} using \eqref{eq_tz_d} with $a_\mathrm{e}=0$, $z_a=0$, $b_\mathrm{e}=2$, $z_b=10$, and $\epsilon=.01$.

\begin{figure}[h]
	\centering
	\includegraphics[width=0.5\linewidth]{plot_rolling_disk_z_d}
	\caption{Plot of the desired path of the disk's GC. The disk's GC starts from rest at $z=0$ at time $t=0$, moves to the right for $0<t<2$, and stops at rest at $z=10$ at time $t=2$.}
	\label{fig:plot_rolling_disk_z_d}
\end{figure}

The optimal control problem for the rolling disk is
\begin{equation}
\min_{a,b,\boldsymbol{u}} J
\mbox{\, s.t. \,}
\left\{
\begin{array}{ll}
\dot {{\boldsymbol {x} }} = \mathbf{f}\left(t,{\boldsymbol {x} },\boldsymbol{u},\mu\right), \\
\boldsymbol{\sigma}\left(a,{\boldsymbol {x} }(a),\mu\right) = \mathbf{0},\\
\boldsymbol{\psi}\left(b,{\boldsymbol {x} }(b),\mu\right) = \mathbf{0}.
\end{array}
\right.
\label{dyn_opt_problem_disk}
\end{equation}
Observe that the optimal control problem encapsulated by \eqref{dyn_opt_problem_disk} ignores path inequality constraints such as $\mathbf{D}\left(t,{\boldsymbol {x} },\boldsymbol{u},\mu\right) \le \mathbf{0}$, where $\mathbf{D}$ is an $r \times 1$ vector-valued function. Path inequality constraints can be incorporated in \eqref{dyn_opt_problem_disk} as soft constraints through penalty functions in the integrand cost function $L$ or the endpoint cost function $p$.

The indirect method, discussed in Subsection~\ref{ssec_pmp}, is applied now to \eqref{dyn_opt_problem_disk} to construct the endpoint function and regular Hamiltonian needed to formulate the ODE TPBVP \eqref{eq_pmp_bvp}, \eqref{eq_pmp_lbc2}, and \eqref{eq_pmp_rbc2}, which render the controlled equations of motion for the rolling disk.
The endpoint function is
\begin{equation} \label{eq_disk_endpoint_fcn}
\begin{split}
G\left(a,{\boldsymbol {x} }(a),\boldsymbol{\xi},b,{\boldsymbol {x} }(b),\boldsymbol{\nu},\mu\right) &= p\left(a,{\boldsymbol {x} }(a),b,{\boldsymbol {x} }(b),\mu\right)+\boldsymbol{\xi}^\mathsf{T} \boldsymbol{\sigma}\left(a,{\boldsymbol {x} }(a),\mu\right)+\boldsymbol{\nu}^\mathsf{T} \boldsymbol{\psi}\left(b,{\boldsymbol {x} }(b),\mu\right) \\
&=\frac{\upsilon_a}{2}\left(a-a_\mathrm{e} \right)^2+\frac{\upsilon_b}{2}\left(b-b_\mathrm{e} \right)^2+ \boldsymbol{\xi}^\mathsf{T}  \begin{bmatrix} \boldsymbol{\theta}(a) - \boldsymbol{\theta}_a \\ \dot \boldsymbol{\theta}(a) - {\dot \boldsymbol{\theta}}_a \\ \phi(a)-\phi_a \\ -r \dot \phi(a) - {\dot z}_a  \end{bmatrix}\\
&\hphantom{=}+\boldsymbol{\nu}^\mathsf{T} \begin{bmatrix} \boldsymbol{\Pi} \left(\tilde \Lambda\left(\phi(b)\right) \left[ \frac{1}{M} \sum_{i=0}^n m_i \boldsymbol{\zeta}_i\left(\theta_i(b)\right) \right] \right) - \boldsymbol{\Delta}_b \\ \dot \boldsymbol{\theta}(b) - {\dot \boldsymbol{\theta}}_b  \\ z_a - r \left(\phi(b)-\phi_a\right) - z_b \\ -r \dot \phi(b) - {\dot z}_b  \end{bmatrix}
\end{split}
\end{equation}
and the Hamiltonian is
\begin{equation} \label{eq_disk_ham}
\begin{split}
H\left(t,{\boldsymbol {x} },\boldsymbol{\lambda},\boldsymbol{u},\mu\right) &= L\left(t,{\boldsymbol {x} },\boldsymbol{u},\mu\right) + \boldsymbol{\lambda}^\mathsf{T} \mathbf{f}\left(t,{\boldsymbol {x} },\boldsymbol{u},\mu\right) \\
&= \frac{\alpha}{2} \left(z_a - r \left(\phi-\phi_a \right) - z_\mathrm{d} \right)^2+ \frac{\beta}{2} \left(- r \dot \phi \right)^2  + \sum_{i=1}^n \frac{\gamma_i}{2} {\ddot \theta}_i^2+\delta + \boldsymbol{\lambda}^\mathsf{T} \begin{bmatrix} \dot \boldsymbol{\theta} \\ \boldsymbol{u}  \\ \dot \phi \\ \kappa\left(t,{\boldsymbol {x} },\boldsymbol{u} \right)  \end{bmatrix}.
\end{split}
\end{equation}
Recall  from \eqref{eqmo_chap_disk_4} that  the function describing the  uncontrolled equation of motion  for the rolling disk is
\begin{equation} \label{eq_disk_kappa}
\kappa\left(t,{\boldsymbol {x} },\boldsymbol{u}\right) = \frac{ -r F_{\mathrm{e},1}+ \sum_{i=0}^n m_i K_i }{d_2+\sum_{i=0}^n m_i \left[\left( r \sin \phi + \zeta_{i,1} \right)^2+\left( r \cos \phi+ \zeta_{i,3} \right)^2 \right]},
\end{equation}
where  $K_i$ for $0 \le i \le n$  is defined  in \eqref{eq_K_i} as
\begin{equation}
\begin{split}
K_i
&\equiv \left(g+ r {\dot \phi}^2 \right) \left(\zeta_{i,3} \sin \phi - \zeta_{i,1} \cos \phi  \right)+
\left(r \cos \phi + \zeta_{i,3} \right) \left(- 2 \dot \phi {\dot \theta}_i \zeta_{i,3}^{\prime} + {\dot \theta}_i^2 \zeta_{i,1}^{\prime \prime} \right)\\
&\hphantom{\equiv}  - \left(r \sin \phi + \zeta_{i,1} \right) \left( 2 \dot \phi {\dot \theta}_i \zeta_{i,1}^{\prime}+ {\dot \theta}_i^2 \zeta_{i,3}^{\prime \prime} \right)
+\left[ \left(r \cos \phi + \zeta_{i,3} \right)\zeta_{i,1}^{\prime}- \left(r \sin \phi + \zeta_{i,1} \right) \zeta_{i,3}^{\prime} \right] {\ddot \theta}_i \, .
\end{split}
\end{equation}
Differentiating the Hamiltonian \eqref{eq_disk_ham} with respect to the components of the control $\boldsymbol{u}$ gives
\begin{equation} \label{eq_H_u_i}
H_{u_i} = H_{\ddot{\theta}_i} = \gamma_i {\ddot \theta}_i + \lambda_{n+i} + \lambda_{2n+2} \frac{m_i \left[ \left(r \cos \phi + \zeta_{i,3} \right)\zeta_{i,1}^{\prime}- \left(r \sin \phi + \zeta_{i,1} \right) \zeta_{i,3}^{\prime} \right]}{d_2+\sum_{i=0}^n m_i \left[\left( r \sin \phi + \zeta_{i,1} \right)^2+\left( r \cos \phi+ \zeta_{i,3} \right)^2 \right]},
\end{equation}
\begin{equation}
H_{u_i u_j} = H_{\ddot{\theta}_i \ddot{\theta}_j} = \gamma_i \delta_{ij},
\end{equation}
and
\begin{equation} \label{eq_Huu_disk}
H_{\boldsymbol{u} \boldsymbol{u}} = \mathrm{ \textbf{diag}} \begin{bmatrix} \gamma_1 & \gamma_2 & \dots & \gamma_n \end{bmatrix}.
\end{equation}
By \eqref{eq_Huu_disk},  $H_{\boldsymbol{u} \boldsymbol{u}}>0$ iff $\gamma_i >0$ for all $1 \le i \le n$. Consequently, the optimal control problem is regular iff $\gamma_i >0$ for all $1 \le i \le n$. Assume that the optimal control problem is regular, so that $\gamma_i >0$ for all $1 \le i \le n$. $H_{\boldsymbol{u}}=0$ iff $H_{u_i}=0$ for all $1 \le i \le n$. From \eqref{eq_H_u_i},
\begin{equation}  \label{eq_ddtheta_exp}
H_{u_i} = 0 \iff  {\ddot \theta}_i = -\gamma_i^{-1} \left\{ \lambda_{n+i} + \lambda_{2n+2} \frac{m_i \left[ \left(r \cos \phi + \zeta_{i,3} \right)\zeta_{i,1}^{\prime}- \left(r \sin \phi + \zeta_{i,1} \right) \zeta_{i,3}^{\prime} \right]}{d_2+\sum_{i=0}^n m_i \left[\left( r \sin \phi + \zeta_{i,1} \right)^2+\left( r \cos \phi+ \zeta_{i,3} \right)^2 \right]} \right\}.
\end{equation}
\eqref{eq_ddtheta_exp} shows that the control $\ddot \boldsymbol{\theta}$ may be expressed as a function $\boldsymbol{\pi}$ of ${\boldsymbol {x} }$, $\boldsymbol{\lambda}$, and $\mu$; to be consistent with the notation of Subsection~\ref{ssec_pmp}, $\boldsymbol{\pi}$ will also depend on $t$ even though in this particular example it does not. The regular Hamiltonian is
\begin{equation} \label{eq_disk_regular_Hamiltonian}
\begin{split}
\hat H\left(t,{\boldsymbol {x} },\boldsymbol{\lambda},\mu\right) &= H\left(t,{\boldsymbol {x} },\boldsymbol{\lambda},\boldsymbol{\pi} \left(t,{\boldsymbol {x} },\boldsymbol{\lambda},\mu\right),\mu\right) \\
&= \frac{\alpha}{2} \left(z_a - r \left(\phi-\phi_a \right) - z_\mathrm{d} \right)^2+ \frac{\beta}{2} \left(- r \dot \phi \right)^2  + \sum_{i=1}^n \frac{\gamma_i}{2} {\pi}_i^2 \left(t,{\boldsymbol {x} },\boldsymbol{\lambda},\mu\right)+\delta \\
&\hphantom{=} + \boldsymbol{\lambda}^\mathsf{T} \begin{bmatrix} \dot \boldsymbol{\theta} \\ \boldsymbol{\pi} \left(t,{\boldsymbol {x} },\boldsymbol{\lambda},\mu\right)  \\ \dot \phi \\ \kappa\left(t,{\boldsymbol {x} },\boldsymbol{\pi} \left(t,{\boldsymbol {x} },\boldsymbol{\lambda},\mu\right) \right)  \end{bmatrix}.
\end{split}
\end{equation}

As explained in Subsection~\ref{ssec_pmp}, one way to solve the optimal control problem \eqref{dyn_opt_problem_disk} for the rolling disk is to solve the ODE TPBVP:
\begin{equation} \label{eq_pmp_bvp_disk}
\begin{split}
\dot {{\boldsymbol {x} }} &= \hat{H}_{\boldsymbol{\lambda}}^\mathsf{T} \left(t,{\boldsymbol {x} },\boldsymbol{\lambda},\mu\right) = \hat{\mathbf{f}}\left(t,{\boldsymbol {x} },\boldsymbol{\lambda},\mu\right), \\
\dot {\boldsymbol{\lambda}} &= - \hat{H}_{{\boldsymbol {x} }}^\mathsf{T} \left(t,{\boldsymbol {x} },\boldsymbol{\lambda},\mu\right), \\
\left. \hat{H} \right|_{t=a} = G_{a}, \quad \left. \boldsymbol{\lambda} \right|_{t=a} &= -G_{{\boldsymbol {x} }(a)}^\mathsf{T}, \quad G_{\boldsymbol{\xi}}^\mathsf{T} = \boldsymbol{\sigma}\left(a,{\boldsymbol {x} }(a),\mu\right) = \mathbf{0}_{k_1 \times 1}, \\
\left. \hat{H} \right|_{t=b} = -G_{b}, \quad \left. \boldsymbol{\lambda} \right|_{t=b} &= G_{{\boldsymbol {x} }(b)}^\mathsf{T}, \quad G_{\boldsymbol{\nu}}^\mathsf{T} = \boldsymbol{\psi}\left(b,{\boldsymbol {x} }(b),\mu\right) = \mathbf{0}_{k_2 \times 1}.
\end{split}
\end{equation}
In order to realize the ODE velocity function in \eqref{eq_pmp_bvp_disk}, $\hat{\mathbf{f}}$ and $\hat{H}_{{\boldsymbol {x} }}^\mathsf{T}$ must be constructed. From its definition \eqref{eq_fhat_def}, $\hat{\mathbf{f}}$ is readily constructed from $\mathbf{f}$ and $\boldsymbol{\pi}$. By \eqref{eq_Hhat_x}, $\hat{H}_{{\boldsymbol {x} }}^\mathsf{T}$ may be obtained from $H_{{\boldsymbol {x} }}^\mathsf{T}$ and $\boldsymbol{\pi}$; Subappendix~\ref{app_ccem_rdisk} derives the formulas for constructing $H_{{\boldsymbol {x} }}^\mathsf{T}$, so that $\hat{H}_{{\boldsymbol {x} }}^\mathsf{T}$ may be realized without resorting to computational tools like symbolic or automatic differentiation.

\rem{One way to solve the optimal control problem \eqref{dyn_opt_problem_disk} for the rolling disk is to solve the ODE TPBVP given by \eqref{eq_pmp_bvp}, \eqref{eq_pmp_lbc2}, and \eqref{eq_pmp_rbc2} using the endpoint function \eqref{eq_disk_endpoint_fcn} and the regular Hamiltonian \eqref{eq_disk_regular_Hamiltonian}. In order to realize the ODE velocity function in \eqref{eq_pmp_bvp}, $\hat{\mathbf{f}}$ and $\hat{H}_{{\boldsymbol {x} }}$ must be constructed. From its definition \eqref{eq_fhat_def}, $\hat{\mathbf{f}}$ is readily constructed from $\mathbf{f}$ and $\boldsymbol{\pi}$. From \eqref{eq_Hhat_x}, $\hat{H}_{{\boldsymbol {x} }}$ may be obtained from $H_{{\boldsymbol {x} }}$ and $\boldsymbol{\pi}$; Subappendix~\ref{app_ccem_rdisk} gives the formulas for constructing $H_{{\boldsymbol {x} }}$, so that $\hat{H}_{{\boldsymbol {x} }}$ may be realized without resorting to computational tools like symbolic or automatic differentiation.}

\subsection{Controlled Equations of Motion for the Rolling Ball} \label{ssec_ball_controlled}
\rem{Before proceeding, some useful terminology is defined or recalled. Given a  vector
\begin{equation}
\boldsymbol{v} = \begin{bmatrix} v_1 \\ v_2 \\ v_3 \end{bmatrix} = \begin{bmatrix} v_1 & v_2 & v_3 \end{bmatrix}^\mathsf{T} \in \mathbb{R}^3,
\end{equation}
the projected vector consisting of the first two components of $\boldsymbol{v}$ is
\begin{equation}
\boldsymbol{v}_{12} = \begin{bmatrix}v_1 \\ v_2 \end{bmatrix} = \begin{bmatrix}v_1 & v_2 \end{bmatrix}^\mathsf{T} \in \mathbb{R}^2.
\end{equation}
Since a versor is used to parameterize the rolling ball's orientation matrix, quaternions and versors are briefly reviewed here; see Appendix~\ref{app_quaternions} and the references mentioned there for a more complete review.
$\mathbb{H}$ denotes the set of quaternions, which is isomorphic to $\mathbb{R}^4$. A quaternion $\mathfrak{p} \in \mathbb{H}$ can be expressed as the column vector
\begin{equation}
\mathfrak{p} =
\begin{bmatrix} p_0 \\ p_1 \\ p_2 \\ p_3 \end{bmatrix}=
\begin{bmatrix} p_0 & p_1 & p_2 & p_3 \end{bmatrix}^\mathsf{T}=\begin{bmatrix} p_0\,;\,p_1\,;\,p_2\,;\,p_3 \end{bmatrix}.
\end{equation}
Given a column vector $\boldsymbol{v} \in \mathbb{R}^3$, $\boldsymbol{v}^\sharp$ is the quaternion $\begin{bmatrix}0\,;\,\boldsymbol{v}\end{bmatrix} \in \mathbb{H}$; that is,
\begin{equation}
\boldsymbol{v}^\sharp = \begin{bmatrix}0 \\ \boldsymbol{v} \end{bmatrix} = \begin{bmatrix}0\,;\,\boldsymbol{v}\end{bmatrix}.
\end{equation}
Given a quaternion $\mathfrak{p} \in \mathbb{H}$, $\mathfrak{p}^\flat \in \mathbb{R}^3$ is the column vector such that
\begin{equation}
\mathfrak{p} = \begin{bmatrix} p_0 \\ \mathfrak{p}^\flat \end{bmatrix}= \begin{bmatrix} p_0\,;\,\mathfrak{p}^\flat \end{bmatrix}.
\end{equation}
Given a column vector $\boldsymbol{v} \in \mathbb{R}^3$, note that
\begin{equation}
\left(\boldsymbol{v}^\sharp\right)^\flat=\boldsymbol{v}.
\end{equation}
However, given a quaternion $\mathfrak{p} \in \mathbb{H}$,
\begin{equation}
\left( \mathfrak{p}^\flat \right)^\sharp = \mathfrak{p} \quad \mathrm{iff} \quad \mathfrak{p} = \begin{bmatrix} 0 \\ \mathfrak{p}^\flat \end{bmatrix}= \begin{bmatrix}0\,;\,\mathfrak{p}^\flat \end{bmatrix}.
\end{equation}
Given quaternions $\mathfrak{p},\mathfrak{q} \in \mathbb{H}$, their product is
\begin{equation}
\mathfrak{p} \mathfrak{q}=\begin{bmatrix} p_0\,;\,\mathfrak{p}^\flat \end{bmatrix} \begin{bmatrix}q_0\,;\,\mathfrak{q}^\flat \end{bmatrix}=\begin{bmatrix}p_0q_0-\mathfrak{p}^\flat \cdot \mathfrak{q}^\flat\,;\,p_0\mathfrak{q}^\flat + q_0\mathfrak{p}^\flat+\mathfrak{p}^\flat \times \mathfrak{q}^\flat \end{bmatrix}
\end{equation}
and their dot product is
\begin{equation}
\mathfrak{p} \cdot \mathfrak{q}=\begin{bmatrix} p_0\,;\,\mathfrak{p}^\flat \end{bmatrix} \cdot \begin{bmatrix}q_0\,;\,\mathfrak{q}^\flat \end{bmatrix}=\begin{bmatrix}p_0\,;\,p_1\,;\,p_2\,;\,p_3\end{bmatrix} \cdot \begin{bmatrix}q_0\,;\,q_1\,;\,q_2\,;\,q_3\end{bmatrix}= p_0 q_0 + \mathfrak{p}^\flat \cdot \mathfrak{q}^\flat=p_0 q_0 +p_1 q_1 + p_2 q_2 + p_3 q_3.
\end{equation}
$\mathscr{S} \subset \mathbb{H}$ denotes the set of unit quaternions, also called versors, which is isomorphic to $\mathbb{S}^3 \subset \mathbb{R}^4$. A versor $\mathfrak{q} \in \mathscr{S}$ can be expressed as the column vector
\begin{equation}
\mathfrak{q} = \begin{bmatrix} q_0 \\ q_1 \\ q_2 \\ q_3 \end{bmatrix}=
\begin{bmatrix} q_0 & q_1 & q_2 & q_3 \end{bmatrix}^\mathsf{T}=\begin{bmatrix} q_0\,;\,q_1\,;\,q_2\,;\,q_3 \end{bmatrix} \quad \mathrm{such \; that} \quad \mathfrak{q} \cdot \mathfrak{q} = q_0^2+q_1^2+q_2^2+q_3^2=1.
\end{equation}
The rolling ball's orientation matrix $\Lambda \in SO(3)$ is parameterized by the versor $\mathfrak{q} \in \mathscr{S}$. If $\boldsymbol{\Omega} \in \mathbb{R}^3$ is the rolling ball's body angular velocity, then the time derivative of $\mathfrak{q}$ is
\begin{equation}
\dot {\mathfrak{q}} = \frac{1}{2} \mathfrak{q} \boldsymbol{\Omega}^\sharp.
\end{equation}
If $\mathbf{Y} \in \mathbb{R}^3$ is a body frame vector, then the rotation of $\mathbf{Y}$ by $\Lambda$ is
\begin{equation}
\Lambda \mathbf{Y} = \left[\mathfrak{q} \mathbf{Y}^\sharp \mathfrak{q}^{-1} \right]^\flat.
\end{equation}
If $\mathbf{y} \in \mathbb{R}^3$ is a spatial frame vector, then the rotation of $\mathbf{y}$ by $\Lambda^{-1}$ is
\begin{equation}
\Lambda^{-1} \mathbf{y} = \left[\mathfrak{q}^{-1} \mathbf{y}^\sharp \mathfrak{q} \right]^\flat.
\end{equation}
}

Having derived the controlled equations of motion for the rolling disk, the controlled equations of motion are now developed for the rolling ball. The rolling ball's orientation matrix $\Lambda \in SO(3)$ is parameterized by $\mathfrak{q} \in \mathscr{S}$, where $\mathscr{S}$ denotes the set of versors (i.e. unit quaternions) \cite{Ho2011_pII,graf2008quaternions,stevens2015aircraft,baraff2001physically}. The properties of and notation used to manipulate versors is explained in Appendix D of \cite{putkaradze2018dynamics}. Since the third coordinate is always $0$, only the first two coordinates, $\boldsymbol{z}$, of the spatial position of the ball's GC are used in the controlled equations of motion for the rolling ball. \rem{Since the third coordinate of the location of the ball's GC is always $0$, only the first two coordinates of the ball's GC are needed to determine the location of the ball's GC. The first two coordinates of the ball's GC are denoted by $\boldsymbol{z}$.} This efficient representaion of the spatial position of the ball's GC motivates the following useful notation for realizing the controlled equations of motion for the rolling ball. Given a  vector
\begin{equation}
\boldsymbol{v} = \begin{bmatrix} v_1 & v_2 & v_3 \end{bmatrix}^\mathsf{T} \in \mathbb{R}^3,
\end{equation}
the projected vector consisting of the first two components of $\boldsymbol{v}$ is
\begin{equation}
\boldsymbol{v}_{12} = \begin{bmatrix}v_1 & v_2 \end{bmatrix}^\mathsf{T} \in \mathbb{R}^2.
\end{equation}

Suppose it is desired to roll the ball from some initial configuration at a prescribed or free initial time $a$ to some final configuration at a prescribed or free final time $b$, without moving the control masses too rapidly along their control rails. In addition, in between the initial and final times, it may be desired that the ball's GC tracks a prescribed spatial $\mathbf{e}_1$-$\mathbf{e}_2$ path $\boldsymbol{z}_\mathrm{d}$ or traces out a minimum energy path, all while avoiding some obstacles. Finally, if the initial or final time is free, it may be desired to minimize the duration $b-a$ of the maneuver. How must the control masses be moved in order to accomplish these tasks? This problem can be solved by posing it as an optimal control problem.

Concretely, at the prescribed or free initial time $a$, the positions of the control mass parameterizations are prescribed to be $\boldsymbol{\theta}(a)=\boldsymbol{\theta}_a$, the velocities of the control mass parameterizations are prescribed to be $\dot \boldsymbol{\theta}(a) = {\dot \boldsymbol{\theta}}_a$, the orientation of the ball is prescribed to be $\mathfrak{q}(a)=\mathfrak{q}_a$, the body angular velocity of the ball is prescribed to be $\boldsymbol{\Omega}(a) = \boldsymbol{\Omega}_a$, and the spatial $\mathbf{e}_1$-$\mathbf{e}_2$ position of the ball's GC is prescribed to be $\boldsymbol{z}(a)=\boldsymbol{z}_a$.

Furthermore, at the prescribed or free final time $b$,  some components (determined by the projection operator $\boldsymbol{\Pi}$) of the ball's center of mass expressed in the spatial frame translated to the GC are prescribed to be
\begin{equation}
\boldsymbol{\Pi} \left(\left[\mathfrak{q}(b) \left[ \frac{1}{M} \sum_{i=0}^n m_i \boldsymbol{\zeta}_i\left(\theta_i(b)\right) \right]^\sharp \mathfrak{q}(b)^{-1} \right]^\flat \right) = \boldsymbol{\Delta}_b,
\end{equation}
the velocities of the control mass parameterizations are prescribed to be $\dot \boldsymbol{\theta}(b) = {\dot \boldsymbol{\theta}}_b$, the body angular velocity of the ball is prescribed to be $\boldsymbol{\Omega}(b) = \boldsymbol{\Omega}_b$, and the spatial $\mathbf{e}_1$-$\mathbf{e}_2$ position of the ball's GC is prescribed to be $\boldsymbol{z}(b)=\boldsymbol{z}_b$.

For example, if it is desired to start and stop the ball at rest, then $\boldsymbol{\Pi}$ is projection onto the first two components, $\boldsymbol{\Delta}_b=\begin{bmatrix} 0 \\ 0 \end{bmatrix}$, $\boldsymbol{\theta}_a$ and $\mathfrak{q}_a$ are such that
\begin{equation} \label{eq_ball_stab_init}
\boldsymbol{\Pi} \left(\left[\mathfrak{q}_a \left[ \frac{1}{M} \sum_{i=0}^n m_i \boldsymbol{\zeta}_i\left(\theta_{a,i}\right) \right]^\sharp \mathfrak{q}_a^{-1} \right]^\flat \right) = \begin{bmatrix} 0 \\ 0 \end{bmatrix},
\end{equation}
${\dot \boldsymbol{\theta}}_a = \mathbf{0}$, $\boldsymbol{\Omega}_a = \mathbf{0}$, $\boldsymbol{\theta}(b)$ and $\mathfrak{q}(b)$ are such that
\begin{equation} \label{eq_ball_stab_fin}
\boldsymbol{\Pi} \left(\left[\mathfrak{q}(b) \left[ \frac{1}{M} \sum_{i=0}^n m_i \boldsymbol{\zeta}_i\left(\theta_i(b)\right) \right]^\sharp \mathfrak{q}(b)^{-1} \right]^\flat \right) = \begin{bmatrix} 0 \\ 0 \end{bmatrix},
\end{equation}
${\dot \boldsymbol{\theta}}_b = \mathbf{0}$, and ${\boldsymbol{\Omega}}_b = \mathbf{0}$. With this choice of $\boldsymbol{\Pi}$, \eqref{eq_ball_stab_init} and \eqref{eq_ball_stab_fin} mean that the CM in the spatial frame translated to the GC is above or below the GC at the initial and final times.

The system state ${\boldsymbol {x} }$ and control $\boldsymbol{u}$ are
\begin{equation}
{\boldsymbol {x} } = \begin{bmatrix} \boldsymbol{\theta} \\ \dot \boldsymbol{\theta} \\ \mathfrak{q} \\ \boldsymbol{\Omega} \\ \boldsymbol{z}  \end{bmatrix} \quad \mathrm{and} \quad \boldsymbol{u} = \ddot \boldsymbol{\theta},
\end{equation}
where $\boldsymbol{\theta}, \, \dot \boldsymbol{\theta}, \, \ddot \boldsymbol{\theta} \in \mathbb{R}^n$, $\mathfrak{q} \in \mathscr{S} \cong \mathbb{S}^3 \subset \mathbb{R}^4$, $\boldsymbol{\Omega} \in \mathbb{R}^3$, and $\boldsymbol{z} \in \mathbb{R}^2$.
The system dynamics defined for $a \le t \le b$ are
\begin{equation} \label{rolling_ball_opt_con_f}
\dot {{\boldsymbol {x} }} = \begin{bmatrix} \dot \boldsymbol{\theta} \\ \ddot \boldsymbol{\theta} \\ \dot {\mathfrak{q}} \\ \dot \boldsymbol{\Omega} \\ \dot \boldsymbol{z}  \end{bmatrix}  = \mathbf{f}\left(t,{\boldsymbol {x} },\boldsymbol{u},\mu\right) \equiv \begin{bmatrix} \dot \boldsymbol{\theta} \\ \boldsymbol{u}  \\ \frac{1}{2} \mathfrak{q} \boldsymbol{\Omega}^\sharp \\ \boldsymbol{\kappa}\left(t,{\boldsymbol {x} },\boldsymbol{u} \right) \\ \left( \left[\mathfrak{q} \boldsymbol{\Omega}^\sharp \mathfrak{q}^{-1} \right]^\flat \times r \mathbf{e}_3  \right)_{12}  \end{bmatrix},
\end{equation}
where $\boldsymbol{\kappa}\left(t,{\boldsymbol {x} },\boldsymbol{u} \right)$ is given by the right-hand side of the formula for $\dot \boldsymbol{\Omega}$ in \eqref{uncon_ball_eqns_explicit_1d}:
\begin{equation} \label{eq_ball_kappa}
\begin{split}
\boldsymbol{\kappa}\left(t,{\boldsymbol {x} },\boldsymbol{u} \right) =
\left[\sum_{i=0}^n m_i \widehat{\mathbf{s}_i}^2  -\inertia \right]^{-1}  \Bigg[&\boldsymbol{\Omega} \times \inertia \boldsymbol{\Omega}+r \tilde \boldsymbol{\Gamma} \times \boldsymbol{\Gamma}\\
&+ \sum_{i=0}^n m_i \mathbf{s}_i \times  \left\{ g \boldsymbol{\Gamma}+ \boldsymbol{\Omega} \times \left(\boldsymbol{\Omega} \times \boldsymbol{\zeta}_i +2 \dot \theta_i \boldsymbol{\zeta}_i^{\prime} \right) + \dot \theta_i^2 \boldsymbol{\zeta}_i^{\prime \prime} + \ddot \theta_i  \boldsymbol{\zeta}_i^{\prime} \right\}  \Bigg].
\end{split}
\end{equation}
Note that in order to construct $\boldsymbol{\kappa}\left(t,{\boldsymbol {x} },\boldsymbol{u} \right)$,  $\boldsymbol{\Gamma} = \Lambda^{-1} \mathbf{e}_3$ and $\tilde \boldsymbol{\Gamma} = \Lambda^{-1} \mathbf{F}_\mathrm{e}$ must be constructed. Given $\mathfrak{q}$, this can be accomplished by first constructing $\Lambda$ from $\mathfrak{q}$ or directly from $\mathfrak{q}$ by using the formulas $\boldsymbol{\Gamma} = \Lambda^{-1} \mathbf{e}_3=\left[\mathfrak{q}^{-1} \mathbf{e}_3^\sharp \mathfrak{q} \right]^\flat$ and $\tilde \boldsymbol{\Gamma} = \Lambda^{-1} \mathbf{F}_\mathrm{e} = \left[\mathfrak{q}^{-1} \mathbf{F}_\mathrm{e}^\sharp \mathfrak{q} \right]^\flat$. Likewise, the final formula in \eqref{rolling_ball_opt_con_f} is $\dot \boldsymbol{z}=\left( \boldsymbol{\omega} \times r \mathbf{e}_3  \right)_{12}$, where $\boldsymbol{\omega} =  \Lambda \boldsymbol{\Omega}=\left[\mathfrak{q} \boldsymbol{\Omega}^\sharp \mathfrak{q}^{-1} \right]^\flat$. Thus, given $\mathfrak{q}$, $\boldsymbol{\omega}$ can be constructed by first constructing $\Lambda$ from $\mathfrak{q}$ or directly from $\mathfrak{q}$ via $\boldsymbol{\omega}=\left[\mathfrak{q} \boldsymbol{\Omega}^\sharp \mathfrak{q}^{-1} \right]^\flat$. The most computationally efficient method to construct $\boldsymbol{\Gamma}= \Lambda^{-1} \mathbf{e}_3=\left[\mathfrak{q}^{-1} \mathbf{e}_3^\sharp \mathfrak{q} \right]^\flat$, $\tilde \boldsymbol{\Gamma}= \Lambda^{-1} \mathbf{F}_\mathrm{e} = \left[\mathfrak{q}^{-1} \mathbf{F}_\mathrm{e}^\sharp \mathfrak{q} \right]^\flat$, and $\boldsymbol{\omega} = \Lambda \boldsymbol{\Omega}=\left[\mathfrak{q} \boldsymbol{\Omega}^\sharp \mathfrak{q}^{-1} \right]^\flat$ is to construct $\Lambda$ from $\mathfrak{q}$ and to then multiply $\Lambda^{-1} = \Lambda^\mathsf{T}$ against $\mathbf{e}_3$ and $\mathbf{F}_\mathrm{e}$ and to multiply $\Lambda$ against $\boldsymbol{\Omega}$.

The prescribed initial conditions at time $t=a$ are
\begin{equation} \label{eq_ball_initial_conds}
\boldsymbol{\sigma}\left(a,{\boldsymbol {x} }(a),\mu\right) = \begin{bmatrix} \boldsymbol{\theta}(a) - \boldsymbol{\theta}_a \\ \dot \boldsymbol{\theta}(a) - {\dot \boldsymbol{\theta}}_a \\ \mathfrak{q}(a)-\mathfrak{q}_a \\ \boldsymbol{\Omega}(a) - \boldsymbol{\Omega}_a \\ \boldsymbol{z}(a)- \boldsymbol{z}_a \end{bmatrix} = \mathbf{0},
\end{equation}
and the prescribed final conditions at time $t=b$ are
\begin{equation} \label{eq_ball_final_conds}
\boldsymbol{\psi}\left(b,{\boldsymbol {x} }(b),\mu\right) = \begin{bmatrix} \boldsymbol{\Pi} \left(\left[\mathfrak{q}(b) \left[ \frac{1}{M} \sum_{i=0}^n m_i \boldsymbol{\zeta}_i\left(\theta_i(b)\right) \right]^\sharp \mathfrak{q}(b)^{-1} \right]^\flat \right) - \boldsymbol{\Delta}_b \\ \dot \boldsymbol{\theta}(b) - {\dot \boldsymbol{\theta}}_b \\ \boldsymbol{\Omega}(b) - \boldsymbol{\Omega}_b \\ \boldsymbol{z}(b)- \boldsymbol{z}_b  \end{bmatrix} = \mathbf{0}.
\end{equation}

Consider the endpoint and integrand cost functions
\begin{equation} \label{eq_ball_endpoint_cost}
p\left(a,{\boldsymbol {x} }(a),b,{\boldsymbol {x} }(b),\mu\right)=\frac{\upsilon_a}{2}\left(a-a_\mathrm{e} \right)^2+\frac{\upsilon_b}{2}\left(b-b_\mathrm{e} \right)^2 \\
\end{equation}
and
\begin{equation} \label{eq_ball_integrand_cost}
L\left(t,{\boldsymbol {x} },\boldsymbol{u},\mu\right) = \frac{\alpha}{2} \left| \boldsymbol{z} - \boldsymbol{z}_\mathrm{d} \right|^2+ \frac{\beta}{2} \left| \left( \left[\mathfrak{q} \boldsymbol{\Omega}^\sharp \mathfrak{q}^{-1} \right]^\flat \times r \mathbf{e}_3  \right)_{12}\right|^2 + \sum_{i=1}^n \frac{\gamma_i}{2} {\ddot \theta}_i^2+\sum_{j=1}^K V_j \left(\boldsymbol{z},\mu \right)+\delta,
\end{equation}
for constants $a_\mathrm{e}$ and $b_\mathrm{e}$ and for fixed nonnegative constants $\upsilon_a$, $\upsilon_b$, $\alpha$, $\beta$, $\gamma_i$, $1 \le i \le n$, and $\delta$ so that the performance index is
\begin{equation} \label{eq_ball_J}
\begin{split}
J &= p\left(a,{\boldsymbol {x} }(a),b,{\boldsymbol {x} }(b),\mu\right)+ \int_a^b L\left(t,{\boldsymbol {x} },\boldsymbol{u},\mu\right) \mathrm{d}t \\
&= \frac{\upsilon_a}{2}\left(a-a_\mathrm{e} \right)^2+\frac{\upsilon_b}{2}\left(b-b_\mathrm{e} \right)^2\\
&\hphantom{=}+\int_a^b \left[ \frac{\alpha}{2} \left| \boldsymbol{z} - \boldsymbol{z}_\mathrm{d} \right|^2+ \frac{\beta}{2} \left| \left( \left[\mathfrak{q} \boldsymbol{\Omega}^\sharp \mathfrak{q}^{-1} \right]^\flat \times r \mathbf{e}_3  \right)_{12}\right|^2 + \sum_{i=1}^n \frac{\gamma_i}{2} {\ddot \theta}_i^2+\sum_{j=1}^K V_j \left(\boldsymbol{z},\mu \right)+\delta \right] \mathrm{d}t.
\end{split}
\end{equation}
The first summand $\frac{\upsilon_a}{2}\left(a-a_\mathrm{e} \right)^2$ in $p$ encourages the initial time $a$ to be near $a_\mathrm{e}$ if the initial time is free, while the second summand $\frac{\upsilon_b}{2}\left(b-b_\mathrm{e} \right)^2$ in $p$ encourages the final time $b$ to be near $b_\mathrm{e}$ if the final time is free. The first summand $\frac{\alpha}{2} \left| \boldsymbol{z} - \boldsymbol{z}_\mathrm{d} \right|^2$ in $L$ encourages the ball's GC to track the desired spatial $\mathbf{e}_1$-$\mathbf{e}_2$ path $\boldsymbol{z}_\mathrm{d}$, the second summand $\frac{\beta}{2} \left| \left( \left[\mathfrak{q} \boldsymbol{\Omega}^\sharp \mathfrak{q}^{-1} \right]^\flat \times r \mathbf{e}_3  \right)_{12}\right|^2$ in $L$ encourages the ball's GC to track a minimum energy path, the next $n$ summands $\frac{\gamma_i}{2} {\ddot \theta}_i^2$, $1 \le i \le n$, in $L$ limit the magnitude of the acceleration of the $i^\mathrm{th}$ control mass parameterization,  the next $K$ summands $V_j\left(\boldsymbol{z},\mu\right)$, $1 \le j \le K$, in $L$ represent obstacles to be avoided, and the final summand $\delta$ in $L$ encourages a minimum time maneuver. Note that a solution  obtained by the optimal control procedure, that minimizes \eqref{eq_disk_J} for the rolling disk or \eqref{eq_ball_J} for the rolling ball, is a ``compromise" between several, often conflicting, components, where some components of the performance index can be made more prominent by making their coefficients appropriately larger. The minimization of the  performance index  does not guarantee the minimization of each  component individually.

For example, the desired spatial $\mathbf{e}_1$-$\mathbf{e}_2$ path might have the form
\begin{equation} \label{eq_ball_z_d}
\boldsymbol{z}_\mathrm{d}(t) = \left[\boldsymbol{z}_a w(t)+\tilde{\boldsymbol{z}}_\mathrm{d}(t) \left(1-w(t)\right) \right] \left(1-y(t)\right)+\boldsymbol{z}_b y(t),
\end{equation}
where $w$, $y$, and $S$ are given by \eqref{eq_tran_sigmoidl}, \eqref{eq_tran_sigmoidr}, and \eqref{eq_sigmoid}, respectively.
$\boldsymbol{z}_\mathrm{d}$ \eqref{eq_ball_z_d} holds steady at $\boldsymbol{z}_a$ for $t < a_\mathrm{e}$, smoothly transitions between $\boldsymbol{z}_a$ and $\tilde{\boldsymbol{z}}_\mathrm{d}$ at $t = a_\mathrm{e}$, follows $\tilde{\boldsymbol{z}}_\mathrm{d}$ for $a_\mathrm{e} < t < b_\mathrm{e}$, smoothly transitions between $\tilde{\boldsymbol{z}}_\mathrm{d}$ and $\boldsymbol{z}_b$ at $t = b_\mathrm{e}$, and holds steady at $\boldsymbol{z}_b$ for $b_\mathrm{e} < t$.  $\tilde{\boldsymbol{z}}_\mathrm{d}$, which appears in \eqref{eq_ball_z_d}, might be
\begin{equation} \label{eq_ball_tz_d}
\tilde{\boldsymbol{z}}_\mathrm{d}(t) = \mathbf{k}_1 \left[ \left\{ -\frac{1}{3}t^3+\frac{1}{2}\left(a_\mathrm{e}+b_\mathrm{e} \right)t-a_\mathrm{e} b_\mathrm{e} t \right\}\begin{bmatrix}  1 \\ 1 \end{bmatrix}+\mathbf{k}_2 \right]= \mathbf{k}_1 \left[q(t) \begin{bmatrix}  1 \\ 1 \end{bmatrix}+\mathbf{k}_2 \right],
\end{equation}
where
\begin{equation} \label{eq_ball_tz_details}
q(t) = -\frac{1}{3}t^3+\frac{1}{2}\left(a_\mathrm{e}+b_\mathrm{e} \right)t-a_\mathrm{e} b_\mathrm{e} t, \quad \mathbf{k}_1 = \frac{\boldsymbol{z}_b-\boldsymbol{z}_a}{q(b_\mathrm{e})-q(a_\mathrm{e})}, \quad \mathrm{and} \quad \mathbf{k}_2 = \frac{\boldsymbol{z}_b}{\mathbf{k}_1}-q(b_\mathrm{e}) \begin{bmatrix}  1 \\ 1 \end{bmatrix}.  \end{equation}
The multiplication between the vectors $\mathbf{k}_1$ and $q(t) \begin{bmatrix}  1 \\ 1 \end{bmatrix}+\mathbf{k}_2$ in \eqref{eq_ball_tz_d} is meant to be performed component-wise.
The division $\frac{\boldsymbol{z}_b}{\mathbf{k}_1}$ used in the construction of $\mathbf{k}_2$ in \eqref{eq_ball_tz_details} is meant to be performed component-wise; to avoid division by zero, if a component of $\mathbf{k}_1$ is zero, then the corresponding component of $\mathbf{k}_2$ is set to zero. $\tilde{\boldsymbol{z}}_\mathrm{d}$ has the special properties $\tilde{\boldsymbol{z}}_\mathrm{d}(a_\mathrm{e})=\boldsymbol{z}_a$, $\dot {\tilde{\boldsymbol{z}}}_\mathrm{d}(a_\mathrm{e})=\begin{bmatrix}  0 & 0 \end{bmatrix}^\mathsf{T}$, $\tilde{\boldsymbol{z}}_\mathrm{d}(b_\mathrm{e})=\boldsymbol{z}_b$, and $\dot {\tilde{\boldsymbol{z}}}_\mathrm{d}(b_\mathrm{e})=\begin{bmatrix}  0 & 0 \end{bmatrix}^\mathsf{T}$, so that the disk's GC is encouraged to start with zero velocity at $\boldsymbol{z}_a$ at $t=a_\mathrm{e}$ and to stop with zero velocity at $\boldsymbol{z}_b$ at $t=b_\mathrm{e}$. Figure~\ref{fig_rolling_ball_z_d} illustrates \eqref{eq_ball_z_d} using \eqref{eq_ball_tz_d} with $a_\mathrm{e}=0$, $\boldsymbol{z}_a=\begin{bmatrix}  0 & 0 \end{bmatrix}^\mathsf{T}$, $b_\mathrm{e}=.5$, $\boldsymbol{z}_b=\begin{bmatrix}  1 & 1 \end{bmatrix}^\mathsf{T}$, and $\epsilon=.01$.

\begin{figure}[h]
	\centering
	\includegraphics[width=0.5\linewidth]{bsim2_zd}
	\caption{Plot of the desired path of the ball's GC. The ball's GC starts from rest at $\boldsymbol{z}=\protect\begin{bmatrix}  0 & 0 \protect\end{bmatrix}^\mathsf{T}$ at time $t=0$ and stops at rest at $\boldsymbol{z}=\protect\begin{bmatrix}  1 & 1 \protect\end{bmatrix}^\mathsf{T}$ at time $t=.5$.}
	\label{fig_rolling_ball_z_d}
\end{figure}

Illustratively, for $1 \le j \le K$, the $j^\mathrm{th}$ obstacle of height $h_j$ and radius $\rho_j$ with center at spatial $\mathbf{e}_1$-$\mathbf{e}_2$  position $\boldsymbol{v}_j = \begin{bmatrix}  v_{j,1} & v_{j,2} \end{bmatrix}^\mathsf{T}$ might be modeled via the function
\begin{equation} \label{eq_obstacle_pot_func}
V_j\left(\boldsymbol{z},\mu\right) = h_j \, S \left(\left| \boldsymbol{z} - \boldsymbol{v}_j \right| - \rho_j \right),
\end{equation}
where
\begin{equation} \label{eq_obst_sigmoid}
S(y)\equiv \frac{1}{2}+\frac{1}{2} \tanh \left(\frac{-y}{\epsilon} \right)
\end{equation}
is a time-reversed sigmoid function or
\begin{equation} \label{eq_obst_cutoff}
S(y)\equiv \left[\max \left\{0,-y \right\} \right]^4
\end{equation}
is a $C^2$ cutoff function.
\rem{where $S$ is given by \eqref{eq_sigmoid}.}
As indicated by \eqref{eq_obstacle_pot_func}, the radial distance from the ball's GC to the obstacle center should exceed the obstacle radius $\rho_j$ for successful obstacle avoidance. In order to encourage the entire ball to avoid the obstacle, the obstacle radius $\rho_j$ must include the ball's radius $r$. That is, if the physical radius of the obstacle is $\epsilon_j$, then set $\rho_j=r+\epsilon_j$ to encourage the entire ball to stay away from the obstacle; if $\rho_j=\epsilon_j$, then only the ball's GC is encouraged to stay away from the obstacle.

The ODE formulation of the optimal control problem for the rolling ball is
\begin{equation}
\min_{a,b,\boldsymbol{u}} J
\mbox{\, s.t. \,}
\left\{
\begin{array}{ll}
\dot {{\boldsymbol {x} }} = \mathbf{f}\left(t,{\boldsymbol {x} },\boldsymbol{u},\mu\right), \\
\boldsymbol{\sigma}\left(a,{\boldsymbol {x} }(a),\mu\right) = \mathbf{0},\\
\boldsymbol{\psi}\left(b,{\boldsymbol {x} }(b),\mu\right) = \mathbf{0}.
\end{array}
\right.
\label{dyn_opt_problem_ball}
\end{equation}
There are also two DAE formulations of the optimal control problem for the rolling ball which explicitly enforce the algebraic versor constraint on $\mathfrak{q}$ and which are mathematically equivalent to \eqref{dyn_opt_problem_ball}. In the first DAE formulation an additional control, $\dot q_0$, is added to the control $\boldsymbol{u}$. The first DAE formulation is
\begin{equation}
\min_{a,b,\boldsymbol{u}_1} J
\mbox{\, s.t. \,}
\left\{
\begin{array}{ll}
\dot {{\boldsymbol {x} }} = \mathbf{f}_1\left(t,{\boldsymbol {x} },\boldsymbol{u}_1,\mu\right), \\
h_1\left({\boldsymbol {x} }\right)=1, \\
\boldsymbol{\sigma}\left(a,{\boldsymbol {x} }(a),\mu\right) = \mathbf{0},\\
\boldsymbol{\psi}\left(b,{\boldsymbol {x} }(b),\mu\right) = \mathbf{0},
\end{array}
\right.
\label{dyn_opt_problem_dae1_ball}
\end{equation}
where
\begin{equation}
\boldsymbol{u}_1 = \begin{bmatrix} \ddot \boldsymbol{\theta} \\ \dot q_0 \end{bmatrix}, \quad \mathbf{f}_1\left(t,{\boldsymbol {x} },\boldsymbol{u}_1,\mu\right) \equiv \begin{bmatrix} \dot \boldsymbol{\theta} \\ \boldsymbol{u}_1  \\ \frac{1}{2} \left( \mathfrak{q} \boldsymbol{\Omega}^\sharp \right)^\flat \\ \boldsymbol{\kappa}\left(t,{\boldsymbol {x} },\ddot \boldsymbol{\theta} \right) \\ \left( \left[\mathfrak{q} \boldsymbol{\Omega}^\sharp \mathfrak{q}^{-1} \right]^\flat \times r \mathbf{e}_3  \right)_{12}  \end{bmatrix}, \quad \mathrm{and} \quad h_1\left({\boldsymbol {x} }\right) =  \mathfrak{q} \cdot \mathfrak{q}.
\end{equation}
In the second DAE formulation, the first component, $q_0$, of the versor $\mathfrak{q}$ is moved from the state ${\boldsymbol {x} }$ to the control $\boldsymbol{u}$ and an imitator state, $\tilde q_0$, is used to replace $q_0$ in ${\boldsymbol {x} }$. $\tilde q_{a,0} = q_{a,0}$, so that with perfect integration (i.e. no numerical integration errors), $\tilde q_0(t) = q_0(t)$ for $a \le t \le b$. Define $\tilde {\mathfrak{q}} = \begin{bmatrix} \tilde q_0 \\  \mathfrak{q}^\flat \end{bmatrix}$; with perfect integration, $\tilde {\mathfrak{q}}(t)=\begin{bmatrix} \tilde q_0(t) \\  \mathfrak{q}^\flat(t) \end{bmatrix}=\begin{bmatrix} q_0(t) \\  \mathfrak{q}^\flat(t) \end{bmatrix}=\mathfrak{q}(t)$ for $a \le t \le b$. $\tilde q_0$ is added to the state since the final conditions require knowledge of $q_0$, which is unavailable if it has been moved to the control since the final conditions are not a function of the control. The second DAE formulation is
\begin{equation}
\min_{a,b,\boldsymbol{u}_2} J
\mbox{\, s.t. \,}
\left\{
\begin{array}{ll}
\dot {{\boldsymbol {x} }}_2 = \mathbf{f}_2\left(t,{\boldsymbol {x} }_2,\boldsymbol{u}_2,\mu\right), \\
h_2\left({\boldsymbol {x} }_2,\boldsymbol{u}_2\right)=1, \\
\boldsymbol{\sigma}_2\left(a,{\boldsymbol {x} }_2(a),\mu\right) = \mathbf{0},\\
\boldsymbol{\psi}_2\left(b,{\boldsymbol {x} }_2(b),\mu\right) = \mathbf{0},
\end{array}
\right.
\label{dyn_opt_problem_dae2_ball}
\end{equation}
where
\begin{equation}
{\boldsymbol {x} }_2 = \begin{bmatrix} \boldsymbol{\theta} \\ \dot \boldsymbol{\theta} \\ \tilde q_0 \\ \mathfrak{q}^\flat \\ \boldsymbol{\Omega} \\ \boldsymbol{z}  \end{bmatrix}, \quad
\boldsymbol{u}_2 = \begin{bmatrix} \ddot \boldsymbol{\theta} \\ q_0 \end{bmatrix}, \quad \mathbf{f}_2\left(t,{\boldsymbol {x} }_2,\boldsymbol{u}_2,\mu\right) \equiv \begin{bmatrix} \dot \boldsymbol{\theta} \\ \ddot \boldsymbol{\theta}  \\ \frac{1}{2} \mathfrak{q} \boldsymbol{\Omega}^\sharp \\ \boldsymbol{\kappa}\left(t,{\boldsymbol {x} },\ddot \boldsymbol{\theta} \right) \\ \left( \left[\mathfrak{q} \boldsymbol{\Omega}^\sharp \mathfrak{q}^{-1} \right]^\flat \times r \mathbf{e}_3  \right)_{12}  \end{bmatrix}, \quad h_2\left({\boldsymbol {x} }_2,\boldsymbol{u}_2\right) =  \mathfrak{q} \cdot \mathfrak{q},
\end{equation}
\begin{equation}
\boldsymbol{\sigma}_2\left(a,{\boldsymbol {x} }_2(a),\mu\right) = \begin{bmatrix} \boldsymbol{\theta}(a) - \boldsymbol{\theta}_a \\ \dot \boldsymbol{\theta}(a) - {\dot \boldsymbol{\theta}}_a \\ \tilde {\mathfrak{q}}(a)-\mathfrak{q}_a \\ \boldsymbol{\Omega}(a) - \boldsymbol{\Omega}_a \\ \boldsymbol{z}(a)- \boldsymbol{z}_a \end{bmatrix} = \mathbf{0},
\end{equation}
and
\begin{equation}
\boldsymbol{\psi}_2\left(b,{\boldsymbol {x} }_2(b),\mu\right) = \begin{bmatrix} \boldsymbol{\Pi} \left(\left[\tilde {\mathfrak{q}}(b) \left[ \frac{1}{M} \sum_{i=0}^n m_i \boldsymbol{\zeta}_i\left(\theta_i(b)\right) \right]^\sharp \tilde {\mathfrak{q}}(b)^{-1} \right]^\flat \right) - \boldsymbol{\Delta}_b \\ \dot \boldsymbol{\theta}(b) - {\dot \boldsymbol{\theta}}_b \\ \boldsymbol{\Omega}(b) - \boldsymbol{\Omega}_b \\ \boldsymbol{z}(b)- \boldsymbol{z}_b  \end{bmatrix} = \mathbf{0}.
\end{equation}
Even though both DAE formulations \eqref{dyn_opt_problem_dae1_ball} and \eqref{dyn_opt_problem_dae2_ball} are mathematically equivalent to the ODE formulation \eqref{dyn_opt_problem_ball}, the DAE formulations \eqref{dyn_opt_problem_dae1_ball} and \eqref{dyn_opt_problem_dae2_ball} tend to be numerically more stable to solve than the ODE formulation \eqref{dyn_opt_problem_ball}, as explained in Example 6.12 ``Reorientation of an Asymmetric Rigid Body'' of \cite{betts2010practical}. While the second DAE formulation \eqref{dyn_opt_problem_dae2_ball} is computationally more efficient (i.e. faster) than the first \eqref{dyn_opt_problem_dae1_ball} because it explicitly constructs the control $q_0$ rather than $\dot q_0$, the second DAE formulation \eqref{dyn_opt_problem_dae2_ball} is not as accurate as the first \eqref{dyn_opt_problem_dae1_ball}, because it only constructs an approximation, $\tilde q_0(b)$, of $q_0(b)$, which is needed for the final conditions. The direct method was used to solve all three formulations of the optimal control problem for the rolling ball. Because DAE TPBVP solvers are not readily available in \mcode{MATLAB}, the indirect method was only applied to the ODE formulation \eqref{dyn_opt_problem_ball}.

Observe that the optimal control problem encapsulated by \eqref{dyn_opt_problem_ball} ignores path inequality constraints such as $\mathbf{D}\left(t,{\boldsymbol {x} },\boldsymbol{u},\mu\right) \le \mathbf{0}$, where $\mathbf{D}$ is an $r \times 1$ vector-valued functions. Path inequality constraints can be incorporated in \eqref{dyn_opt_problem_ball} and additional path inequality constraints can be incorporated in \eqref{dyn_opt_problem_dae1_ball} and \eqref{dyn_opt_problem_dae2_ball} as soft constraints through penalty functions in the integrand cost function $L$ or the endpoint cost function $p$.

The indirect method, discussed in Subsection~\ref{ssec_pmp}, is applied now to \eqref{dyn_opt_problem_ball} to construct the endpoint function and regular Hamiltonian needed to formulate the ODE TPBVP \eqref{eq_pmp_bvp}, \eqref{eq_pmp_lbc2}, and \eqref{eq_pmp_rbc2}, which render the controlled equations of motion for the rolling ball.

The endpoint function is
\begin{equation} \label{eq_ball_endpoint_fcn}
\begin{split}
G\left(a,{\boldsymbol {x} }(a),\boldsymbol{\xi},b,{\boldsymbol {x} }(b),\boldsymbol{\nu},\mu\right) &= p\left(a,{\boldsymbol {x} }(a),b,{\boldsymbol {x} }(b),\mu\right)+\boldsymbol{\xi}^\mathsf{T} \boldsymbol{\sigma}\left(a,{\boldsymbol {x} }(a),\mu\right)+\boldsymbol{\nu}^\mathsf{T} \boldsymbol{\psi}\left(b,{\boldsymbol {x} }(b),\mu\right) \\
&=\frac{\upsilon_a}{2}\left(a-a_\mathrm{e} \right)^2+\frac{\upsilon_b}{2}\left(b-b_\mathrm{e} \right)^2+ \boldsymbol{\xi}^\mathsf{T} \begin{bmatrix} \boldsymbol{\theta}(a) - \boldsymbol{\theta}_a \\ \dot \boldsymbol{\theta}(a) - {\dot \boldsymbol{\theta}}_a \\ \mathfrak{q}(a)-\mathfrak{q}_a \\ \boldsymbol{\Omega}(a) - \boldsymbol{\Omega}_a \\ \boldsymbol{z}(a)- \boldsymbol{z}_a \end{bmatrix}\\
&\hphantom{=}+\boldsymbol{\nu}^\mathsf{T} \begin{bmatrix} \boldsymbol{\Pi} \left(\left[\mathfrak{q}(b) \left[ \frac{1}{M} \sum_{i=0}^n m_i \boldsymbol{\zeta}_i\left(\theta_i(b)\right) \right]^\sharp \mathfrak{q}(b)^{-1} \right]^\flat \right) - \boldsymbol{\Delta}_b \\ \dot \boldsymbol{\theta}(b) - {\dot \boldsymbol{\theta}}_b \\ \boldsymbol{\Omega}(b) - \boldsymbol{\Omega}_b \\ \boldsymbol{z}(b)- \boldsymbol{z}_b  \end{bmatrix}
\end{split}
\end{equation}
and the Hamiltonian is
\begin{equation} \label{eq_ball_ham}
\begin{split}
H\left(t,{\boldsymbol {x} },\boldsymbol{\lambda},\boldsymbol{u},\mu\right) &= L\left(t,{\boldsymbol {x} },\boldsymbol{u},\mu\right) + \boldsymbol{\lambda}^\mathsf{T} \mathbf{f}\left(t,{\boldsymbol {x} },\boldsymbol{u},\mu\right) \\
&= \frac{\alpha}{2} \left| \boldsymbol{z} - \boldsymbol{z}_\mathrm{d} \right|^2+ \frac{\beta}{2} \left| \left( \left[\mathfrak{q} \boldsymbol{\Omega}^\sharp \mathfrak{q}^{-1} \right]^\flat \times r \mathbf{e}_3  \right)_{12}\right|^2  + \sum_{i=1}^n \frac{\gamma_i}{2} {\ddot \theta}_i^2+\sum_{j=1}^K V_j \left(\boldsymbol{z},\mu \right)+\delta\\
&\hphantom{=} + \boldsymbol{\lambda}^\mathsf{T} \begin{bmatrix} \dot \boldsymbol{\theta} \\ \boldsymbol{u}  \\ \frac{1}{2} \mathfrak{q} \boldsymbol{\Omega}^\sharp \\ \boldsymbol{\kappa}\left(t,{\boldsymbol {x} },\boldsymbol{u} \right) \\ \left( \left[\mathfrak{q} \boldsymbol{\Omega}^\sharp \mathfrak{q}^{-1} \right]^\flat \times r \mathbf{e}_3  \right)_{12}  \end{bmatrix}.
\end{split}
\end{equation}
Let $\boldsymbol{\lambda}_{\boldsymbol{\Omega}}=\begin{bmatrix} \lambda_{2n+5} & \lambda_{2n+6} & \lambda_{2n+7} \end{bmatrix}^\mathsf{T}$. Recall that
\begin{equation}
\begin{split}
\boldsymbol{\kappa}\left(t,{\boldsymbol {x} },\boldsymbol{u} \right) =
\left[\sum_{i=0}^n m_i \widehat{\mathbf{s}_i}^2  -\inertia \right]^{-1}  \Bigg[&\boldsymbol{\Omega} \times \inertia \boldsymbol{\Omega}+r \tilde \boldsymbol{\Gamma} \times \boldsymbol{\Gamma}\\
&+ \sum_{i=0}^n m_i \mathbf{s}_i \times  \left\{ g \boldsymbol{\Gamma}+ \boldsymbol{\Omega} \times \left(\boldsymbol{\Omega} \times \boldsymbol{\zeta}_i +2 \dot \theta_i \boldsymbol{\zeta}_i^{\prime} \right) + \dot \theta_i^2 \boldsymbol{\zeta}_i^{\prime \prime} + \ddot \theta_i  \boldsymbol{\zeta}_i^{\prime} \right\}  \Bigg].
\end{split}
\end{equation}
Differentiating the Hamiltonian \eqref{eq_ball_ham} with respect to the components of the control $\boldsymbol{u}$ gives
\begin{equation} \label{eq_H_u_i_rball}
H_{u_i} = H_{\ddot{\theta}_i} = \gamma_i {\ddot \theta}_i + \lambda_{n+i} + \boldsymbol{\lambda}_{\boldsymbol{\Omega}}^\mathsf{T} \left[\sum_{i=0}^n m_i \widehat{\mathbf{s}_i}^2  -\inertia \right]^{-1}  \left[ m_i \mathbf{s}_i \times  \boldsymbol{\zeta}_i^{\prime}  \right],
\end{equation}
\begin{equation}
H_{u_i u_j} = H_{\ddot{\theta}_i \ddot{\theta}_j} = \gamma_i \delta_{ij},
\end{equation}
and
\begin{equation} \label{eq_Huu_ball}
H_{\boldsymbol{u} \boldsymbol{u}} = \mathrm{ \textbf{diag}} \begin{bmatrix} \gamma_1 & \gamma_2 & \dots & \gamma_n \end{bmatrix}.
\end{equation}
By \eqref{eq_Huu_ball},  $H_{\boldsymbol{u} \boldsymbol{u}}>0$ iff $\gamma_i >0$ for all $1 \le i \le n$. Consequently, the optimal control problem is regular iff $\gamma_i >0$ for all $1 \le i \le n$. Assume that the optimal control problem is regular, so that $\gamma_i >0$ for all $1 \le i \le n$.
$H_{\boldsymbol{u}}=0$ iff $H_{u_i}=0$ for all $1 \le i \le n$. From \eqref{eq_H_u_i_rball},
\begin{equation}  \label{eq_ddtheta_exp_rball}
H_{u_i} = 0 \iff  {\ddot \theta}_i = -\gamma_i^{-1} \left\{ \lambda_{n+i} + \boldsymbol{\lambda}_{\boldsymbol{\Omega}}^\mathsf{T} \left[\sum_{i=0}^n m_i \widehat{\mathbf{s}_i}^2  -\inertia \right]^{-1}  \left[ m_i \mathbf{s}_i \times  \boldsymbol{\zeta}_i^{\prime}  \right]  \right\}.
\end{equation}
\eqref{eq_ddtheta_exp_rball} shows that $\ddot \boldsymbol{\theta}$ may be expressed as a function $\boldsymbol{\pi}$ of ${\boldsymbol {x} }$, $\boldsymbol{\lambda}$, and $\mu$; to be consistent with the notation of Subsection~\ref{ssec_pmp}, $\boldsymbol{\pi}$ will also depend on $t$ even though in this particular example it does not. The regular Hamiltonian is
\begin{equation} \label{eq_ball_regular_Hamiltonian}
\begin{split}
\hat H\left(t,{\boldsymbol {x} },\boldsymbol{\lambda},\mu\right) &= H\left(t,{\boldsymbol {x} },\boldsymbol{\lambda},\boldsymbol{\pi} \left(t,{\boldsymbol {x} },\boldsymbol{\lambda},\mu\right),\mu\right) \\
&=  \frac{\alpha}{2} \left| \boldsymbol{z} - \boldsymbol{z}_\mathrm{d} \right|^2+ \frac{\beta}{2} \left| \left( \left[\mathfrak{q} \boldsymbol{\Omega}^\sharp \mathfrak{q}^{-1} \right]^\flat \times r \mathbf{e}_3  \right)_{12}\right|^2 + \sum_{i=1}^n \frac{\gamma_i}{2} {\pi}_i^2 \left(t,{\boldsymbol {x} },\boldsymbol{\lambda},\mu\right)+\sum_{j=1}^K V_j \left(\boldsymbol{z},\mu \right)+\delta\\
&\hphantom{=} + \boldsymbol{\lambda}^\mathsf{T} \begin{bmatrix} \dot \boldsymbol{\theta} \\ \boldsymbol{\pi} \left(t,{\boldsymbol {x} },\boldsymbol{\lambda},\mu\right)  \\ \frac{1}{2} \mathfrak{q} \boldsymbol{\Omega}^\sharp \\ \boldsymbol{\kappa}\left(t,{\boldsymbol {x} },\boldsymbol{\pi} \left(t,{\boldsymbol {x} },\boldsymbol{\lambda},\mu\right) \right) \\ \left( \left[\mathfrak{q} \boldsymbol{\Omega}^\sharp \mathfrak{q}^{-1} \right]^\flat \times r \mathbf{e}_3  \right)_{12}  \end{bmatrix}.
\end{split}
\end{equation}

As explained in Subsection~\ref{ssec_pmp}, one way to solve the optimal control problem \eqref{dyn_opt_problem_ball} for the rolling ball is to solve the ODE TPBVP:
\begin{equation} \label{eq_pmp_bvp_ball}
\begin{split}
\dot {{\boldsymbol {x} }} &= \hat{H}_{\boldsymbol{\lambda}}^\mathsf{T} \left(t,{\boldsymbol {x} },\boldsymbol{\lambda},\mu\right) = \hat{\mathbf{f}}\left(t,{\boldsymbol {x} },\boldsymbol{\lambda},\mu\right), \\
\dot {\boldsymbol{\lambda}} &= - \hat{H}_{{\boldsymbol {x} }}^\mathsf{T} \left(t,{\boldsymbol {x} },\boldsymbol{\lambda},\mu\right), \\
\left. \hat{H} \right|_{t=a} = G_{a}, \quad \left. \boldsymbol{\lambda} \right|_{t=a} &= -G_{{\boldsymbol {x} }(a)}^\mathsf{T}, \quad G_{\boldsymbol{\xi}}^\mathsf{T} = \boldsymbol{\sigma}\left(a,{\boldsymbol {x} }(a),\mu\right) = \mathbf{0}_{k_1 \times 1}, \\
\left. \hat{H} \right|_{t=b} = -G_{b}, \quad \left. \boldsymbol{\lambda} \right|_{t=b} &= G_{{\boldsymbol {x} }(b)}^\mathsf{T}, \quad G_{\boldsymbol{\nu}}^\mathsf{T} = \boldsymbol{\psi}\left(b,{\boldsymbol {x} }(b),\mu\right) = \mathbf{0}_{k_2 \times 1}.
\end{split}
\end{equation}
In order to realize the ODE velocity function in \eqref{eq_pmp_bvp_ball}, $\hat{\mathbf{f}}$ and $\hat{H}_{{\boldsymbol {x} }}^\mathsf{T}$ must be constructed. From its definition \eqref{eq_fhat_def}, $\hat{\mathbf{f}}$ is readily constructed from $\mathbf{f}$ and $\boldsymbol{\pi}$. By \eqref{eq_Hhat_x}, $\hat{H}_{{\boldsymbol {x} }}^\mathsf{T}$ may be obtained from $H_{{\boldsymbol {x} }}^\mathsf{T}$ and $\boldsymbol{\pi}$; Subappendix~\ref{app_ccem_rball} derives the formulas for constructing $H_{{\boldsymbol {x} }}^\mathsf{T}$, so that $\hat{H}_{{\boldsymbol {x} }}^\mathsf{T}$ may be realized without resorting to computational tools like symbolic or automatic differentiation.

\rem{One way to solve the optimal control problem \eqref{dyn_opt_problem_ball} for the rolling ball is to solve the ODE TPBVP given by \eqref{eq_pmp_bvp}, \eqref{eq_pmp_lbc2}, and \eqref{eq_pmp_rbc2} using the endpoint function \eqref{eq_ball_endpoint_fcn} and the regular Hamiltonian \eqref{eq_ball_regular_Hamiltonian}. In order to realize the ODE velocity function in \eqref{eq_pmp_bvp}, $\hat{\mathbf{f}}$ and $\hat{H}_{{\boldsymbol {x} }}$ must be constructed. From its definition \eqref{eq_fhat_def}, $\hat{\mathbf{f}}$ is readily constructed from $\mathbf{f}$ and $\boldsymbol{\pi}$. From \eqref{eq_Hhat_x}, $\hat{H}_{{\boldsymbol {x} }}$ may be obtained from $H_{{\boldsymbol {x} }}$ and $\boldsymbol{\pi}$; Subappendix~\ref{app_ccem_rball} gives the formulas for constructing $H_{{\boldsymbol {x} }}$, so that $\hat{H}_{{\boldsymbol {x} }}$ may be realized without resorting to computational tools like symbolic or automatic differentiation.}

These controlled equations of motion for the rolling ball actuated by internal point masses that move along arbitrarily-shaped rails fixed within the ball are new and have not appeared previously in the literature, as far as we know. Note that these controlled equations of motion track the ball's orientation (i.e. the mapping from body to spatial frames) and involve many specialized parameters and terms that enable the ball to execute a wide variety of interesting and useful maneuvers. These controlled equations of motion constitute a novel contribution of this paper.

\section{Numerical Solutions of the Controlled Equations of Motion}
\label{sec_ball_numerics}

In this section, the motions of the rolling disk and ball are simulated in \mcode{MATLAB} R2017b by numerically solving the controlled equations of motion \eqref{eq_pmp_bvp_disk} and \eqref{eq_pmp_bvp_ball} corresponding to the optimal control problems \eqref{dyn_opt_problem_disk} and \eqref{dyn_opt_problem_ball} for the rolling disk and ball, respectively. Subsections~\ref{ssec_disk_sim} and \ref{ssec_disk_sim_redux} simulate the rolling disk, while Subsections~\ref{ssec_ball_sim} and \ref{ssec_ball_sim_redux} simulate the rolling ball. Because the controlled equations of motion have a very small radius of convergence \cite{betts2010practical,bryson1999dynamic,BrHo1975applied}, a direct method, namely the \mcode{MATLAB} toolbox \mcode{GPOPS-II} \cite{patterson2014gpops} version 2.4, is first used to construct a good initial guess. \mcode{GPOPS-II} relies on the NLP solvers IPOPT \cite{wachter2006implementation} version 3.12.6 or SNOPT \cite{snopt76,GilMS05} version 7.6.0; IPOPT utilizes the sparse linear solvers PARDISO, the version provided in Intel MKL version 11.3 Update 4, or MA57 \cite{duff2004ma57}, the version provided in \mcode{MATLAB} R2017b. For the rolling disk, the direct method is used to solve the rolling disk optimal control problem \eqref{dyn_opt_problem_disk}. When using the direct method to solve the rolling ball optimal control problem, one of the DAE formulations \eqref{dyn_opt_problem_dae1_ball} or \eqref{dyn_opt_problem_dae2_ball} is solved first. The direct method solution to the DAE formulation is then used as an initial guess to solve the ODE formulation \eqref{dyn_opt_problem_ball}, which is consistent with the controlled equations of motion for the rolling ball, by the direct method; recall that the controlled equations of motion for the rolling ball were obtained from the ODE formulation \eqref{dyn_opt_problem_ball} of the rolling ball optimal control problem. The \mcode{MATLAB} automatic differentiation toolbox \mcode{ADiGator} \cite{weinstein2017algorithm,weinstein2015utilizing} version 1.4.1 is used to supply vectorized first and second derivatives (i.e. Jacobians and Hessians) to the direct method solver \mcode{GPOPS-II}.

Starting from the initial guess provided by the direct method, the controlled equations of motion \eqref{eq_pmp_bvp_disk} and \eqref{eq_pmp_bvp_ball} are solved by predictor-corrector continuation in the parameter $\mu$, utilizing the algorithm described in Appendix~\ref{app_sweep_predictor_corrector}. The predictor-corrector continuation method uses the \mcode{MATLAB} global method ODE TPBVP solvers \mcode{sbvp} \cite{auzinger2003collocation} version 1.0 or \mcode{bvptwp} \cite{cash2013algorithm} version 1.0. By vectorized automatic differentiation of $H_{{\boldsymbol {x} }}$, $\boldsymbol{\pi}$, and $\hat{\mathbf{f}}$, \mcode{ADiGator} is used to numerically construct the Jacobians of the normalized ODE velocity function \eqref{eq_DPhi_z} and \eqref{eq_DPhi_mu}. By non-vectorized automatic differentiation of the Hamiltonian $H$, the initial condition function $\boldsymbol{\sigma}$, the final condition function $\boldsymbol{\psi}$, and the endpoint function $G$, \mcode{ADiGator} is used to numerically construct the normalized BC function \eqref{eq_tUps} and the Jacobians of the normalized BC function \eqref{eq_Jac_tUps_tz0}, \eqref{eq_Jac_tUps_tz1}, and \eqref{eq_Jac_tUps_mu}. These functions are needed by the ODE TPBVP solvers \mcode{sbvp} and \mcode{bvptwp} to solve the controlled equations of motion \eqref{eq_pmp_bvp_disk} and \eqref{eq_pmp_bvp_ball} by predictor-corrector continuation in the parameter $\mu$.

In contrast to the direct method, the controlled equations of motion obtained via the indirect method have a very small radius of convergence \cite{betts2010practical,bryson1999dynamic,BrHo1975applied}.Therefore, the direct method is needed to initialize the predictor-corrector continuation of the controlled equations of motion. Predictor-corrector continuation is used in conjunction with the indirect, rather than direct, method, because a predictor-corrector continuation direct method requires a predictor-corrector continuation NLP solver. Even though predictor-corrector continuation NLP solver algorithms are provided in \cite{zangwill1981pathways,kungurtsev2017predictor}, there do not seem to be any publicly available predictor-corrector continuation NLP solvers.

\subsection{Trajectory Tracking for the Rolling Disk} \label{ssec_disk_sim}
In this and the next subsection, numerical solutions of the controlled equations of motion for the rolling disk are presented, where the goal is to move the disk between a pair of points while the disk's GC tracks a prescribed trajectory. A rolling disk of mass $m_0=1$, radius $r=1$, polar moment of inertia $d_2=1$, and with the CM coinciding with the GC (i.e. $\boldsymbol{\zeta}_0=\mathbf{0}$) is simulated. There are $n=4$ control masses, each of mass $1$ so that $m_1=m_2=m_3=m_4=1$, located on concentric circular control rails centered on the GC of radii $r_1=.9$, $r_2=.6\overline{3}$, $r_3=.3\overline{6}$, and $r_4=.1$, as shown in Figure~\ref{fig_dsim1_control_masses_rails}. The specific rail parameterizations expressed in the body frame centered on the GC are given in \cite{putkaradze2018dynamics}. The total system mass is $M=5$. Gravity is $g=1$. The initial time is fixed to $a=a_\mathrm{e}=0$ and the final time is fixed to $b=b_\mathrm{e}=2$. The disk's GC starts at rest at $z_a=0$ at time $a=a_\mathrm{e}=0$ and stops at rest at $z_b=1$ at time $b=b_\mathrm{e}=2$. Table~\ref{table_disk_ICs} shows parameter values used in the rolling disk's initial conditions \eqref{eq_disk_initial_conds} and final conditions \eqref{eq_disk_final_conds}. Since the initial orientation of the disk is $\phi_a=0$ and since the initial configurations of the control masses are given by $\boldsymbol{\theta}_a=\begin{bmatrix} \scalebox{0.75}[1.0]{\( - \)} \frac{\pi}{2} & \scalebox{0.75}[1.0]{\( - \)} \frac{\pi}{2} & \scalebox{0.75}[1.0]{\( - \)} \frac{\pi}{2} & \scalebox{0.75}[1.0]{\( - \)} \frac{\pi}{2} \end{bmatrix}^\mathsf{T}$, all the control masses are initially located directly below the GC. Table~\ref{table_disk_FCs} shows parameter values used in the rolling ball's final conditions \eqref{eq_disk_final_conds}. Moreover, for the final conditions \eqref{eq_disk_final_conds}, $\boldsymbol{\Pi}$ is projection onto the first coordinate. $\boldsymbol{\Delta}_b$ and $\boldsymbol{\Pi}$ are selected so that the total system CM in the spatial frame translated to the GC is located above or below the GC at the final time $b$.

\begin{table}[h!]
	\centering
	{
		\setlength{\extrarowheight}{1.5pt}
		\begin{tabular}{| c | c |}
			\hline
			\textbf{Parameter} & \textbf{Value} \\
			\hline\hline
			$\boldsymbol{\theta}_a$ & $\begin{bmatrix} \scalebox{0.75}[1.0]{\( - \)} \frac{\pi}{2} & \scalebox{0.75}[1.0]{\( - \)} \frac{\pi}{2} & \scalebox{0.75}[1.0]{\( - \)} \frac{\pi}{2} & \scalebox{0.75}[1.0]{\( - \)} \frac{\pi}{2} \end{bmatrix}^\mathsf{T}$  \\
			\hline
			$\dot \boldsymbol{\theta}_a$ & $\begin{bmatrix} 0 & 0 & 0 & 0 \end{bmatrix}^\mathsf{T}$ \\
			\hline
			$\phi_a$ & $0$ \\
			\hline
			$z_a$ & $0$ \\
			\hline
			$\dot z_a$ & $0$ \\
			\hline
		\end{tabular}
	}
	\caption{Initial condition parameter values for the rolling disk. Refer to \eqref{eq_disk_initial_conds} and \eqref{eq_disk_final_conds}.}
	\label{table_disk_ICs}
\end{table}

\begin{table}[h!]
	\centering
	{
		\setlength{\extrarowheight}{1.5pt}
		\begin{tabular}{| c | c |}
			\hline
			\textbf{Parameter} & \textbf{Value} \\
			\hline\hline
			$\boldsymbol{\Delta}_b$ & $0$ \\
			\hline
			$\dot \boldsymbol{\theta}_b$ & $\begin{bmatrix} 0 & 0 & 0 & 0 \end{bmatrix}^\mathsf{T}$ \\
			\hline
			$z_b$ & $1$ \\
			\hline
			$\dot z_b$ & $0$ \\
			\hline
		\end{tabular}
	}
	\caption{Final condition parameter values for the rolling disk. Refer to \eqref{eq_disk_final_conds}.}
	\label{table_disk_FCs}
\end{table}

The desired GC path $z_\mathrm{d}$ in the integrand cost function \eqref{eq_disk_integrand_cost} is depicted by the red curve in Figures~\ref{fig_dsim1_dm_gc_path} and \ref{fig_dsim1_pc_gc_path}. $z_\mathrm{d}$ encourages the disk's GC to track a sinusoidally-modulated linear trajectory connecting $z=0$ at $t=0$ with $z=1$ at $t=2$. That is, the disk is encouraged to roll right, then left, then right, then left, and finally to the right, with the amplitude of each successive roll increasing from the previous one. Specifically, $z_\mathrm{d}$ is given by \eqref{eq_z_d}, with $\epsilon=.01$ in \eqref{eq_sigmoid} and where $\tilde{z}_\mathrm{d}$ is given by
\begin{equation}
\tilde{z}_\mathrm{d}(t) = \left[ z_a+\left(z_b-z_a\right) \frac{t-a_\mathrm{e}}{b_\mathrm{e}-a_\mathrm{e}} \right] \sin\left(\frac{9\pi}{2}\frac{t-a_\mathrm{e}}{b_\mathrm{e}-a_\mathrm{e}}\right).
\end{equation}

Table~\ref{table_disk_integrand} shows the values set for the integrand cost function coefficients in \eqref{eq_disk_integrand_cost}. Since the initial and final times are fixed, the integrand cost function coefficient $\delta$ in \eqref{eq_disk_integrand_cost} and the endpoint cost function coefficients $\upsilon_a$ and $\upsilon_b$ in \eqref{eq_disk_endpoint_cost} are irrelevant. There is no external force acting on the disk's GC, so that $F_{\mathrm{e},1}=0$ in \eqref{eq_disk_kappa}.

\begin{table}[h!]
	\centering
	{
		\setlength{\extrarowheight}{1.5pt}
		\begin{tabular}{| c | c |}
			\hline
			\textbf{Parameter} & \textbf{Value} \\
			\hline\hline
			$\alpha(\mu)$ & $20+\frac{.95-\mu}{.95-.00001}\left(5000-20\right)$ \\
			\hline
			$\beta$ & $0$ \\
			\hline
			$\gamma_1$ & $.1$ \\
			\hline
			$\gamma_2$ & $.1$ \\
			\hline
			$\gamma_3$ & $.1$ \\
			\hline
			$\gamma_4$ & $.1$ \\
			\hline
		\end{tabular}
	}
	\caption{Integrand cost function coefficient values for the rolling disk when predictor-corrector continuation is performed in $\alpha$. Refer to \eqref{eq_disk_integrand_cost}.}
	\label{table_disk_integrand}
\end{table}

The direct method solver \mcode{GPOPS-II} is used to solve the optimal control problem \eqref{dyn_opt_problem_disk} when the integrand cost function coefficient is $\alpha=20$. Predictor-corrector continuation  is then used to solve the controlled equations of motion \eqref{eq_pmp_bvp_disk}, starting from the direct method solution. The continuation parameter is $\mu$, which is used to adjust $\alpha$ according to the linear homotopy given in Table~\ref{table_disk_integrand}, so that $\alpha=20$ when $\mu=.95$ and $\alpha=5{,}000$ when $\mu=.00001$. The predictor-corrector continuation begins at $\mu=.95$, which is consistent with the direct method solution obtained at $\alpha=20$.

For the direct method, \mcode{GPOPS-II} was run using the IPOPT NLP solver with the MA57 linear solver. The \mcode{GPOPS-II} mesh tolerance is $1\mathrm{e}\scalebox{0.75}[1.0]{\( - \)} 6$ and the IPOPT error tolerance is $1\mathrm{e}\scalebox{0.75}[1.0]{\( - \)} 7$. The sweep predictor-corrector method discussed in Appendix~\ref{app_sweep_predictor_corrector} was used by the indirect method. For the sweep predictor-corrector continuation method, the maximum tangent steplength is $\sigma_\mathrm{max}=30$, there are $250$ predictor-corrector steps, the direction of the initial unit tangent is determined by setting $d=\scalebox{0.75}[1.0]{\( - \)} 2$ to force the continuation parameter $\mu$ to initially decrease, the relative error tolerance is $1\mathrm{e}\scalebox{0.75}[1.0]{\( - \)} 8$, the unit tangent solver is \mcode{twpbvpc_m}, and the monotonic ``sweep'' continuation solver is \mcode{acdcc}. The numerical results are shown in Figures~\ref{fig_dsim1} and \ref{fig_dsim1_pc}. As $\mu$ decreases from $.95$ below $\scalebox{0.75}[1.0]{\( - \)} 8$ during continuation (see Figure~\ref{fig_dsim1_pc_cont_param}), $\alpha$ increases from $20$ up to $54{,}866$ (see Figure~\ref{fig_dsim1_pc_alpha}). Since $\alpha$ is ratcheted up during continuation, thereby increasing the penalty in the integrand cost function \eqref{eq_disk_integrand_cost} for deviation between the disk's GC and $z_d$, by the end of continuation, the disk's GC tracks $z_d$ much more accurately (compare Figures~\ref{fig_dsim1_dm_gc_path} vs \ref{fig_dsim1_pc_gc_path}), at the expense of wild control mass trajectories (compare Figures~\ref{fig_dsim1_dm_cm_bf} vs \ref{fig_dsim1_pc_cm_bf}) and large magnitude controls (compare Figures~\ref{fig_dsim1_dm_controls} vs \ref{fig_dsim1_pc_controls}). Note the turning points at solutions $14$ and $19$ in Figures~\ref{fig_dsim1_pc_cont_param}-\ref{fig_dsim1_pc_J}. In Figure~\ref{fig_dsim1_pc_sigma}, note that the predictor-corrector continuation method has to reduce the tangent steplength below $\sigma_\mathrm{max}=30$ to get around the turning points encountered at solutions $14$ and $19$.

\begin{figure}[h]
	\centering
	\includegraphics[scale=.6]{dsim1_control_masses_rails}
	\caption{The disk of radius $r=1$ actuated by $4$ control masses, $m_1$, $m_2$, $m_3$, and $m_4$, each on its own circular control rail. The control rail radii are $r_1=.9$, $r_2=.6\overline{3}$, $r_3=.3\overline{6}$, and $r_4=.1$. The location of the disk's CM is denoted by $m_0$.}
	\label{fig_dsim1_control_masses_rails}
\end{figure}

\begin{figure}[!ht]
	\centering
	\subfloat[The GC hardly tracks the desired path when $\alpha=20$.]{\includegraphics[scale=.5]{dsim1_dm_gc_path}\label{fig_dsim1_dm_gc_path}}
	\hspace{5mm}
	\subfloat[The GC tracks the desired path much more accurately when $\alpha=54{,}866$.]{\includegraphics[scale=.5]{dsim1_pc_gc_path}\label{fig_dsim1_pc_gc_path}}
	\\
	\subfloat[The motion of the center of masses is modest when $\alpha=20$.]{\includegraphics[scale=.5]{dsim1_dm_cm_bf}\label{fig_dsim1_dm_cm_bf}}
	\hspace{5mm}
	\subfloat[The motion of the center of masses is much wilder when $\alpha=54{,}866$.]{\includegraphics[scale=.5]{dsim1_pc_cm_bf}\label{fig_dsim1_pc_cm_bf}}
	\\
	\subfloat[The controls have relatively small magnitudes when $\alpha=20$.]{\includegraphics[scale=.5]{dsim1_dm_controls}\label{fig_dsim1_dm_controls}}
	\hspace{5mm}
	\subfloat[The controls have large magnitudes when $\alpha=54{,}866$.]{\includegraphics[scale=.5]{dsim1_pc_controls}\label{fig_dsim1_pc_controls}}
	\caption{Numerical solutions of the rolling disk optimal control problem \eqref{dyn_opt_problem_disk} using $4$ control masses for $\beta=0,\, \gamma_1=\gamma_2=\gamma_3=\gamma_4=.1$, and fixed initial and final times. The direct method results for $\alpha=20$ are shown in the left column, while the predictor-corrector continuation indirect method results for $\alpha=54{,}866$ are shown in the right column.}
	\label{fig_dsim1}
\end{figure}

\begin{figure}[!ht]
	\centering
	\subfloat[Evolution of the continuation parameter $\mu$.]{\includegraphics[scale=.5]{dsim1_pc_cont_param}\label{fig_dsim1_pc_cont_param}}
	\hspace{5mm}
	\subfloat[Evolution of the tangent steplength $\sigma$.]{\includegraphics[scale=.5]{dsim1_pc_sigma}\label{fig_dsim1_pc_sigma}}
	\\
	\subfloat[Evolution of $\alpha$ from $20$ to $54{,}866$.]{\includegraphics[scale=.5]{dsim1_pc_alpha}\label{fig_dsim1_pc_alpha}}
	\hspace{5mm}
	\subfloat[Evolution of the performance index $J$.]{\includegraphics[scale=.5]{dsim1_pc_J}\label{fig_dsim1_pc_J}}
	\caption{Evolution of various parameters during the predictor-corrector continuation indirect method, which starts from the direct method solution, used to solve the rolling disk optimal control problem \eqref{dyn_opt_problem_disk}. Note the pair of turning points at solutions 14 and 19. The maximum tangent steplength $\sigma_\mathrm{max}$ is fixed to $30$.}
	\label{fig_dsim1_pc}
\end{figure}

\subsection{Trajectory Tracking for the Rolling Disk, Redux} \label{ssec_disk_sim_redux}
Based on the turning points unveiled in the previous simulation, the disk is re-simulated using different parameters for the sweep predictor-corrector method in order to ratchet up $\alpha$ even more while decreasing the total simulation runtime. In particular, the maximum tangent steplength used by the sweep predictor-corrector method is adjusted based on the location of the turning points revealed by the previous simulation. Until the turning points (which occur at solutions $14$ and $19$) are passed, $\sigma_\mathrm{max}=30$, after which $\sigma_\mathrm{max}$ is increased linearly to a maximum of $3{,}000$, as depicted in Figure~\ref{fig_dsim2_sigma}. Because $\sigma_\mathrm{max}$ is increased dramatically, only $42$ predictor-corrector steps are executed. The numerical results are shown in Figures~\ref{fig_dsim2} and \ref{fig_dsim2_pc}.  As $\mu$ decreases from $.95$ below $\scalebox{0.75}[1.0]{\( - \)} 200$ during continuation (see Figure~\ref{fig_dsim2_pc_cont_param}), $\alpha$ increases from $20$ up to $1{,}229{,}199$ (see Figure~\ref{fig_dsim2_pc_alpha}). Since $\alpha$ is ratcheted up during continuation, thereby increasing the penalty in the integrand cost function \eqref{eq_disk_integrand_cost} for deviation between the disk's GC and $z_d$, by the end of continuation, the disk's GC tracks $z_d$ very accurately (compare Figures~\ref{fig_dsim2_dm_gc_path} vs \ref{fig_dsim2_pc_gc_path}), at the expense of extremely wild control mass trajectories (compare Figures~\ref{fig_dsim2_dm_cm_bf} vs \ref{fig_dsim2_pc_cm_bf}) and large magnitude controls (compare Figures~\ref{fig_dsim2_dm_controls} vs \ref{fig_dsim2_pc_controls}).

\begin{figure}[!ht]
	\centering
	\subfloat[The GC hardly tracks the desired path when $\alpha=20$.]{\includegraphics[scale=.5]{dsim1_dm_gc_path}\label{fig_dsim2_dm_gc_path}}
	\hspace{5mm}
	\subfloat[The GC tracks the desired path very accurately when $\alpha=1{,}229{,}199$.]{\includegraphics[scale=.5]{dsim2_pc_gc_path}\label{fig_dsim2_pc_gc_path}}
	\\
	\subfloat[The motion of the center of masses is modest when $\alpha=20$.]{\includegraphics[scale=.5]{dsim1_dm_cm_bf}\label{fig_dsim2_dm_cm_bf}}
	\hspace{5mm}
	\subfloat[The motion of the center of masses is extremely wild when $\alpha=1{,}229{,}199$.]{\includegraphics[scale=.5]{dsim2_pc_cm_bf}\label{fig_dsim2_pc_cm_bf}}
	\\
	\subfloat[The controls have relatively small magnitude when $\alpha=20$.]{\includegraphics[scale=.5]{dsim1_dm_controls}\label{fig_dsim2_dm_controls}}
	\hspace{5mm}
	\subfloat[The controls have large magnitudes when $\alpha=1{,}229{,}199$.]{\includegraphics[scale=.5]{dsim2_pc_controls}\label{fig_dsim2_pc_controls}}
	\caption{Numerical solutions of the rolling disk optimal control problem \eqref{dyn_opt_problem_disk} using $4$ control masses for $\beta=0,\, \gamma_1=\gamma_2=\gamma_3=\gamma_4=.1$, and fixed initial and final times. The direct method results for $\alpha=20$ are shown in the left column, while the predictor-corrector continuation indirect method results for $\alpha=1{,}229{,}199$ are shown in the right column.}
	\label{fig_dsim2}
\end{figure}

\begin{figure}[h]
	\centering
	\subfloat[Evolution of the continuation parameter $\mu$.]{\includegraphics[scale=.5]{dsim2_pc_cont_param}\label{fig_dsim2_pc_cont_param}}
	\hspace{5mm}
	\subfloat[Evolution of the tangent steplength $\sigma$.]{\includegraphics[scale=.5]{dsim2_pc_sigma}\label{fig_dsim2_sigma}}
	\\
	\subfloat[Evolution of $\alpha$ from $20$ to $1{,}229{,}199$.]{\includegraphics[scale=.5]{dsim2_pc_alpha}\label{fig_dsim2_pc_alpha}}
	\hspace{5mm}
	\subfloat[Evolution of the performance index $J$.]{\includegraphics[scale=.5]{dsim2_pc_J}\label{fig_dsim2_pc_J}}
	\caption{Evolution of various parameters during the predictor-corrector continuation indirect method, which starts from the direct method solution, used to solve the rolling disk optimal control problem \eqref{dyn_opt_problem_disk}. The maximum tangent steplength $\sigma_\mathrm{max}$ is increased linearly after passing the turning points.}
	\label{fig_dsim2_pc}
\end{figure}

\subsection{Obstacle Avoidance for the Rolling Ball} \label{ssec_ball_sim}
Numerical solutions of the controlled equations of motion for the rolling ball are presented here, where the goal is to move the ball between a pair of points while avoiding a pair of obstacles using the obstacle avoidance function \eqref{eq_obstacle_pot_func} with the sigmoid boundary transition function \eqref{eq_obst_sigmoid}. A rolling ball of mass $m_0=1$, radius $r=1$, principle moments of inertia $d_1=d_2=d_3=1$, and with the CM coinciding with the GC (i.e. $\boldsymbol{\zeta}_0=\mathbf{0}$) is simulated. There are $n=3$ control masses, each of mass $1$ so that $m_1=m_2=m_3=1$, located on circular control rails centered on the GC of radii $r_1=.95$, $r_2=.9$, and $r_3=.85$, oriented as shown in Figure~\ref{fig_bsim2_control_masses_rails}. The specific rail parameterizations expressed in the body frame centered on the GC are given in \cite{putkaradze2018dynamics}. The total system mass is $M=4$. Gravity is $g=1$. The initial time is fixed to $a=a_\mathrm{e}=0$ and the final time is fixed to $b=b_\mathrm{e}=.5$. The ball's GC starts at rest at $\boldsymbol{z}_a=\begin{bmatrix}  0 & 0 \end{bmatrix}^\mathsf{T}$ at time $a=a_\mathrm{e}=0$ and stops at rest at $\boldsymbol{z}_b=\begin{bmatrix}  1 & 1 \end{bmatrix}^\mathsf{T}$ at time $b=b_\mathrm{e}=.5$. Table~\ref{table_ball_ICs} shows the parameter values used in the rolling ball's initial conditions \eqref{eq_ball_initial_conds}. The initial configurations of the control masses are selected so that the total system CM in the spatial frame translated to the GC is initially located above or below the GC. Table~\ref{table_ball_FCs} shows the parameter values used in the rolling ball's final conditions \eqref{eq_ball_final_conds}. Moreover, for the final conditions in \eqref{eq_ball_final_conds}, $\boldsymbol{\Pi}$ is projection onto the first and second coordinates. $\boldsymbol{\Delta}_b$ and $\boldsymbol{\Pi}$ are selected so that the total system CM in the spatial frame translated to the GC is located above or below the GC at the final time $b$.

\begin{table}[h!]
	\centering
	{
		\setlength{\extrarowheight}{1.5pt}
		\begin{tabular}{| c | c |}
			\hline
			\textbf{Parameter} & \textbf{Value} \\
			\hline\hline
			$\boldsymbol{\theta}_a$ & $\begin{bmatrix}  0 & 2.0369 & .7044 \end{bmatrix}^\mathsf{T}$  \\
			\hline
			$\dot \boldsymbol{\theta}_a$ & $\begin{bmatrix} 0 & 0 & 0 \end{bmatrix}^\mathsf{T}$ \\
			\hline
			$\mathfrak{q}_a$ & $\begin{bmatrix} 1 &  0 & 0 & 0 \end{bmatrix}^\mathsf{T}$ \\
			\hline
			$\boldsymbol{\Omega}_a$ & $\begin{bmatrix}  0 & 0 & 0 \end{bmatrix}^\mathsf{T}$ \\
			\hline
			$\boldsymbol{z}_a$ & $\begin{bmatrix}  0 & 0 \end{bmatrix}^\mathsf{T}$ \\
			\hline
		\end{tabular}
	}
	\caption{Initial condition parameter values for the rolling ball. Refer to \eqref{eq_ball_initial_conds}.}
	\label{table_ball_ICs}
\end{table}

\begin{table}[h!]
	\centering
	{
		\setlength{\extrarowheight}{1.5pt}
		\begin{tabular}{| c | c |}
			\hline
			\textbf{Parameter} & \textbf{Value} \\
			\hline\hline
			$\boldsymbol{\Delta}_b$ & $\begin{bmatrix}  0 & 0 \end{bmatrix}^\mathsf{T}$  \\
			\hline
			$\dot \boldsymbol{\theta}_b$ & $\begin{bmatrix} 0 & 0 & 0 \end{bmatrix}^\mathsf{T}$ \\
			\hline
			$\boldsymbol{\Omega}_b$ & $\begin{bmatrix}  0 & 0 & 0 \end{bmatrix}^\mathsf{T}$ \\
			\hline
			$\boldsymbol{z}_b$ & $\begin{bmatrix}  1 & 1 \end{bmatrix}^\mathsf{T}$ \\
			\hline
		\end{tabular}
	}
	\caption{Final condition parameter values for the rolling ball. Refer to \eqref{eq_ball_final_conds}.}
	\label{table_ball_FCs}
\end{table}

The desired GC path $\boldsymbol{z}_\mathrm{d}$ in the integrand cost function \eqref{eq_ball_integrand_cost} is depicted by the red curve in Figures~\ref{fig_bsim2_dm_gc_path}, \ref{fig_bsim2_pc_gc_path}, and \ref{fig_rolling_ball_z_d}. $\boldsymbol{z}_\mathrm{d}$ encourages the ball's GC to track a trajectory in the $\mathbf{e}_1$-$\mathbf{e}_2$ plane connecting $\boldsymbol{z}=\begin{bmatrix} 0 & 0 \end{bmatrix}^\mathsf{T}$ at $t=0$ with $\boldsymbol{z}=\begin{bmatrix} 1 & 1 \end{bmatrix}^\mathsf{T}$ at $t=.5$. That is, the ball is encouraged to start at $\boldsymbol{z}=\begin{bmatrix} 0 & 0 \end{bmatrix}^\mathsf{T}$ at $t=0$ with zero velocity and to stop at $\boldsymbol{z}=\begin{bmatrix} 1 & 1 \end{bmatrix}^\mathsf{T}$ at $t=.5$ with zero velocity. Specifically, $\boldsymbol{z}_\mathrm{d}$ is given by \eqref{eq_ball_z_d}, with $\epsilon=.01$ in \eqref{eq_sigmoid} and where $\tilde{\boldsymbol{z}}_\mathrm{d}$ is given by \eqref{eq_ball_tz_d}.

Table~\ref{table_ball_integrand} shows the values set for the integrand cost function coefficients in \eqref{eq_ball_integrand_cost}. Since the initial and final times are fixed, the integrand cost function coefficient $\delta$ in \eqref{eq_ball_integrand_cost} and the endpoint cost function coefficients $\upsilon_a$ and $\upsilon_b$ in \eqref{eq_ball_endpoint_cost} are irrelevant. There are $2$ circular obstacles which the ball's GC should avoid, depicted in Figures~\ref{fig_bsim2_dm_gc_path} and \ref{fig_bsim2_pc_gc_path}. The obstacles appearing in \eqref{eq_ball_integrand_cost} are modeled by \eqref{eq_obstacle_pot_func} with \eqref{eq_obst_sigmoid}; the obstacle centers, radii, and heights are also shown in Table~\ref{table_ball_integrand}. There is no external force acting on the disk's GC, so that $\tilde \boldsymbol{\Gamma} = \Lambda^{-1} \mathbf{F}_\mathrm{e}=\mathbf{0}$ in \eqref{eq_ball_kappa}.

\begin{table}[h!]
	\centering
	{
		\setlength{\extrarowheight}{1.5pt}
		\begin{tabular}{| c | c |}
			\hline
			\textbf{Parameter} & \textbf{Value} \\
			\hline\hline
			$\alpha$ & $20$ \\
			\hline
			$\beta$ & $0$ \\
			\hline
			$\gamma_1$ & $10$ \\
			\hline
			$\gamma_2$ & $10$ \\
			\hline
			$\gamma_3$ & $10$ \\
			\hline
			$\boldsymbol{v}_1$ & $\begin{bmatrix}  .3 & .3 \end{bmatrix}^\mathsf{T}$ \\
			\hline
			$\boldsymbol{v}_2$ & $\begin{bmatrix}  .7 & .7 \end{bmatrix}^\mathsf{T}$ \\
			\hline
			$\rho_1$ & $.2$ \\
			\hline
			$\rho_2$ & $.2$ \\
			\hline
			$h_1(\mu)$ & $.1+\frac{.95-\mu}{.95-.00001}\left(1000-.1\right)$ \\
			\hline
			$h_2(\mu)$ & $.1+\frac{.95-\mu}{.95-.00001}\left(1000-.1\right)$ \\
			\hline
		\end{tabular}
	}
	\caption{Integrand cost function coefficient values for the rolling ball when predictor-corrector continuation is performed in the obstacle heights. Refer to \eqref{eq_ball_integrand_cost} and \eqref{eq_obstacle_pot_func}.}
	\label{table_ball_integrand}
\end{table}

The direct method solver \mcode{GPOPS-II} is used to solve the optimal control problem \eqref{dyn_opt_problem_ball} when the obstacle heights appearing in the integrand cost function are $h_1=h_2=.1$, given an initial solution that solves the optimal control problem \eqref{dyn_opt_problem_dae2_ball}. Predictor-corrector continuation is then used to solve the controlled equations of motion \eqref{eq_pmp_bvp_ball}, starting from the direct method solution. The continuation parameter is $\mu$, which is used to adjust $h_1=h_2$ according to the linear homotopy shown in Table~\ref{table_ball_integrand}, so that $h_1=h_2=.1$ when $\mu=.95$ and $h_1=h_2=1{,}000$ when $\mu=.00001$. The predictor-corrector continuation begins at $\mu=.95$, which is consistent with the direct method solution obtained at $h_1=h_2=.1$.

For the direct method, \mcode{GPOPS-II} was run using the IPOPT NLP solver with the MKL PARDISO linear solver. The \mcode{GPOPS-II} mesh tolerance is $1\mathrm{e}\scalebox{0.75}[1.0]{\( - \)} 6$ and the IPOPT error tolerance is $1\mathrm{e}\scalebox{0.75}[1.0]{\( - \)} 7$. The sweep predictor-corrector method discussed in Appendix~\ref{app_sweep_predictor_corrector} was used by the indirect method. For the sweep predictor-corrector continuation method, the maximum tangent steplength $\sigma_\mathrm{max}$ is adjusted according to Figure~\ref{fig_bsim2_pc_sigma} over the course of $6$ predictor-corrector steps, the direction of the initial unit tangent is determined by setting $d=\scalebox{0.75}[1.0]{\( - \)} 2$ to force the continuation parameter $\mu$ to initially decrease, the relative error tolerance is $1\mathrm{e}\scalebox{0.75}[1.0]{\( - \)} 6$, the unit tangent solver is \mcode{twpbvpc_m}, and the monotonic ``sweep'' continuation solver is \mcode{acdcc}. The numerical results are shown in Figures~\ref{fig_bsim2} and \ref{fig_bsim2_pc}. As $\mu$ decreases from $.95$ below $\scalebox{0.75}[1.0]{\( - \)} 1{,}400$ during continuation (see Figure~\ref{fig_bsim2_pc_cont_param}), $h_1=h_2$ increases from $.1$ up to $1{,}495{,}740$ (see Figure~\ref{fig_bsim2_pc_obst_height}). Since $h_1=h_2$ is ratcheted up during continuation, thereby increasing the penalty in the integrand cost function \eqref{eq_ball_integrand_cost} when the GC intrudes into the obstacles, by the end of continuation, the ball's GC veers tightly around both obstacles (compare Figures~\ref{fig_bsim2_dm_gc_path} vs \ref{fig_bsim2_pc_gc_path}), at the expense of large magnitude controls (compare Figures~\ref{fig_bsim2_dm_controls} vs \ref{fig_bsim2_pc_controls}). Note the turning points in the continuation parameter $\mu$ at solutions $3$ and $5$ in Figure~\ref{fig_bsim2_pc_cont_param}.

\begin{figure}[h]
	\centering
	\includegraphics[scale=.5]{bsim2_control_masses_rails_png}
	\caption{The ball of radius $r=1$ actuated by $3$ control masses, $m_1$, $m_2$, and $m_3$, each on its own circular control rail. The control rail radii are $r_1=.95$, $r_2=.9$, and $r_3=.85$. The location of the ball's CM is denoted by $m_0$.}
	\label{fig_bsim2_control_masses_rails}
\end{figure}

\begin{figure}[!ht]
	\centering
	\subfloat[The GC plows through the obstacles when $h_1=h_2=.1$.]{\includegraphics[scale=.4]{bsim2_dm_gc_path_png}\label{fig_bsim2_dm_gc_path}}
	\hspace{5mm}
	\subfloat[The GC veers tightly around the obstacles when $h_1=h_2=1{,}495{,}740$.]{\includegraphics[scale=.4]{bsim2_pc_gc_path_png}\label{fig_bsim2_pc_gc_path}}
	\\
	\subfloat[Motion of the center of masses when $h_1=h_2=.1$.]{\includegraphics[scale=.5]{bsim2_dm_cm_bf}\label{fig_bsim2_dm_cm_bf}}
	\hspace{5mm}
	\subfloat[Motion of the center of masses when $h_1=h_2=1{,}495{,}740$.]{\includegraphics[scale=.5]{bsim2_pc_cm_bf}\label{fig_bsim2_pc_cm_bf}}
	\\
	\subfloat[The controls when $h_1=h_2=.1$.]{\includegraphics[scale=.5]{bsim2_dm_controls}\label{fig_bsim2_dm_controls}}
	\hspace{5mm}
	\subfloat[The controls increase in magnitude when $h_1=h_2=1{,}495{,}740$.]{\includegraphics[scale=.5]{bsim2_pc_controls}\label{fig_bsim2_pc_controls}}
	\caption{Numerical solutions of the rolling ball optimal control problem \eqref{dyn_opt_problem_ball} using $3$ control masses for $\alpha=20,\, \beta=0,\, \gamma_1=\gamma_2=\gamma_3=10$, and fixed initial and final times. The obstacle centers are located at $\boldsymbol{v}_1=\protect\begin{bmatrix}  v_{1,1} & v_{1,2} \protect\end{bmatrix}^\mathsf{T} = \protect\begin{bmatrix}  .3 & .3 \protect\end{bmatrix}^\mathsf{T}$ and $\boldsymbol{v}_2 = \protect\begin{bmatrix}  v_{2,1} & v_{2,2} \protect\end{bmatrix}^\mathsf{T} = \protect\begin{bmatrix}  .7 & .7 \protect\end{bmatrix}^\mathsf{T}$ and the obstacle radii are $\rho_1=\rho_2=.2$. The direct method results for obstacle heights at $h_1=h_2=.1$ are shown in the left column, while the predictor-corrector continuation indirect method results for obstacle heights at $h_1=h_2=1{,}495{,}740$ are shown in the right column.}
	\label{fig_bsim2}
\end{figure}

\begin{figure}[!ht]
	\centering
	\subfloat[Evolution of the continuation parameter $\mu$. Note the turning points at solutions $3$ and $5$.]{\includegraphics[scale=.5]{bsim2_pc_cont_param}\label{fig_bsim2_pc_cont_param}}
	\hspace{5mm}
	\subfloat[Evolution of the tangent steplength $\sigma$. The maximum tangent steplength is not realized to obtain solution 4.]{\includegraphics[scale=.5]{bsim2_pc_sigma}\label{fig_bsim2_pc_sigma}}
	\\
	\subfloat[Evolution of the obstacle heights $h_1=h_2$ from $.1$ to $1{,}495{,}740$.]{\includegraphics[scale=.5]{bsim2_pc_obst_height}\label{fig_bsim2_pc_obst_height}}
	\hspace{5mm}
	\subfloat[Evolution of the performance index $J$.]{\includegraphics[scale=.5]{bsim2_pc_J}\label{fig_bsim2_pc_J}}
	\caption{Evolution of various parameters during the predictor-corrector continuation indirect method, which starts from the direct method solution, used to solve the rolling ball optimal control problem \eqref{dyn_opt_problem_ball}.}
	\label{fig_bsim2_pc}
\end{figure}

\subsection{Obstacle Avoidance for the Rolling Ball, Redux} \label{ssec_ball_sim_redux}
The controlled equations of motion for the rolling ball are solved numerically again to move the ball between a pair of points while avoiding a pair of obstacles, but this time the obstacle avoidance function \eqref{eq_obstacle_pot_func} uses the cutoff boundary transition function \eqref{eq_obst_cutoff}. First, the direct method solver \mcode{GPOPS-II} is used with the NLP solver SNOPT to solve the optimal control problem \eqref{dyn_opt_problem_dae2_ball} with the mesh tolerance set to $1\mathrm{e}{-3}$ and the NLP error tolerance set to $1\mathrm{e}{-4}$.
Starting from the solution to this problem as an initial guess, \mcode{GPOPS-II} is used with SNOPT to solve \eqref{dyn_opt_problem_ball} with the same tolerances. Starting from the solution to this problem as an initial guess, \mcode{GPOPS-II} is again used with SNOPT to solve \eqref{dyn_opt_problem_ball}, but with the mesh tolerance set to $1\mathrm{e}{-6}$ and the NLP error tolerance set to $1\mathrm{e}{-7}$. Starting from this direct method solution, predictor-corrector continuation is then used to solve the controlled equations of motion \eqref{eq_pmp_bvp_ball}, where the continuation parameter is $\mu$, which is used to adjust $h_1=h_2$ according to the linear homotopy shown in Table~\ref{table_ball_integrand_pc1}, so that $h_1=h_2=1\mathrm{e}{-10}$ when $\mu=.95$ and $h_1=h_2=1{,}000$ when $\mu=.00001$. This predictor-corrector continuation begins at $\mu=.95$, which is consistent with the direct method solution obtained at $h_1=h_2=1\mathrm{e}{-10}$. Starting from this predictor-corrector continuation solution, a second predictor-corrector continuation is used to solve the controlled equations of motion \eqref{eq_pmp_bvp_ball}, where the continuation parameter is $\mu$, which is now used to adjust $\gamma_1=\gamma_2=\gamma_3$ according to the linear homotopy shown in Table~\ref{table_ball_integrand_pc2}, so that $\gamma_1=\gamma_2=\gamma_3=10$ when $\mu=.95$ and $\gamma_1=\gamma_2=\gamma_3=\unaryminus1{,}000$ when $\mu=.00001$. The second predictor-corrector continuation begins at $\mu=.95$, which is consistent with the previous predictor-corrector continuation solution obtained at $\gamma_1=\gamma_2=\gamma_3=10$.

The sweep predictor-corrector method discussed in Appendix~\ref{app_sweep_predictor_corrector} was used by both instances of the indirect method. For the sweep predictor-corrector continuation method, the maximum tangent steplength $\sigma_\mathrm{max}$ is adjusted according to Figures~\ref{fig_bsim6_pc_h_sigma} and \ref{fig_bsim6_pc_gamma_sigma} over the course of $18$ and $4$ predictor-corrector steps, the direction of the initial unit tangent is determined by setting $d=\scalebox{0.75}[1.0]{\( - \)} 2$ to force the continuation parameter $\mu$ to initially decrease, the relative error tolerance is $1\mathrm{e}\scalebox{0.75}[1.0]{\( - \)} 6$, the unit tangent solver is \mcode{twpbvpc_m}, and the monotonic ``sweep'' continuation solver is \mcode{acdcc}.

The numerical results are shown in Figures~\ref{fig_bsim6} and \ref{fig_bsim6_pc}. As $\mu$ decreases from $.95$ down to $\scalebox{0.75}[1.0]{\( - \)} 8.638\mathrm{e}{11}$ during the first round of predictor-corrector continuation, $h_1=h_2$ increases from $1\mathrm{e}{-10}$ up to $9.09\mathrm{e}{14}$ (see Figure~\ref{fig_bsim6_pc_h_obst_height}). Since $h_1=h_2$ is ratcheted up during continuation, thereby increasing the penalty in the integrand cost function \eqref{eq_ball_integrand_cost} when the GC intrudes into the obstacles, by the end of continuation, the ball's GC approaches the boundaries of both obstacles (compare Figures~\ref{fig_bsim6_dm_gc_path_plain} vs \ref{fig_bsim6_pc_h_gc_path_plain}), at the expense of large magnitude controls (compare Figures~\ref{fig_bsim6_dm_controls} vs \ref{fig_bsim6_pc_h_controls}). As $\mu$ decreases from $.95$ down to $.94059432759$ during the second round of predictor-corrector continuation, $\gamma_1=\gamma_2=\gamma_3$ decreases from $10$ down to $1.8\mathrm{e}{-4}$ (see Figure~\ref{fig_bsim6_pc_gamma_weights}). Since $\gamma_1=\gamma_2=\gamma_3$ is ratcheted down during continuation, thereby decreasing the penalty in the integrand cost function \eqref{eq_ball_integrand_cost} for large magnitude control mass accelerations, and since the obstacle heights are held fixed at $9.09\mathrm{e}{14}$, by the end of continuation, the ball's GC veers smartly around both obstacles (compare Figures~\ref{fig_bsim6_pc_h_gc_path_plain} vs \ref{fig_bsim6_pc_gamma_gc_path_plain}). Figure~\ref{fig_bsim6_pc_h_J}  shows that the performance index $J$ increases from $1.3\mathrm{e}{5}$ up to $4.72\mathrm{e}{5}$ as the obstacle heights are ramped up in the first round of predictor-corrector continuation; Figure~\ref{fig_bsim6_pc_gamma_J} shows that the performance index $J$ then decreases down to $34.3$ as the control weights are ramped down and the ball's GC fully departs the tall obstacles in the second round of predictor-corrector continuation.

\begin{table}[h!]
	\centering
	{
		\setlength{\extrarowheight}{1.5pt}
		\begin{tabular}{| c | c |}
			\hline
			\textbf{Parameter} & \textbf{Value} \\
			\hline\hline
			$\alpha$ & $30$ \\
			\hline
			$\beta$ & $0$ \\
			\hline
			$\gamma_1$ & $10$ \\
			\hline
			$\gamma_2$ & $10$ \\
			\hline
			$\gamma_3$ & $10$ \\
			\hline
			$\boldsymbol{v}_1$ & $\begin{bmatrix}  .3 & .3 \end{bmatrix}^\mathsf{T}$ \\
			\hline
			$\boldsymbol{v}_2$ & $\begin{bmatrix}  .7 & .7 \end{bmatrix}^\mathsf{T}$ \\
			\hline
			$\rho_1$ & $.2$ \\
			\hline
			$\rho_2$ & $.2$ \\
			\hline
			$h_1(\mu)$ & $\left[1\mathrm{e}{-10}\right]+\frac{.95-\mu}{.95-.00001}\left(1000-\left[1\mathrm{e}{-10}\right]\right)$ \\
			\hline
			$h_2(\mu)$ & $\left[1\mathrm{e}{-10}\right]+\frac{.95-\mu}{.95-.00001}\left(1000-\left[1\mathrm{e}{-10}\right]\right)$ \\
			\hline
		\end{tabular}
	}
	\caption{Integrand cost function coefficient values for the rolling ball when a first round of predictor-corrector continuation is performed in the obstacle heights. Refer to \eqref{eq_ball_integrand_cost} and \eqref{eq_obstacle_pot_func}.}
	\label{table_ball_integrand_pc1}
\end{table}

\begin{table}[h!]
	\centering
	{
		\setlength{\extrarowheight}{1.5pt}
		\begin{tabular}{| c | c |}
			\hline
			\textbf{Parameter} & \textbf{Value} \\
			\hline\hline
			$\alpha$ & $30$ \\
			\hline
			$\beta$ & $0$ \\
			\hline
			$h_1$ & $9.1\mathrm{e}{14}$ \\
			\hline
			$h_2$ & $9.1\mathrm{e}{14}$ \\
			\hline
			$\boldsymbol{v}_1$ & $\begin{bmatrix}  .3 & .3 \end{bmatrix}^\mathsf{T}$ \\
			\hline
			$\boldsymbol{v}_2$ & $\begin{bmatrix}  .7 & .7 \end{bmatrix}^\mathsf{T}$ \\
			\hline
			$\rho_1$ & $.2$ \\
			\hline
			$\rho_2$ & $.2$ \\
			\hline
			$\gamma_1(\mu)$ & $10+\frac{.95-\mu}{.95-.00001}\left(-1000-10\right)$ \\
			\hline
			$\gamma_2(\mu)$ & $10+\frac{.95-\mu}{.95-.00001}\left(-1000-10\right)$ \\
			\hline
			$\gamma_3(\mu)$ & $10+\frac{.95-\mu}{.95-.00001}\left(-1000-10\right)$ \\
			\hline
		\end{tabular}
	}
	\caption{Integrand cost function coefficient values for the rolling ball when a second round of predictor-corrector continuation is performed in the control weights. Refer to \eqref{eq_ball_integrand_cost} and \eqref{eq_obstacle_pot_func}.}
	\label{table_ball_integrand_pc2}
\end{table}

\begin{figure}[!ht]
	\centering
	\subfloat[The GC plows through the obstacles when $h_1=h_2=1\mathrm{e}{-10}$.]{\includegraphics[scale=.35]{bsim6_dm_gc_path_plain}\label{fig_bsim6_dm_gc_path_plain}}
	\hspace{5mm}
	\subfloat[The GC nearly clears the obstacles after ramping the obstacle heights $h_1=h_2$ up from $1\mathrm{e}{-10}$ to $9.09\mathrm{e}{14}$.]{\includegraphics[scale=.35]{bsim6_pc_h_gc_path_plain}\label{fig_bsim6_pc_h_gc_path_plain}}
	\hspace{5mm}
	\subfloat[The GC veers tightly around the obstacle boundaries after relaxing the control weights $\gamma_1=\gamma_2=\gamma_3$ from $10$ down to $1.8\mathrm{e}{-4}$.]{\includegraphics[scale=.35]{bsim6_pc_gamma_gc_path_plain}\label{fig_bsim6_pc_gamma_gc_path_plain}}
	\\
	\subfloat[Motion of the center of masses when $h_1=h_2=1\mathrm{e}{-10}$.]{\includegraphics[scale=.35]{bsim6_dm_cm_bf}\label{fig_bsim6_dm_cm_bf}}
	\hspace{5mm}
	\subfloat[Motion of the center of masses after ramping $h_1=h_2$ up from $1\mathrm{e}{-10}$ to $9.09\mathrm{e}{14}$.]{\includegraphics[scale=.35]{bsim6_pc_h_cm_bf}\label{fig_bsim6_pc_h_cm_bf}}
	\hspace{5mm}
	\subfloat[Motion of the center of masses after relaxing the control weights $\gamma_1=\gamma_2=\gamma_3$ from $10$ down to $1.8\mathrm{e}{-4}$.]{\includegraphics[scale=.35]{bsim6_pc_gamma_cm_bf}\label{fig_bsim6_pc_gamma_cm_bf}}
	\\
	\subfloat[The controls when $h_1=h_2=1\mathrm{e}{-10}$.]{\includegraphics[scale=.35]{bsim6_dm_controls}\label{fig_bsim6_dm_controls}}
	\hspace{5mm}
	\subfloat[The controls after ramping $h_1=h_2$ up from $1\mathrm{e}{-10}$ to $9.09\mathrm{e}{14}$.]{\includegraphics[scale=.35]{bsim6_pc_h_controls}\label{fig_bsim6_pc_h_controls}}
	\hspace{5mm}
	\subfloat[The controls after relaxing $\gamma_1=\gamma_2=\gamma_3$ from $10$ down to $1.8\mathrm{e}{-4}$.]{\includegraphics[scale=.35]{bsim6_pc_gamma_controls}\label{fig_bsim6_pc_gamma_controls}}
	\caption{Direct and predictor-corrector indirect methods realize an obstacle avoidance maneuver for the rolling ball. First, a direct method (left column) solves to provide an initial solution that plows through the obstacles. Then, predictor-corrector continuation in the obstacle heights $h_1=h_2$ (middle column) solves to transform the ball's trajectory to the obstacle boundaries. Finally, predictor-corrector continuation in the control weights $\gamma_1=\gamma_2=\gamma_3$ (right column) solves so that the ball's trajectory is just outside the obstacle boundaries. }

	\label{fig_bsim6}
\end{figure}

\begin{figure}[!ht]
	\centering
	\subfloat[Evolution of the obstacle heights $h_1=h_2$ as they increase from $1\mathrm{e}{-10}$ to $9.09\mathrm{e}{14}$.]{\includegraphics[scale=.5]{bsim6_pc_h_obst_height}\label{fig_bsim6_pc_h_obst_height}}
	\hspace{5mm}
	\subfloat[Evolution of the control weights $\gamma_1=\gamma_2=\gamma_3$ as they decrease from $10$ to $1.8\mathrm{e}{-4}$.]{\includegraphics[scale=.5]{bsim6_pc_gamma_weights}\label{fig_bsim6_pc_gamma_weights}}
	\\
	\subfloat[Evolution of the tangent steplength $\sigma$ as the obstacle heights $h_1=h_2$ increase from $1\mathrm{e}{-10}$ to $9.09\mathrm{e}{14}$.]{\includegraphics[scale=.5]{bsim6_pc_h_sigma}\label{fig_bsim6_pc_h_sigma}}
	\hspace{5mm}
	\subfloat[Evolution of the tangent steplength $\sigma$ as the control weights $\gamma_1=\gamma_2=\gamma_3$ decrease from $10$ to $1.8\mathrm{e}{-4}$.]{\includegraphics[scale=.5]{bsim6_pc_gamma_sigma}\label{fig_bsim6_pc_gamma_sigma}}
	\\
	\subfloat[Evolution of the performance index $J$ from $1.3\mathrm{e}{5}$ to $4.72\mathrm{e}{5}$ as the obstacle heights $h_1=h_2$ increase from $1\mathrm{e}{-10}$ to $9.09\mathrm{e}{14}$.]{\includegraphics[scale=.5]{bsim6_pc_h_J}\label{fig_bsim6_pc_h_J}}
	\hspace{5mm}
	\subfloat[Evolution of the performance index $J$ from $4.72\mathrm{e}{5}$ to $34.3$ as the control weights $\gamma_1=\gamma_2=\gamma_3$  decrease from $10$ to $1.8\mathrm{e}{-4}$.]{\includegraphics[scale=.5]{bsim6_pc_gamma_J}\label{fig_bsim6_pc_gamma_J}}
	\caption{Predictor-corrector continuation in the obstacle heights $h_1=h_2$ (left column) is followed by predictor-corrector continuation in the control weights $\gamma_1=\gamma_2=\gamma_3$ (right column) to realize an obstacle avoidance maneuver for the rolling ball.}
	\label{fig_bsim6_pc}
\end{figure}

\section{Summary, Discussion, and Future Work} \label{sec_conclusions}

The controlled equations of motion for the rolling disk and ball have been derived using the variational Pontryagin's minimum principle. These controlled equations of motion were solved numerically using predictor-corrector continuation, starting from an initial solution obtained via a direct method, to solve trajectory tracking problems for the rolling disk and obstacle avoidance problems for the rolling ball. These optimal control maneuvers were achieved by performing predictor-corrector continuation in weighting factors that scale penalty functions in the integrand cost function of the performance index.

This paper focused on the indirect, rather than direct, method to numerically solve the optimal control problems. Because the indirect and direct methods only converge to a local minimum solution near the initial guess, a robust continuation algorithm capable of handling turning points is needed to obtain indirect and direct method solutions of complicated, nonconvex optimal control problems. A continuation indirect method requires a continuation ODE or DAE TPBVP solver, while a continuation direct method requires a continuation NLP solver. Predictor-corrector continuation ODE TPBVP algorithms were presented in Appendices~\ref{app_predictor_corrector} and \ref{app_sweep_predictor_corrector} and implemented in \mcode{MATLAB} to realize the continuation indirect method used to solve the rolling disk and ball optimal control problems. Even though predictor-corrector continuation NLP solver algorithms are provided in the literature (e.g. see \cite{zangwill1981pathways,kungurtsev2017predictor}), there do not seem to be any publicly available predictor-corrector continuation NLP solvers, which inhibited the use of a continuation direct method in this paper. When compared against the direct method, the indirect method suffers from two major deficiencies:
\begin{enumerate}
	\item Unlike the direct method, the indirect method has a very small radius of convergence and therefore requires a very accurate initial solution guess \cite{betts2010practical,bryson1999dynamic,BrHo1975applied}. Moreover, unlike the direct method, the indirect method requires a guess of the costates, which are unphysical.
	\item Unlike the direct method, the indirect method is unable to construct the switching structure (i.e. the times when the states and/or controls enter and exit the boundary) of an optimal control problem having path inequality constraints.
\end{enumerate}
Since predictor-corrector continuation was used in this paper, the first deficiency in the indirect method only applied when constructing the solution of the initial ODE TPBVP, and this deficiency was circumvented by using a direct method to solve the optimal control problem corresponding to that initial ODE TPBVP. To circumvent the second deficiency in the indirect method, path inequality constraints were incorporated into the optimal control problems as soft constraints through penalty functions in the integrand and endpoint cost functions.

In future work, instead of using \mcode{MATLAB}, the simulation code could be reimplemented in the higher performance programming languages Julia or C++, while relying on Fortran routines like COLNEW \cite{bader1987new}, COLMOD \cite{cash2001automatic}, TWPBVP(C) \cite{cash1991deferred,cash2005new}, TWPBVPL(C) \cite{bashir1998lobatto,cash2006hybrid}, and ACDC(C) \cite{cash2001automatic,cash2013algorithm} to solve the underlying ODE TPBVPs, to obtain faster numerical results. Julia and C++ feature several mature and efficient automatic differentiation libraries \cite{autodiff} capable of constructing the Jacobians and Hessians needed by the ODE TPBVP solvers. In addition, a more efficient and robust predictor-corrector adaptive tangent steplength algorithm, such as described in \cite{bader1989continuation,deuflhard2011newton}, could be implemented.

Another avenue for future investigation is to use a neighboring extremal optimal control (NEOC) method \cite{gupta2017combined}, which constructs a homotopy between the controlled equations of motion and their linearization about a nominal solution; however, the NEOC method in \cite{gupta2017combined} could be made more robust by using predictor-corrector, rather than monotonic, continuation in the homotopy parameter. Yet another avenue for future investigation is to perform  predictor-corrector continuation in a weighting factor that scales a term in the endpoint cost function measuring the deviation between the actual and prescribed final conditions.

The use of rolling  ball robots in practical, real-life applications poses additional challenges. Some of the most interesting challenges that can be tackled by our method are, in our opinion,  the navigation of a  rolling ball robot over complex terrains and  imposing the  associated constraints on the  ball's motion.  Complex terrains will affect both the  uncontrolled equations of motion due to gravity and the performance index, for example, through penalty functions that discourage the ball from  ascending steep slopes. Constraints on the ball's motion may  arise during operation on a slippery surface, in which case the force at the contact  point must be bounded to avoid slippage. These constraints can be  imposed as soft constraints through penalty functions in the performance index, although it would   also  be interesting to consider the possibility of  imposing hard constraints, which represents a much more complex problem. These interesting questions will be considered in future work.

\section*{Acknowledgements}

We are indebted to our colleagues  A.M. Bloch, D.M. de Diego, F. Gay-Balmaz, D.D. Holm, M. Leok, A. Lewis, T. Ohsawa, and D.V. Zenkov for useful and fruitful discussions.
M.J. Weinsten provided copious advice on using the \mcode{MATLAB} automatic differentiation toolbox \mcode{ADiGator} and fixed numerous bugs in \mcode{ADiGator} that were revealed in the course of this research. A.V. Rao provided a free license to use the \mcode{MATLAB} direct method optimal control solver \mcode{GPOPS-II} for some of this research. We are grateful for the comments and suggestions provided by two anonymous reviewers of an earlier version of this paper.

This research was partially  supported by  the NSERC Discovery Grant, the University of Alberta Centennial Fund, and the Alberta Innovates Technology Funding (AITF) which came through the Alberta Centre for Earth Observation Sciences (CEOS). S.M. Rogers also received support from the University of Alberta Doctoral Recruitment Scholarship, the FGSR Graduate Travel Award, the IGR Travel Award, the GSA Academic Travel Award, and the AMS Fall Sectional Graduate Student Travel Grant.

\phantomsection
\addcontentsline{toc}{section}{References}
\printbibliography
\hypertarget{References}{}

redpajama_set_name: RedPajamaArXiv