Extended Operational Space Kinematics, Dynamics, and Control of Redundant Non-Serial Compound Robotic Manipulators

1. Introduction

Redundant robotic manipulators offer flexibility for achieving tasks such as obstacle avoidance, joint limit avoidance, and posture optimization. However, classical modelling and control strategies have been limited to serial manipulators and rely on velocity-level inverse kinematics using Jacobian pseudoinverses and null-space projection techniques [3,4,23,24]. These approaches are inherently local and can suffer from numerical drift, instability near singularities, and difficulties in maintaining secondary objectives over extended trajectories. Recent papers by the authors employs differential geometry and modern methods of multibody dynamics that avoid use of Lagrange multipliers and resolve these deficiencies for redundant robotic manipulators [17,18,20]. This paper extends these results to a dynamically consistent approach that is globally valid on singularity free components of non-serial compound manipulator configuration space. Building on a configuration-level inverse kinematic mapping that presents all inverse kinematic solutions as functions of operational space parameters, coupled second-order ordinary differential equations (ODE) of dynamics are constructed in the product space of task variables and self-motion coordinates. This framework eliminates the need for projection-based redundancy resolution and provides a control-ready ODE formulation that is applicable for a broad spectrum of non-serial compound manipulator architectures.

Goals of the paper are twofold: (i) present a new dynamics and control formulation in an accessible style for researchers and control engineers, and (ii) benchmark its performance using a multi-degree of redundancy example. This work aims to bridge a significant gap between geometric modeling of redundant non-serial compound manipulators and their application in planning and control.

2. Background on Redundant Non-Serial Compound Manipulator Modeling and Control

2.1. Methods Adapted from Redundant Serial Manipulators

In redundant serial manipulators, an explicit input-output forward kinematic relation exists, in the form [4,20]

$$\mathit{z}=\mathit{G}(\mathit{y})$$

(2.1)

where input and output coordinates are denoted $\mathit{y}\in R^n \mathrm{and} \mathit{z}\in R^m$, with $n\text{>}m$. Here, ${\mathrm{R}}^{\mathrm{k}}$ is k-dimensional Euclidean space with elements $\mathit{x}\in {\mathit{R}}^{\mathit{k}}$ in the form of column vectors $\mathit{x}={\left[\begin{array}{ccc}{x}_1& \cdots & x_{\mathrm{k}}\end{array}\right]}^{\mathrm{T}}$. Bold symbols are vectors and matrices.

The inverse kinematics problem associated with Eq. (2.1) has infinitely many solutions. Classical approaches resolve this redundancy by operating at the velocity level [3,33] ,

$$\dot{\mathit{y}}={\mathit{J}}^{†}\dot{\mathit{z}}+\left(\mathit{I}-{\mathit{J}}^{†}\mathit{J}\right){\dot{\mathit{y}}}^0$$

(2.2)

where ${\mathit{J}}^{†}$ is the Moore-Penrose pseudoinverse [4,32] of $\mathit{J}=\left[𝜕G_i(\mathit{y})/𝜕y_j\right]$, and ${\dot{\mathit{y}}}^0$ is arbitrary. Although effective in some non-serial settings, such methods are differential in nature and require integration to recover configurations. They can exhibit numerical instability and are often sensitive to singularities. This velocity space generalized inverse approach has been adapted for use in parallel redundant manipulators, but without notable success [14,15].

Task-priority methods and control in operational space originated in the serial manipulator setting; e.g., [23,30], provide hierarchical frameworks for multiple objectives, but rely on local Jacobian-based approximations and projections that lead to serious problems, especially in non-serial compound manipulators.

The concept of self-motion of serial manipulators introduced in [3] has been adapted in the present research for use with non-serial compound manipulator applications.

2.2. Constrained Operational Space Dynamics

In a formulation introduced in [8], the multiplier form of the constrained equation of motion is mapped into task space using the dynamically consistent inverse of a Jacobian that characterizes both task and constraint spaces. The resulting task space equation of motion is then partitioned into an equation corresponding to the motion, and an equation corresponding to constraint forces. One begins by expressing the multiplier form of the constrained equation of motion. The d’Alembert variational equation of motion for the manipulator with holonomic constraints $\mathit{\Phi }(\mathit{q})=0$ is

$$\mathrm{\delta }{\mathit{q}}^{\mathrm{T}}\left[\mathit{M}(\mathit{q})\ddot{\mathit{q}}-\mathit{S}(\mathit{q},\dot{\mathit{q}})-{\mathit{Q}}^{\mathrm{A}}(\mathit{q},\dot{\mathit{q}},\mathrm{t})-{\mathit{P}}^{\mathit{q}}\right]=0$$

for all $\mathrm{\delta }\mathit{q}\in \ker ({\mathit{\Phi }}_{\mathit{q}})$; i.e., for all $\delta \mathit{q}$ such that ${\mathit{\Phi }}_{\mathit{q}}\delta \mathit{q}=0$ [10,13,21,28]. This implies

$${\mathit{P}}^{\mathit{q}}=\mathit{M}(\mathit{q})\ddot{\mathit{q}}-\mathit{S}(\mathit{q},\dot{\mathit{q}})-{\mathit{Q}}^{\mathrm{A}}(\mathit{q},\dot{\mathit{q}},t)-{\mathit{\Phi }}_{\mathit{q}}{(\mathit{q})}^{\mathit{T}}\mathit{\lambda }$$

(2.3)

subject to holonomic constraints,

$$\mathit{\Phi }(\mathit{q})=0$$

(2.4)

where $\mathit{\lambda }$ is a vector of Lagrange multipliers that determine constraint forces. The task velocity relationship is

$$\dot{\mathrm{z}}=\mathit{J}\dot{\mathit{q}}$$

(2.5)

and the constraint velocity condition is

$$\dot{\mathit{\Phi }}={\mathit{\Phi }}_{\mathit{q}}\dot{\mathit{q}}=0.$$

(2.6)

Concatenating these relationships into a single matrix relation,

$$\dot{\stackrel{⌣}{\mathit{z}}}=\left[\begin{array}{c}\dot{\mathit{z}}\\ \dot{\mathit{\Phi }}\end{array}\right]=\left[\begin{array}{c}\mathit{J}\\ {\mathit{\Phi }}_{\mathit{q}}\end{array}\right]\dot{\mathit{q}}=\stackrel{⌣}{\mathit{J}}\dot{\mathit{q}}$$

(2.7)

where the notation $\stackrel{⌣}{\square }$ represents a quantity that is formed from the composition of task and constraint terms. The applied generalized force can be decomposed into a task/constraint space component and a null space component, as in [8],

$${\mathit{P}}^{\mathit{q}}={\stackrel{⌣}{\mathit{J}}}^{\mathit{T}}{\stackrel{⌣}{\mathit{P}}}^{\mathit{z}}+{\stackrel{⌣}{\mathit{N}}}^{\mathit{T}}{\mathit{P}}_{\mathit{o}}^{\mathit{q}}=\left[\begin{array}{cc}{\mathit{J}}^{\mathit{T}}& {\mathit{\Phi }}_{\mathit{q}}^{\mathit{T}}\end{array}\right]\left[\begin{array}{c}{\mathit{P}}^{\mathit{z}}\\ {\mathit{P}}_{\mathit{C}}^{\mathit{z}}\end{array}\right]+{\stackrel{⌣}{\mathit{N}}}^{\mathit{T}}{\mathit{P}}_{\mathit{o}}^{\mathit{q}}$$

(2.8)

where ${\mathit{P}}_{\mathit{C}}^{\mathit{z}}$ is the component of applied task space force acting along the constraint direction. Equation (2.8) can thus be written as

$${\stackrel{⌣}{\mathit{J}}}^{\mathit{T}}\left[\begin{array}{c}{\mathit{P}}^{\mathit{z}}\\ {\mathit{P}}_{\mathit{C}}^{\mathit{z}}\end{array}\right]+{\stackrel{⌣}{\mathit{N}}}^{\mathit{T}}{\mathit{P}}_{\mathit{o}}^{\mathit{q}}=\mathit{M}\ddot{\mathit{q}}-\mathit{S}-{\mathit{Q}}^{\mathit{A}}-{\stackrel{⌣}{\mathit{J}}}^{\mathit{T}}\left[\begin{array}{c}0\\ \mathit{\lambda }\end{array}\right]$$

(2.9)

Mapping Eq. (2.9) into task space by premultiplying by $\stackrel{⌣}{\mathit{\Gamma }}\stackrel{⌣}{\mathit{J}}{\mathit{M}}^{-1}$,

$$\underset{\mathit{I}}{\underbrace{\stackrel{⌣}{\mathit{\Gamma }}\stackrel{⌣}{\mathit{J}}{\mathit{M}}^{-1}{\stackrel{⌣}{\mathit{J}}}^{\mathit{T}}}}\left[\begin{array}{c}{\mathit{P}}^{\mathit{z}}\\ {\mathit{P}}_{\mathit{C}}^{\mathit{z}}\end{array}\right]+\underset{0}{\underbrace{\stackrel{⌣}{\mathit{\Gamma }}\stackrel{⌣}{\mathit{J}}{\mathit{M}}^{-1}{\stackrel{⌣}{\mathit{N}}}^{\mathit{T}}}}{\mathit{P}}_0^{\mathit{q}}=\stackrel{⌣}{\mathit{\Gamma }}\stackrel{⌣}{\mathit{J}}{\mathit{M}}^{-1}\left(\mathit{M}\ddot{\mathit{q}}-\mathit{S}-{\mathit{Q}}^{\mathit{A}}\right)-\underset{\mathit{I}}{\underbrace{\stackrel{⌣}{\mathit{\Gamma }}\stackrel{⌣}{\mathit{J}}{\mathit{M}}^{-1}{\stackrel{⌣}{\mathit{J}}}^{\mathit{T}}}}\left[\begin{array}{c}0\\ \mathit{\lambda }\end{array}\right]$$

(2.10)

This yields

$$\begin{array}{cc}\hfill \left[\begin{array}{c}{\mathit{P}}^{\mathit{z}}\\ {\mathit{P}}_{\mathit{C}}^{\mathit{z}}\end{array}\right]& =\stackrel{⌣}{\mathit{\Gamma }}\stackrel{⌣}{\mathit{J}}\ddot{\mathit{q}}-\stackrel{⌣}{\mathit{\Gamma }}\stackrel{⌣}{\mathit{J}}{\mathit{M}}^{-1}\mathit{S}-\stackrel{⌣}{\mathit{\Gamma }}\stackrel{⌣}{\mathit{J}}{\mathit{M}}^{-1}{\mathit{Q}}^{\mathit{A}}-\left[\begin{array}{c}0\\ \mathit{\lambda }\end{array}\right]\hfill \\ & =\stackrel{⌣}{\mathit{\Gamma }}\left[\begin{array}{c}\ddot{\mathit{z}}-\dot{\stackrel{⌣}{\mathit{J}}}\dot{\mathit{q}}\\ \dot{\mathit{\Phi }}-{\dot{\mathit{\Phi }}}_{\mathit{q}}\dot{\mathit{q}}\end{array}\right]-\stackrel{⌣}{\mathit{\Gamma }}\stackrel{⌣}{\mathit{J}}{\mathit{M}}^{-1}\mathit{S}-\stackrel{⌣}{\mathit{\Gamma }}\stackrel{⌣}{\mathit{J}}{\mathit{M}}^{-1}{\mathit{Q}}^{\mathit{A}}-\left[\begin{array}{c}0\\ \mathit{\lambda }\end{array}\right]\hfill \end{array}$$

(2.11)

and

$$\left[\begin{array}{c}{\mathit{P}}^{\mathit{z}}\\ {\mathit{P}}_{\mathit{C}}^{\mathit{z}}\end{array}\right]=\stackrel{⌣}{\mathit{\Gamma }}(\mathit{q})\left[\begin{array}{c}\ddot{\mathit{z}}\\ 0\end{array}\right]+\stackrel{⌣}{\mathit{\mu }}(\mathit{q},\dot{\mathit{q}})+\stackrel{⌣}{\mathit{p}}(\mathit{q})-\left[\begin{array}{c}0\\ \mathit{\lambda }\end{array}\right]$$

(2.12)

where the constraint condition, $\ddot{\mathit{\Phi }}=0$ , has been imposed. This yields the following identities:

