1 Introduction

With recent advances in robot technology, there is an increasing demand toward flexible operations in the real world. Since utmost practical tasks are defined in operational space such as Cartesian coordinates, kinematically redundant structure and hierarchical task control are requested for better dexterity and versatility. A specific aim of this paper deals with the redundant robot with under-actuation, i.e., the number of actuators is less than its degrees of freedom (DoFs), which concerns not only advanced forms of floating-based robots such as humanoids, quadrupeds, space robots, and aerial manipulators but also a conventional manipulator with passive joints. Besides, it can be of great interest how to recover/maximize task-performing capability and to elevate the safety [1], when certain joints of the robot lost actuation under minimum maintenance condition, e.g., fault-tolerant control of manipulator [2].

In the 1980s, there was a breakthrough to afford the robot to effectively perform operational space tasks, called the operational space formulation (OSF) [3], which mitigates the representation of a complex nonlinear dynamics by introducing compact and straightforward form. This has been further advanced for applications to highly redundant robot systems with physical constraints such as contacts [4,5,6,7] or non-holonomic constraints [8]. Particularly for under-actuated systems, Arai et al. propose the OSF employing a fixed joint constraint at the un-actuated joint which is a passive joint equipped with a brake [9]. Liu et al. introduce the OSF for multi-arm robotic systems forming a closed-chain mechanism [10], and the OSF controllers for floating base robots constrained by environment contacts are proposed in [11,12,13], which successfully demonstrate control results on bipedal and humanoid robots. Nevertheless, the aforementioned approaches necessitate specific constraints to cope with the control of un-actuated joints, e.g., a firmly grasped condition in the closed-chain mechanism and contact constraints. It is still challenging to control the under-actuated robotic system in the absence of constrained conditions such as free-floating robots and the manipulator with passive joints.

Meanwhile, dealing with un-actuated joints in the OSF-based control is of importance since the robot bodies are dynamically coupled with the other joints [14,15,16], for instance, the un-actuated joint due to even a single actuator failure in the manipulator substantially disturbs its motion and force task execution. In addition, since it is a highly nonlinear system and controllable conditions are dependent on the robot configuration as introduced in early studies [17,18,19,20], it is further difficult to control the under-actuated robot in the operational space without any constraints. Interestingly, the author in [21] proposes the dynamically consistent OSF without the contact constraint to control space robots in zero gravity based on the approach suggested by [11, 12], while the authors in [22] formulate a computationally efficient operational space control and apply it to three degrees-of-freedom (DoFs) planner robot with one un-actuated joint. However, the dynamic impacts induced by the bias force such as Coriolis/centrifugal and gravity forces may be imposed on the reference torque at un-actuated joints which is physically infeasible. As also discussed in relevant studies [20, 23, 24], this issue is a critical problem and thus to be further analyzed to exploit operational space control for the robot with the un-actuated joint without explicit constraints.

The main contribution of this study lies in the analysis of the nonlinear dynamic effect stemmed from un-actuated joints on the robot manipulator. We strive to rigorously analyze and to reveal the dynamics components affecting the operational space task which has been overlooked in previous methods for under-actuated robots [21, 22]. It is the first time to derive the explicit form of the dynamic disturbance effect emerged in the operational space tasks. The analysis result is then applied to the robot controller. It is another contribution of this study to provide a practical control approach that can improve the control performance of robots with un-actuated joints from the analysis results. To verify the potential applicability to enhance control performance, we adopt the quadratic programming (QP) optimization method to minimize the dynamic effect from the un-actuated joints, i.e., the analysis results of this paper, with consideration of other practical constraints such as torque and acceleration limits of the robot. The correctness of the analysis and effectiveness of the proposed control are then demonstrated under a free-swinging failure [23] scenario, where certain joints suddenly lose their actuation and passively swing due to a gravity force during the task execution. Moreover, this paper offers several experimental case studies with a 7-DoFs torque-controlled arm such as accuracy comparisons and robustness tests and further discusses on impacts of weightings in the optimization problem and existing limitations.

2 Problem definition

This section reviews dynamic control approaches for the nonlinear robot system with un-actuated joints based on both joint space and operational space formulations and clarifies the problem to be focused on in this paper.

2.1 Baseline: joint space control with un-actuated joints

The joint space dynamics equation of the robot with k rotational joints can be described as follows:

$$\begin{aligned} {\mathbf {A}}({\mathbf {q}}) \ddot{{\mathbf {q}}} + {\mathbf {b}}({\mathbf {q}},\dot{{\mathbf {q}}}) +{\mathbf{g}({\mathbf {q}})} =\varvec{\varGamma }, \end{aligned}$$
(1)

where \({\mathbf {A}}({\mathbf {q}},\dot{{\mathbf {q}}}) \in {\mathbb {R}}^{k \times k}\) is the inertia matrix, \({\mathbf {q}} \in {\mathbb {R}}^{k}\) is the joint angle vector, \(\dot{\bullet }\) denote time derivative of \(\bullet \), \({\mathbf {b}}({\mathbf {q}},\dot{{\mathbf {q}}}) \in {\mathbb {R}}^{k}\) is the Coriolis/centrifugal torque vector, \({\mathbf {g}}({\mathbf {q}}) \in {\mathbb {R}}^{k}\) is the gravity torque vector, and \(\varvec{\varGamma } \in {\mathbb {R}}^{k}\) is the joint torque vector. Hereinafter, \({\mathbf {A}}({\mathbf {q}})\), \({\mathbf {b}}({\mathbf {q}},\dot{{\mathbf {q}}})\), and \({\mathbf {g}}({\mathbf {q}})\) are indicated as \({\mathbf {A}}\), \({\mathbf {b}}\), and \({\mathbf {g}}\), respectively, for the sake of brevity.

The robot system shown in (1) includes highly nonlinear and coupled dynamics. This can be controlled through a nonlinear dynamic decoupling approach [3] when dynamics parameters are perfectly modeled. For the robot with u un-actuated joints whose actuation torques are zero, a joint-level controller can be designed with torques at the actuated joint, \(\varvec{\varGamma }_{a} \in {\mathbb {R}}^{k-u}\), as follows:

$$\begin{aligned} \hat{{\mathbf {A}}} \ddot{{\mathbf {q}}} + \hat{{\mathbf {b}}} + \hat{{\mathbf {g}}} ={\mathbf {S}}^{T} \varvec{\varGamma }_{a}, \end{aligned}$$
(2)

where \({\hat{\bullet }}\) denotes modeled dynamics parameters and \(\mathbf{{S}} \in {\mathbb {R}}^{(k-u) \times k}\) is the mapping matrix to select actuated joints. For example, when the first u joints are un-actuated, \({\mathbf {S}}\) can be defined as \( {\mathbf {S}} = [{\mathbf {0}}^{(k-u) \times u} {\mathbf {I}}^{(k-u) \times (k-u)} ]\), where \({\mathbf {0}}\) and \({\mathbf {I}}\) are the zero and identity matrices with the corresponding size, respectively.

From (2), a joint space controller for actuated joints can be designed as

$$\begin{aligned} \varvec{\varGamma }_{a} = \bar{{\mathbf {S}}}^{T} (\hat{{\mathbf {A}}} \ddot{{\mathbf {q}}}^{*} + \hat{{\mathbf {b}}} +\hat{{\mathbf {g}}}), \end{aligned}$$
(3)

where \(\ddot{{\mathbf {q}}}^{*} \in {\mathbb {R}}^{k}\) is the reference joint acceleration vector and \(\bar{{\mathbf {S}}}^{T}\) is the dynamically consistent inverse of \({\mathbf {S}}^{T}\) expressed as

$$\begin{aligned} \bar{{\mathbf {S}}}^{T} = ({\mathbf {S}} \hat{{\mathbf {A}}}^{-1} {\mathbf {S}}^{T})^{-1} {\mathbf {S}} \hat{{\mathbf {A}}}^{-1}. \end{aligned}$$
(4)