$$\begin{array}{c}\stackrel{⌣}{\mathit{\Gamma }}(\mathit{q})={\left[\begin{array}{cc}\mathit{J}{\mathit{M}}^{-1}{\mathit{J}}^{\mathit{T}}& \mathit{J}{\mathit{M}}^{-1}{{\mathit{\Phi }}_{\mathit{q}}}^{\mathit{T}}\\ {\mathit{\Phi }}_{\mathit{q}}{\mathit{M}}^{-1}{\mathit{J}}^{\mathit{T}}& {\mathit{\Phi }}_{\mathit{q}}{\mathit{M}}^{-1}{{\mathit{\Phi }}_{\mathit{q}}}^{\mathit{T}}\end{array}\right]}^{-1}\\ \stackrel{⌣}{\mathit{\mu }}(\mathit{q},\dot{\mathit{q}})=\stackrel{⌣}{\mathit{\Gamma }}\left[\begin{array}{c}-\mathit{J}{\mathit{M}}^{-1}\mathit{S}-\dot{\stackrel{⌣}{\mathit{J}}}\dot{\mathit{q}}\\ -{\mathit{\Phi }}_{\mathit{q}}{\mathit{M}}^{-1}\mathit{S}-{\dot{\mathit{\Phi }}}_{\mathit{q}}\dot{\mathit{q}}\end{array}\right]\\ \stackrel{⌣}{\mathit{p}}(\mathit{q})=\stackrel{⌣}{\mathit{\Gamma }}\left[\begin{array}{c}-\mathit{J}{\mathit{M}}^{-1}{\mathit{Q}}^{\mathit{A}}\\ -{\mathit{\Phi }}_{\mathit{q}}{\mathit{M}}^{-1}{\mathit{Q}}^{\mathit{A}}\end{array}\right]\end{array}$$

(2.13)

While Eq. (2.12) expresses system task-constraint dynamics, it is useful to partition it in the following manner:

$$\left[\begin{array}{c}{\mathit{P}}^{\mathit{z}}\\ {\mathit{P}}_{\mathit{C}}^{\mathit{z}}\end{array}\right]=\left[\begin{array}{cc}{\stackrel{⌣}{\mathit{\Gamma }}}_{11}& {\stackrel{⌣}{\mathit{\Gamma }}}_{12}\\ {\stackrel{⌣}{\mathit{\Gamma }}}_{21}& {\stackrel{⌣}{\mathit{\Gamma }}}_{22}\end{array}\right]\left[\begin{array}{c}\ddot{\mathit{z}}\\ 0\end{array}\right]+\left[\begin{array}{c}{\stackrel{⌣}{\mathit{\mu }}}_1\\ {\stackrel{⌣}{\mathit{\mu }}}_2\end{array}\right]+\left[\begin{array}{c}{\stackrel{⌣}{\mathit{p}}}_1\\ {\stackrel{⌣}{\mathit{p}}}_2\end{array}\right]-\left[\begin{array}{c}0\\ \mathit{\lambda }\end{array}\right]$$

(2.14)

This partitioning yields an equation corresponding to task space control force,

$${\mathit{P}}^{\mathit{z}}={\stackrel{⌣}{\mathit{\Gamma }}}_{11}\ddot{\mathit{z}}+{\stackrel{⌣}{\mathit{\mu }}}_1+{\stackrel{⌣}{\mathit{p}}}_1$$

(2.15)

and an equation corresponding to constraint forces,

$${\mathit{P}}_{\mathit{C}}^{\mathit{z}}+\mathit{\lambda }={\stackrel{⌣}{\mathit{\Gamma }}}_{21}\ddot{\mathit{z}}+{\stackrel{⌣}{\mathit{\mu }}}_2+{\stackrel{⌣}{\mathit{p}}}_2$$

(2.16)

The constraint force vector $\mathit{\lambda }$ will always arise so as to satisfy Eq. (2.15), as dictated by constraint consistency. The component of applied task space force, ${\mathit{P}}_{\mathit{C}}^{\mathit{z}}$, acting along the constraint direction has no impact on motion of the task. Its only effect is on constraint forces (values of λ) that arise. Thus, all choices for ${\mathit{P}}_{\mathit{C}}^{\mathit{z}}$ result in equivalent motion of the system.

The dynamics are derived from Eqs. (2.15) and (2.16), as demonstrated in [9],

$${\mathit{P}}^{\mathit{q}}={\mathit{\Theta }}^{\mathit{T}}{\mathit{J}}^{\mathit{T}}\left({\mathit{\Gamma }}_{\mathit{c}}\ddot{\mathit{z}}+{\mathit{\mu }}_{\mathit{c}}+{\mathit{p}}_{\mathit{c}}\right)+{{\mathit{\Phi }}_{\mathit{q}}}^{\mathit{T}}\left(\mathit{\alpha }+\mathit{\rho }\right)+{\mathit{N}}_{\mathit{c}}^{\mathit{T}}{\mathit{P}}_{\mathit{o}}^{\mathit{q}}-{{\mathit{\Phi }}_{\mathit{q}}}^{\mathit{T}}\mathit{\lambda }$$

(2.17)

The term $\mathit{\Theta }\left(\mathit{q}\right)\in {\mathit{R}}^{\mathit{k}\times \mathit{k}}$ is the constraint null space matrix, ${\mathit{\Gamma }}_{\mathit{c}}\left(\mathit{q}\right)\in {\mathit{R}}^{\mathit{m}\times \mathit{m}}$ is the task/constraint space mass matrix, ${\mathit{\mu }}_{\mathit{c}}\left(\mathit{q}, \mathit{q}\dot{}\right)\in {\mathit{R}}^{\mathit{m}}$ is the task/constraint space centrifugal and Coriolis force vector, ${\mathit{p}}_{\mathit{c}}\left(\mathit{q}\right)\in {\mathit{R}}^{\mathit{m}}$ is the task/constraint space gravity vector, and ${\mathit{N}}_{\mathit{c}}{\left(\mathit{q}\right)}^{\mathit{T}}\in {\mathit{R}}^{\mathit{n}\times \mathit{n}}$ is the task/constraint null space projection matrix. These terms are given in [8,9] as

$$\mathit{\Theta }(\mathit{q})=1-{\mathit{M}}^{-1}{{\mathit{\Phi }}_{\mathit{q}}}^{\mathit{T}}{\mathit{L}}^{\mathit{T}}{\mathit{\Phi }}_{\mathit{q}}$$

$${\mathit{\Gamma }}_{\mathit{c}}(\mathit{q})={\left(\mathit{J}{\mathit{M}}^{-1}{\mathit{\Theta }}^{\mathit{T}}{\mathit{J}}^{\mathit{T}}\right)}^{-1}$$

(2.18)

$${\mathit{\mu }}_{\mathit{c}}(\mathit{q},\dot{\mathit{q}})=-{\mathit{\Gamma }}_{\mathit{c}}\mathit{J}{\mathit{M}}^{-1}{\mathit{\Theta }}^{\mathit{T}}\mathit{S}-{\mathit{\Gamma }}_{\mathit{c}}\left(\dot{\mathit{J}}-\mathit{J}{\mathit{M}}^{-1}{{\mathit{\Phi }}_{\mathit{q}}}^{\mathit{T}}\mathit{L}{\dot{\mathit{\Phi }}}_{\mathit{q}}\right)\dot{\mathit{q}}$$

(2.19)

$${\mathit{p}}_{\mathit{c}}(\mathit{q})=-{\mathit{\Gamma }}_{\mathit{c}}\mathit{J}{\mathit{M}}^{-1}{\mathit{\Theta }}^{\mathit{T}}{\mathit{Q}}^{\mathit{A}}$$

(2.20)

$${\mathit{N}}_{\mathit{c}}{(\mathit{q})}^{\mathit{T}}={\mathit{\Theta }}^{\mathit{T}}\left(1-{\mathit{J}}^{\mathit{T}}{\mathit{\Gamma }}_{\mathit{c}}\mathit{J}\mathit{\Theta }{\mathit{M}}^{-1}\right)$$

(2.21)

The term $\mathit{L}(\mathit{q})\in {\mathit{R}}^{\mathit{k}-\mathit{n}\times \mathit{k}-\mathit{n}}$ is the constraint space mass matrix, which reflects system inertia projected onto the constraint, $\mathit{\alpha }\left(\mathit{q}, \mathit{q}\dot{}\right)\in {\mathit{R}}^{\mathit{k}-\mathit{n}}$ is the vector of centrifugal and Coriolis forces projected onto the constraint, and $\mathit{\rho }(\mathit{q})\in {\mathit{R}}^{\mathit{k}-\mathit{n}}$ is the vector of gravity forces projected onto the constraint,

$$\mathit{L}={\left({\mathit{\Phi }}_{\mathit{q}}{\mathit{M}}^{-1}{{\mathit{\Phi }}_{\mathit{q}}}^{\mathit{T}}\right)}^{-1}$$

(2.22)

$$\mathit{\alpha }=-\mathit{L}{\mathit{\Phi }}_{\mathit{q}}{\mathit{M}}^{-1}\mathit{S}-\mathit{L}{\dot{\mathit{\Phi }}}_{\mathit{q}}\dot{\mathit{q}}$$

(2.23)

$$\mathit{\rho }=-\mathit{L}{\mathit{\Phi }}_{\mathit{q}}{\mathit{M}}^{-1}{\mathit{Q}}^{\mathit{A}}$$

(2.24)

2.3. Redundant Parallel Manipulators

Redundant manipulators comprised of a single moving platform as output with six spatial degrees of freedom, whose position and orientation are controlled by parallel legs with more than six input coordinates have received significant attention in the engineering literature [14,15,26]. Inverse kinematics approaches in these applications have, however, been ad hoc or adaptations of problematic generalized inverse velocity approaches adapted from the serial manipulator literature. A contribution to the redundant parallel manipulator literature [5] established the reality that excess generalized coordinates are required to treat this class of redundant manipulator. This reality is addressed in redundant non-serial compound manipulators addressed herein.

3. A Redundant Non-Serial Compound Material Loader Manipulator

In contrast with the foregoing, the approach presented in this paper constructs an explicit inverse kinematic mapping at the position/configuration level. A unified approach is employed for redundant non-serial compound manipulators, allowing the full set of inverse kinematic solutions to be represented analytically [18]. This enables direct formulation of dynamics in a form that is suitable for implementation of control laws in the space of task and self-motion redundancy coordinates, without projection into a Jacobian null space. The unified approach presented herein for non-serial redundant manipulators is an extension of prior developments that used idealized small-scale manipulators to illustrate the formulation [18]. A multi-degree of redundancy example is used herein to motivate the approach presented, in a rigorous but tutorial style, with numerical results that verify accuracy and practicality of the approach.

Kinematics of a five-body planar material loading compound manipulator [18] of Figure 3.1 are defined by unit length horizontal and vertical vectors ${\mathit{u}}_{\mathit{x}} \mathrm{and} {\mathit{u}}_{\mathit{y}}$, and the following vectors that locate key points on the manipulator in the global ${\mathit{x}}_0^{\text{′}}-{\mathit{y}}_0^{\text{′}}$ frame as functions of generalized coordinates $q_1 \mathrm{through} q_7$:

$$\begin{array}{l}{\mathit{r}}_{{\mathit{A}}_1}=q_1{\mathit{u}}_{\mathit{y}}, {\mathit{r}}_{{\mathit{A}}_2}=q_1{\mathit{u}}_{\mathit{x}} +q_2{\mathit{u}}_{\mathit{y}}\\ {\mathit{r}}_{{\mathit{A}}_3}=q_1{\mathit{u}}_{\mathit{x}} +q_2{\mathit{u}}_{\mathit{y}}+{\mathit{s}}_{A_2{A}_3}(\mathit{cos}{q}_4{\mathit{u}}_{\mathit{y}}+\mathit{sin}{q}_4{\mathit{u}}_{\mathit{y}})\\ {\mathit{r}}_{{\mathit{B}}_1}=q_1{\mathit{u}}_{\mathit{x}} +q_2{\mathit{u}}_{\mathit{y}}+q_5(\mathit{cos}{q}_4{\mathit{u}}_{\mathit{y}}+\mathit{sin}{q}_4{\mathit{u}}_{\mathit{y}})\\ {\mathit{r}}_{\mathit{C}}={\mathit{r}}_{{\mathit{B}}_1}+q_6(\mathit{cos}{q}_4{\mathit{u}}_{\mathit{y}}+\mathit{sin}{q}_4{\mathit{u}}_{\mathit{y}})\\ {\mathit{r}}_{{\mathit{B}}_2}={\mathit{r}}_{{\mathit{B}}_1}+{\mathit{L}}_3(\mathit{cos}{q}_4{\mathit{u}}_{\mathit{y}}+\mathit{sin}{q}_4{\mathit{u}}_{\mathit{y}})\\ {\mathit{r}}_{{\mathit{D}}_2}={\mathit{r}}_{{\mathit{B}}_2}+\mathit{A}(q_4+q_7){\mathit{s}}_{{\mathit{D}}_1{\mathit{D}}_2}{\mathit{u}}_{\mathit{y}}\end{array}$$

(3.1)

where ${\mathit{s}}_{\mathit{AB}}$ is the distance between points A and B, ${\mathit{L}}_3$ is the length of body 3, and $\mathit{A}(q_4+q_7)=\left[\begin{array}{cc}\mathit{cos}(q_4+q_7)& -\mathit{sin}(q_4+q_7)\\ \mathit{sin}(q_4+q_7)& \mathit{cos}(q_4+q_7)\end{array}\right]$.

Figure 3.1. Compound Material Loader Loops comprised of (1) bodies 0, 1, 2, and slider ${\mathit{A}}_2$ that define the length of actuator ${\mathit{q}}_3$, which is not contained in a joint (2) bodies 3, 4, and 5 that define the fixed length of bar ${\mathit{L}}_{\mathit{CD}}$ are written in algebraic form as.

Figure 3.1. Compound Material Loader Loops comprised of (1) bodies 0, 1, 2, and slider ${\mathit{A}}_2$ that define the length of actuator ${\mathit{q}}_3$, which is not contained in a joint (2) bodies 3, 4, and 5 that define the fixed length of bar ${\mathit{L}}_{\mathit{CD}}$ are written in algebraic form as.

$$\mathit{\Phi }(\mathit{q})=(1/2)\left[\begin{array}{c}{({\mathit{r}}_{{\mathit{A}}_2}-{\mathit{r}}_{{\mathit{A}}_1})}^{\mathit{T}}({\mathit{r}}_{{\mathit{A}}_2}-{\mathit{r}}_{{\mathit{A}}_1})-q_3^2\\ {({\mathit{r}}_{{\mathit{D}}_2}-{\mathit{r}}_{\mathit{C}})}^{\mathrm{T}}({\mathit{r}}_{{\mathit{D}}_2}-{\mathit{r}}_{\mathit{C}})-L_{\mathit{CD}}^2\end{array}\right]=0$$

(3.2)

Input and output coordinates $\mathit{y}\in R^5$ and $\mathit{z}\in R^3$ are functions of $\mathit{q}\in R^7$,

$$\begin{array}{l}\mathit{y}=\mathit{r}(\mathit{q})={\left[\begin{array}{ccccc}{q}_1& q_2& q_3& q_4& q_5\end{array}\right]}^{\mathit{T}}\\ \mathit{z}=\mathit{s}(\mathit{q})=\left[\begin{array}{c}{q}_1+(q_5+L_3)\mathit{cos}{q}_4\\ q_2+(q_5+L_3)\mathit{sin}{q}_4\\ q_7\end{array}\right]\end{array}$$

(3.3)

where z is the vector of generalized coordinates of the bucket. The manipulator has two degrees of redundancy, and its kinematics cannot be reduced to the form of Eq. (2.1) that defines the kinematics of a serial manipulator.

4. Redundant Compound Manipulator Kinematics, Dynamics, and Control

4.1. Basics of Compound Redundant Manipulator Kinematics

Many important redundant compound manipulators, or simply compound manipulators, contain kinematic and/or actuator closed loops and are not characterized by kinematic models of redundant serial manipulators. Their kinematics are defined by input coordinates $\mathit{y}\in R^n$, output coordinates $\mathit{z}\in R^m$, $\mathrm{m}<\mathrm{n}$, and generalized coordinates $\mathit{q}\in R^k$, $k\text{>}n$. Generalized coordinates satisfy $\mathrm{k}-\mathrm{n}$ twice continuously differentiable constraints,

$$\mathit{\Phi }(\mathit{q})=0$$

(4.1)

and determine input and output coordinates as

$$\begin{array}{l}\mathit{y}=\mathit{r}(\mathit{q})\\ \mathit{z}=\mathit{s}(\mathit{q})\end{array}$$

(4.2)

where $\mathit{r}(\mathit{q}) \mathrm{and} \mathit{s}(\mathit{q})$ are twice continuously differentiable functions.

Redundant compound manipulators are encountered in applications involving heavy loads and high precision requirements. They also arise when Euler parameters with excess variables are employed that are subject to a normalization constraint to define orientation in space [21].

There is little literature on redundant compound manipulators. Conconi and Carricato [5] studied redundant manipulators using screw coordinates and showed that manipulators containing closed kinematic loops require inclusion of generalized coordinates and holonomic constraints in the kinematic model. De Sapio, Khatib, Jain, and coworkers addressed redundant compound manipulators using an operational space methodology based on a velocity formulation and Lagrange multipliers [9,22,23]. While some positive results have been obtained, well known problems associated with the velocity approach [7,19,31] remain unresolved in compound manipulator kinematics and dynamics.

4.1.1. Manipulator Configuration Space

Compound manipulator configuration coordinates are defined as $\mathit{x}={\left[\begin{array}{ccc}{\mathit{y}}^{\mathrm{T}}& {\mathit{q}}^{\mathrm{T}}& {\mathit{z}}^{\mathrm{T}}\end{array}\right]}^{\mathrm{T}}\in R^{\mathrm{n}+\mathrm{k}+\mathrm{m}}$, and form the compound manipulator configuration space

$$X=\left\{\mathit{x}\in {\mathit{R}}^{\mathrm{n}+\mathrm{k}+\mathrm{m}}: \mathit{\Phi }(\mathit{q})=0, \mathit{r}(\mathit{q})-\mathit{y}=0, \mathit{and} \mathit{s}(\mathit{q})-\mathit{z}=0\right\}$$

(4.3)

that may contain singular configurations. This configuration space contains important information that defines the differential geometry and topology of the manipulator [25,27,29].

4.1.2. Assembly Components

It is shown in [18] that if ${\mathit{\Phi }}_{\mathit{q}}(\mathit{q})$ has full rank, X of Eq. (4.3) is a topological space that may be partitioned into maximal, disjoint, path connected assembly components ${AC}_i$ that cover X, shown schematically in Figure 4.1. Configurations in disjoint closures of assembly components cannot be connected by a continuous path in X.

Figure 4.1. Assembly Components of Manipulator Configuration Space.

Figure 4.1. Assembly Components of Manipulator Configuration Space.

4.1.3. Forward and Inverse Kinematics

Forward and inverse kinematics of compound manipulators are defined by composite equations comprised of Eqs. (4.1) and (4.2),

$$\mathit{\Omega }(\mathit{q},\mathit{y})=\left[\begin{array}{c}\mathit{\Phi }(\mathit{q})\\ \mathit{r}(\mathit{q})-\mathit{y}\end{array}\right]=0$$

(4.4)

$$\mathit{\Lambda }(\mathit{q},\mathit{z})=\left[\begin{array}{c}\mathit{\Phi }(\mathit{q})\\ \mathit{s}(\mathit{q})-\mathit{z}\end{array}\right]=0$$

(4.5)

If $\left|{\mathit{\Omega }}_{\mathit{q}}(\overline{\mathit{q}},\overline{\mathit{y}})\right|\ne 0$ for $\overline{\mathit{x}}\in X$, the implicit function theorem [6] assures existence of a unique twice continuously differentiable solution

$$\mathit{q}=\mathit{f}(\mathit{y})$$

(4.6)

of Eq. (4.4) in a neighborhood of $\overline{\mathit{y}}$. Combining this with the second of Eqs. (4.2),

$$\mathit{z}=\mathit{s}(\mathit{f}(\mathit{y}))$$

(4.7)

is a twice continuously differentiable forward kinematic mapping in a neighborhood of $\overline{\mathit{y}}$ . While the mapping of Eq. (4.7) cannot generally be analytically defined, it can be evaluated accurately using Newton-Raphson iteration [1].

In contrast with the foregoing single-valued forward kinematic mapping, if $\mathrm{rank}({\mathit{\Lambda }}_{\mathit{q}}(\overline{\mathit{q}},\overline{\mathit{z}}))=k-(\mathrm{n}-\mathrm{m})<k$ for $\mathit{x}\in X$, the implicit function theorem assures existence of a twice continuously differentiable multi-valued solution

$$\mathit{q}=\mathit{g}(\mathit{z},\mathit{v})$$

(4.8)

of Eq. (4.5), where $\mathit{v}\in R^{\mathrm{n}-\mathrm{m}}$ is a vector of arbitrary coordinates, in a neighborhood of $\mathit{z}=\overline{\mathit{z}}$ and $\mathit{v}=0$. Combining this result with the first of Eqs. (4.2),

$$\mathit{y}=\mathit{r}(\mathit{g}(\mathit{z},\mathit{v}))$$

(4.9)

is a twice continuously differentiable multi-valued inverse kinematic mapping in a neighborhood of $\mathit{z}=\overline{\mathit{z}} \mathrm{and} \mathit{v}=0$. This theoretical result can seldom be implemented analytically. It is implemented numerically in the following.

4.1.4. Singularity Free Assembly Components

It is shown in [18] that the regular compound manipulator configuration space

$$C=\left\{\mathit{x}\in \mathit{X}:\left|{\mathit{\Omega }}_{\mathit{q}}(\mathit{q},\mathit{y})\right|\ne 0, \mathrm{rank}({\mathit{\Lambda }}_{\mathit{q}}(\overline{\mathit{q}},\overline{\mathit{z}}))=k-(\mathrm{n}-\mathrm{m})\right\}$$

(4.10)

is a differentiable manifold that is partitioned into singularity free, path connected, assembly components ${\mathit{SFAC}}_{ij}\subset A{C}_i$ that cover C, as shown schematically in Figure 4.2. Kinematics and dynamics may be carried out in each ${\mathit{SFAC}}_{ij}$ without encountering a singular configuration. It is impossible to connect configurations in different ${\mathit{SFAC}}_{ij}$ with singularity free continuous paths in C. These important results cannot be obtained if manipulator kinematics are defined in velocity space.

Figure 4.2. Singularity Free Assembly Components.

Figure 4.2. Singularity Free Assembly Components.

4.1.5. Numerical Construction of a Multi-Valued Inverse Kinematic Mapping

To construct a multi-valued inverse kinematic mapping on a subset ${\mathrm{N}}^{\mathrm{j}}$ of ${\mathit{SFAC}}_{ij}$ with base point ${\overline{\mathit{x}}}^{\mathrm{j}}\in {\mathrm{N}}^{\mathrm{j}}$, define constant matrices ${\mathit{U}}^{\mathrm{j}}$ and ${\mathit{V}}^{\mathrm{j}}$ on ${\mathrm{N}}^{\mathrm{j}}$ such that

$${\mathit{U}}^{\mathrm{j}}={\mathit{\Lambda }}_{\mathit{q}}^{\mathrm{T}}({\overline{\mathit{q}}}^{\mathrm{j}},{\overline{\mathit{z}}}^{\mathrm{j}})\hspace{1em}{\mathit{\Lambda }}_{\mathit{q}}({\overline{\mathit{q}}}^{\mathrm{j}},{\overline{\mathit{z}}}^{\mathrm{j}}){\mathit{V}}^{\mathrm{j}}=0\hspace{1em}{\mathit{V}}^{\mathrm{j}\mathrm{T}}{\mathit{V}}^{\mathrm{j}}=\mathit{I}$$

(4.11)

The matrix ${\mathit{V}}^{\mathrm{j}}$ is computed using singular value decomposition [1,32]. Since the k linearly independent columns of ${\mathit{U}}^{\mathrm{j}} \mathrm{and} {\mathit{V}}^{\mathrm{j}}$ span ${\mathrm{R}}^{\mathrm{k}}$, every solution q of Eq. (4.5) in a neighborhood of ${\overline{\mathit{q}}}^{\mathrm{j}}$ may be represented as

$$\mathit{q}={\overline{\mathit{q}}}^{\mathrm{j}}+{\mathit{V}}^{\mathrm{j}}(\mathit{v}-{\overline{\mathit{v}}}^{\mathrm{j}})-{\mathit{U}}^{\mathrm{j}}(\mathit{u}-{\overline{\mathit{u}}}^{\mathrm{j}})$$

(4.12)

where $\mathit{v}\in {\mathit{R}}^{\mathit{n}-\mathit{m}}$ and $\mathit{u}\in {\mathit{R}}^{\mathit{k}-\mathit{n}+\mathit{m}}$. Note that $\mathit{v}={\overline{\mathit{v}}}^{\mathrm{j}}$ and $\mathit{u}={\overline{\mathit{u}}}^{\mathrm{j}}$ at $\mathit{q}={\overline{\mathit{q}}}^{\mathit{j}}$.

The goal is to solve Eq. (4.5) with q of Eq. (4.12); i.e.,