To this end, one can control the robot with un-actuated joints by applying (3) with the relation of \(\varvec{\varGamma } = \mathbf{{S}}^{T} \varvec{\varGamma }_{a}\). This control solution then creates the acceleration as a reference command for the actuation joints while minimizing the acceleration energy of the joints according to the dynamically consistent inverse (4).

2.2 Operational space control with un-actuated joints

For the operational space control, e.g., controlling an end-effector (EE) of robot in the Cartesian space, the Jacobian matrix \({\mathbf {J}} ({\mathbf {q}}) \in {\mathbb {R}}^{n \times k}\) is generally utilized to linearize the nonlinear system as

$$\begin{aligned} \dot{{\mathbf {x}}}= {\mathbf {J}}({\mathbf {q}})\dot{{\mathbf {q}}}, \end{aligned}$$

where \({\mathbf {x}} \in {\mathbb {R}}^{n}\) is a vector describing position and orientation of the EE in the operational space coordinate. Hereinafter, \({\mathbf {J}} ({\mathbf {q}})\) is indicated as \({\mathbf {J}}\) for the sake of brevity. The operational space dynamic equation then can be derived as follows [3]:

$$\begin{aligned} \varvec{\varLambda }\ddot{{\mathbf {x}}} +\varvec{\mu } +{\mathbf {p}} = {\mathbf {F}}, \end{aligned}$$
(5)

where \({\mathbf {F}} \in {\mathbb {R}}^{n}\) is the operational space force vector, \(\varvec{\varLambda } \in {\mathbb {R}}^{n \times n}\) denotes the inertia matrix given as

$$\begin{aligned} \varvec{\varLambda } = ({\mathbf {J}} {\mathbf {A}}^{-1} {\mathbf {J}}^{T})^{-1}, \end{aligned}$$
(6)

while \(\varvec{\mu } = \bar{{\mathbf {J}}}^{T} {\mathbf {b}} - \varvec{\varLambda }\dot{{\mathbf {J}}}\dot{{\mathbf {q}}} \in {\mathbb {R}}^{n}\) and \({\mathbf {p}} = \bar{{\mathbf {J}}}^{T} {\mathbf {g}}\) denote the operational space Coriolis/centrifugal force vector and the operational space gravity force vector, respectively, where \(\bar{{\mathbf {J}}}^{T}\) is the dynamically consistent inverse of \({{\mathbf {J}}}^{T}\) given as

$$\begin{aligned} \bar{{\mathbf {J}}}^{T} = \varvec{\varLambda }{\mathbf {J}}{\mathbf {A}}^{-1}. \end{aligned}$$
(7)

To control a given reference operational space acceleration \(\ddot{{\mathbf {x}}}^{*} \in {\mathbb {R}}^{n}\), the force vector \({\mathbf {F}}\) in the operational space is expressed as

$$\begin{aligned} {\mathbf {F}} = \hat{\varvec{\varLambda }}\ddot{{\mathbf {x}}}^{*} +\hat{\varvec{\mu }} +\hat{{\mathbf {p}}}. \end{aligned}$$
(8)

The relationship between \({\mathbf {F}}\) and \(\varvec{\varGamma }_{a}\) of the under-actuation robotic system is expressed as follows [21]:

$$\begin{aligned} {\mathbf {F}} = \bar{{\mathbf {J}}}^{T}{\mathbf {S}}^{T}\varvec{\varGamma }_{a}. \end{aligned}$$
(9)

Substituting (9) into (8) yields the control torque of the actuated joints, \(\varvec{\varGamma }_{a}\), as follows:

$$\begin{aligned} \varvec{\varGamma }_{a} = \overline{\bar{{\mathbf {J}}}^{T}{\mathbf {S}}^{T}}(\hat{\varvec{\varLambda }}\ddot{{\mathbf {x}}}^{*}+\hat{{\mathbf {p}}}+\hat{\varvec{\mu }}). \end{aligned}$$
(10)

To track the desired operational space position \({{\mathbf {x}}}_{d}\), velocity \(\dot{{\mathbf {x}}}_{d}\), and acceleration \(\ddot{{\mathbf {x}}}_{d}\), one can select the reference acceleration as

$$\begin{aligned} {\ddot{\mathbf {x}}^{*}} = {{k}}_{p}({\mathbf {x}}_{d} - {\mathbf {x}}) + {{k}}_{v}(\dot{{\mathbf {x}}}_{d} - \dot{{\mathbf {x}}}) + \ddot{{\mathbf {x}}}_{d}, \end{aligned}$$
(11)

where \(k_{p}\) and \(k_{v}\) are control gains.

For a redundant robot with un-actuated joints as of \(\bar{{\mathbf {J}}}^{T}{\mathbf {S}}^{T}\) in (10), \(\overline{\bar{{\mathbf {J}}}^{T}{\mathbf {S}}^{T}}\) is achieved by the weighted pseudo inverse. The weighting matrix \({\mathbf {W}} \in {\mathbb {R}}^{(k-u) \times (k-u)}\) is defined to minimize the joint acceleration energy [25] as follows:

$$\begin{aligned} {\mathbf {W}} = {\mathbf {S}}{\mathbf {A}}^{-1}{\mathbf {S}}^{T}, \end{aligned}$$
(12)

where the inertia matrix \({\mathbf {A}}\) is always full-rank and pre- and post-multiplications of \({\mathbf {S}}\) and \({\mathbf {S}}^{T}\) with \({\mathbf {A}}^{-1}\) decrease both the rank and dimension of the matrix by the number of un-actuated joints u; accordingly, \({\mathbf {W}}\) is a full-rank matrix which is invertible, i.e., \({\mathbf {W}}^{-1}\) exists. The dynamic consistent inverse of \(\bar{{\mathbf {J}}}^{T}{\mathbf {S}}^{T}\) can then be obtained as follows:

$$\begin{aligned} \overline{\bar{{\mathbf {J}}}^{T}{\mathbf {S}}^{T}} = {\mathbf {W}}^{-1} (\bar{{\mathbf {J}}}^{T}{\mathbf {S}}^{T})^{T} \{ \bar{{\mathbf {J}}}^{T}{\mathbf {S}}^{T}{\mathbf {W}}^{-1}(\bar{{\mathbf {J}}}^{T}{\mathbf {S}}^{T})^{T}\}^{-1}. \end{aligned}$$
(13)

Task can then be controlled with torque \(\varvec{\varGamma } = {\mathbf {S}}^{T} \varvec{\varGamma }_{a}\) with (10) same as the joint space control.

Hierarchical control structure can be achieved when there is redundancy, i.e., \(n < k -u\), by utilizing the null space projection matrix which can be described as follows:

$$\begin{aligned} {\mathbf {N}}^{T} = {\mathbf {I}} - \overline{\bar{{\mathbf {J}}}^{T}{\mathbf {S}}^{T}}(\bar{{\mathbf {J}}}^{T}{\mathbf {S}}^{T}). \end{aligned}$$
(14)

Then, the hierarchical control structure for under-actuated robot can be composed as

$$\begin{aligned} \varvec{\varGamma } = {\mathbf {S}}^{T} \{ \overline{\bar{{\mathbf {J}}}^{T}{\mathbf {S}}^{T}} (\hat{\varvec{\varLambda }} \ddot{{\mathbf {x}}}^{*} +\hat{\varvec{\mu }} +\hat{{\mathbf {p}}})+ {\mathbf {N}}^{T} \varvec{\varGamma }_{a,0} \}, \end{aligned}$$
(15)

where \(\varvec{\varGamma }_{a,0} \in {\mathbb {R}}^{k-u}\) is an arbitrary torque vector for actuated joints. For better efficiency, the Coriolis/centrifugal torque and the gravity torque can be compensated in the joint space [26] as follows:

$$\begin{aligned} \varvec{\varGamma } = {\mathbf {S}}^{T} \{ \overline{\bar{{\mathbf {J}}}^{T}{\mathbf {S}}^{T}} \hat{\varvec{\varLambda }} \ddot{{\mathbf {x}}}^{*} + {\mathbf {N}}^{T} \varvec{\varGamma }_{a,0} +\bar{{\mathbf {S}}}^{T} (\hat{{\mathbf {b}}} +\hat{{\mathbf {g}}}) \}. \end{aligned}$$
(16)

2.3 Problem statement in operational space control

For controlling the under-actuated robot, the joint space control formulation shown in (3) is straightforward and it is feasible to set a zero-torque constraint at un-actuated joints while maintaining dynamic consistency, whereas, in the OSF, the controller (16) requests the un-actuated joints not to create the joint acceleration while producing torques to compensate the nonlinear dynamic effect of bias forces such from \({\mathbf {b}}\) and \({\mathbf {g}}\) acting at the un-actuated joints, which is physically infeasible. (More detailed analyses are found in the following section.) Note that while the operational space control of under-actuated robot has been studied in existing literature (e.g., [21, 22]), they consider employing physical constraints forcing the system fully controllable (e.g., contact constraints), or to restrict to the case when \({\mathbf {b}}\) and \({\mathbf {g}}\) can be ignored (e.g., planar robot or space robot in zero-gravity).

Accordingly, we tackle this problem to analyze the nonlinear dynamic effect caused by un-actuated joints when the redundant robot performs operational space tasks, and exploit the outcome to derive an improved operational space control solution mitigating constrained conditions.

3 Nonlinear dynamic effects from un-actuated joints

3.1 Theoretical analysis

3.1.1 Derivation of joint space response

To analyze the behavior of robot controlled by the inverse dynamics torque resolution, we substitute the control torque (16) into the robot dynamics (1) with the assumption that the dynamics parameters in the controller are identical to those of the robot, i.e., \(\hat{\varvec{\varLambda }} = \varvec{\varLambda }\), \(\hat{{\mathbf {A}}} = {\mathbf {A}}\), \(\hat{{\mathbf {b}}} = {\mathbf {b}}\), and \(\hat{{\mathbf {g}}} = {\mathbf {g}}\). The closed-loop responses of the robot then can be expressed as follows:

$$\begin{aligned} {\mathbf {A}} \ddot{{\mathbf {q}}}= & {} {\mathbf {S}}^{T} \overline{\bar{{\mathbf {J}}}^{T}{\mathbf {S}}^{T}} \varvec{\varLambda }\ddot{{\mathbf {x}}}^{*} + {\mathbf {S}}^{T} {\mathbf {N}}^{T}\varvec{\varGamma }_{a,0} \nonumber \\&+ {\mathbf {S}}^{T} \bar{{\mathbf {S}}}^{T}({\mathbf {b}} + {\mathbf {g}})-({\mathbf {b}} + {\mathbf {g}}). \end{aligned}$$
(17)

The joint acceleration \(\ddot{{\mathbf {q}}}\) in the left-hand side of (17) describes joint response when the control references for hierarchical tasks, i.e., \(\ddot{{\mathbf {x}}}^{*}\) and \(\varvec{\varGamma }_{a,0}\), are commanded. However, the operational space response caused by the un-actuated joints is unrevealed yet in the above equation. Thus, we investigate (17) to reveal the nonlinear behavior of un-actuated joints and their dynamical effects on the operational space task associated with the control references. For this purpose, we re-formulate the above equation with the projection matrices to remove the terms not affecting the operational space in the following.

First, the term \({\mathbf {S}}^{T} \overline{\bar{{\mathbf {J}}}^{T}{\mathbf {S}}^{T}}\varvec{\varLambda }\ddot{{\mathbf {x}}}^{*}\) in (17) can be re-described by substituting (6) and \(\ddot{{\mathbf {x}}}^{*} = {\mathbf {J}}\ddot{{\mathbf {q}}}^{*}+ \dot{{\mathbf {J}}}\dot{{\mathbf {q}}}\) as follows:

$$\begin{aligned} {\mathbf {S}}^{T} \overline{\bar{{\mathbf {J}}}^{T}{\mathbf {S}}^{T}}\varvec{\varLambda }\ddot{{\mathbf {x}}}^{*}&= {\mathbf {S}}^{T} (\overline{\bar{{\mathbf {J}}}^{T}{\mathbf {S}}^{T}})({\mathbf {J}} {\mathbf {A}}^{-1} {\mathbf {J}})^{T}({\mathbf {J}} \ddot{{\mathbf {q}}}^{*} + \dot{{\mathbf {J}}}\dot{{\mathbf {q}}})\nonumber \\&= {\mathbf {S}}^{T} (\overline{\bar{{\mathbf {J}}}^{T}{\mathbf {S}}^{T}})\bar{{\mathbf {J}}}^{T} {\mathbf {A}} \bar{{\mathbf {J}}}({\mathbf {J}} \ddot{{\mathbf {q}}}^{*} + \dot{{\mathbf {J}}}\dot{{\mathbf {q}}})\nonumber \\&= {\mathbf {P}}_{o} {\mathbf {A}} ({\mathbf {P}}_{t} \ddot{{\mathbf {q}}}^{*} + \bar{{\mathbf {J}}}\dot{{\mathbf {J}}}\dot{{\mathbf {q}}}), \end{aligned}$$
(18)

where \({\mathbf {P}}_{o}= \overline{\bar{{\mathbf {J}}}^{T}} \bar{{\mathbf {J}}}^{T}\) and \({\mathbf {P}}_{t} = \bar{{\mathbf {J}}}{\mathbf {J}}\) are the projection matrices and \(\overline{\bar{{\mathbf {J}}}^{T}}\) is the dynamically consistent inverse of \(\bar{{\mathbf {J}}}^{T}\), while the following Property 1 is used.

Property 1

\({\mathbf {S}}^{T} (\overline{\bar{{\mathbf {J}}}^{T}{\mathbf {S}}^{T}})\bar{{\mathbf {J}}}^{T} = \overline{\bar{{\mathbf {J}}}^{T}} \bar{{\mathbf {J}}}^{T}\), and its proof is shown as follows:

Proof

$$\begin{aligned} {\mathbf {S}}^{T} (\overline{\bar{{\mathbf {J}}}^{T}{\mathbf {S}}^{T}})\bar{{\mathbf {J}}}^{T} =&{\mathbf {S}}^{T} {\mathbf {W}}^{-1} {{S}} \bar{{\mathbf {J}}} \{ \bar{{\mathbf {J}}}^{T}{\mathbf {S}}^{T}{\mathbf {W}}^{-1}(\bar{{\mathbf {J}}}^{T}{\mathbf {S}}^{T})^{T}\}^{-1} \bar{{\mathbf {J}}}^{T} \\ =&\{ {\mathbf {W}}^{+}_{s}\bar{{\mathbf {J}}}(\bar{{\mathbf {J}}}^{T}{\mathbf {W}}^{+}_{s}\bar{{\mathbf {J}}})^{-1} \}\bar{{\mathbf {J}}}^{T}, \end{aligned}$$

where \({\mathbf {W}}_{s}^{+} = {{\mathbf {S}}^{T}}{\mathbf {W}}^{-1}{\mathbf {S}}\), defined as the pseudo-inverse of \({\mathbf {W}}_{s}\) with the superscript \(+\), because it is semi-positive definite. Then, \({\mathbf {W}}^{+}_{s}\bar{{\mathbf {J}}}(\bar{{\mathbf {J}}}^{T}{\mathbf {W}}^{+}_{s}\bar{{\mathbf {J}}})^{-1}\) can be considered as \(\overline{\bar{{\mathbf {J}}}^{T}}\) which is the weighted pseudo-inverse of \(\bar{{\mathbf {J}}}^{T}\) with the weighting matrix \({\mathbf {W}}_{s}\) [27]. Therefore, the following can be achieved:

$$\begin{aligned}&{\mathbf {S}}^{T} (\overline{\bar{{\mathbf {J}}}^{T}{\mathbf {S}}^{T}})\bar{{\mathbf {J}}}^{T} = \overline{\bar{{\mathbf {J}}}^{T}} \bar{{\mathbf {J}}}^{T} = {\mathbf {P}}_{o}. \end{aligned}$$

\(\square \)