$$\mathit{\Lambda }({\overline{\mathit{q}}}^{\mathrm{j}}+{\mathit{V}}^{\mathrm{j}}(\mathit{v}-{\overline{\mathit{v}}}^{\mathrm{j}})-{\mathit{U}}^{\mathrm{j}}(\mathit{u}-{\overline{\mathit{u}}}^{\mathrm{j}}),\mathit{z})=0$$

(4.13)

for u as a function of z and v in a neighborhood of $\mathit{z}={\overline{\mathit{z}}}^{\mathrm{j}} \mathrm{and} \mathit{v}={\overline{\mathit{v}}}^{\mathrm{j}}$. The derivative of the left side of Eq. (4.13) with respect to u, evaluated at ${\overline{\mathit{x}}}^{\mathrm{j}}$ is $-{\mathit{\Lambda }}_{\mathit{q}}({\overline{\mathit{q}}}^{\mathrm{j}},{\overline{\mathit{z}}}^{\mathrm{j}}){\mathit{U}}^{\mathrm{j}}=-{\mathit{U}}^{\mathrm{j}\mathrm{T}}{\mathit{U}}^{\mathrm{j}}$, which is nonsingular. Thus, the implicit function theorem [6] implies existence of a unique, twice continuously differentiable solution

$$\mathit{u}={\mathit{h}}^{\mathrm{j}}(\mathit{z},\mathit{v})$$

(4.14)

of Eq. (4.13) in a neighborhood of $\mathit{z}={\overline{\mathit{z}}}^{\mathrm{j}} \mathrm{and} \mathit{v}={\overline{\mathit{v}}}^{\mathrm{j}}$.

In a neighborhood of $\mathit{z}={\overline{\mathit{z}}}^{\mathrm{j}} \mathrm{and} \mathit{v}={\overline{\mathit{v}}}^{\mathrm{j}}$, in the space of operational coordinates $\mathit{w}={\left[\begin{array}{cc}{\mathit{z}}^{\mathrm{T}}& {\mathit{v}}^{\mathrm{T}}\end{array}\right]}^{\mathrm{T}}$, substituting u of Eq. (4.14) into Eq. (4.12),

$$\mathit{q}(\mathit{w})={\overline{\mathit{q}}}^{\mathrm{j}}+{\mathit{V}}^{\mathrm{j}}(\mathit{v}-{\overline{\mathit{v}}}^{\mathrm{j}})-{\mathit{U}}^{\mathrm{j}}({\mathit{h}}^{\mathrm{j}}(\mathit{z},\mathit{v})-{\overline{\mathit{u}}}^{\mathrm{j}})$$

(4.15)

Using the first of Eqs. (4.2),

$$\mathit{y}(\mathit{w})=\mathit{y}(\mathit{z},\mathit{v})=\mathit{r}({\overline{\mathit{q}}}^{\mathrm{j}}+{\mathit{V}}^{\mathrm{j}}(\mathit{v}-{\overline{\mathit{v}}}^{\mathrm{j}})-{\mathit{U}}^{\mathrm{j}}({\mathit{h}}^{\mathrm{j}}(\mathit{z},\mathit{v})-{\overline{\mathit{u}}}^{\mathrm{j}}))\equiv {\mathit{\psi }}^j(\mathit{w})$$

(4.16)

is a twice continuously differentiable, multi-valued, inverse kinematic mapping in a neighborhood of $\mathit{z}={\overline{\mathit{z}}}^{\mathrm{j}} \mathrm{and} \mathit{v}={\overline{\mathit{v}}}^{\mathrm{j}}$. Multiplying Eq. (4.16) on the left by ${\mathit{V}}^{j\mathit{T}}$ and using Eq. (4.7) yields a twice continuously, 1-to-1 relation between $\mathit{w} \mathrm{and} \mathit{y}$,

$$\mathit{w}={\left[\begin{array}{cc}{\mathit{z}}^{\mathit{T}}& {\mathit{v}}^{\mathit{T}}\end{array}\right]}^{\mathit{T}}={\left[\mathit{s}{(\mathit{f}(\mathit{y}))}^{\mathit{T}}\hspace{1em}{({\overline{\mathit{v}}}^j+{\mathit{V}}^{j\mathit{T}}(\mathit{y}-{\overline{\mathit{y}}}^j))}^{\mathit{T}}\right]}^{\mathit{T}}={\mathit{\psi }}^{j(-1)}(\mathit{y})$$

(4.17)

Analysis in ${\mathit{SFAC}}_{\mathit{ij}}$ must be carried out on individual charts and transitioned to adjacent charts as manipulator configurations progress along a trajectory in ${\mathit{SFAC}}_{\mathit{ij}}$, as shown schematically in Figure 4.3.

Figure 4.3. Trajectory Along Charts in ${\mathit{SFAC}}_{\mathit{ij}}$.

Figure 4.3. Trajectory Along Charts in ${\mathit{SFAC}}_{\mathit{ij}}$.

Note that if $\mathit{v}(t) \mathrm{and} \mathit{z}(t)$ are periodic functions of time, so is $\mathit{y}(\mathit{v}(t),\mathit{z}(t))$; i.e., this inverse kinematic mapping is locally cyclic, a property not enjoyed by inverse kinematics based on generalized matrix inverses [19,31].

With output fixed as z and $\mathit{v}\in {\mathit{R}}^{\mathrm{n}-\mathrm{m}}$ arbitrary in a neighborhood of $\mathit{v}={\overline{\mathit{v}}}^{\mathrm{j}}$, Eq. (4.16) defines an infinite set of input coordinates,

$$\begin{array}{l}\mathit{SMM}(\mathit{z})=\{\mathit{y}=\mathit{r}({\overline{\mathit{q}}}^{\mathrm{j}}+{\mathit{V}}^{\mathrm{j}}(\mathit{v}-{\overline{\mathit{v}}}^{\mathrm{j}})-{\mathit{U}}^{\mathrm{j}}({\mathit{h}}^{\mathrm{j}}(\mathit{z},\mathit{v})-{\overline{\mathit{u}}}^{\mathrm{j}})):\\ \mathrm{for} \mathrm{all} \mathit{v} \mathrm{in} \mathrm{a} \mathrm{neighborhood} \mathrm{of} {\overline{\mathit{v}}}^{\mathrm{j}}\}\end{array}$$

(4.18)

This set is the manipulator self-motion manifold at z, for all v in a neighborhood of $\mathit{v}={\overline{\mathit{v}}}^{\mathrm{j}}$[29].

4.2. Compound Manipulator Dynamics

4.2.1. Input Space ODE of Dynamics

The d’Alembert variational equation of motion for the manipulator is

$$\mathrm{\delta }{\mathit{q}}^{\mathrm{T}}\left[\mathit{M}(\mathit{q})\ddot{\mathit{q}}-\mathit{S}(\mathit{q},\dot{\mathit{q}})-{\mathit{Q}}^{\mathrm{A}}(\mathit{q},\dot{\mathit{q}},\mathrm{t})\right]-\mathrm{\delta }{\mathit{y}}^{\mathrm{T}}{\mathit{P}}^{\mathit{y}}(\mathit{y},\dot{\mathit{y}})-\mathrm{\delta }{\mathit{z}}^{\mathrm{T}}{\mathit{P}}^{\mathit{z}}(\mathit{z},\dot{\mathit{z}})=0$$

(4.19)

for all $\mathrm{\delta }\mathit{q}, \mathrm{\delta }\mathit{y}, \mathrm{and} \mathrm{\delta }\mathit{z}$ that are consistent with linearized forms of Eqs. (4.1) and (4.2) [21].

In ${\mathit{SFAC}}_{\mathit{ij}}$, the solution $\mathit{q}=\mathit{f}(\mathit{y})$ of Eq. (4.4) satisfies $\mathit{\Omega }(\mathit{f}(\mathit{y}),\mathit{y})=0$. Differentiating with respect to y, ${\mathit{\Omega }}_{\mathit{q}}(\mathit{f}(\mathit{y}),\mathit{y}){\mathit{f}}^{\prime }(\mathit{y})+{\mathit{\Psi }}_{\mathit{y}}(\mathit{f}(\mathit{y}),\mathit{y})=0$ so ${\mathit{f}}^{\prime }(\mathit{y})=-{\mathit{\Omega }}_{\mathit{q}}^{-1}(\mathit{f}(\mathit{y}),\mathit{y}){\mathit{\Omega }}_{\mathit{y}}(\mathit{f}(\mathit{y}),\mathit{y})$, which has full rank, since ${\mathit{\Omega }}_{\mathit{q}}^{-1}(\mathit{f}(\mathit{y}),\mathit{y})$ is nonsingular in ${\mathit{SFAC}}_{\mathit{ij}}$ and ${\mathit{\Omega }}_{\mathit{y}}(\mathit{f}(\mathit{y}),\mathit{y})$ has full rank. Thus,

$$\dot{\mathit{q}}={\mathit{f}}^{\prime }(\mathit{y})\dot{\mathit{y}}=-{\mathit{\Omega }}_{\mathit{q}}^{-1}(\mathit{f}(\mathit{y}),\mathit{y}){\mathit{\Omega }}_{\mathit{y}}(\mathit{f}(\mathit{y}),\mathit{y})\dot{\mathit{y}}$$

(4.20)

Differentiating this result with respect to t,

$$\ddot{\mathit{q}}={\mathit{f}}^{\prime }(\mathit{y})\ddot{\mathit{y}}-{\dot{\mathit{\Omega }}}_{\mathit{q}}^{-1}{\mathit{\Omega }}_{\mathit{y}}\dot{\mathit{y}}-{\mathit{\Omega }}_{\mathit{q}}^{-1}\left[{\left({\mathit{\Omega }}_{\mathit{y}}(\mathit{q},\widehat{\mathit{y}})\widehat{\dot{\mathit{y}}}\right)}_{\mathit{q}}\dot{\mathit{q}}+{\left({\mathit{\Omega }}_{\mathit{y}}(\widehat{\mathit{q}},\mathit{y})\widehat{\dot{\mathit{y}}}\right)}_{\mathit{y}}\dot{\mathit{y}}\right]$$

(4.21)

Since ${\mathit{\Omega }}_{\mathit{q}}(\mathit{q},\mathit{y}){\mathit{\Omega }}_{\mathit{q}}^{-1}(\mathit{q},\mathit{y})\mathit{\beta }=\mathit{\beta }$, with $\mathit{\beta }$ constant, differentiation with respect to t and manipulating yields [18]

$$\begin{array}{l}\ddot{\mathit{q}}={\mathit{f}}^{\prime }(\mathit{y})\ddot{\mathit{y}}+\mathit{K}(\mathit{y},\dot{\mathit{y}})\\ \mathit{K}(\mathit{y},\dot{\mathit{y}})={\mathit{\Omega }}_{\mathit{q}}^{-1}\left[\begin{array}{l}{\left({\mathit{\Omega }}_{\mathit{q}}(\mathit{q},\widehat{\mathit{y}}){\widehat{\mathit{\Omega }}}_{\mathit{q}}^{-1}{\widehat{\mathit{\Psi }}}_{\mathit{y}}\widehat{\dot{\mathit{y}}}\right)}_{\mathit{q}}\dot{\mathit{q}}\\ +{\left({\mathit{\Omega }}_{\mathit{q}}(\widehat{\mathit{q}},\mathit{y}){\widehat{\mathit{\Omega }}}_{\mathit{q}}^{-1}{\widehat{\mathit{\Omega }}}_{\mathit{y}}\widehat{\dot{\mathit{y}}}\right)}_{\mathit{y}}\dot{\mathit{y}}\end{array}\right]-{\mathit{\Omega }}_{\mathit{q}}^{-1}\left[\begin{array}{l}{\left({\mathit{\Omega }}_{\mathit{y}}(\mathit{q},\widehat{\mathit{y}})\widehat{\dot{\mathit{y}}}\right)}_{\mathit{q}}\dot{\mathit{q}}\\ +{\left({\mathit{\Omega }}_{\mathit{y}}(\widehat{\mathit{q}},\mathit{y})\widehat{\dot{\mathit{y}}}\right)}_{\mathit{y}}\dot{\mathit{y}}\end{array}\right]\end{array}$$

(4.22)

where $\mathit{q}=\mathit{f}(\mathit{y})$ and $\dot{\mathit{q}}={\mathit{f}}^{\prime }(\mathit{y})\dot{\mathit{y}}$.