Therefore, one can conclude the term \({\mathbf {S}}^{T} \overline{\bar{{\mathbf {J}}}^{T}{\mathbf {S}}^{T}}\varvec{\varLambda }\ddot{{\mathbf {x}}}^{*}\) in (17) affects only the operational space since they are projected onto that space by \({\mathbf {P}}_{o}\).

Second, the term \({\mathbf {S}}^{T}{\mathbf {N}}^{T}\varvec{\varGamma }_{a,0}\) in (17), the lower priority task control torque, can be expressed with (14) and Property 1 as follows:

$$\begin{aligned} {\mathbf {S}}^{T}{\mathbf {N}}^{T}\varvec{\varGamma }_{a,0}&= {\mathbf {S}}^{T}\{ {\mathbf {I}} - (\overline{\bar{{\mathbf {J}}}^{T}{\mathbf {S}}^{T}})\bar{{\mathbf {J}}}^{T} {\mathbf {S}}^{T} \}\varvec{\varGamma }_{a,0} \nonumber \\&= \{{\mathbf {S}}^{T} - {\mathbf {S}}^{T}(\overline{\bar{{\mathbf {J}}}^{T}{\mathbf {S}}^{T}})\bar{{\mathbf {J}}}^{T} {\mathbf {S}}^{T} \}\varvec{\varGamma }_{a,0} \nonumber \\&= ({\mathbf {S}}^{T} - {\mathbf {P}}_{o} {\mathbf {S}}^{T})\varvec{\varGamma }_{a,0} \nonumber \\&= ({\mathbf {I}} - {\mathbf {P}}_{o}){\mathbf {S}}^{T}\varvec{\varGamma }_{a,0}. \end{aligned}$$
(19)

At the left-hand side of (19), it is unclear how \({\mathbf {S}}^{T}\) influences the null space projected torque \({\mathbf {N}}^{T}\varvec{\varGamma }_{a,0}\) since the null space projection matrix \({\mathbf {N}}^{T}\) is pre-multiplied by \({\mathbf {S}}^{T}\). On the other hand, at the right-hand side of (19), it is explicitly seen that \({\mathbf {S}}^{T}\varvec{\varGamma }_{a,0}\) is projected onto the null space of the operational space by the null space projection matrix \(({\mathbf {I}} - {\mathbf {P}}_{o})\), and one can observe where the lower priority task torque is projected, that is, it does not affect the operational space.

Third, the remaining terms in (17) are expressed as

$$\begin{aligned} {\mathbf {S}}^{T}\bar{{\mathbf {S}}}^{T}({\mathbf {b}} + {\mathbf {g}})-({\mathbf {b}}+{\mathbf {g}}) = -({\mathbf {I}}-{\mathbf {P}}_{a})({\mathbf {b}} + {\mathbf {g}}), \end{aligned}$$
(20)

where the projection matrix \({\mathbf {P}}_{a} ={\mathbf {S}}^{T}\bar{{\mathbf {S}}}^{T}\). At the right-hand side of (20), the term \(({\mathbf {b}}+{\mathbf {g}})\) is projected into the null space of the actuation joint space, \(({\mathbf {I}}-{\mathbf {P}}_{a})\), which straightforwardly indicates they do not affect the actuation joints. Accordingly, one can notice that the nonlinear torques \({\mathbf {b}}\) and \({\mathbf {g}}\) acting on the un-actuated joints cannot be compensated by OSC references, while they produce free-swinging motion as dynamic disturbances.

As a result, substituting (18), (19), and (20) into (17) leads to the following joint space equation:

$$\begin{aligned} {\mathbf {A}} \ddot{{\mathbf {q}}}&= {\mathbf {P}}_{o} {\mathbf {A}} ({\mathbf {P}}_{t} \ddot{{\mathbf {q}}}^{*} + \bar{{\mathbf {J}}}\dot{{\mathbf {J}}}\dot{{\mathbf {q}}}) + ({\mathbf {I}} - {\mathbf {P}}_{o}){\mathbf {S}}^{T}\varvec{\varGamma }_{a,0} \nonumber \\&\quad - ({\mathbf {I}}-{\mathbf {P}}_{a})({\mathbf {b}} + {\mathbf {g}}). \end{aligned}$$
(21)

3.1.2 Derivation of operational space response

A re-formulated robot response (21) describes joint space behavior that can affect the operational space. In this subsection, (21) is further analyzed to explicitly show operational space behavior. To observe the operational space response, the matrix \(\bar{{\mathbf {J}}}^{T}\) which defined in (7) is multiplied to (21) as follows:

$$\begin{aligned} \varvec{\varLambda }\ddot{{\mathbf {x}}} =&\bar{{\mathbf {J}}}^{T}{\mathbf {P}}_{o}{\mathbf {A}} ({\mathbf {P}}_{t}\ddot{{\mathbf {q}}}^{*} + \bar{{\mathbf {J}}}\dot{{\mathbf {J}}}\dot{{\mathbf {q}}}) - \bar{{\mathbf {J}}}^{T}({\mathbf {I}}-{\mathbf {P}}_{a})({\mathbf {b}} + {\mathbf {g}}). \end{aligned}$$
(22)

The term corresponding to \({\mathbf {I}}-{\mathbf {P}}_{o}\) in (21), projected onto the orthogonal space of the operational space \({\mathbf {P}}_{o}\), is removed according to the following Property 2:

Property 2

\(\bar{{\mathbf {J}}}^{T}({\mathbf {I}}-{\mathbf {P}}_{o})={\mathbf {0}}\) and its proof is shown as follows:

Proof

$$\begin{aligned} \bar{{\mathbf {J}}}^{T}({\mathbf {I}}-{\mathbf {P}}_{o}) =&\bar{{\mathbf {J}}}^{T} - \bar{{\mathbf {J}}}^{T}{\mathbf {P}}_{o}&\\ =&\bar{{\mathbf {J}}}^{T} - \bar{{\mathbf {J}}}^{T}\overline{\bar{{\mathbf {J}}}^{T}} \bar{{\mathbf {J}}}^{T}&\\ =&\bar{{\mathbf {J}}}^{T} - {\mathbf {I}}\bar{{\mathbf {J}}}^{T} = \mathbf{{0}}. \end{aligned}$$

\(\square \)

Note that the lower priority task need not be considered from (22) because the lower priority task term, \(({\mathbf {I}} - {\mathbf {P}}_{o}){\mathbf {S}}^{T}\varvec{\varGamma }_{a,0}\), affects neither the higher priority operational space task nor the un-actuated joint space according to Property 2 and the following Property 3, respectively:

Property 3

\(({\mathbf {I}}-{\mathbf {P}}_{a})({\mathbf {I}}-{\mathbf {P}}_{o}){\mathbf {S}}^{T}={\mathbf {0}}\) and its proof is shown as follows:

Proof

$$\begin{aligned} ({\mathbf {I}}-{\mathbf {P}}_{a})({\mathbf {I}}-{\mathbf {P}}_{o}){\mathbf {S}}^{T} =&\,\,\, ({\mathbf {I}} - {\mathbf {P}}_{a} - {\mathbf {P}}_{o} + \underbrace{{\mathbf {P}}_{a}{\mathbf {P}}_{o}}_{={\mathbf{{P}}_{o}}}){\mathbf {S}}^{T}\\ =&\,\,\, ({\mathbf {I}}-{\mathbf {P}}_{a}){\mathbf {S}}^{T}\\ =&\,\,\,{\mathbf {S}}^{T} - {\mathbf {S}}^{T}\underbrace{\bar{{\mathbf {S}}}^{T}{\mathbf {S}}^{T}}_{={\mathbf {I}}} = {\mathbf {0}}, \end{aligned}$$

where the property \({\mathbf {P}}_{a}{\mathbf {P}}_{o} = {\mathbf {P}}_{o}\) can be obtained as follows:

$$\begin{aligned} {\mathbf {P}}_{a}{\mathbf {P}}_{o}&= {\mathbf {S}}^{T}\bar{{\mathbf {S}}}^{T} \overline{\bar{{\mathbf {J}}}^{T}} \bar{{\mathbf {J}}}^{T}\\&= {\mathbf {S}}^{T}\underbrace{\bar{{\mathbf {S}}}^{T} {\mathbf {S}}^{T}}_{={\mathbf {I}}} (\overline{\bar{{\mathbf {J}}}^{T}{\mathbf {S}}^{T}})\bar{{\mathbf {J}}}^{T}\\&={\mathbf {S}}^{T}(\overline{\bar{{\mathbf {J}}}^{T}{\mathbf {S}}^{T}})\bar{{\mathbf {J}}}^{T} = {\mathbf {P}}_{o}. \end{aligned}$$

\(\square \)

From (22), one can notice that the operational space is affected by two components: one is the operational space control torque, i.e., the first term of the right-hand side of (22), and the other is the uncompensated gravity torque and Coriolis/centrifugal torque at the un-actuated joint, i.e., the second term of the right-hand side of (22).

3.1.3 Dynamic effect from un-actuated joints

In the analyzed equation (22), the dynamic effects caused by actuation and un-actuated joints are combined since the actuation joint and un-actuated joint components are not separated from the reference joint acceleration vector \(\ddot{{\mathbf {q}}}^{*}\). To clarify the effect from un-actuated joints, \(\ddot{{\mathbf {q}}}^{*}\) is decomposed into the reference acceleration of actuation joints \(\ddot{{\mathbf {q}}}_{a}^{*}\) and that of un-actuated joints \(\ddot{{\mathbf {q}}}_{u}^{*}\) from the inversion of the relationship between the task and joint space, i.e., inversion of \(\ddot{{\mathbf {x}}}^{*} = {\mathbf{J}}\ddot{{\mathbf {q}}}^{*}+ \dot{{\mathbf {J}}}\dot{{\mathbf {q}}}\), as follows:

$$\begin{aligned} \ddot{{\mathbf {q}}}^{*}&= \bar{{\mathbf {J}}}(\ddot{{\mathbf {x}}}^{*}-\dot{{\mathbf {J}}}\dot{{\mathbf {q}}})\nonumber \\&= {\mathbf {S}}^{T}{\mathbf{S}}\bar{{\mathbf {J}}}(\ddot{{\mathbf {x}}}^{*}-\dot{{\mathbf{J}}}\dot{{\mathbf {q}}}) + ({\mathbf {I}} - {\mathbf {S}}^{T}{\mathbf {S}})\bar{{\mathbf {J}}}(\ddot{{\mathbf {x}}}^{*}-\dot{{\mathbf {J}}}\dot{{\mathbf {q}}})\nonumber \\&=\underbrace{{\mathbf {S}}^{T}{\mathbf {S}}\ddot{{\mathbf {q}}}^{*}}_{:=\ddot{{\mathbf {q}}}^{*}_{a}} + \underbrace{({\mathbf {I}} - {\mathbf {S}}^{T}{\mathbf {S}})\ddot{{\mathbf {q}}}^{*}}_{:=\ddot{{\mathbf {q}}}_{u}^{*}}, \end{aligned}$$
(23)

where \(\bar{{\mathbf {J}}}\) is the dynamically consistent inverse of \({\mathbf {J}}\). In (23), \(\ddot{{\mathbf {q}}}^{*}_{u}\) cannot be generated as desired since it is in accordance with the un-actuated joints, unlike \(\ddot{{\mathbf {q}}}^{*}_{a}\). To reveal the physical meaning of un-actuated joints explicitly, \(\ddot{{\mathbf {q}}}^{*}_{u}\) is re-described as an equation of un-actuated joint acceleration which is defined as \(\ddot{{\mathbf {q}}}_{u} = ({\mathbf {I}} - {\mathbf {S}}^{T}{\mathbf{S}})\ddot{{\mathbf {q}}}\) as follows:

$$\begin{aligned} \ddot{{\mathbf {q}}}_{u}+(\ddot{{\mathbf {q}}}^{*}_{u} - \ddot{{\mathbf {q}}}_{u}). \end{aligned}$$
(24)

Then, (24) describes \(\ddot{{\mathbf {q}}}^{*}_{u}\) as the summation of acceleration and acceleration error of the un-actuated joints. Finally, by substituting (7), (23), and (24) into (22), the operational space acceleration response is obtained as follows:

$$\begin{aligned} \varvec{\varLambda }\ddot{{\mathbf {x}}}&= \varvec{\varLambda } [ \underbrace{{\mathbf {J}}\ddot{{\mathbf {q}}}^{*}_{a}}_{:=\ddot{{\mathbf {x}}}_{a}}+\underbrace{{\mathbf {J}}\{\ddot{{\mathbf {q}}}_{u}+(\ddot{{\mathbf {q}}}^{*}_{u} - \ddot{{\mathbf {q}}}_{u})\}}_{:=\ddot{{\mathbf {x}}}_{u1}} \nonumber \\&\quad \underbrace{- {\mathbf {J}}{\mathbf {A}}^{-1}({\mathbf {I}}-{\mathbf {P}}_{a})({\mathbf {b}} + {\mathbf {g}})}_{:=\ddot{{\mathbf {x}}}_{u2}} + \dot{{\mathbf {J}}}\dot{{\mathbf {q}}} ], \end{aligned}$$
(25)

where \(\ddot{{\mathbf {x}}}_{a}\) is the only producible reference acceleration vector that commands actuated joints, \(\ddot{{\mathbf {x}}}_{u1}\) is the disturbance acceleration affected by the joint acceleration error due to the un-actuated joints, and \(\ddot{{\mathbf {x}}}_{u2}\) is the passive disturbance term caused by uncompensated nonlinear bias torques at the un-actuated joints.

3.1.4 Discussion

The final result (25) explicitly describes the dynamic effect of the un-actuated joints. From a further investigation of (25), one can notice that the torque command of the conventional OSC, shown in (16), cannot generate desired task force and acceleration due to the nonlinear dynamic disturbance effect \(\ddot{{\mathbf {x}}}_{u1}\) and \(\ddot{{\mathbf {x}}}_{u2}\) stemmed from the un-actuated joints. The term \(\ddot{{\mathbf {x}}}_{u1}\) complicates the operational space control problem since it is coupled with the controllable acceleration \(\ddot{{\mathbf {x}}}_{a}\). Therefore, in most casesFootnote 1, the disturbance force from \(\ddot{{\mathbf {x}}}_{u1}\) is accompanied by the reference \(\ddot{{\mathbf {x}}}^{*}\) even when controlling slow motion in zero-gravity space although \(\ddot{{\mathbf {x}}}_{u2}\) can be ignored as zero.

The term \(\ddot{{\mathbf {x}}}_{u2}\) also disturbs the operational space task but it has different characteristics from \(\ddot{{\mathbf {x}}}_{u1}\). The effect from uncompensated \({\mathbf {g}}\) at un-actuated joints can induce a large influence depending on the robot posture, while that from uncompensated \({\mathbf {b}}\) at un-actuated joints occurs during the high-speed motion. Especially, the former can be critical because a large amount of \({\mathbf {g}}\) directly affects the operational space and it also creates a free-swinging motion increasing \({\mathbf {b}}\). Meanwhile, in most cases, \(\ddot{{\mathbf {x}}}_{u2}\) converges to a small value over time regardless of control since the potential energy keeps forcing un-actuated joints to converge to equilibrium while creating free-swinging motion.

The analyzed result describes the expected behavior of the robot when the conventional control method [21, 22] is applied for the robot arm control. Accordingly, it is proved that the under-actuated robot arm may not be successfully controlled with the conventional method by showing the disturbance terms \(\ddot{{\mathbf {x}}}_{u1}\) and \(\ddot{{\mathbf {x}}}_{u2}\). Thus, an improved control solution is required to overcome the shown limitation of the previous method [21, 22].

3.2 Verification with numerical experiments

In this section, numerical analysis is performed instead of actual robot experiments in order to examine results isolated from robot modeling and state-estimation errors. For verification of the analysis result (25), measured accelerations and computed terms in (25) are compared in simulations.