Substituting $\ddot{\mathit{q}}$ of Eq. (4.22), $\mathit{q}=\mathit{f}(\mathit{y})$ of Eq.(4.6), Eq. (4.20), $\mathrm{\delta }\mathit{z}={\mathit{s}}^{\prime }(\mathit{f}(\mathit{y})){\mathit{f}}^{\prime }(\mathit{y})\mathrm{\delta }\mathit{y}$ from Eq. (4.7), and $\mathrm{\delta }\mathit{q}={\mathit{f}}^{\prime }(\mathit{y})\mathrm{\delta }\mathit{y}$ into Eq. (4.19), $\mathrm{\delta }{\mathit{y}}^{\mathrm{T}}\left\{{{\mathit{f}}^{\prime }}^{\mathrm{T}}\mathit{M}{\mathit{f}}^{\prime }+{{\mathit{f}}^{\prime }}^{\mathrm{T}}\left(\mathit{M}\mathit{K}-\mathit{S}-{\mathit{Q}}^{\mathrm{A}}\right)-{\mathit{P}}^{\mathit{y}}-{{\mathit{f}}^{\prime }}^{\mathrm{T}}{{\mathit{s}}^{\prime }}^{\mathrm{T}}{\mathit{P}}^{\mathit{z}}\right\}=0$. Since $\mathrm{\delta }\mathit{y}$ is arbitrary, the compound manipulator input space ODE of motion is

$${{\mathit{f}}^{\prime }}^{\mathrm{T}}\mathit{M}{\mathit{f}}^{\prime } \ddot{\mathit{y}}+{{\mathit{f}}^{\prime }}^{\mathrm{T}}\left(\mathit{M}\mathit{K}-\mathit{S}-{\mathit{Q}}^{\mathrm{A}}\right)-{\mathit{P}}^{\mathit{y}}-{{\mathit{f}}^{\prime }}^{\mathrm{T}}{{\mathit{s}}^{\prime }}^{\mathrm{T}}{\mathit{P}}^{\mathit{z}}=0$$

(4.23)

Since ${\mathit{\Phi }}_{\mathit{q}}\mathrm{\delta }\mathit{q}={\mathit{\Phi }}_{\mathit{q}}{\mathit{f}}^{\prime }\mathrm{\delta }\mathit{y}=0$ and $\mathrm{\delta }\mathit{y}$ is arbitrary, ${\mathit{\Phi }}_{\mathit{q}}{\mathit{f}}^{\prime }=0$ and the columns of ${\mathit{f}}^{\prime }$ span the null space of ${\mathit{\Phi }}_{\mathit{q}}$, ${{\mathit{f}}^{\prime }}^{\mathrm{T}}\mathit{M}{\mathit{f}}^{\prime }$ is positive definite [21], hence nonsingular, so the ODE of Eq. (4.23) is well-posed [21].

4.2.2. Operational Space Velocity and Acceleration Kinematics

Differentiating Eq. (4.15) with respect to t,

$$\dot{\mathit{q}}=\mathit{V}\dot{\mathit{v}}-\mathit{U}\dot{\mathit{h}}$$

(4.24)

Since $\mathit{h}=\mathit{u}$ is the solution of Eq. (4.13), $\mathit{\Lambda }(\overline{\mathit{q}}+\mathit{V}(\mathit{v}-\overline{\mathit{v}})-\mathit{U}(\mathit{h}-\overline{\mathit{u}}),\mathit{z})=0$. Differentiating this identity with respect to t,

$${\mathit{\Lambda }}_{\mathit{q}}(\mathit{q},\mathit{z})(\mathit{V}\dot{\mathit{v}}-\mathit{U}\dot{\mathit{h}})+{\mathit{\Lambda }}_{\mathit{z}}(\mathit{q},\mathit{z})\dot{\mathit{z}}=0$$

(4.25)

From Eq. (4.11), $-{\mathit{\Lambda }}_{\mathit{q}}(\overline{\mathit{q}},\overline{\mathit{z}})\mathit{U}=-{\mathit{U}}^{\mathrm{T}}\mathit{U}$ is nonsingular. Since ${\mathit{\Lambda }}_{\mathit{q}}(\mathit{q},\mathit{z})$ is continuous, ${\mathit{\Lambda }}_{\mathit{q}}(\mathit{q},\mathit{z})\mathit{U}$ is nonsingular in a neighborhood of $\overline{\mathit{x}}\in {\mathit{SFAC}}_{\mathrm{j}}$ and

$$\mathit{B}(\mathit{q},\mathit{z})={\left({\mathit{\Lambda }}_{\mathit{q}}(\mathit{q},\mathit{z})\mathit{U}\right)}^{-1}$$

(4.26)

is well defined and continuously differentiable in a neighborhood of $\overline{\mathit{x}}\in {\mathit{SFAC}}_j$.

Using Eq. (4.26) in Eq. (4.25),

$$\dot{\mathit{h}}=\mathit{B}(\mathit{q},\mathit{z})\left({\mathit{\Lambda }}_{\mathit{q}}(\mathit{q},\mathit{z})\mathit{V}\dot{\mathit{v}}+{\mathit{\Lambda }}_{\mathit{z}}(\mathit{q},\mathit{z})\dot{\mathit{z}}\right)$$

(4.27)

In Eq. (4.24), this yields

$$\begin{array}{l}\dot{\mathit{q}}=-\mathit{U}\mathit{B}(\mathit{q},\mathit{z}){\mathit{\Lambda }}_{\mathit{z}}(\mathit{q},\mathit{z})\dot{\mathit{z}}+\left(\mathit{I}-\mathit{U}\mathit{B}(\mathit{q},\mathit{z}){\mathit{\Lambda }}_{\mathit{q}}(\mathit{q},\mathit{z})\right)\mathit{V}\dot{\mathit{v}}\\ \hspace{1em}=-\mathit{U}\mathit{B}(\mathit{q},\mathit{z}){\mathit{\Lambda }}_{\mathit{z}}(\mathit{q},\mathit{z})\dot{\mathit{z}}+\mathit{D}(\mathit{q},\mathit{z})\dot{\mathit{v}}=\mathit{H}(\mathit{w})\dot{\mathit{w}}\\ \mathit{D}(\mathit{q},\mathit{z})\equiv \left(\mathit{I}-\mathit{U}\mathit{B}(\mathit{q},\mathit{z}){\mathit{\Lambda }}_{\mathit{q}}(\mathit{q},\mathit{z})\right)\mathit{V}\\ \mathit{H}(\mathit{w})\equiv {\left[-\mathit{U}\mathit{B}(\mathit{q},\mathit{z}){\mathit{\Lambda }}_{\mathit{z}}(\mathit{q},\mathit{z})\hspace{1em}\mathit{D}(\mathit{q},\mathit{z})\right]}_{\mathit{k}\times \mathit{n}}\end{array}$$

(4.28)

In terms of differentials, this is

$$\mathit{\delta }\mathit{q}=-\mathit{U}\mathit{B}(\mathit{q},\mathit{z}){\mathit{\Lambda }}_{\mathit{z}}(\mathit{q},\mathit{z})\mathit{\delta }\mathit{z}+\mathit{D}(\mathit{q},\mathit{z})\mathit{\delta }\mathit{v}=\mathit{H}(\mathit{w})\mathit{\delta }\mathit{w}$$

(4.29)

Suppressing arguments and manipulating the product on the left,

$$\left[\begin{array}{c}-{\mathit{\Lambda }}_{\mathit{z}}^{-1}{\mathit{\Lambda }}_{\mathit{q}}\\ {\mathit{V}}^{\mathrm{T}}\end{array}\right]\mathit{H}=\left[\begin{array}{c}-{\mathit{\Lambda }}_{\mathit{z}}^{-1}{\mathit{\Lambda }}_{\mathit{q}}\\ {\mathit{V}}^{\mathrm{T}}\end{array}\right]\left[\begin{array}{cc}-\mathit{U}\mathit{B}{\mathit{\Lambda }}_{\mathit{z}}& \mathit{D}\end{array}\right]=\left[\begin{array}{cc}\mathit{I}& {\mathit{\Lambda }}_{\mathit{z}}^{-1}({\mathit{\Lambda }}_{\mathit{q}}-{\mathit{\Lambda }}_{\mathit{q}})\mathit{V}\\ 0& {\mathit{V}}^{\mathrm{T}}\mathit{V}\end{array}\right]=\left[\begin{array}{cc}\mathit{I}& 0\\ 0& \mathit{I}\end{array}\right]=\mathit{I}$$

Thus, $\mathit{H}(\mathit{w})$ has full column rank, with left inverse

$${\mathit{H}}^{-L}(\mathit{w})=\left[\begin{array}{c}-{\mathit{\Lambda }}_{\mathit{z}}^{-1}(\mathit{q}(\mathit{w}),\mathit{z}){\mathit{\Lambda }}_{\mathit{q}}(\mathit{q}(\mathit{w}),\mathit{z})\\ {\mathit{V}}^{\mathrm{T}}\end{array}\right]$$

(4.30)

Differentiating Eq. (4.28) with respect to t,

$$\begin{array}{l}\ddot{\mathit{q}}=\mathit{H}(\mathit{w})\ddot{\mathit{w}}-\mathit{U}\dot{\mathit{B}}{\mathit{\Lambda }}_{\mathit{z}}\dot{\mathit{z}}-\mathit{U}\dot{\mathit{B}}{\mathit{\Lambda }}_{\mathit{q}}\mathit{V}\dot{\mathit{v}}-\mathit{U}\mathit{B}\left({({\mathit{\Lambda }}_{\mathit{z}}(\mathit{q},\widehat{\mathit{z}})\widehat{\dot{\mathit{z}}})}_{\mathit{q}}\dot{\mathit{q}}+{({\mathit{\Lambda }}_{\mathit{z}}(\widehat{\mathit{q}},\mathit{z})\widehat{\dot{\mathit{z}}})}_{\mathit{z}}\dot{\mathit{z}}\right)\\ -\mathit{U}\mathit{B}\left({({\mathit{\Lambda }}_{\mathit{q}}(\mathit{q},\widehat{\mathit{z}})\mathit{V}\widehat{\dot{\mathit{v}}})}_{\mathit{q}}\dot{\mathit{q}}+{({\mathit{\Lambda }}_{\mathit{q}}(\widehat{\mathit{q}},\mathit{z})\mathit{V}\widehat{\dot{\mathit{v}}})}_{\mathit{z}}\dot{\mathit{z}}\right)\end{array}$$

(4.31)

where, from Eq. (4.28), $\dot{\mathit{q}}=\mathit{H}(\mathit{w})\dot{\mathit{w}}$. Writing Eq. (4.26) in the form ${\mathit{\Lambda }}_{\mathit{q}}(\mathit{q},\mathit{z})\mathit{U}\mathit{B}(\mathit{q},\mathit{z})\mathit{\beta }=\mathit{\beta }$, with $\mathit{\beta }$ constant, differentiating with respect to t, and manipulating [18],

$$\begin{array}{l}\ddot{\mathit{q}}=\mathit{H}(\mathit{w})\ddot{\mathit{w}}+\mathit{E}(\mathit{w},\dot{\mathit{w}})\\ \mathit{E}(\mathit{w},\dot{\mathit{w}})=\mathit{U}\mathit{B}\left(\begin{array}{l}\left[\begin{array}{l}{\left({\mathit{\Lambda }}_{\mathit{q}}(\mathit{q},\widehat{\mathit{z}})\mathit{U}\widehat{\mathit{B}}({\widehat{\mathit{\Lambda }}}_{\mathit{z}}\widehat{\dot{\mathit{z}}}+{\widehat{\mathit{\Lambda }}}_{\mathit{q}}\mathit{V}\widehat{\dot{\mathit{v}}})\right)}_{\mathit{q}}\dot{\mathit{q}}\\ +{\left({\mathit{\Lambda }}_{\mathit{q}}(\widehat{\mathit{q}},\mathit{z})\mathit{U}\widehat{\mathit{B}}({\widehat{\mathit{\Lambda }}}_{\mathit{z}}\widehat{\dot{\mathit{z}}}+{\widehat{\mathit{\Lambda }}}_{\mathit{q}}\mathit{V}\widehat{\dot{\mathit{v}}})\right)}_{\mathit{z}}\dot{\mathit{z}}\end{array}\right]\\ -\left[\begin{array}{l}\left({({\mathit{\Lambda }}_{\mathit{z}}(\mathit{q},\widehat{\mathit{z}})\widehat{\dot{\mathit{z}}})}_{\mathit{q}}\dot{\mathit{q}}+{({\mathit{\Lambda }}_{\mathit{z}}(\widehat{\mathit{q}},\mathit{z})\widehat{\dot{\mathit{z}}})}_{\mathit{z}}\dot{\mathit{z}}\right)\\ +\left({({\mathit{\Lambda }}_{\mathit{q}}(\mathit{q},\widehat{\mathit{z}})\mathit{V}\widehat{\dot{\mathit{v}}})}_{\mathit{q}}\dot{\mathit{q}}+{({\mathit{\Lambda }}_{\mathit{q}}(\widehat{\mathit{q}},\mathit{z})\mathit{V}\widehat{\dot{\mathit{v}}})}_{\mathit{z}}\dot{\mathit{z}}\right)\end{array}\right]\end{array}\right)\end{array}$$