Fig. 1
figure 1

Robot system for case studies: a the humanoid robot COMAN+; b its dynamic simulation model, where the global coordinate is depicted with red, green, and blue arrows for x-, y-, and z-axis, respectively; and c the kinematic structure of the 7 DoFs arm. (Color figure online)

Full size image
Fig. 2
figure 2

Results of the gravity and Coriolis/centrifugal force compensation case with \(\varvec{\varGamma } = {\mathbf {S}}^{T}\bar{{\mathbf {S}}}^{T} (\hat{{\mathbf {b}}} +\hat{{\mathbf {g}}})\): a acceleration responses, b position responses, and c snapshots of the robot arm motion, where the coordinate system of the lower right corner denotes the x-, y-, and z-axis. (Color figure online)

Full size image

3.2.1 Simulation settings

Two simulations are implemented with an arm of the human-sized humanoid robot COMAN+ [28] depicted in Fig. 1a. The dynamic simulator V-REP with the URDF model of the COMAN+ shown in Fig. 1b and 1c is employed for numerical verification.

In simulations, the robot arm has 7 DoFs of torque controllable actuators, while the base body and joints of the other arm, waist, and legs are fixed to remove the effect from the motion of the joints. The ball-shaped hand is the EE which center point is controlled in the Cartesian space. Accordingly, the operational space is defined as \({\mathbf {x}} = [x; y; z]^{T} \in {\mathbb {R}}^{3}\), where x, y,  and z denote the position of the EE at the Cartesian coordinate. The origin and the orientation of the coordinate system are shown in Fig. 1b.

We adopt a free-swinging joint failure scenario of performing fault-tolerant control which is a practical control application. In the beginning, the robot is controlled to keep the initial position shown in Fig. 1b with a fully actuated state and a shoulder pitch (ShP) joint becomes an actuation failure state at \(t=0.3\) s. After the actuation failure, the commanding torque is calculated by (16) which the OSF for the under-actuated robot with the selection matrix \({\mathbf {S}} = [{\mathbf {0}}^{6 \times 1} \ \ {\mathbf {I}}^{6 \times 6}]\).

The kinematics parameter (\({\mathbf {J}}\)) and dynamics parameters (\({\mathbf {A}}, {\mathbf {b}}, {\mathbf {g}}\)) of the robot are calculated based on the method introduced in [29], and they are modeled perfectly the same as the simulation model. The vectors regarding robot joint states (\({\mathbf {q}}, \dot{{\mathbf {q}}}, \ddot{{\mathbf {q}}}\)) are obtained from the simulator. Control reference \(\ddot{{\mathbf {x}}}\) is designed with (11). Accordingly, all the other parameters can be calculated with these known and designed parameters.

3.2.2 Dynamic effect without motion control

In the first simulation, correctness of \((\ddot{{\mathbf {x}}}_{u2} + \dot{{\mathbf {J}}}\dot{{\mathbf {q}}})\) in the analyzed equation (25) is examined. To this end, the robot is controlled by OSC without applying task commands, i.e., \(\ddot{{\mathbf {x}}}^{*}={\mathbf{0}}\) and \(\varvec{\varGamma }_{a,0} = {\mathbf{0}}\) are set in (16) to make \(\ddot{{\mathbf {x}}}_{a}\) and \(\ddot{{\mathbf {x}}}_{u1}\) zero. Accordingly, free-swinging motion is expected to occur at the ShP joint while \({\mathbf {b}}\) and \({\mathbf {g}}\) components of the other joints are to be compensated by the feedforward term in (16).

Simulation results are shown in Fig. 2. The robot arm motion at the operational space (x, y, z) during the simulation is shown in Fig. 2b and 2c . The resulting motion of the entire arm is visualized with snapshots in Fig. 2c. It can be seen that the whole arm including the end-effector moves back and forth perturbed by the free-swinging motion at the ShP joint. Corresponding EE motion in the Cartesian coordinate is described in Fig. 2b. As can be seen in the figures, the robot arm generates pendulum-like motion rotating around ShP joint affected by the un-actuated ShP joint. The motion stops at a specific posture after the magnitude of \(\ddot{\mathbf {{x}}}_{u2} + \dot{{\mathbf {J}}}\dot{{\mathbf {q}}}\) becomes small. This is a natural result since the potential energy of the un-actuated joint reduces to the specific value while generating the free-swinging motion and the joint angle converges to the angle where the gravity force acting on it is zero, as aforementioned.

Acceleration results during the simulation are shown in Fig. 2a. The terms \(\ddot{{\mathbf {x}}}_{u2}\) and \(\dot{{\mathbf {J}}}\dot{{\mathbf {q}}}\) from (25), and the measured acceleration of the EE at the operational space \(\ddot{{\mathbf {x}}} = [\ddot{{{x}}},\ddot{{{y}}},\ddot{{{z}}}]^{T}\) are plotted. In the result, one can observe that the summation of the analytically calculated term (\(\ddot{{\mathbf {x}}}_{u2} + \dot{{\mathbf {J}}}\dot{{\mathbf {q}}}\)) is almost identical to the measured acceleration of the EE. A little difference is mostly caused by a numerical error occurring in kinematic derivatives when measuring the acceleration. This result supports that the terms \(\ddot{{\mathbf {x}}}_{u2}\) and \(\dot{{\mathbf {J}}}\dot{{\mathbf {q}}}\) of the analyzed equation (25) are correct.

Fig. 3
figure 3

Results of the end-effector position regulation control with (16): a acceleration responses, b position responses, and c snapshots of the robot arm motion, where the coordinate system of the lower right corner denotes the x-, y-, and z-axis. (Color figure online)

Full size image

3.2.3 Dynamic effect with motion control

In the second simulation, correctness of the other terms \((\ddot{{\mathbf {x}}}_{a}+\ddot{{\mathbf {x}}}_{u1})\) in (25) is verified. For this purpose, the 3 DoFs Cartesian position of the EE is controlled to regulate its position as the first priority task with PD control scheme (11), and the joint damping is generated as the lower priority task to stabilize self-motion with \(\varvec{\varGamma }_{a,0} = \bar{{\mathbf {S}}}^{T}(-{{k}}_{v_j}\dot{{\mathbf {q}}} )\), where gain \({{k}}_{v_j}\) is set as 40. The gains for the operational space task are set as \({{k}}_{p}=400\) and \({{k}}_{v}=40\). Simulation results are shown in Fig. 3.

The motion of the entire arm during the simulation is shown in Fig. 3c. It can be seen that the free-swinging ShP joint causes unwanted motion on the entire arm including the EE. Nevertheless, due to the control torque, it is successfully controlled so the EE position almost converges at the initial position after \(t\approx 1.5\) s. Detailed EE motion at the Cartesian coordinate is shown in Fig. 3b. Due to the large influence stemmed by the un-actuated joint, the control error of the EE increases during \(t \approx 0.3\)-0.7 s. After \(t\approx 0.7\) s, the EE moves back to the desired position since the control torque can generate acceleration \(\ddot{{\mathbf {x}}}_{a}\) in the desired direction while compensating disturbance acceleration \(\ddot{{\mathbf {x}}}_{u1} + \ddot{{\mathbf {x}}}_{u2}\). Acceleration results are shown in Fig. 3a. Although the control error after \(t\approx 1.5\) s is small, the EE is continuously perturbed by the passive motion caused by nonlinear forces at the un-actuated joint. Thus, it does not converge at the desired position perfectly but slightly oscillates. This result shows that \(\ddot{{\mathbf {x}}}_{u2}\) is the dynamic component critically affecting the operational space task by creating free-swinging motion, as aforementioned.

Similar to the previous simulation, EE acceleration \(\ddot{{\mathbf {x}}}\) calculated by (25) is almost identical to the measured value during the OSC, as shown in Fig 3a. Therefore, the term \(\ddot{{\mathbf {x}}}_{a}+\ddot{{\mathbf {x}}}_{u1}\) in (25) is also verified to be correct since the other term, \(\ddot{{\mathbf {x}}}_{u2} + \dot{{\mathbf {J}}}\dot{{\mathbf {q}}}\), has been already verified in the previous simulation. As a result, we proved through the numerical simulations that the analyzed equation (25) is correct.

4 Application study: OSC exploiting dynamic effect analysis of un-actuated joints

From the analysis result (25) in the previous section, one can expect that reducing the effect of disturbance accelerations \(\ddot{{\mathbf {x}}}_{u1}\) and \(\ddot{{\mathbf {x}}}_{u2}\) can lead to better control performance for the operational space task, whereas it is difficult to mitigate the disturbance terms by the linearized operational space controller, i.e., by the Jacobian matrix-based control approach, since the analyzed dynamic terms are coupled and nonlinear as discussed in Sect. 3. Thus, to verify that the analyzed result can be effectively utilized for the control problem, we present an optimization-based control method that can instantaneously decrease the disturbance terms by calculating amended acceleration reference for the operational space control.

We design a control strategy to minimize negative effects for the critical tasks at the expense of less important tasks. By sacrificing certain task components, i.e., by allowing motion error of non-critical task components, the disturbance acceleration affecting the important task component can be compensated. A real-time controller is formulated with numerical optimization as introduced in the following subsection. Then, through the physical robot experiment, the efficacy of the employed control strategy exploiting the analysis result is verified.

4.1 Controller formulation

The operational space task can be controlled as desired when the following condition is satisfied according to (25).

$$\begin{aligned} \ddot{{\mathbf {x}}}^{*} = \ddot{{\mathbf {x}}}_{a} +\ddot{{\mathbf {x}}}_{u1}+\ddot{{\mathbf {x}}}_{u2} + \dot{{\mathbf {J}}}\dot{{\mathbf{q}}}. \end{aligned}$$

Here, one of an instantaneous controllable condition of the robot that satisfying the above condition can be expressed as follows:

$$\begin{aligned}&\ddot{{\mathbf {x}}}^{*} - \ddot{{\mathbf {x}}}_{a} - \dot{{\mathbf {J}}}\dot{{\mathbf {q}}}={\mathbf{0}}, \end{aligned}$$
(26)
$$\begin{aligned}&\ddot{{\mathbf {x}}}_{u1}+\ddot{{\mathbf {x}}}_{u2} = {\mathbf{0}}. \end{aligned}$$
(27)

When the above conditions (26) and (27) cannot be satisfied at a given instant, minimizing the magnitude of left-hand side terms of those conditions can produce better control performance, i.e., minimizing \(\Vert \ddot{{\mathbf {x}}}^{*} - \ddot{{\mathbf {x}}}_{a}- \dot{{\mathbf {J}}}\dot{{\mathbf {q}}}\Vert _2\), and \(\Vert \ddot{{\mathbf {x}}}_{u1}+\ddot{{\mathbf {x}}}_{u2}\Vert _2\).

To obtain the minimized solution, QP optimization is employed to propose a task-optimization approach that produces a new optimized reference acceleration \(\ddot{{\mathbf {x}}}'\) with the cost function that describes the above condition as

$$\begin{aligned} \displaystyle \min _{\ddot{{\mathbf {x}}}'} \frac{1}{2}(\varvec{\chi }_{a}^{T}{\mathbf{H}}_{a}\varvec{\chi }_{a} + \varvec{\chi }_{u}^{T}{\mathbf {H}}_{u}\varvec{\chi }_{u}), \end{aligned}$$
(28)

where \(\ddot{{\mathbf {x}}}'\) is the decision variable vector; \({\mathbf {H}}_{a}\) and \({\mathbf {H}}_{u}\) are the diagonal matrices for weighting. To compute \(\ddot{{\mathbf {x}}}'\) that can minimize the two conditions (26) and (27), the vectors \(\varvec{\chi }_{a}\) and \(\varvec{\chi }_{u}\) are defined as follows:

$$\begin{aligned} \varvec{\chi }_{a}&= \ddot{{\mathbf {x}}}^{*} - (\ddot{{\mathbf {x}}}_{a}' + \dot{{\mathbf {J}}}\dot{{\mathbf {q}}})\\&= \ddot{{\mathbf {x}}}^{*} - \{{\mathbf {J}}{\mathbf {S}}^{T}{\mathbf {S}}\bar{{\mathbf {J}}}(\ddot{{\mathbf{x}}}'-\dot{{\mathbf {J}}}\dot{{\mathbf {q}}}) + \dot{{\mathbf {J}}}\dot{{\mathbf {q}}}\},\\ \varvec{\chi }_{u}&= \ddot{{\mathbf {x}}}_{u1}'+\ddot{{\mathbf {x}}}_{u2}\\&={\mathbf {J}}({\mathbf {I}} - {\mathbf {S}}^{T}{\mathbf {S}})\bar{{\mathbf {J}}}(\ddot{{\mathbf {x}}}'-\dot{{\mathbf {J}}}\dot{{\mathbf {q}}})+\ddot{{\mathbf {x}}}_{u2}, \end{aligned}$$

where \(\ddot{{\mathbf {x}}}_{a}'\) and \(\ddot{{\mathbf {x}}}_{u1}'\) are the operational space acceleration reference vectors corresponding to actuation joints and un-actuated joints, respectively. Note that \(\ddot{{\mathbf {x}}}_{u2}\) is a constant vector at a given instant since it depends on the given state of the robot.

The cost function (28) is then minimized subject to