(4.32)

Differentiating y of Eq. (4.16) with respect to t and using Eq. (4.27),

$$\begin{array}{cc}\hfill \dot{\mathit{y}}& ={\mathit{r}}^{\prime }(\left(\mathit{q}\right)\left[{\mathit{V}}^j\dot{\mathit{v}}-{\mathit{U}}^j\left(\mathit{B}(\mathit{q},\mathit{z})\left({\mathit{\Lambda }}_{\mathit{q}}(\mathit{q},\mathit{z}){\mathit{V}}^j\dot{\mathit{v}}+{\mathit{\Lambda }}_{\mathit{z}}(\mathit{q},\mathit{z})\dot{\mathit{z}}\right)\right)\right]\hfill \\ & ={\mathit{r}}^{\prime }(\left(\mathit{q}\right)\left[-{\mathit{U}}^j\mathit{B}(\mathit{q},\mathit{z}){\mathit{\Lambda }}_{\mathit{z}}(\mathit{q},\mathit{z})\dot{\mathit{z}}+\left(\mathit{I}-{\mathit{U}}^j\mathit{B}(\mathit{q},\mathit{z}){\mathit{\Lambda }}_{\mathit{q}}(\mathit{q},\mathit{z})\right){\mathit{V}}^j\dot{\mathit{v}}\right]\hfill \\ & ={\mathit{r}}^{\prime }(\left(\mathit{q}\right)\left[-{\mathit{U}}^j\mathit{B}(\mathit{q},\mathit{z}){\mathit{\Lambda }}_{\mathit{z}}(\mathit{q},\mathit{z})\dot{\mathit{z}}+\mathit{D}(\mathit{q},\mathit{z})\dot{\mathit{v}}\right]={\mathit{r}}^{\prime }(\left(\mathit{q}\right)\mathit{H}(\mathit{w})\dot{\mathit{w}}\hfill \end{array}$$

(4.33)

In terms of differentials, this is

$$\mathit{\delta }\mathit{y}={\mathit{r}}^{\prime }(\left(\mathit{q}\right)\left[-{\mathit{U}}^j\mathit{B}(\mathit{q},\mathit{z}){\mathit{\Lambda }}_{\mathit{z}}(\mathit{q},\mathit{z})\mathit{\delta }\mathit{z}+\mathit{D}(\mathit{q},\mathit{z})\mathit{\delta }\mathit{v}\right]={\mathit{r}}^{\prime }(\left(\mathit{q}\right)\mathit{H}(\mathit{w})\mathit{\delta }\mathit{w}$$

(4.34)

4.2.3. Operational Space ODE of Dynamics

From Eq. (4.29), Eq. (4.34), and $\mathit{w}={\left[\begin{array}{cc}{\mathit{z}}^{\mathrm{T}}& {\mathit{v}}^{\mathrm{T}}\end{array}\right]}^{\mathrm{T}}$, $\mathrm{\delta }\mathit{q}=\mathit{H}(\mathit{w})\mathrm{\delta }\mathit{w}$, $\mathrm{\delta }\mathit{y}={\mathit{r}}^{\prime }(\mathit{q}(\mathit{w}))\mathit{H}(\mathit{w})\mathrm{\delta }\mathit{w}$, and $\mathrm{\delta }\mathit{z}=\left[\begin{array}{cc}\mathit{I}& 0\end{array}\right]\mathrm{\delta }\mathit{w}$. Substituting these relations and Eq. (4.32) into Eq. (4.19), $\mathrm{\delta }{\mathit{w}}^{\mathrm{T}}\left[{\mathit{H}}^{\mathrm{T}}\mathit{M}\mathit{H}\ddot{\mathit{w}}+{\mathit{H}}^{\mathrm{T}}\left(\mathit{M}\mathit{E}-\mathit{S}-{\mathit{Q}}^{\mathrm{A}}-{{\mathit{r}}^{\prime }}^{\mathrm{T}}{\mathit{P}}^{\mathit{y}}\right)-{\left[\begin{array}{cc}\mathit{I}& 0\end{array}\right]}^{\mathrm{T}}{\mathit{P}}^{\mathit{z}}\right]=0$. Since $\mathrm{\delta }\mathit{w}$ is arbitrary, the compound manipulator operational space ODE of motion is

$${\mathit{H}}^{\mathrm{T}}\mathit{M}\mathit{H}\ddot{\mathit{w}}+{\mathit{H}}^{\mathrm{T}}\left(\mathit{M}\mathit{E}-\mathit{S}-{\mathit{Q}}^{\mathrm{A}}-{{\mathit{r}}^{\prime }}^{\mathrm{T}}{\mathit{P}}^{\mathit{y}}\right)-{\left[\begin{array}{cc}\mathit{I}& 0\end{array}\right]}^{\mathrm{T}}{\mathit{P}}^{\mathit{z}}=0$$

(4.35)

Since M is positive definite in the null space of ${\mathit{\Phi }}_{\mathit{q}}$ [21] and ${\mathit{\Phi }}_{\mathit{q}}\mathrm{\delta }\mathit{q}={\mathit{\Phi }}_{\mathit{q}}\mathit{H}\mathrm{\delta }\mathit{w}=0$ for arbitrary $\mathrm{\delta }\mathit{w}$, the linearly independent columns of the matrix H are in the null space of ${\mathit{\Phi }}_{\mathit{q}}$ and ${\mathit{H}}^{\mathrm{T}}\mathit{M}\mathit{H}$ is positive definite, hence nonsingular. Thus, the operational space ODE of Eq. (4.35) is well-posed [21].

For manipulator control, it may be helpful to write second order ODE in terms of z and v. Substituting $\ddot{\mathit{q}}$ of Eq. (4.31) and manipulating [18]

$$\begin{array}{l}-{\mathit{\Lambda }}_{\mathit{z}}^{\mathrm{T}}{\mathit{B}}^{\mathrm{T}}{\mathit{U}}^{\mathrm{T}}\mathit{M}\mathit{U}\mathit{B}{\mathit{\Lambda }}_{\mathit{z}}\ddot{\mathit{z}}+{\mathit{\Lambda }}_{\mathit{z}}^{\mathrm{T}}{\mathit{B}}^{\mathrm{T}}{\mathit{U}}^{\mathrm{T}}\mathit{M}\mathit{D}\ddot{\mathit{v}}+{\mathit{\Lambda }}_{\mathit{z}}^{\mathrm{T}}{\mathit{B}}^{\mathrm{T}}{\mathit{U}}^{\mathrm{T}}\left(\mathit{M}\mathit{E}+\mathit{S}+{\mathit{Q}}^{\mathit{A}}-{\mathit{P}}^{\mathit{y}}\right)-{\mathit{P}}^{\mathit{z}}=0\\ {\mathit{D}}^{\mathrm{T}}\mathit{M}\mathit{U}\mathit{B}{\mathit{\Lambda }}_{\mathit{z}}\ddot{\mathit{z}}-{\mathit{D}}^{\mathrm{T}}\mathit{M}\mathit{D}\ddot{\mathit{v}}-{\mathit{D}}^{\mathrm{T}}\left(\mathit{M}\mathit{E}+\mathit{S}+{\mathit{Q}}^{\mathit{A}}-{\mathit{P}}^{\mathit{y}}\right)=0\end{array}$$

(4.36)

This operational space ODE of compound manipulator dynamics forms the foundation for a new approach to control of compound manipulators.

4.3.4. Computation of $\mathit{h}(\mathit{z},\mathit{v})$ and $\mathit{B}(\mathit{q},\mathit{z})$

At $\overline{\mathit{x}}\in {\mathit{SFAC}}_{\mathrm{j}}$, $\mathit{B}(\overline{\mathit{q}},\overline{\mathit{z}})={\left({\mathit{\Lambda }}_{\mathit{q}}(\overline{\mathit{q}},\overline{\mathit{z}})\mathit{U}\right)}^{-1}={\left({\mathit{U}}^{\mathrm{T}}\mathit{U}\right)}^{-1}$ is numerically evaluated. For $({\mathit{Q}}^{\mathrm{i}},{\mathit{z}}^{\mathrm{i}})$ at time ${\mathit{t}}^{\mathit{i}}$, $\mathit{B}({\mathit{Q}}^{\mathrm{i}},{\mathit{z}}^{\mathrm{i}})$ satisfies Eq. (4.26), in the form

$${\mathit{\Lambda }}_{\mathit{q}}(\mathit{q},\mathit{z})\mathit{U}\mathit{B}(\mathit{q},\mathit{z})-\mathit{I}=0$$

(4.37)

With an approximation ${\mathit{B}}^{\left(1\right)}\approx \mathit{B}({\mathit{q}}^{\mathrm{i}-1},{\mathit{z}}^{\mathrm{i}-1})$of the solution of Eq. (4.37) , a matrix version of Newton-Raphson iteration in solution of Eq. (4.37) is [21]

$${\mathit{B}}^{(\mathrm{j}+1)}=2{\mathit{B}}^{\left(\mathrm{j}\right)}-{\mathit{B}}^{\left(\mathrm{j}\right)}{\mathit{\Lambda }}_{\mathit{q}}\mathit{U}{\mathit{B}}^{\left(\mathrm{j}\right)}, \mathrm{j}=1, 2,\cdots , \mathrm{until} ‖{\mathit{\Lambda }}_{\mathit{q}}\mathit{U}{\mathit{B}}^{(\mathrm{j}+1)}-\mathit{I}‖\le \mathrm{Btol}$$

(4.38)

where Btol is an error tolerance.

Similarly, $\mathit{h}({\mathit{z}}^{\mathrm{i}},{\mathit{v}}^{\mathrm{i}})$ can be efficiently evaluated as accurately as desired using Newton-Raphson iteration to solve Eq. (4.13) for $\mathit{u}=\mathit{h}({\mathit{z}}^{\mathrm{i}},{\mathit{v}}^{\mathrm{i}})$,

$$\begin{array}{l}{\mathit{u}}^{(\mathrm{j}+1)}={\mathit{u}}^{\left(\mathrm{j}\right)}+\mathit{B}\mathit{\Lambda }(\overline{\mathit{q}}+\mathit{V}\mathit{v}-\mathit{U}{\mathit{u}}^{\left(\mathrm{j}\right)},\mathit{z}), \mathrm{j}=1, 2,\mathrm{...} \\ \mathrm{until} ‖\mathit{\Lambda }(\overline{\mathit{q}}+\mathit{V}\mathit{v}-\mathit{U}{\mathit{u}}^{(\mathrm{j}+1)},\mathit{z})‖\le \mathrm{utol}\end{array}$$

(4.39)

where utol is a specified error tolerance.

5. Material Loader Dynamics and Control

5.1. Material Loader Input and Operational Space Dynamics

The equivalence of input space dynamics and extended operational space dynamics is next demonstrated for the loader example. For input space dynamics, the differential-algebraic equation (DAE) form of the constrained equations of motion (Eq 2.3) is

$${\mathit{P}}^{\mathit{q}}+{\mathit{\Phi }}_{\mathit{q}}{(\mathit{q})}^{\mathit{T}}\mathit{\lambda }=\mathit{M}(\mathit{q})\ddot{\mathit{q}}-\mathit{S}(\mathit{q},\dot{\mathit{q}})-{\mathit{Q}}^{\mathrm{A}}(\mathit{q},\dot{\mathit{q}},t)-{\mathit{P}}^{\mathit{j}}+10{\nabla }_{\mathit{q}}T$$

subject to the holonomic constraint

$$\mathit{\Phi }(\mathit{q})=0$$

where a structural joint limit response force ${\mathit{P}}^{\mathit{j}}$ is specified to prevent hyper-extension of the actuator cylinders. A dissipative force ${\nabla }_{\mathit{q}}T$ associated with the gradient of the kinetic energy has also been added to the standard input space forces. This represents a set of DAE. These DAE can be represented as a set of ODEs by differentiating the constraint equations twice and solving for $\ddot{\mathit{q}}$, yielding.

$$\ddot{\mathit{q}}=\mathit{\Theta }{\mathit{M}}^{-1}\left(\mathit{S}+{\mathit{Q}}^{\mathrm{A}}+{\mathit{P}}^{\mathit{j}}-10{\nabla }_{\mathit{q}}T\right)-{\overline{\mathit{\Phi }}}_{\mathit{q}}{\dot{\mathit{\Phi }}}_{\mathit{q}}\dot{\mathit{q}}-{\overline{\mathit{\Phi }}}_{\mathit{q}}(\mathit{\alpha }\mathit{\Phi }+\mathit{\beta }{\mathit{\Phi }}_{\mathit{q}}\dot{\mathit{q}})$$

Where ${\overline{\mathit{\Phi }}}_{\mathit{q}}\equiv {\mathit{M}}^{-1}{{\mathit{\Phi }}_{\mathit{q}}}^{\mathit{T}}{\left({\mathit{\Phi }}_{\mathit{q}}{\mathit{M}}^{-1}{{\mathit{\Phi }}_{\mathit{q}}}^{\mathit{T}}\right)}^{-1}$and $\mathit{\alpha }=100$ and $\mathit{\beta }=20$ are Baumgarte stabilization constants [2]. Specifying initial conditions

$$\begin{array}{l}{\mathit{q}}_0={\left[\begin{array}{ccccccc}0& 2& \sqrt{5}& 0& 0.5& 1.1725& 0\end{array}\right]}^{\mathit{T}}\\ {\dot{\mathit{q}}}_0=0\end{array}$$

and an integration procedure, yields [10]

$$\begin{array}{l}\mathrm{For} \mathit{i}=0 \mathrm{to} \mathit{n},\\ {\left.{\ddot{\mathit{q}}}_{\mathit{i}}=\mathit{\Theta }{\mathit{M}}^{-1}\left(\mathit{S}+{\mathit{Q}}^{\mathrm{A}}+{\mathit{P}}^{\mathit{j}}-10{\nabla }_{\mathit{q}}T\right)-{\overline{\mathit{\Phi }}}_{\mathit{q}}{\dot{\mathit{\Phi }}}_{\mathit{q}}\dot{\mathit{q}}-{\overline{\mathit{\Phi }}}_{\mathit{q}}(\mathit{\alpha }\mathit{\Phi }+\mathit{\beta }{\mathit{\Phi }}_{\mathit{q}}\dot{\mathit{q}})\right|}_{\mathit{i}}\\ {\dot{\mathit{q}}}_{\mathit{i}+1}={\dot{\mathit{q}}}_{\mathit{i}}+{\ddot{\mathit{q}}}_{\mathit{i}}\mathrm{\Delta }\mathit{t}\\ {\mathit{q}}_{\mathit{i}+1}={\mathit{q}}_{\mathit{i}}+{\dot{\mathit{q}}}_{\mathit{i}}\mathrm{\Delta }\mathit{t}+{\displaystyle \frac{1}{2}}{\ddot{\mathit{q}}}_{\mathit{i}}\mathrm{\Delta }\mathit{t}\end{array}$$

Running this simple second order integrator for three seconds yields results shown in Figures 5.1 and 5.2. Each image in Figure 5.1 depicts the manipulator configuration at a specific time. The images occur at regular intervals, with the earliest images fading away; i.e., becoming more transparent. Figure 5.2 displays generalized coordinates over the three second interval. Comparable results could be obtained by solving Eq. (4.23) for $\mathit{y}(t) \mathrm{and} \dot{\mathit{y}}(t)$ and using $\mathit{q}=\mathit{f}(\mathit{y}) \mathrm{and} \dot{\mathit{q}}={\mathit{f}}^{\prime }(\mathit{y})\dot{\mathit{y}}$ to recover $\mathit{q} \mathrm{and} \dot{\mathit{q}}$.

Figure 5.1. Composite Rendering of Manipulator Configurations at Specific Times.

Figure 5.1. Composite Rendering of Manipulator Configurations at Specific Times.

Figure 5.2. Time Histories of Generalized Coordinates, Representing Free-Fall of the Manipulator Under Gravity, Using Input Space Dynamics.

Figure 5.2. Time Histories of Generalized Coordinates, Representing Free-Fall of the Manipulator Under Gravity, Using Input Space Dynamics.

The operational space equation of motion of Eq. (4.35) is

$${\mathit{H}}^{\mathrm{T}}\mathit{M}\mathit{H}\ddot{\mathit{w}}+{\mathit{H}}^{\mathrm{T}}\left(\mathit{M}\mathit{E}-\mathit{S}-{\mathit{Q}}^{\mathrm{A}}-{\mathit{P}}^{\mathit{j}}+10{\nabla }_{\mathit{q}}T\right)=0$$

This represents a set of ODE. With the same initial conditions and integration procedure [10]

$$\begin{array}{l}\mathrm{For} \mathit{i}=0 \mathrm{to} \mathit{n}\\ {\ddot{\mathit{w}}}_{\mathit{i}}={\left.-{({\mathit{H}}^{\mathrm{T}}\mathit{M}\mathit{H})}^{-1}{\mathit{H}}^{\mathrm{T}}\left(\mathit{M}\mathit{E}-\mathit{S}-{\mathit{Q}}^{\mathrm{A}}-{\mathit{P}}^{\mathit{j}}+10{\nabla }_{\mathit{q}}T\right)\right|}_{\mathit{i}}\\ {\ddot{\mathit{q}}}_{\mathit{i}}={\left.\mathit{H}\ddot{\mathit{w}}+\mathit{E}\right|}_{\mathit{i}}\\ {\dot{\mathit{q}}}_{\mathit{i}+1}={\dot{\mathit{q}}}_{\mathit{i}}+{\ddot{\mathit{q}}}_{\mathit{i}}\mathrm{\Delta }\mathit{t}\\ {\mathit{q}}_{\mathit{i}+1}={\mathit{q}}_{\mathit{i}}+{\dot{\mathit{q}}}_{\mathit{i}}\mathrm{\Delta }\mathit{t}+{\displaystyle \frac{1}{2}}{\ddot{\mathit{q}}}_{\mathit{i}}\mathrm{\Delta }\mathit{t}\end{array}$$

where the acceleration is computed in extended operational space coordinates and transformed to generalized accelerations, for comparison with the foregoing input space integration results.

Running this simple second order integrator for three seconds yields results shown in Figure 5.3. The results are essentially identical to the input space results, demonstrating equivalence of input space and extended operational space dynamics.

Figure 5.3. Time Histories of Generalized Coordinates, Representing Free-Fall of the Manipulator Under Gravity, Using Extended Operational Space Dynamics.

Figure 5.3. Time Histories of Generalized Coordinates, Representing Free-Fall of the Manipulator Under Gravity, Using Extended Operational Space Dynamics.

5.2. Tracking Task Trajectory, with Kinetic Energy Minimization and Joint Limit Avoidance

Two operational space controllers will be compared in tracking a desired task trajectory z_d. For both traditional and extended operational space control, initial conditions of the manipulator are

$$\begin{array}{l}\mathit{q}(0)={\left[\begin{array}{ccccccc}0& 2& \sqrt{5}& 0& 0.5& 1.1725& 0\end{array}\right]}^{\mathit{T}}\\ \dot{\mathit{q}}\left(0\right)={\left[\begin{array}{c}\mathit{J}\\ \mathit{\Phi }\end{array}\right]}^{+}\left[\begin{array}{c}{\dot{\mathit{x}}}_{\mathit{d}}\\ 0\end{array}\right]\end{array}$$

The desired task trajectory is shown in the left of Figure 5.4, with output coordinate time histories on the right.

Figure 5.4. Desired Reference Signals for Task and Self-Motion Tracking of Traditional and Extended Operational Space Controllers. (Left) Planar Trajectory Passing Through a Set of Points. (Right) Task Output Coordinates.

Figure 5.4. Desired Reference Signals for Task and Self-Motion Tracking of Traditional and Extended Operational Space Controllers. (Left) Planar Trajectory Passing Through a Set of Points. (Right) Task Output Coordinates.

5.2.1. Traditional Operational Space Control

The multiplier form of the constrained operational space equations is restated from Eq. (2.17),

$${\mathit{P}}^{\mathit{q}}+{{\mathit{\Phi }}_{\mathit{q}}}^{\mathit{T}}\mathit{\lambda }={\mathit{\Theta }}^{\mathit{T}}{\mathit{J}}^{\mathit{T}}\left({\mathit{\Gamma }}_{\mathit{c}}\ddot{\mathit{z}}+{\mathit{\mu }}_{\mathit{c}}+{\mathit{p}}_{\mathit{c}}\right)+{{\mathit{\Phi }}_{\mathit{q}}}^{\mathit{T}}\left(\mathit{\alpha }+\mathit{\rho }\right)+{\mathit{N}}_{\mathit{c}}^{\mathit{T}}{\mathit{P}}_{\mathit{o}}^{\mathit{q}}$$

where terms are defined in Eqs. (2.18) thru (2.24). These equations need to be complemented by the passivity condition on the unactuated joints [8],

$${\mathit{S}}_{\mathit{p}}{\mathit{P}}^{\mathit{q}}=0$$

where ${\mathit{S}}_{\mathit{p}}\in {\mathit{R}}^{\mathit{k}-\mathit{n}\times \mathit{k}}$ is the selection matrix that specifies the unactuated joints. For example, in this case,

$${\mathit{S}}_{\mathit{p}}=\left[\begin{array}{ccccccc}0& 0& 0& 1& 0& 0& 0\\ 0& 0& 0& 0& 0& 0& 1\end{array}\right]$$

which indicates that the 4^th and 7^th generalized coordinates are unactuated.

The control input to the plant is then

$$\left[\begin{array}{c}{\mathit{P}}^{\mathit{q}}\\ \mathit{\lambda }\end{array}\right]={\left[\begin{array}{cc}1& {{\mathit{\Phi }}_{\mathit{q}}}^{\mathit{T}}\\ {\mathit{S}}_{\mathit{p}}& 0\end{array}\right]}^{-1}\left[\begin{array}{c}{\mathit{\Theta }}^{\mathit{T}}{\mathit{J}}^{\mathit{T}}\left({\mathit{\Gamma }}_{\mathit{c}}{\mathit{P}}^{\ast }+{\mathit{\mu }}_{\mathit{c}}+{\mathit{p}}_{\mathit{c}}\right)+{{\mathit{\Phi }}_{\mathit{q}}}^{\mathit{T}}\left(\mathit{\alpha }+\mathit{\rho }\right)+{\mathit{N}}_{\mathit{c}}^{\mathit{T}}{\mathit{P}}_{\mathit{o}}^{\mathit{q}}\\ 0\end{array}\right]$$

where

$${\mathit{P}}^{\ast }=100({\mathit{z}}_{\mathit{d}}-\mathit{z})+20({\dot{\mathit{z}}}_{\mathit{d}}-\dot{\mathit{z}})+{\ddot{\mathit{z}}}_{\mathit{d}} \mathrm{and} {\mathit{P}}_{\mathit{o}}^{\mathit{q}}={\mathit{P}}^{\mathit{j}}-10{\nabla }_{\dot{\mathit{q}}}T$$

Joint limit avoidance forces ${\mathit{P}}^{\mathit{j}}$, modeled using exponential springs, are specified. These forces are filtered through the null space projection matrix to ensure that they are dynamically decoupled from the task control. Additionally, a kinetic energy minimization term $10{\nabla }_{\mathit{q}}T$ is added.

Application of this controller generates the results shown in Figures 5.5 through 5.8. Each image in Figure 5.5 depicts the manipulator configuration at a specific time. The high frequency artifacts in Figures 5.7 and 5.8 are due to joint limits repeatedly being hit.

Figure 5.5. Snapshots of System Configuration.

Figure 5.5. Snapshots of System Configuration.

Figure 5.6. Operational Space Trajectory Using the Traditional Operational Space Controller.

Figure 5.6. Operational Space Trajectory Using the Traditional Operational Space Controller.

Figure 5.7. Time Histories 0f Generalized Coordinates Using the Traditional Operational Space Controller.

Figure 5.7. Time Histories 0f Generalized Coordinates Using the Traditional Operational Space Controller.

Figure 5.8. Time Histories of Generalized Forces Under Traditional Operational Space Control.

Figure 5.8. Time Histories of Generalized Forces Under Traditional Operational Space Control.

5.2.2. Extended Operational Space Control

The compound extended operational space equations are restated from Eq. (4.35) as ${\mathit{P}}^{\mathrm{y}}={\left({\mathit{H}}^{\mathrm{T}}{{\mathit{r}}^{\prime }}^{\mathrm{T}}\right)}^{-1}{\mathit{H}}^{\mathrm{T}}\mathit{M}\mathit{H}\ddot{\mathit{w}}+{\left({\mathit{H}}^{\mathrm{T}}{{\mathit{r}}^{\prime }}^{\mathrm{T}}\right)}^{-1}{\mathit{H}}^{\mathrm{T}}\left(\mathit{M}\mathit{E}-\mathit{S}-{\mathit{Q}}^{\mathrm{A}}\right)$. The control input to the plant is then

$${\mathit{P}}^{\mathrm{y}}={\left({\mathit{H}}^{\mathrm{T}}{{\mathit{r}}^{\prime }}^{\mathrm{T}}\right)}^{-1}{\mathit{H}}^{\mathrm{T}}\mathit{M}\mathit{H}{\mathit{P}}^{\ast }\mathit{z}+{\left({\mathit{H}}^{\mathrm{T}}{{\mathit{r}}^{\prime }}^{\mathrm{T}}\right)}^{-1}{\mathit{H}}^{\mathrm{T}}\left(\mathit{M}\mathit{E}-\mathit{S}-{\mathit{Q}}^{\mathrm{A}}\right)$$

where

$${\mathit{P}}^{\ast }=\left[\begin{array}{c}100({\mathit{z}}_{\mathit{d}}-\mathit{z})+20({\dot{\mathit{z}}}_{\mathit{d}}-\dot{\mathit{z}})+{\ddot{\mathit{z}}}_{\mathit{d}}\\ {\mathit{V}}^{\mathit{T}}({\mathit{P}}^{\mathit{j}}-10{\nabla }_{\dot{\mathit{q}}}T)\end{array}\right]$$

The same joint limit avoidance forces and kinetic energy minimization forces are applied as in the previous example. Application of this controller generates results shown in Figures 5.9 through 5.11, indicating that high frequency artifacts are not present (or are significantly reduced).

Figure 5.9. Snapshots of System Configuration.

Figure 5.9. Snapshots of System Configuration.

Figure 5.10. Time Histories of Generalized Coordinates, Using the Extended Operational Space Control, Showing that High Frequency Artifacts Are Not Present.

Figure 5.10. Time Histories of Generalized Coordinates, Using the Extended Operational Space Control, Showing that High Frequency Artifacts Are Not Present.

Figure 5.11. Time Histories of Generalized Forces, Using the Extended Operational Space Controller, Showing that Limited High Frequency Artifacts Are Present.

Figure 5.11. Time Histories of Generalized Forces, Using the Extended Operational Space Controller, Showing that Limited High Frequency Artifacts Are Present.

5.3. Comparison of Performance Metrics

A number of performance metrics have been implemented to establish an objective comparison between the extended operational space control formulation presented herein and the traditional operational space control approaches [11]. These metrics include the kinetic energy, defined as

$$\mathit{T}={\displaystyle \frac{1}{2}}{\dot{\mathit{q}}}^{\mathit{T}}\mathit{M}\dot{\mathit{q}}$$

and plotted in Figure 5.12, showing a significantly greater system kinetic energy $T_{\mathit{os}}$ throughout the entire time history in the traditional operational space controller, compared to the small stable stored kinetic energy $T_{\mathit{eos}}$ in the extended operational space controller.

Figure 5.12. Stored Kinetic Energy of the System.

Figure 5.12. Stored Kinetic Energy of the System.

Power is the product of input space force and input space velocity. If this product is positive the flow of power is from actuator to joint, and joint to actuator if the product is negative. If the actuator is perfectly regenerative then all power flowing to it from the joint is recovered. Actuators that are not regenerative will be considered, in which case the power is given by

$$\stackrel{\rightharpoonup }{P}={\displaystyle \sum _{\mathit{i}=1}^{\mathit{n}}\mathit{Ramp}({\mathit{P}}_{\mathit{i}}^{\mathit{q}}{\dot{\mathit{q}}}_{\mathit{i}})} \mathrm{where} \mathit{Ramp}(\mathit{x})=\left\{\begin{array}{c}\mathit{x} \mathrm{if} \mathit{x}>0\\ 0 \mathrm{if} \mathit{x}\le 0\end{array}\right.$$

The plot of Figure 5.13 shows significantly greater system power ${\overline{P}}_{\mathit{os}}$ generated by the actuators throughout the entire time history in the traditional operational space controller, compared with the small stable power ${\overline{P}}_{\mathit{eos}}$ generated in the extended operational space controller.

Figure 5.13. Power Generation for the System.

Figure 5.13. Power Generation for the System.

Sample entropy is a measure of complexity [11]. It is used here to characterize the disorder of the internal motion of the manipulator under different control approaches. Sample entropy has been used in biomedical applications and biomechanical research, for example to evaluate postural control [16]. Given a continuous time series $\mathit{x}(\mathit{t})$ over the interval $[0,{\mathit{t}}_{\mathit{f}}]$, the discrete time series is defined as

$${\mathit{X}}^{\mathit{N}}=\left.\left\{\mathit{x}({\mathit{t}}_{\mathit{f}}/\mathit{N}),\mathit{x}(2{\mathit{t}}_{\mathit{f}}/\mathit{N}),\cdots ,\mathit{x}({\mathit{t}}_{\mathit{f}})\right.\right\}$$

where $\mathit{N}$ is the number of time samples. The sample entropy is given by the numerical procedure $\mathit{SampEn}(\mathit{m},\mathit{r},{\mathit{X}}^{\mathit{N}})$, where $\mathit{m}$ is the embedding dimension that is usually taken to be equal to 2. The term $\mathit{r}$ mis the tolerance, usually taken to be equal to $0.2\mathit{\sigma }$, where $\mathit{\sigma }$ is the standard deviation of the time series samples. The cumulative entropy of the time series is

$$\left\{\mathit{SampEn}(\mathit{m},\mathit{r},{\mathit{X}}^{\mathit{N}}(1),\mathit{SampEn}(\mathit{m},\mathit{r},{\mathit{X}}^{\mathit{N}}(1,\cdots ,2),\mathit{SampEn}(\mathit{m},\mathit{r},{\mathit{X}}^{\mathit{N}}(1,\cdots ,\mathit{N})\right\}$$

For the input space time series ${\mathit{q}}_{\mathit{i}}(\mathit{t})$,

$$\begin{array}{l}\left\{\mathit{SampEn}(\mathit{m},\mathit{r},{\mathit{Q}}_{\mathit{i}}^{\mathit{N}}(1),\mathit{SampEn}(\mathit{m},\mathit{r},{\mathit{Q}}_{\mathit{i}}^{\mathit{N}}(1,\cdots ,2),\mathit{SampEn}(\mathit{m},\mathit{r},{\mathit{Q}}_{\mathit{i}}^{\mathit{N}}(1,\cdots ,\mathit{N})\right\}\\ \mathrm{for} \mathit{i}=1,2,\dots ,7\end{array}$$

As shown in Figure 5.14, a higher general level of entropy, reflecting higher disorder, occurs in the traditional operational space controller (left) compared to the lower entropy in the extended operational space controller (right).

Figure 5.14. Sample Entropy Computed on Cumulative Sampling (1000 Total Samples Per Signal, Minimum of 100 Samples).

Figure 5.14. Sample Entropy Computed on Cumulative Sampling (1000 Total Samples Per Signal, Minimum of 100 Samples).

5.4. Self Motion Coordinate Tracking

Self-motion coordinate tracking, in addition to task coordinate tracking, is next demonstrated. The reference trajectories used are shown in Figure 5.15. Note that these are used only for extended operational space control, since traditional operational space control does not allow control of self-motion coordinates. Initial conditions are $\mathit{q}(0)={\left[\begin{array}{ccccccc}0& 2& \sqrt{5}& 0& 0.5& 1.1725& 0\end{array}\right]}^{\mathit{T}}$and $\dot{\mathit{q}}\left(0\right)=-\mathit{U}{({\mathit{\Lambda }}_{\mathit{q}}\mathit{U})}^{-1}{\mathit{\Lambda }}_{\mathit{z}}{\mathit{z}}_{\mathit{d}}+(1_7-\mathit{U}{({\mathit{\Lambda }}_{\mathit{q}}\mathit{U})}^{-1}{\mathit{\Lambda }}_{\mathit{q}}\mathit{V}{\dot{\mathit{v}}}_{\mathit{d}}$.

The compound extended operational space equations are restated from Eq. (4.35) as ${\mathit{P}}^{\mathrm{y}}={\left({\mathit{H}}^{\mathrm{T}}{{\mathit{r}}^{\prime }}^{\mathrm{T}}\right)}^{-1}{\mathit{H}}^{\mathrm{T}}\mathit{M}\mathit{H}\ddot{\mathit{w}}+{\left({\mathit{H}}^{\mathrm{T}}{{\mathit{r}}^{\prime }}^{\mathrm{T}}\right)}^{-1}{\mathit{H}}^{\mathrm{T}}\left(\mathit{M}\mathit{E}-\mathit{S}-{\mathit{Q}}^{\mathrm{A}}\right)$. The control input to the plant is then

$${\mathit{P}}^{\mathrm{y}}={\left({\mathit{H}}^{\mathrm{T}}{{\mathit{r}}^{\prime }}^{\mathrm{T}}\right)}^{-1}{\mathit{H}}^{\mathrm{T}}\mathit{M}\mathit{H}{\mathit{P}}^{\ast }\mathit{z}+{\left({\mathit{H}}^{\mathrm{T}}{{\mathit{r}}^{\prime }}^{\mathrm{T}}\right)}^{-1}{\mathit{H}}^{\mathrm{T}}\left(\mathit{M}\mathit{E}-\mathit{S}-{\mathit{Q}}^{\mathrm{A}}\right)$$

Where ${\mathit{P}}^{\ast }=\left[\begin{array}{c}100({\mathit{z}}_{\mathit{d}}-\mathit{z})+20({\dot{\mathit{z}}}_{\mathit{d}}-\dot{\mathit{z}})+{\ddot{\mathit{z}}}_{\mathit{d}}\\ 100({\mathit{v}}_{\mathit{d}}-\mathit{v})+20({\dot{\mathit{v}}}_{\mathit{d}}-\dot{\mathit{v}})+{\ddot{\mathit{v}}}_{\mathit{d}}+{\mathit{V}}^{\mathit{T}}({\mathit{P}}^{\mathit{j}}-10{\nabla }_{\dot{\mathit{q}}}T)\end{array}\right]$.

Figure 5.15. Self Motion Output Coordinates.

Figure 5.15. Self Motion Output Coordinates.

The same joint limit avoidance forces and kinetic energy minimization forces are applied as in previous example. Application of this controller generates results shown in Figures 5.16 through 5.19, indicating tracking of the self-motion coordinates.

Figure 5.16. Snapshots of System Configuration.

Figure 5.16. Snapshots of System Configuration.

Figure 5.17. Control Results Using the Extended Operational Space Controller, With Cyclic Evolution of the Self-Motion Coordinates.

Figure 5.17. Control Results Using the Extended Operational Space Controller, With Cyclic Evolution of the Self-Motion Coordinates.

Figure 5.18. Time Histories of Generalized Coordinates, Using the Extended Operational Space Controller.

Figure 5.18. Time Histories of Generalized Coordinates, Using the Extended Operational Space Controller.

Figure 5.19. Time Histories of Generalized Forces, Using the Extended Operational Space Controller.

Figure 5.19. Time Histories of Generalized Forces, Using the Extended Operational Space Controller.

6. Summary and Conclusions

Results from the previous sections have demonstrated the effectiveness of the extended operational space control formulation in tracking both self-motion and task coordinates. Additionally, gradient minimization of kinetic energy has been demonstrated. Most importantly, In the absence of direct specification of self-motion coordinates, the extended operational formulation achieved lower kinetic energy, power consumption, and sample entropy than the traditional operational space formulation.

It is important to note that a dynamically consistent generalized inverse of the task Jacobian for a serial manipulator is defined [10,12,20] to minimize kinetic energy of the solution of the velocity kinematic equation. In fact, the dynamically consistent inverse has the property of yielding the least velocity norm solution. The mass-weighted inverse solution of the velocity equation is precisely the least kinetic energy (analogous to least norm). Despite this property of the dynamically consistent inverse, it can be seen in Figure 5.13 that control based on dynamic consistency does not produce minimal kinetic energy over a time serie, for either serial or compound manipulators. As discussed in [20] for serial manipulators, it should not be expected to do so, since the kinetic energy property that it satisfies is an instantaneous one that is dependent upon the specific configuration.

An area for future investigation will be to establish global properties for the extended operational control formulation, based on analysis of the differential geometry of self-motion manifolds. For example, Hertz’s principle of least curvature [10] reveals that under certain conditions, motion on the constraint manifold is restricted to geodesic (straightest) paths. Without specification of explicit control for the self-motion space, the manner in which the extended operational space approach implicitly controls self-motion is worthy of future investigation.