$$\begin{aligned} \varvec{\varGamma }&= {\mathbf {S}}^{T} \{ \overline{\bar{{\mathbf {J}}}^{T}{\mathbf {S}}^{T}} \hat{\varvec{\varLambda }} \ddot{{\mathbf {x}}}' + {\mathbf {N}}^{T} \varvec{\varGamma }_{a,0} + \bar{{\mathbf {S}}}^{T} (\hat{{\mathbf {b}}} +\hat{{\mathbf {g}}}) \}, \end{aligned}$$
(29)
$$\begin{aligned} \varvec{\varGamma }&\in [\varvec{\varGamma }_{\text {lb}}, \ \varvec{\varGamma }_{\text {ub}}], \end{aligned}$$
(30)
$$\begin{aligned} \ddot{{\mathbf {x}}}'&\in [\ddot{{\mathbf {x}}}_{\text {lb}}, \ \ddot{{\mathbf {x}}}_{\text {ub}}], \end{aligned}$$
(31)

where \(\varvec{\varGamma }_{\text {lb}}\), \(\varvec{\varGamma }_{\text {ub}}\), \(\ddot{{\mathbf {x}}}_{\text {lb}}\), \(\ddot{{\mathbf {x}}}_{\text {ub}}\) are lower and upper boundary of joint torque vector, lower and upper boundary of task reference acceleration vector, respectively. The equality constraints (29) from (16) provide the operational space control rule. The inequality constraint (30) is for physical limitation of joint torque, and the inequality constraint (31) is applied to limit the magnitude of the reference acceleration. Finally, by substituting optimized task acceleration \(\ddot{{\mathbf {x}}}'\) into (29), control torque can be achieved.

Fig. 4
figure 4

Snapshots of operational space control experiments when the ShP joint is un-actuated: a position trajectory tracking, b position regulation, and c robust regulation under disturbances, where red-colored dot denotes the desired position and the green-colored arrow denotes the direction of external force. (Also see other experiments in the video, Online Resource 1). (Color figure online)

Full size image

4.2 Experiments under actuation failure scenarios

In this section, experiments with actual robot hardware are performed in order to verify the efficacy of the analyzed equation (25) and application proposed in Sect. 4.1.

4.2.1 Experimental settings

Actual robot experiments with COMAN+ arm are conducted with the QP-based control approach [30, 31], where the robot is controlled in the sampling frequency of 1 kHz with real-time software XBotCore [32]. The QP optimization is solved in real time by the open-source library qpOASES [33].

In experiments, trajectory tracking control, position regulation control, and position regulation control under perturbation are performed with the QP optimization-based OSF approach as shown in Fig. 4. To be consistent with the simulations in Sect. 3, the 3-DoFs EE position in Cartesian space \({\mathbf {x}} = [x; y; z]^{T}\) is controlled to track the given desired position while the damping of joints is actively generated in the null space. The origin of the coordinate is positioned at the center of two feet which is the same as the simulation.

As the actuation failure scenario, the ShP joint suddenly becomes un-actuated at t=0 s, while the robot is controlled to maintain the initial position before the un-actuation of the joint is triggered. Accordingly, the selection matrix \({\mathbf {S}} = [{\mathbf {0}}^{6 \times 1} \ \ {\mathbf {I}}^{6 \times 6}]\) is used after joint failure occurs.

During the experiments, kinematics (\({\mathbf {J}}\)) and dynamics parameters (\({\mathbf {A}}, {\mathbf {b}}, {\mathbf {g}}\)) are calculated with the method in [29]. The joint angle vector \({\mathbf {q}}\) is measured by the encoders at joints, \(\dot{{\mathbf {q}}}\) and \(\ddot{{\mathbf {q}}}\) are calculated by differentiating \({\mathbf {q}}\) with time, and the control reference \(\ddot{{\mathbf {x}}}\) is designed by (11). The other parameters are calculated based on these parameters. It is notable that the modeled and measured parameters are not perfectly the same as the real robot unlike simulations in Sect. 3.2.

4.2.2 Trajectory tracking control

To evaluate the control performance, the trajectory tracking control is performed as shown in Fig. 4a. In the experiment, the trajectory generated by a cubic spline function provides \({\mathbf {x}}_{d}\), \(\dot{{\mathbf {x}}}_{d}\), and \(\ddot{{\mathbf {x}}}_{d}\) to draw a triangle in the y-z plane for 3 seconds while the x-axis position is commanded to be constant. To control the y- and z-axis position more precisely than the x-axis position, weighting matrices for the optimization are set as \({\mathbf {H}}_{a}\) \(=\) \({\mathbf {H}}_{u}\) \(=\) \({\mathbf {diag}}(1, 100, 100)\), where \({\mathbf {diag}}(w_{x},w_{y},w_{z})\) denotes the diagonal matrix with weighting value \(w_{\bullet }\) for the \(\bullet \)-axis motion, to control the y- and z-axis position more precisely than the x-axis position.

The position control result without the QP optimization and the result with the QP optimization approach are shown in Fig. 5. In the results, position error rapidly increases with the beginning, and then, the error reduces according to the reduction in the disturbance acceleration caused by the free-swinging motion of the un-actuated joint. These are the similar tendency compared to the position regulation simulation result in the previous section, and this supports that (25) we derived is valid also for the actual robot system.

Fig. 5
figure 5

Plots of end-effector position during the QP optimization-based trajectory tracking control compared to the control without optimization: a time responses and b position in yz- and xz-planes. The red-colored dashed line denotes position trajectory, the blue-colored line denotes the position result when controlled without optimization, and the black-colored dash-dotted line denotes the position result when controlled with the proposed optimization-based method. (Color figure online)

Full size image
Fig. 6
figure 6

Plot of optimized cost during trajectory tracking control. (Color figure online)

Full size image

One can investigate that the z-axis motion error reduces as intended by weighting matrices when the proposed optimization is adopted. On the other hand, the x-axis error increases as a trade-off of reducing the disturbance acceleration effect at the z-axis component because the optimized cost cannot be minimized as zero as shown in Fig 6. Also, one can notice that the position error becomes larger when the optimized cost shown in Fig 6 is large since the cost is proportional to the disturbance acceleration from the un-actuated joints.

In the experiment, the control result without the QP optimization shows the result with the previous method for under-actuated robots in [21, 22]. As can be seen in the experiment, a large control error occurs when the previous method is applied for control. On the other hand, it is shown that the proposed method can manage the control performance of the specific direction by reducing the analyzed dynamic effect of the un-actuated joint. Through this result, one can notice that the developed method can advance the previous method.

Fig. 7
figure 7

Plots of end-effector position during the regulation control with various weighting matrices. (Color figure online)

Full size image

4.2.3 3D Position regulation with different weightings

The strategic selection of the important task components by assigning high weighting factors to the corresponding components can be effectively utilized for avoiding critical situations such as self-collision or workspace deviation by controlling the important task more precisely when the un-actuated joint exists. Here, to verify the influence of the weightings and effectiveness of the strategy, we implemented the EE position regulation experiments as shown in Fig. 4b when the ShP joint is un-actuated with four different weightings as \({\mathbf {H}}_{a}={\mathbf {H}}_{u}={\mathbf {diag}}(100, 100, 100)\), \({\mathbf {H}}_{a}={\mathbf {H}}_{u}={\mathbf {diag}}(1, 100, 100)\), \({\mathbf {H}}_{a}={\mathbf {H}}_{u}={\mathbf {diag}}(100, 1, 100)\), and \({\mathbf {H}}_{a}={\mathbf {H}}_{u}={\mathbf {diag}}(100, 100, 1)\).

As shown in Fig. 7, similar to all the other experiments and simulations, position rapidly changes in the beginning and then converges to the desired position while the position error results vary according to the weighting matrices. In the case when the weightings are \({\mathbf {diag}}(100, 100, 100)\), the control error deviation in each axis is the smallest compared to the other cases. When \(w_{x}\) is small, the x-axis position error is large while the other direction error is small. Similarly, when \(w_{y}\) or \(w_{z}\) is small, the same axis error is large and the other direction error tends to be small.

The control result with weightings \({\mathbf {diag}}(100, 100, 100)\) presents the best results with respect to the average error in all axes. The root mean square (rms) of the average control error during \(t=0\)-1.3 s for this case is approximately 0.6 cm while rms error for the other weightings is between 1.2 and 1.4 cm. However, in some situations, accuracy in a specific axis might be more desirable than the best average. For example, in this actuation failure case, the system is vulnerable against the gravity force in z-axis; it is thus better to lower \(w_{x}\) or \(w_{y}\) which relaxes x, y-axes motion yet allows greater control capability of z-axis motion. When \({\mathbf {diag}}(100, 1, 100)\) is applied for the weightings, the z-axis rms error and the peak error are approximately 0.3 cm and 0.6 cm, respectively, which are smaller than the results when \({\mathbf {diag}}(100, 100, 100)\) is applied where the z-axis rms error and the peak error are approximately 0.5 cm and 1.6 cm, respectively. In other words, when \(w_{z}\) is low, it can be the worse choice since in the z-axis position error increases and the x- and y-axis motion is almost the same as the result when the weighting matrix is \({\mathbf {diag}}(100, 100, 100)\). As seen from the experimental results, more effective control is possible by selecting the combination of weightings with careful consideration of given task conditions and the state of the robot.

In addition to the trajectory tracking and regulation control, we performed more experiments with the QP optimization-based controller, where disturbance forces are randomly applied to the robot during position regulation as shown in Fig. 4c. In the experiments, the QP optimization-based control method successfully copes with un-modeled external forces creating compliant self-motion. It is also successfully tested under actuation failure in other joints such as roll and yaw joints at the shoulder. Those extra experimental demonstrations can be found in the associated media file. Through these experiments, we verified how practical the analysis we performed can be used for robotic arm control applications.

5 Conclusion and future work

In this study, we investigate the robot’s multi-body dynamics, which encompasses complicated nonlinear dynamic phenomena, particularly associated with the effects of un-actuated joints. Then, we propose a task-optimization method to alleviate unwanted disturbance from un-actuated joints and to ameliorate control performance of the selected operational space motion exploiting the derived dynamic effect analysis. Those are verified by a number of simulations and experiments with the torque-controllable 7 DoFs robot arm under the fault-tolerant control of the free-swinging joint failure scenario.

This paper provides a theoretical foundation for practical control engineers to develop fault-tolerant control methods associated with complicated nonlinear problems of the free-swinging joint failure. Additionally, there is an interesting extension from this study to look into behavior and properties of the OSF in floating base robots and to control or plan free-flying motion such for the aerial or under-water manipulator, and jumping motion of the legged robots, which cover the under-actuated and unconstrained cases.