Control Barrier Functions for Mechanical Systems: Theory and Application to Robotic Grasping

TL;DR

This paper develops robust control barrier functions for second-order, sampled-data mechanical systems with uncertainties, and applies them to robotic grasping to ensure safety and task constraints are met.

Contribution

It introduces novel control barrier functions for relative degree two, sampled-data systems with uncertainties, and demonstrates their effectiveness in robotic grasping tasks.

Findings

Validated through simulations and experiments

Ensures no slip, over-extension, or fingertip roll-off during grasping

Compatible with existing manipulation controllers

Abstract

Control barrier functions have been demonstrated to be a useful method of ensuring constraint satisfaction for a wide class of controllers, however existing results are mostly restricted to continuous time systems of relative degree one. Mechanical systems, including robots, are typically second-order systems in which the control occurs at the force/torque level. These systems have velocity and position constraints (i.e. relative degree two) that are vital for safety and/or task execution. Additionally, mechanical systems are typically controlled digitally as sampled-data systems. The contribution of this work is two-fold. First, is the development of novel, robust control barrier functions that ensure constraint satisfaction for relative degree two, sampled-data systems in the presence of model uncertainty. Second, is the application of the proposed method to the challenging problem of robotic grasping in which a robotic hand must ensure an object remains inside the grasp while manipulating it to a desired reference trajectory. A grasp constraint satisfying controller is proposed that can admit existing nominal manipulation controllers from the literature, while simultaneously ensuring no slip, no over-extension (e.g. singular configurations), and no rolling off of the fingertips. Simulation and experimental results validate the proposed control for the robotic hand application.

Tables

Table 1. TABLE I : Simulation Parameters

Link dimensions	$0.05\text{m}\times 0.05\text{m}\times 0.3\text{m}$
Link mass	$0.25\text{kg}$
Link moment of inertia	$\text{diag}\left([0.0019,0.0001,0.0019]\right){\text{kg}\text{m}}^2$
Fingertip radius	$0.06\text{m}$
Object dimensions	$0.260\text{m}\times 0.260\text{m}\times 0.260\text{m}$
Object mass	$0.11\text{kg}$
Object moment of inertia	$\text{diag}\left([0.0058,0.0214,0.0214]\right){\text{kg}\text{m}}^2$
Friction coefficient	$\mu =0.9$
Initial ${𝒑}_o$	$[0.00,0.00,0.41]\text{m}$
Initial ${𝜸}_o$	$[0.0,0.0,0.0]\text{rad}$

Table 2. TABLE II : Allegro Hand: Model Parameters

Dimensions (m)	Link 1	Link 2	Link 3
Length (index, middle, ring)	$0.0540$	$0.0384$	$0.0250$
Length (thumb)	$0.0554$	$0.0514$	$0.0400$
Width/Depth (all)	$0.0196$	$0.0196$	$0.0196$
Mass (kg)	Link 1	Link 2	Link 3
(index, middle, ring)	$0.0444$	$0.0325$	$0.0619$
(thumb)	$0.0176$	$0.0499$	$0.0556$

Equations

$$\dot{\mathbf{x}}=\mathbf{f}(\mathbf{x})+\mathbf{g}(\mathbf{x})\mathbf{u}+\mathbf{d}$$

$${\hat{h}}_j(\mathbf{x})=h_j(\mathbf{x})-{\delta }_j,j\in [1,l]$$

$$\begin{array}{r}{\hat{\mathcal{C}}}_j=\{\mathbf{x}\in {\mathbb{R}}^p:{\hat{h}}_j(\mathbf{x})\ge 0\},j\in [1,l]\\ \partial {\hat{\mathcal{C}}}_j=\{\mathbf{x}\in {\mathbb{R}}^p:{\hat{h}}_j(\mathbf{x})=0\},j\in [1,l]\\ \text{Int}({\hat{\mathcal{C}}}_j)=\{\mathbf{x}\in {\mathbb{R}}^p:{\hat{h}}_j(\mathbf{x})>0\},j\in [1,l]\end{array}$$

$${\hat{B}}_j(\mathbf{x})={\dot{\hat{h}}}_j(\mathbf{x})+{\alpha }_1({\hat{h}}_j(\mathbf{x}))-{\beta }_j,j\in [1,l]$$

$${\hat{\mathcal{B}}}_j=\{\mathbf{x}\in {\mathbb{R}}^p:{\hat{B}}_j(\mathbf{x})\ge 0\}$$

$$\begin{gathered} {\hat{\mathcal{S}}}_{u_j}(\mathbf{x})=\{\mathbf{u}\in U: \\ {L}_f{\hat{B}}_j(\mathbf{x})+L_g{\hat{B}}_j(\mathbf{x})\mathbf{u}+{\alpha }_2({\hat{B}}_j(\mathbf{x}))\ge 0\},j\in [1,l] \end{gathered}$$

$$V_{{\hat{\mathcal{B}}}_j}(\mathbf{x})=\{\begin{array}{ll}0,& \text{if}\mathbf{x}\in {\hat{\mathcal{B}}}_j\\ -{\hat{B}}_j(\mathbf{x}),& \text{if}\mathbf{x}\in {\mathcal{E}}_j\setminus {\hat{\mathcal{B}}}_j\end{array}$$

$$V_{{\hat{\mathcal{C}}}_j}(\mathbf{x})=\{\begin{array}{ll}0,& \text{if}\mathbf{x}\in {\hat{\mathcal{C}}}_j\\ -{\hat{h}}_j(\mathbf{x}),& \text{if}\mathbf{x}\in {\mathcal{D}}_j\setminus {\hat{\mathcal{C}}}_j\end{array}$$

$${\mathcal{B}}_{{\gamma }_j}=\{\mathbf{x}\in {\mathbb{R}}^p:{\hat{B}}_j(\mathbf{x})\ge -\gamma ({\Vert \mathbf{d}\Vert }_{\infty })\}$$

$${\dot{x}}_1=x_2$$

$${\dot{x}}_2=f(\mathbf{x})+g(\mathbf{x})\mathbf{u}$$

$${\alpha }_1(x_1-1)\le x_2\le {\alpha }_1(4-x_1)$$

$$\begin{array}{rl}{\mathbf{u}}^{\ast }=& \underset{\mathbf{u}\in {\mathbb{R}}^m}{\mathrm{argmin}}{\mathbf{u}}^T\mathbf{u}-2{\mathbf{u}}_{\text{nom}}^T\mathbf{u}\\ & \text{s.t.}\hat{A}(\mathbf{x})\mathbf{u}\ge \hat{\mathbf{b}}(\mathbf{x})\\ & {\mathbf{u}}_{\text{min}}\le \mathbf{u}\le {\mathbf{u}}_{\text{max}}\end{array}$$

$$\hat{A}(\bm{x})=\left[\begin{array}[]{ccc}L_{g}\hat{B}_{1}(\bm{x})^{T},&...,&L_{g}\hat{B}_{l}(\bm{x})^{T}\end{array}\right]^{T}$$

$$\hat{\bm{b}}(\bm{x})=\left[\begin{array}[]{c}-L_{f}\hat{B}_{1}(\bm{x})-\alpha_{2}(\hat{B}_{1}(\bm{x}))\\ ...\\ -L_{f}\hat{B}_{l}(\bm{x})-\alpha_{2}(\hat{B}_{l}(\bm{x}))\end{array}\right]$$

$$\dot{\mathbf{x}}=\mathbf{f}(\mathbf{x})+\mathbf{g}(\mathbf{x}){\mathbf{u}}_k+\mathbf{d}$$

$$\begin{gathered} {\hat{\mathcal{S}}}_{u_{k_j}}(\mathbf{x})=\{{\mathbf{u}}_k\in U:k\in [0,N]: \\ {L}_f{\hat{B}}_j({\mathbf{x}}_k)+L_g{\hat{B}}_j({\mathbf{x}}_k){\mathbf{u}}_k+{\alpha }_2({\hat{B}}_j({\mathbf{x}}_k))\ge \nu (T)\},j\in [1,l] \end{gathered}$$

$$m=\Big{(}L_{f}\hat{B}_{j}(\bm{x})-L_{f}\hat{B}_{j}(\bm{x}_{k})\Big{)}+\Big{(}\alpha_{2}(\hat{B}_{j}(\bm{x}))-\alpha_{2}(\hat{B}_{j}(\bm{x}_{k}))\Big{)}\\ +\Big{(}L_{g}\hat{B}_{j}(\bm{x})-L_{g}\hat{B}_{j}(\bm{x}_{k})\Big{)}\bm{u}_{k},j\in[1,l]$$

$$\begin{gathered} ||m||\le (c_1+c_2+c_3||{\mathbf{u}}_k||)||\mathbf{x}-{\mathbf{x}}_k||, \\ \forall t\in [kT,(k+1)T),j\in [1,l] \end{gathered}$$

$$\begin{gathered} ||m||\le \frac{(c_1+c_2+c_3{c}_4)c_5}{c_1+c_2{c}_4}(e^{(c_1+c_2{c}_4)(t-kT)}-1), \\ \forall t\in [kT,(k+1)T),j\in [1,l] \end{gathered}$$

$${\nu }_{k_j}(T):=\frac{a}{b}(e^{bT}-1),\forall t\in [kT,(k+1)T),j\in [1,l]$$

$$\begin{gathered} L_f{\hat{B}}_j({\mathbf{x}}_k)+L_g{\hat{B}}_j({\mathbf{x}}_k){\mathbf{u}}_k+{\alpha }_2({\hat{B}}_j({\mathbf{x}}_k))\ge \nu (T), \\ \forall t\in [kT,(k+1)T),\forall j\in [1,l] \end{gathered}$$

$$\begin{gathered} L_f{\hat{B}}_j(\mathbf{x})+L_g{\hat{B}}_j(\mathbf{x}){\mathbf{u}}_k+{\alpha }_2({\hat{B}}_j(\mathbf{x}))+L_d{\hat{B}}_j(\mathbf{x}) \\ \ge \nu (T)+m+L_d{\hat{B}}_j(\mathbf{x}) \end{gathered}$$

$${\dot{\hat{B}}}_j(\mathbf{x})+{\alpha }_2({\hat{B}}_j(\mathbf{x}))\ge L_d{\hat{B}}_j(\mathbf{x})$$

$${\dot{\hat{B}}}_j(\mathbf{x})+{\alpha }_3(B_j(\mathbf{x}))\ge -{\alpha }_4(-{\beta }_j)+L_d{\hat{B}}_j(\mathbf{x})$$

$$\begin{array}{rl}{\mathbf{u}}_k^{\ast }=& \underset{\mathbf{u}\in {\mathbb{R}}^m}{\mathrm{argmin}}{\mathbf{u}}^T\mathbf{u}-2{\mathbf{u}}_{{\text{nom}}_k}^T\mathbf{u}\\ & \text{s.t.}\hat{A}({\mathbf{x}}_k)\mathbf{u}\ge \hat{\mathbf{b}}({\mathbf{x}}_k)+\hat{\nu }1\\ & {\mathbf{u}}_{\text{min}}\le \mathbf{u}\le {\mathbf{u}}_{\text{max}}\end{array}$$

$$M_h\ddot{\mathbf{q}}+C_h\dot{\mathbf{q}}=-J_h^T{\mathbf{f}}_c+{\mathbf{\tau }}_e+\mathbf{u}$$

$$J_{h_{i}}(\bm{q}_{i},\bm{p}_{fc_{i}})=\left[\begin{array}[]{cc}I_{3\times 3}&-(\bm{p}_{fc_{i}})\times\end{array}\right]J_{s_{i}}(\bm{q}_{i})$$

$$M_o{\ddot{\mathbf{x}}}_o+C_o{\dot{\mathbf{x}}}_o=G{\mathbf{f}}_c+{\mathbf{w}}_e$$

$$G(\bm{p}_{c},\bm{p}_{o})=\left[\begin{array}[]{cccc}I_{3\times 3},&...,&I_{3\times 3},\\ (\bm{p}_{c_{1}}-\bm{p}_{o})\times,&...,&(\bm{p}_{c_{n}}-\bm{p}_{o})\times\end{array}\right]$$

Full text

I Introduction

Mechanical systems, such as robots, comprise many of the engineered systems used in the physical world. Their dynamics are conventionally defined as second-order, nonlinear affine systems, with generalized states associated with position, velocity, and acceleration. The control of these systems is usually applied as a force/torque at the acceleration level, and most, if not all, mechanical systems must satisfy workspace constraints. Examples include collision avoidance of autonomous vehicles in traffic [1], a robotic manipulator avoiding obstacles in the environment [2], and of particular note here is a robotic hand manipulating an object [3]. Workspace constraints are position dependent, which due to the second order dynamics, makes them relative degree two. Additionally, these mechanical systems are controlled digitally. That is, at each sampling period, sensors collect measurements of the system and a control input is implemented constantly over the sampling time. Thus mechanical systems can be equivalently defined as sampled-data systems of relative degree two.

An existing approach for formally handling constraints is via control barrier functions. Control barrier functions can be classified as reciprocal control barrier functions and zeroing control barrier functions [4]. The former was developed with applications towards bipedal walking [5], systems evolving on manifolds [6], and control of constrained robots [2]. However the latter, zeroing control barrier functions, have been shown to not only be more practical for implementation, but also robust to model uncertainties [4, 7].

Zeroing control barrier functions are attractive due to their robustness qualities, however existing zeroing control barrier functions are mostly restricted to continuous-time, relative degree one systems. The implementation of those methods is conventionally done by formulating the control input as a quadratic program, and requires local Lipschitz continuity of the ensuing control input [4]. That implementation requires restrictive properties, namely linear independent constraint qualification and complimentary slackness, to ensure local Lipschitz continuity of the quadratic program [8, 9]. Those requirements are difficult to guarantee in practice when multiple constraints must be considered for mechanical systems. Furthermore the continuous-time formulation does not allow for robustness to sampling effects that arise during implementation on mechanical systems. Only [10] has addressed (reciprocal) control barrier function for sampled data systems, however that approach was restricted to double integrator systems, which does not apply to the mechanical systems considered here, nor does it consider robustness to perturbations.

An additional concern is that mechanical systems have a physical relationship between position, velocity, and actuation effort, which is not considered in existing methods for relative degree two systems. This relationship is apparent as the position of a mechanical system approaches the constraint boundary. If there is no consideration of the position/velocity relationship, then a large velocity may be permitted near the constraint boundary. Due to the inertia of the system, large control effort is then required to keep the system inside the constraint set. This large control effort may lead to actuator saturation, which compromises the systems ability of ensuring constraint satisfaction. This issue becomes exacerbated when considering external perturbations on the system that may arise from model uncertainty commonly found in practice. Thus it is important to systematically consider velocity profiles in the zeroing control barrier function formulation, which has not yet been addressed in existing work [4, 7].

The first contribution of this paper is the development of a novel zeroing control barrier function for mechanical systems. The proposed method formally ensures constraint satisfaction for relative degree two systems and is robust to sampling effects and perturbations. The proposed method also allows for the designer to tune the velocity bounds to reduce excessive actuator effort near the constraint boundary and help comply with actuator limitations of the mechanical system. A constraint satisfying controller is then defined to stay minimally close to a nominal control, but prioritize constraint satisfaction. The second contribution is the application of the proposed technique to a non-trivial problem: robotic grasping.

Robotic grasping consists of a robotic hand manipulating an object to a desired reference pose trajectory. This task is commonly referred to as in-hand manipulation. In-hand manipulation consists of moving an object to track a desired trajectory, while simultaneously ensuring the object remains within the grasp. For successful in-hand manipulation, it is paramount to guarantee that the object remains in the grasp during the manipulation motion.

A failed grasp can result from slipping, joint over-extension, and excessive rolling. Slipping is an obvious grasping concern, which has been extensively addressed in the literature [3, 11, 12]. Joint over-extension relates to joints exceeding feasible joint angles (e.g joint workspace and singular hand configurations), which inhibits the robotic hand from applying necessary contact forces on the object [13]. Excessive rolling is when the contact points roll off of the fingertip surface. In-hand manipulation inherently relies on rolling motion for object manipulation [14]. However excessive rolling motion may cause the contact points to leave the fingertip surface, resulting in loss of contact with the object. Thus for successful manipulation, the object must not slip, the joints must remain inside a feasible workspace, and the contact points must remain in the fingertip workspace. These conditions are referred to as the grasp constraints.

To date, there exist an abundance of object manipulation controllers for robotic hands. Early work developed manipulation controllers based on linearization or feedback linearization for an exact object model [15, 16]. In [17] a more robust manipulation controller was developed to handle gravity, uncertain contact locations/kinematics. Later work developed passivity-based controllers [18], impedance-based controllers [19], and visual/tactile-based controllers [20]. Other bio-inspired methods have also been developed by extracting human information to perform manipulation on robotic hands [21, 22, 23, 24, 25, 26]. A more extensive review of object manipulation control methods can be found in [27]. However, those existing techniques are only valid if the grasp constraints are satisfied.

There exist few methods that address grasp constraint satisfaction. In grasp force optimization, an optimization problem is solved in-the-loop in which the constraints ensure slip is prevented. Early work in grasp force optimization primarily focused on computational speed due to limited computational capabilities of the time [12, 28, 29, 30, 31, 32]. Later work addressed robustness properties of grasp force optimization [33], and considered the dynamics of the hand-object system [11, 34, 3]. However those methods only address a subset of the grasp constraint satisfaction problem, that of no slip, and still required unfounded assumptions that singular configurations were avoided and excessive rolling did not occur.

Existing methods of addressing all grasping constraints are via motion planning approaches. Motion planning approaches typically assume the exact knowledge of the hand-object model and assume the system is quasi-static to search for feasible manipulation trajectories [35, 36, 37]. In [38], the knowledge of the object is relaxed by requiring a mesh of the object surface, but still assumes a quasi-static nature of the system. Those methods rely on quasi-static assumptions that generally do not hold in a dynamic manipulation setting. Furthermore those approaches are subject to large computational resources, which may not be conducive to real-time applications [37].

Furthermore, existing methods of addressing grasping constraints are not robust to model uncertainties and/or unknown disturbances that may act on the system. The literature contains numerous controllers that aim to extend manipulation capabilities outside of a laboratory setting by restricting the required sensors to on-board sensing modalities [3, 27]. A common theme in related work is referred to as “blind grasping” or similarly “tactile-based blind grasping” where the robotic hand only has access to sensors that can be physically integrated into the hand (i.e. joint angle sensors and tactile sensors). Those approaches still depend on satisfaction of the grasp constraint, but with the added restriction to on-board sensors and no a priori knowledge of the object model. This motivates the need for a robust, active method of ensuring grasp constraint satisfaction that does not require exact knowledge of the object model.

The second contribution of this paper is a novel grasp controller to actively ensure grasp constraint satisfaction. The proposed control is designed to be implemented alongside existing manipulation controllers found in the literature. The idea here is to support the many existing methods from the literature such that in the case when no constraint violation occurs, the proposed control outputs the original manipulation controller from the literature. However, should constraint violation be imminent, the proposed control deviates from the manipulation controller to prioritize constraint satisfaction (i.e. no slip, no over-extension, no excessive rolling). The proposed control is implemented in simulation and hardware to demonstrate the efficacy of the proposed approach. Note this paper extends the contribution from [39] with application to sampled-data systems, robustness to model uncertainties, associated formal proofs, and implementation on hardware.

Notation

Throughout this paper, an indexed vector $\bm{v}_{i}\in\mathbb{R}^{p}$ has an associated concatenated vector $\bm{v}\in\mathbb{R}^{pk}$, where the index $i$ is specifically used to index over the $n$ contact points in the grasp. The notation $\bm{v}^{\mathcal{\mathcal{E}}}$ indicates that the vector $\bm{v}$ is written with respect to a frame $\mathcal{E}$, and if there is no explicit frame defined, $\bm{v}$ is written with respect to the inertial frame, $\mathcal{P}$. The operator $(\cdot)\times$ denotes the skew-symmetric matrix representation of the cross-product. $SO(3)$ denotes the special orthogonal group of dimension 3. The $r\times r$ identity matrix is denoted $I_{r\times r}$. The term $\bm{e}_{j}\in\mathbb{R}^{1,r}$ denotes the $j$th row of $I_{r\times r}$. The Lie derivatives of a function $h(\bm{x})$ for the system $\dot{\bm{x}}=\bm{f}(\bm{x})+g(\bm{x})\bm{u}$ are denoted by $L_{f}h$ and $L_{g}h$, respectively. The intersection of sets $\mathscr{C}_{j}$ for $j\in[1,l]$ is denoted \underaccent{\bar}{\mathscr{C}}. The distance between a point $\bm{x}$ and set $\mathscr{A}$ is $\left\lVert\bm{x}\right\rVert_{\mathscr{A}}\mathrel{\mathop{\ordinarycolon}}=\underset{\bm{y}\in\mathscr{A}}{\text{inf}}$ $\left\lVert\bm{x-y}\right\rVert$. A $\mathcal{KL}$ function as defined in [40] is denoted by $\mathcal{KL}(\cdot,\cdot)$. When discussing model uncertainty, the approximation of a variable $\bm{v}$ is denoted with a hat, $\hat{\bm{v}}$, and the associated error is denoted by $\Delta(\bm{v})$.

II Zeroing Control Barrier Functions for Sampled-Data Systems of Relative Degree Two

In this section, the zeroing control barrier functions from [4] are extended to relative degree two, sampled-data systems and address robustness to perturbations.

II-A Robust Barrier Functions for Relative Degree Two Systems

Consider the following nonlinear affine control system in continuous time:

[TABLE]

where $\bm{u}\in U\subseteq\mathbb{R}^{m}$ is the control input, $\bm{f},\bm{g}$ are locally Lipschitz continuous functions of $\bm{x}\in\mathbb{R}^{p}$, and $\bm{d}\in\mathbb{R}^{p}$ is a bounded, locally Lipschitz disturbance. Let $\bm{x}(t,\bm{x}_{0})\in\mathbb{R}^{n}$ be the solution of (1), which for ease of notation is denoted by $\bm{x}$.

The goal of constraint satisfaction is to ensure the states $\bm{x}$ stay within a set of constraint-admissible states. Let $h_{j}(\bm{x})\mathrel{\mathop{\ordinarycolon}}\mathbb{R}^{p}\to\mathbb{R}$ be a twice-continuously differentiable, relative degree two function for constraint $j\in[1,l]$. Robustness is addressed using margins, $\delta_{j}\in\mathbb{R}_{\geq 0}$ in $h_{j}$, which leads to the following definition of $\hat{h}_{j}$:

[TABLE]

Let the set of constraint-admissible states be:

[TABLE]

For ease of notation, \hat{\underaccent{\bar}{\mathscr{C}}} represents the intersection of all $\hat{\mathscr{C}}_{j}$ for $j\in[1,l]$. Also, $\mathscr{C}_{j}$ is used to denote $\hat{\mathscr{C}}_{j}$ with $\delta_{j}\equiv 0$.

The following definition for extended class-$\mathcal{K}$ function is now introduced:

Definition 1**.**

[4]**: A continuous function, $\alpha\mathrel{\mathop{\ordinarycolon}}(-b,a)\to(-\infty,\infty)$ for $a,b\in\mathbb{R}_{>0}$ is an extended class-$\mathcal{K}$ function if it is strictly increasing and $\alpha(0)=0$.

Note for clarity, the extended class-$K$ functions addressed here will be defined for $a,b=\infty$.

Constraint satisfaction is ensured by showing that on the constraint boundary, the system states are directed into the interior or along the boundary of the constraint set [41]. This is equivalent to guaranteeing that $\dot{\hat{h}}(\bm{x})\geq-\alpha_{1}(\hat{h}(\bm{x}))$ for a continuously differentiable, extended class-$\mathcal{K}$ function $\alpha_{1}$ [4].

The novel approach taken here is to introduce a continuously differentiable function $\hat{B}_{j}\mathrel{\mathop{\ordinarycolon}}\mathbb{R}^{p}\to\mathbb{R}$ defined by:

[TABLE]

where $\beta_{j}\in\mathbb{R}_{\geq 0}$ is a robustness margin. By construction of $\hat{B}_{j}$, constraint satisfaction regarding $\hat{\mathscr{C}}_{j}$ is equivalent to ensuring $\hat{B}_{j}\geq 0$ for all $t\geq 0$. Let $\hat{\mathscr{B}}_{j}$ denote the set where $\hat{B}_{j}\geq 0$:

[TABLE]

As with $\hat{h}_{j}$ and $\hat{\mathscr{C}}_{j}$, $B_{j}$ and $\mathscr{B}_{j}$ will be used to denote $\hat{B}_{j}$, $\hat{\mathscr{B}}_{j}$ for $\beta_{j}\equiv 0$. To implement the control barrier functions, let $\hat{\mathscr{S}}_{u_{j}}$ denote the set of constraint-admissable control inputs:

[TABLE]

By ensuring forward invariance of $\hat{\mathscr{B}}_{j}$, it follows that $\hat{B}_{j}\geq 0$ to ensure that $\hat{\mathscr{C}}_{j}$ is forward invariant. The function $\hat{B}_{j}$ is considered the zeroing control barrier function. Here, input-to-state stability properties of zeroing control barrier functions are exploited to ensure forward invariance of \underaccent{\bar}{\mathscr{C}} in the presence of bounded perturbations. This is accomplished by using robustness margins $\beta_{j}$, $\delta_{j}$ for compact sets \underaccent{\bar}{\mathscr{B}}, \underaccent{\bar}{\mathscr{C}}. Forward invariance of \underaccent{\bar}{\mathscr{C}} for the proposed method is formally guaranteed in the following theorem:

Theorem 1**.**

Consider the controllable system (1). Let $\hat{\mathscr{C}}_{j}$ be defined by (2), (3), for the twice-continuously differentiable, relative degree two functions $h_{j}\mathrel{\mathop{\ordinarycolon}}\mathscr{D}_{j}\to\mathbb{R}$, defined on the open set $\mathscr{D}_{j}\supset\mathscr{C}_{j}\supset\hat{\mathscr{C}}_{j}$. Suppose there exist a continuously differentiable extended class-$\mathcal{K}$ function $\alpha_{1}$ and extended class-$\mathcal{K}$ function $\alpha_{2}$ such that for $\hat{B}_{j}\mathrel{\mathop{\ordinarycolon}}\mathscr{E}_{j}\to\mathbb{R}$ (defined by (4)) on the open set $\mathscr{E}_{j}\supset\hat{\mathscr{B}}_{j}$ (defined by (5)), and \hat{\underaccent{\bar}{\mathscr{S}}}_{u} (defined by (6)), \hat{\underaccent{\bar}{\mathscr{B}}} and \hat{\underaccent{\bar}{\mathscr{C}}} are compact, and \hat{\underaccent{\bar}{\mathscr{B}}}\cap\hat{\underaccent{\bar}{\mathscr{C}}}, \hat{\underaccent{\bar}{\mathscr{S}}}_{u} are non-empty. Then for any \bm{x}(0)\in\hat{\underaccent{\bar}{\mathscr{B}}}\bigcap\hat{\underaccent{\bar}{\mathscr{C}}}, and locally Lipschitz control \bm{u}(\bm{x})\in\underaccent{\bar}{\mathscr{S}}_{u}(\bm{x}), there exist $\theta,\beta_{j},\delta_{j}\in\mathbb{R}_{\geq 0}$, $j\in[1,l]$ such that for $\left\lVert\bm{d}\right\rVert_{\infty}\leq\theta$, $\bm{x}$ remains in \underaccent{\bar}{\mathscr{B}}\bigcap\underaccent{\bar}{\mathscr{C}} for $t\geq 0$.

Proof.

First, asymptotic stability of \hat{\underaccent{\bar}{\mathscr{B}}} is considered for $\bm{d}\equiv 0$. Let V_{\hat{\underaccent{\bar}{\mathscr{B}}}}\mathrel{\mathop{\ordinarycolon}}\mathbb{R}^{p}\to\mathbb{R}_{\geq 0} be the Lyapunov function defined by V_{\hat{\underaccent{\bar}{\mathscr{B}}}}=\sum_{j}V_{\hat{\mathscr{B}}_{j}} on the open set \underaccent{\bar}{\mathscr{E}} where $V_{\hat{\mathscr{B}}_{j}}$ is defined by:

[TABLE]

To show negative definiteness of \dot{V}_{\hat{\underaccent{\bar}{\mathscr{B}}}}, note that for $\bm{x}\in\mathscr{E}_{j}\setminus\hat{\mathscr{B}}_{j}$, $\hat{B}_{j}(\bm{x})<0$. For $\bm{u}\in\hat{\mathscr{S}}_{u_{j}}$, differentiation of $V_{\hat{\mathscr{B}}_{j}}$ for $\bm{x}\in\mathscr{E}_{j}\setminus\hat{\mathscr{B}}_{j}$ results in $\dot{V}_{\hat{\mathscr{B}}_{j}}=-\dot{\hat{B}}_{j}\leq\alpha_{2}(\hat{B}_{j})=\alpha_{2}(-V_{\hat{\mathscr{B}}_{j}})<0$ for each $j\in[1,l]$. Thus \dot{V}_{\hat{\underaccent{\bar}{\mathscr{B}}}}<0 holds by definitino of V_{\hat{\underaccent{\bar}{\mathscr{B}}}} and asymptotic stability of the compact set \hat{\underaccent{\bar}{\mathscr{B}}} follows from Theorem 2.8 of [42]. Smoothness of the Lyapunov function is addressed in Proposition 4.2 of [42].

Next, asymptotic stability of \hat{\underaccent{\bar}{\mathscr{C}}} is addressed. Consider the following Lyapunov function, V_{\hat{\underaccent{\bar}{\mathscr{C}}}}\mathrel{\mathop{\ordinarycolon}}\mathbb{R}^{p}\to\mathbb{R}_{\geq 0}, defined by V_{\hat{\underaccent{\bar}{\mathscr{C}}}}=\sum_{j}V_{\hat{\mathscr{C}}_{j}} on the open set \underaccent{\bar}{\mathscr{D}} where $V_{\hat{\mathscr{C}}_{j}}$ is defined by:

[TABLE]

From $\bm{x}\in\hat{\mathscr{B}}_{j}$, it follows that $\hat{B}_{j}\geq 0$ holds. Thus $V_{\hat{\mathscr{C}}_{j}}(\bm{x})=0$ for $\bm{x}\in\hat{\mathscr{C}}_{j}$, $V_{\hat{\mathscr{C}}_{j}}(\bm{x})>0$ for $\bm{x}\in\mathscr{D}_{j}\setminus\hat{\mathscr{C}}_{j}$. Differentiation of $V_{\hat{\mathscr{C}}_{j}}$ for $\bm{x}\in\mathscr{D}_{j}\setminus\hat{\mathscr{C}}_{j}$ results in $\dot{V}_{\hat{\mathscr{C}}_{j}}=-\dot{\hat{h}}_{j}\leq\alpha_{1}(\hat{h}_{j})=\alpha_{1}(-V_{\hat{\mathscr{C}}_{j}})<0$. Similarly with \hat{\underaccent{\bar}{\mathscr{B}}}, asymptotic stability of \hat{\underaccent{\bar}{\mathscr{C}}} follows.

Now consider $\bm{d}\neq 0$. It follows from Proposition 5 of [7] that for a class-$\mathcal{K}$ function $\gamma$, the set:

[TABLE]

is asymptotically stable for $\left\lVert\bm{d}\right\rVert_{\infty}\leq\theta$ such that $\left\lVert\bm{x}\right\rVert_{\mathscr{B}_{\gamma}}\leq\mathcal{KL}(\left\lVert\bm{x}(0)\right\rVert_{\mathscr{B}_{\gamma}},t)$ for all $t\geq 0$. Choose $\beta_{j}=\gamma(\left\lVert\bm{d}\right\rVert_{\infty})$. By (4), it follows that $B_{j}=\hat{B}_{j}+\beta_{j}$ and consequently $\mathscr{B}_{j}=\mathscr{B}_{\gamma_{j}}$. Thus $\mathscr{B}_{j}$ is asymptotically stable such that $\left\lVert\bm{x}\right\rVert_{\mathscr{B}_{j}}\leq\mathcal{KL}(\left\lVert\bm{x}(0)\right\rVert_{\mathscr{B}_{j}},t)$. By the condition that $\bm{x}(0)\in\hat{\mathscr{B}}_{j}\subset\mathscr{B}_{j}$, it follows that $\left\lVert\bm{x}\right\rVert_{\mathscr{B}_{j}}=0$ for all $t\geq 0$, and thus $\bm{x}$ remains in $\mathscr{B}_{j}$ for $t\geq 0$.

Forward invariance of $\mathscr{B}_{j}$ implies that $\mathscr{C}_{j}$ is asymptotically stable. Thus by the same argument, it follows that there exists a $\delta_{j}$ such that $\bm{x}$ remains in $\mathscr{C}_{j}$ for $t\geq 0$. Since $\bm{x}$ remains in $\mathscr{B}_{j}$ and $\mathscr{C}_{j}$ concurrently, $\bm{x}$ must remain in $\mathscr{B}_{j}\bigcap\mathscr{C}_{j}$ for $t\geq 0$. Repeated application for $j\in[1,l]$ completes the proof. ∎

Theorem 1 ensures constraint satisfaction via the proposed zeroing control barrier functions in the presence of disturbances. The development of the proposed zeroing control barrier functions is also advantageous for implementation on mechanical systems of relative degree two. The following example highlights this advantage by demonstrating how the propose method naturally restricts large velocities from occurring at the constraint boundary:

Example 1**.**

One advantage of the zeroing control barrier functions presented here over existing methods [4] is that it naturally constrains the velocity of the relative-degree two system for desired behavior near the constraint boundary. This is particularly advantageous for mechanical systems with workspace constraints. Consider the following system:

[TABLE]

[TABLE]

where $\bm{x}=(x_{1},x_{2})\in\mathbb{R}^{2}$, $\bm{u}\in\mathbb{R}^{2}$. The system is constrained by: $1\leq x_{1}\leq 4$.

Let $h_{\text{min}}(x)=x-1$, $h_{\text{max}}=4-x$. From (4), the resulting control barrier functions are $B_{\text{min}}=x_{2}+\alpha_{1}(x-1)$ and $B_{\text{max}}=-x_{2}+\alpha_{1}(4-x)$ for a given extended class-$\mathcal{K}$ function, $\alpha_{1}$. The zeroing control barrier functions naturally bound the velocity, $x_{2}$, with respect to the extended class-$\mathcal{K}$ function $\alpha_{1}$:

[TABLE]

The bounds on $x_{2}$ are shown in Figure 1 for various choices of $\alpha_{1}$. Figure 1 shows that without additional structure imposed on the proposed control barrier functions, the velocity bounds prevent large velocities at the boundary that would require large control effort to ensure constraint satisfaction. Furthermore, the designer has the ability to tune these velocity bounds by an appropriate choice of $\alpha_{1}$. The gray regions in Figure 1 depict the set of constraint-admissible states $\bm{x}$.

Remark 1**.**

It is worth mentioning that the proposed zeroing control barrier function method is extendable to larger relative degrees by repeated applications of (4). For example, for a relative degree 3 system, a new function $\hat{E}_{j}=\dot{\hat{B}}_{j}+\alpha_{3}(\hat{B}_{j})-\eta_{j}$, with $\eta_{j}\in\mathbb{R}_{\geq 0}$, would be defined as the zeroing control barrier function and the same results would follow.

II-B Control Barrier Functions for Sampled-Data Systems

Theorem 1 ensures constraint satisfaction for some locally Lipschitz continuous \bm{u}\in\hat{\underaccent{\bar}{\mathscr{S}}}_{u}. However insofar there is no discussion of how such a $\bm{u}$ is to be constructed. In related work [4, 2], the control $\bm{u}$ is defined as the solution to a quadratic program, which takes the following form for the zeroing control barrier functions presented here:

[TABLE]

where $\bm{u}_{\text{nom}}\in\mathbb{R}^{m}$ is a nominal control and

[TABLE]

[TABLE]

The idea behind the quadratic program formulation is to simplify the control design by first designing a stabilizing, nominal control for the system without consideration of constraint satisfaction. Then the quadratic program (13) ensures constraint satisfaction as the nominal control stabilizes the system to a desired point. This implementation prioritizes constraint satisfaction over the stability/performance guarantees associated with the nominal controller. It is important to emphasize that existing control barrier function technqiues require local Lipschitz continuity of $\bm{u}^{*}$ [4, 2]. However the quadratic program formulation alone is not sufficient to ensure local Lipschitz continuity as discussed in [9]. To ensure local Lipschitzness of the quadratic program (13), the following properties must hold [43, 8]:

Property 1**.**

(Linear Independence Constraint Qualification) The active constraints of the quadratic program (13) have full row rank.

Property 2**.**

(Strict Complimentary Slackness) Let $\lambda^{*}\in\mathbb{R}^{m}$ denote the Lagrange multiplier associated with $\bm{u}^{*}$. Strict complimentary slackness is satisfied if there does not exist any $j$ such that both $\lambda_{j}^{*}=0$ and $A_{j}\bm{u}_{j}^{*}=0$. ($A_{j}$ refers to the $j$th row of $A$).

Note that Properties 1 and 2 can be overly restrictive and difficult to ensure. Linear independence in particular can be troublesome when there are more constraints than decision variables in the optimization problem leading to potential redundant, active constraints. This occurs in robotic grasping, as will be discussed later, in which slip and workspace constraints outnumber the control input of the system.

The quadratic program formulation from (13) is valuable for mechanical systems, particularly robotic systems, in which constraint satisfaction is paramount for safety and successful execution of a task. However (13) is not directly implementable for mechanical systems. Mechanical systems are typically controlled digitally, in which sensors are used to take measurements, the quadratic program (13) is solved, and the computed control is implemented over a sampling period. This sample-hold operation is a common characteristic in most mechanical systems, yet is neglected in existing control barrier function methods.

Here, sampling effects are considered to extend the proposed zeroing control barrier function method for implementation on mechanical systems. Consider the sampled-data version of (1), where a zero-order hold is placed on $\bm{u}$:

[TABLE]

and $\bm{u}_{k}\in\mathbb{R}^{m}$ is a piece-wise constant control with sampling time of $T\in\mathbb{R}_{>0}$. The sampled-data system takes measurements of $\bm{x}_{k}\mathrel{\mathop{\ordinarycolon}}=\bm{x}(kT)$, which are constant in each sampling period. Uniqueness and existence of the solution $\bm{x}(t,\bm{x}(0))$ for (16) is ensured by Caratheodory’s Theorem for a time interval $t\in[0,NT)$, $N\in\mathbb{Z}_{>0}$ [44].

The extension of zeroing control barrier functions to sampled-data systems is as follows. First, inter-sampling behavior is addressed by incorporating a margin $\nu(T)$ in the zeroing control barrier function condition (6). The margin $\nu(T)$ is an extended class-$\mathcal{K}$ function, which acts to negate inter-sampling effects. Let the set of constraint-admissible controls $\bm{u}_{k}$ for sampled-data systems be defined by:

[TABLE]

Following Nagumo’s Theorem [41], the use of $\nu(T)$ is to ensure $\dot{\hat{B}}_{j}\geq-\alpha(\hat{B}_{j})$, which ensures asymptotic stability of $\hat{\mathscr{B}}_{j}$ (see Theorem 1) for forward invariance of $\mathscr{B}_{j}$. The following theorem ensures forward invariance of \underaccent{\bar}{\mathscr{B}}\cap\underaccent{\bar}{\mathscr{C}} in the presence of sampling and external perturbations:

Theorem 2**.**

Consider the controllable system (16). Let $\hat{\mathscr{C}}_{j}$ be defined by (2), (3) for the thrice-continuously differentiable, relative degree two functions $h_{j}(\bm{x})\mathrel{\mathop{\ordinarycolon}}\mathscr{D}_{j}\to\mathbb{R}$, defined on the open set $\mathscr{D}_{j}\supset\mathscr{C}_{j}\supset\hat{\mathscr{C}}_{j}$. Suppose there exists a twice-continuously differentiable, extended class-$\mathcal{K}$ function $\alpha_{1}$ and locally Lipschitz continuous extended class-$\mathcal{K}$ function $\alpha_{2}$, such that for $\hat{B}_{j}\mathrel{\mathop{\ordinarycolon}}\mathscr{E}_{j}\to\mathbb{R}$ (defined by (4)) on the open set $\mathscr{E}_{j}\supset\hat{\mathscr{B}}_{j}$ (defined by (5)), \hat{\underaccent{\bar}{\mathscr{B}}} and \hat{\underaccent{\bar}{\mathscr{C}}} are compact, and \hat{\underaccent{\bar}{\mathscr{B}}}\cap\hat{\underaccent{\bar}{\mathscr{C}}} is non-empty. Then for a given $T\in\mathbb{R}_{>0}$, $N\in\mathbb{Z}_{>0}$, there exists an extended class-$\mathcal{K}$ function $\nu(T)$, and $\theta$, $\beta_{j}$, $\delta_{j}\in\mathbb{R}_{\geq 0}$, $j\in[1,l]$ such that for all \bm{x}(0)\in\hat{\underaccent{\bar}{\mathscr{B}}}\bigcap\hat{\underaccent{\bar}{\mathscr{C}}}, $\left\lVert\bm{d}\right\rVert_{\infty}\leq\theta$, and any bounded, piece-wise constant control \bm{u}_{k}(\bm{x})\in\hat{\underaccent{\bar}{\mathscr{S}}}_{u_{k}}(\bm{x}) (defined by (17)), $\bm{x}$ remains in $\mathscr{B}\bigcap\mathscr{C}$ for $t\in[0,NT)$.

Proof.

First, under the local Lipschitz continuity of $\bm{f}$, $\bm{g}$, and $\bm{d}$, Caratheodory’s Theorem [44] ensures existence and uniqueness of the solution to (16) for the bounded piece-wise constant control $\bm{u}_{k}$ on the interval $t\in[0,NT)$.

Next, the extended class-$\mathcal{K}$ function $\nu(T)$ is constructed. Let $m$ be defined by:

[TABLE]

By locally Lipschitz properties of $\bm{f}(\bm{x})$, $\bm{g}(\bm{x})$, $\hat{B}_{j}(\bm{x})$, and $\alpha_{2}(\hat{B}_{j}(\bm{x}))$ it follows that:

[TABLE]

where $c_{1},c_{2},c_{3}\in\mathbb{R}_{>0}$ are the respective Lipschitz constants for $L_{f}\hat{B}_{j}$, $\alpha_{2}$, and $L_{g}\hat{B}_{j}$. By closeness of solutions between $\bm{x}$ and $\bm{x}_{k}$ (Theorem 3.4 of [40]) and boundedness of $\bm{u}_{k}$ for $||\bm{u}_{k}||\leq c_{4}$, $c_{4}\in\mathbb{R}_{>0}$, it follows that

[TABLE]

where $c_{5}\geq||\bm{f}(\bm{x})+\bm{g}(\bm{x})\bm{u}_{k}||$. Note $c_{5}$ is guaranteed to exist over the interval $t\in[kT,(k+1)T)$ due to local Lipschitz continuity of $\bm{f}$, $\bm{g}$ and boundedness of $\bm{u}_{k}$. Let $\nu_{k_{j}}(T)$ be defined by:

[TABLE]

where $a=(c_{1}+c_{2}+c_{3}c_{4})c_{5}$ and $b=c_{1}+c_{2}c_{4}$. Let $\nu(T)\mathrel{\mathop{\ordinarycolon}}=\underset{k,j}{\text{max}}\ \nu_{k_{j}}(T)$ such that $\nu(T)\geq\nu_{k_{j}}(T)$ for all $j\in[1,l],k\in[0,N]$.

From \bm{u}_{k}\in\hat{\underaccent{\bar}{\mathscr{S}}}_{u_{k}}, it follows that:

[TABLE]

Addition of $L_{f}\hat{B}_{j}(\bm{x})+L_{g}\hat{B}_{j}(\bm{x})\bm{u}_{k}+\alpha_{2}(\hat{B}_{j}(\bm{x}))+L_{d}\hat{B}_{j}(\bm{x})$ to each side of (19) results in:

[TABLE]

From the previous derivation of $\nu(T)$, it follows that $\nu(T)\geq m$ such that the following holds:

[TABLE]

Next, the robustness margin, $\beta_{j}$ is used to account for the perturbation associated with $\bm{d}$. It is trivial to show that there exists extended class-$\mathcal{K}$ functions $\alpha_{3}$, $\alpha_{4}$ such that $\alpha_{2}(\hat{B}_{j})\leq\alpha_{3}(B_{j})+\alpha_{4}(-\beta_{j})$, which results in:

[TABLE]

Choose $\beta_{j}\geq\alpha_{4}^{-1}\circ c_{6}\theta$, where $\frac{\partial\hat{B}}{\partial\bm{x}}\leq c_{6}$, such that $-\alpha_{4}(-\beta_{j})\geq L_{d}\hat{B}_{j}(\bm{x})$. From $\hat{B}_{j}=B_{j}-\beta_{j}$, it is straightforward to see that $\dot{\hat{B}}_{j}=\dot{B}_{j}$, and consequently $\dot{B}_{j}\geq-\alpha_{3}(B_{j}(\bm{x}))$. According to Nagumo’s theorem [41], $\mathscr{B}_{j}$ is forward invariant on $t\in[kT,(k+1)T)$. Repeated application for $j\in[1,l]$, $t\in[0,NT)$ ensures forward invariance of \underaccent{\bar}{\mathscr{B}} for $t\in[0,NT)$. The same procedure is repeated for $\hat{h}_{j}$ and $\delta_{j}$, which concurrently ensures forward invariance of \underaccent{\bar}{\mathscr{C}} and completes the proof. ∎

Following the result from Theorem 2, the following control is proposed for mechanical systems:

[TABLE]

where $\bm{u}_{\text{nom}_{k}}\in\mathbb{R}^{m}$ is the sampled nominal control and $\hat{\nu}\in\mathbb{R}_{\geq 0}$ is a tuning parameter estimating (18) to negate the effects of sampling.

The following theorem guarantees constraint satisfaction via the proposed control (20) for mechanical systems:

Theorem 3**.**

Consider the sampled-data system (16). Let $\hat{\mathscr{C}}_{j}$ be defined by (2), (3) for the thrice-continuously differentiable, relative degree two functions $h_{j}(\bm{x})\mathrel{\mathop{\ordinarycolon}}\mathscr{D}_{j}\to\mathbb{R}$, defined on the open set $\mathscr{D}_{j}\supset\mathscr{C}_{j}\supset\hat{\mathscr{C}}_{j}$. Suppose there exists a twice-continuously differentiable, extended class-$\mathcal{K}$ function $\alpha_{1}$ and locally Lipschitz continuous extended class-$\mathcal{K}$ function $\alpha_{2}$, such that for $\hat{B}_{j}\mathrel{\mathop{\ordinarycolon}}\mathscr{E}_{j}\to\mathbb{R}$ (defined by (4)) on the open set $\mathscr{E}_{j}\supset\hat{\mathscr{B}}_{j}$ (defined by (5)), \hat{\underaccent{\bar}{\mathscr{B}}} and \hat{\underaccent{\bar}{\mathscr{C}}} are compact, and \hat{\underaccent{\bar}{\mathscr{B}}}\cap\hat{\underaccent{\bar}{\mathscr{C}}} is non-empty. Let $\bm{u}_{nom}$ be a nominal control with $\bm{u}_{nom_{k}}=\bm{u}_{nom}(t=kT)$. Then for a given $T$, there exists $\theta$, $\hat{\nu}$, $\beta_{j}$, $\delta_{j}\in\mathbb{R}_{\geq 0}$, $j\in[1,l]$ such that for $\left\lVert\bm{d}\right\rVert_{\infty}\leq\theta$, the control (20) applied to (16) ensures $\bm{x}$ remains in \underaccent{\bar}{\mathscr{B}}\bigcap\underaccent{\bar}{\mathscr{C}} for $t\in[0,NT)$.

Proof.

Existence and uniqueness of $\bm{u}_{k}^{*}$ is ensured by the linear constraints with respect to $\bm{u}$ in (20) [45]. Let $\nu(T)$ be defined from the proof of Theorem 2, and choose $\hat{\nu}$ sufficiently large such that $\hat{\nu}\geq\nu(T)$ for all $j\in[1,l]$, $t\in[0,NT)$. Now, the constraints of (20) ensures $\bm{u}_{k}^{*}$ is bounded and \bm{u}_{k}^{*}\in\underaccent{\bar}{\mathscr{S}}_{u_{k}} such that $\bm{x}$ remains in \underaccent{\bar}{\mathscr{B}}\cap\underaccent{\bar}{\mathscr{C}} for $t\in[0,NT)$ via Theorem 2. ∎

Remark 2**.**

An alternative approach to addressing control barrier functions for sampled-data systems is to employ existing emulation techniques for set stability to the continuous-time controller (13) [46]. However this alternative requires more restrictive conditions including sufficient smoothness of all components of (13) in addition to Properties 1 and 2. On the other hand, by exploiting the properties of sampled-data systems, the proposed method does not require those unnecessary conditions and is thus applicable to more general systems.

III Grasp Constraint Satisfaction

In the previous section, a novel control barrier function method was proposed to ensure constraint satisfaction for mechanical systems. In this section, that method is applied to the challenging problem of robotic grasping, in which a robotic hand is required to grasp and manipulate an object to a desired reference pose trajectory. The application of the proposed zeroing control barrier method to this problem is not trivial, and is an additional contribution of this paper. The following sections develop the appropriate model and conventional assumptions required in related grasping work. Then, appropriate analysis is presented to ensure constraint satisfaction for robotic grasping.

III-A Hand-Object System

Consider a fully-actuated, multi-fingered hand grasping a rigid, convex object at $n\in\mathbb{Z}_{>0}$ contact points. Each finger consists of $m_{i}\in\mathbb{Z}_{>0}$ revolute joints with smooth, convex fingertips of high stiffness. Let the finger joint configuration be described by the joint angles, $\bm{q}_{i}\in\mathbb{R}^{m_{i}}$. The full hand configuration is defined by the joint angle vector, $\bm{q}=(\bm{q}_{1},\bm{q}_{2},...,\bm{q}_{n})^{T}\in\mathbb{R}^{m}$, where $m=\sum_{i=1}^{n}m_{i}$ is the total number of joints. Let the inertial frame, $\mathcal{P}$, be fixed on the palm of the hand, and a fingertip frame, $\mathcal{F}_{i}$, fixed at the point $\bm{p}_{f_{i}}\in\mathbb{R}^{3}$. The translational and rotational velocities of $\mathcal{F}_{i}$ with respect to $\mathcal{P}$ are denoted $\bm{v}_{f_{i}},\bm{\omega}_{f_{i}}\in\mathbb{R}^{3}$, respectively. The rotation matrix from $\mathcal{F}_{i}$ to $\mathcal{P}$ is $R_{pf_{i}}\mathrel{\mathop{\ordinarycolon}}=R_{pf_{i}}(\bm{q}_{i})\in SO(3)$. The contact frame, $\mathcal{C}_{i}$, is located at the contact point, $\bm{p}_{c_{i}}\in\mathbb{R}^{3}$. A visual representation of the contact geometry for the $i$th finger is shown in Figure 2(a).

The hand dynamics incorporating each finger (i.e. robotic manipulator) of the grasp is as follows [13]:

[TABLE]

where $M_{h}\mathrel{\mathop{\ordinarycolon}}=M_{h}(\bm{q})\in\mathbb{R}^{m\times m}$ is the inertia matrix, $C_{h}\mathrel{\mathop{\ordinarycolon}}=C_{h}(\bm{q},\dot{\bm{q}})\in\mathbb{R}^{m\times m}$ is the Coriolis/centrifugal matrix, $J_{h}\mathrel{\mathop{\ordinarycolon}}=J_{h}(\bm{q},\bm{p}_{fc})\in\mathbb{R}^{3n\times m}$ is the hand Jacobian, $\bm{f}_{c}\in\mathbb{R}^{3n}$ is the contact force, $\bm{\tau}_{e}\mathrel{\mathop{\ordinarycolon}}=\bm{\tau}_{e}(t)\in\mathbb{R}^{m}$ is the disturbance torque acting on the joints, and $\bm{u}\in\mathbb{R}^{m}$ is the joint torque control input. Note each $M_{h}$, $C_{h}$, $J_{h}$ is a block diagonal matrix of the individual inertia, Coriolis/centrifugal, and hand Jacobian matrix respectively, and each $\bm{f}_{c}$, $\bm{\tau}_{e}$, $\bm{u}$ is a vector concatenation of the individual contact forces, $\bm{f}_{c_{i}}$, external torques $\bm{\tau}_{e_{i}}$, and control inputs $\bm{u}_{i}$, respectively. For ease of notation, the matrix $E_{i}\in\mathbb{R}^{m_{i}\times m}$ is used to map from the full concatenated vector form to the individual vector, such that for example, $\dot{\bm{q}}_{i}=E_{i}\dot{\bm{q}}$. Note the individual hand Jacobian matrix, $J_{h_{i}}$, is defined by:

[TABLE]

where $J_{s_{i}}(\bm{q}_{i})\in\mathbb{R}^{6\times mi}$ is the spatial manipulator Jacobian that maps $\dot{\bm{q}}_{i}\mapsto(\bm{v}_{f_{i}},\bm{\omega}_{f_{i}})$ [13].

Let $\mathcal{O}$ be a reference frame fixed at the object center of mass $\bm{p}_{o}\in\mathbb{R}^{3}$, and $R_{po}\in SO(3)$ is the rotation matrix, which maps from $\mathcal{O}$ to $\mathcal{P}$. The respective inertial translation and rotational velocities of the object are $\bm{v}_{o},\bm{\omega}_{o}\in\mathbb{R}^{3}$. The object state is $\bm{x}_{o}\in\mathbb{R}^{6}$, with $\dot{\bm{x}}_{o}=(\bm{v}_{o},\bm{\omega}_{o})$. The position vector from $\mathcal{O}$ to the respective contact point is $\bm{p}_{oc_{i}}\in\mathbb{R}^{3}$.

The object dynamics are given by [13]:

[TABLE]

where $M_{o}\mathrel{\mathop{\ordinarycolon}}=M_{o}(\bm{x}_{o})\in\mathbb{R}^{6\times 6}$ is the object inertia matrix, $C_{o}\mathrel{\mathop{\ordinarycolon}}=C_{o}(\bm{x}_{o},\dot{\bm{x}}_{o})\in\mathbb{R}^{6\times 6}$ is the object Coriolis and centrifugal matrices, $G\mathrel{\mathop{\ordinarycolon}}=G(\bm{p}_{oc})\in\mathbb{R}^{6\times 3n}$ is the grasp map, and $\bm{w}_{e}\mathrel{\mathop{\ordinarycolon}}=\bm{w}_{e}(t)\in\mathbb{R}^{6}$ is an external wrench disturbing the object. The grasp map, $G$, maps the contact force, $\bm{f}_{c}$, to the net wrench acting on the object and is defined by:

[TABLE]

When grasping an object, it is important to prevent slip from occurring by ensuring each contact force remains inside the friction cone defined by:

[TABLE]

where $\bm{f}^{\mathcal{C}_{i}}_{c_{i}}=(f_{x_{i}},f_{y_{i}},f_{n_{i}})$ is the contact force at $i$ written in frame $\mathcal{C}_{i}$ with tangential force components $f_{x_{i}},f_{y_{i}}\in\mathbb{R}$ and normal force component $f_{n_{i}}\in\mathbb{R}$, and $\mu\in\mathbb{R}_{>0}$ is the friction coefficient. The full friction cone is the Cartesian product of all the friction cones: $F_{c}=F_{c_{1}}\times...\times F_{c_{n}}$.

When the contact points do not slip, the following grasp relation holds [16]:

[TABLE]

The following assumptions are made for the grasp:

Assumption 1**.**

The multi-fingered hand has $m\geq 3n$ joints.

Assumption 2**.**

The given multi-fingered grasp is such that $G$ is full rank and $\mathcal{R}(G)\bigcap\text{Int}(FC)\neq\emptyset$.

Assumption 3**.**

The system dynamics (21), (23), and local fingertip/object contact surfaces are smooth.

Remark 3**.**

Assumption 2 ensures the grasp is force-closure, which means that for any given object wrench, there exists a contact force that produces the given wrench and also lies inside the friction cone [16]. This force-closure condition is a conventional assumption in robotic grasping literature, and can be ensured by a high-level grasp planner [47].

III-B Hand-Contact Kinematics

Here the differential geometric modeling of rolling contacts is reviewed as presented in [13, 14]. Note, the subscript $co$ will refer to the object surface of the contact, and the subscript $cf$ refers to the fingertip surface of the contact. At each contact point, the contact surfaces are parameterized by local coordinates $\bm{\xi}_{co_{i}}=(a_{co_{i}},b_{co_{i}}),\bm{\xi}_{cf_{i}}=(a_{cf_{i}},b_{cf_{i}})$. The relation between the local coordinates and contact position vectors are defined by smooth mappings: $\bm{p}^{\mathcal{F}_{i}}_{fc_{i}}=\bm{c}_{cf_{i}}(\bm{\xi}_{cf_{i}}),\bm{p}^{\mathcal{O}}_{oc_{i}}=\bm{c}_{co_{i}}(\bm{\xi}_{co_{i}})$.

The geometric parameters including the metric tensor, curvature tensor, and torsion tensor are used to define the rolling contact kinematics. For ease of notation, $\bm{c}_{fa},\bm{c}_{fb}$ respectively denote $\frac{\partial\bm{c}_{cf_{i}}}{\partial a_{cf_{i}}}$ and $\frac{\partial\bm{c}_{cf_{i}}}{\partial b_{cf_{i}}}$. Similarly let $\bm{c}_{oa},\bm{c}_{ob}$ respectively denote $\frac{\partial\bm{c}_{co_{i}}}{\partial a_{co_{i}}}$ and $\frac{\partial\bm{c}_{co_{i}}}{\partial b_{co_{i}}}$.

The Gauss frame is used to define the contact frame $\mathcal{C}_{i}$, which is depicted in Figure 2(b):

[TABLE]

where $R_{fc_{i}}\in SO(3)$ maps $\mathcal{C}_{i}$ to $\mathcal{F}_{i}$.

The metric tensor, $M_{cf_{i}}\mathrel{\mathop{\ordinarycolon}}=M_{cf_{i}}(\bm{\xi}_{cf_{i}})\in\mathbb{R}^{2\times 2}$, curvature tensor, $K_{cf_{i}}\mathrel{\mathop{\ordinarycolon}}=K_{cf_{i}}(\bm{\xi}_{cf_{i}})\in\mathbb{R}^{2\times 2}$, and torsion tensor, $T_{cf_{i}}\mathrel{\mathop{\ordinarycolon}}=T_{cf_{i}}(\bm{\xi}_{cf_{i}})\in\mathbb{R}^{2\times 1}$ are defined by:

[TABLE]

The same geometric parameters for the object, $M_{co_{i}}\mathrel{\mathop{\ordinarycolon}}=M_{co_{i}}(\bm{\xi}_{co_{i}})\in\mathbb{R}^{2\times 2},K_{co_{i}}\mathrel{\mathop{\ordinarycolon}}=K_{co_{i}}(\bm{\xi}_{co_{i}})\in\mathbb{R}^{2\times 2},T_{co_{i}}=T_{co_{i}}(\bm{\xi}_{co_{i}})\in\mathbb{R}^{2\times 1}$, are defined by appropriate substitution of $\bm{\xi}_{cf_{i}}$ with $\bm{\xi}_{co_{i}}$ in (27)-(28):

[TABLE]

Now the equations of motion for $\bm{\xi}_{cf_{i}}$ and $\bm{\xi}_{co_{i}}$ are defined as follows:

[TABLE]

[TABLE]

where

[TABLE]

and $R_{c_{i}p}=R_{fc_{i}}^{T}R_{pf_{i}}^{T}$ maps $\mathcal{P}$ to $\mathcal{C}_{i}$. The contact angle dynamics is defined by:

[TABLE]

where $\psi_{i}\in\mathbb{R}$ is the angle between $\frac{\partial\bm{c}_{co_{i}}}{\partial a_{co_{i}}}$ and $\frac{\partial\bm{c}_{cf_{i}}}{\partial a_{cf_{i}}}$ (see Figure 2(b)).

It is important to note the chosen parameterizations must satisfy the following assumption for (30), (31) to be well-defined [13]:

Assumption 4**.**

The parameterizations are orthogonal such that $\frac{\partial\bm{c}_{f_{i}}}{\partial a_{cf_{i}}}^{T}\frac{\partial\bm{c}_{f_{i}}}{\partial b_{f_{i}}}=0$, $\frac{\partial\bm{c}_{o_{i}}}{\partial a_{o_{i}}}^{T}\frac{\partial\bm{c}_{o_{i}}}{\partial b_{o_{i}}}=0$, and $M_{cf_{i}},K_{cf_{i}},T_{cf_{i}},M_{co_{i}},K_{co_{i}},T_{co_{i}}$ are defined for all $\bm{\xi}_{cf_{i}}$ on the fingertip surface, and $\bm{\xi}_{co_{i}}$ on the object surface, respectively.

III-C Problem Formulation

The formal definition of the grasp constraint satisfaction problem is presented here.

Contact Force Set

No slip is ensured by guaranteeing that the contact forces remain inside the friction cone, $\mathcal{FC}$. A well-known technique is to approximate the friction cone by a pyramid, which results in the following linear constraint condition [12]:

[TABLE]

where $\Lambda(\mu)\in\mathbb{R}^{l_{s}n\times 3n}$ defines a pyramid of $l_{s}\in\mathbb{Z}_{>0}$ faces used to a approximate the friction cone [12], and $R_{cp}\in\mathbb{R}^{3n\times 3n}$ is the block diagonal matrix of all $R_{c_{i}p}$ for $i\in[1,n]$. Let the set of constraint admissible contact forces be defined as:

[TABLE]

Joint Angle Set

Over-extension of the joints is prevented by constraining the joint angles within a feasible workspace. The constraints on the joint angles are defined by the following box constraints:

[TABLE]

where $\bm{i}_{j}\in\mathbb{R}^{1\times m}$ is the $j$th row of $I_{m\times m}$ and $q_{\text{max}_{j}},q_{\text{min}_{j}}\in\mathbb{R}_{\geq 0}$ define the joint angle limits, which omit singular hand configurations. The set of constraint admissible joint angles is defined by:

[TABLE]

Contact Location Set

Excessive rolling occurs when the contact locations exceed the workspace of the fingertip. Many existing tactile sensors are designed as fingertips with flat, hemispherical, or other relatively simple geometric surface [48], which can be appropriately modeled with geometric parameterizations [13]. The benefit of the geometric modeling is not only that it can be applied to these fingertip shapes, but the fingertip workspace can be defined as box constraints:

[TABLE]

where $a_{\text{min}},a_{\text{max}},b_{\text{min}},b_{\text{max}}\in\mathbb{R}$ define the boundary of the fingertip surface. Each $h_{r_{j}}$ defines the box constraints such that if $h_{r_{j}}\geq 0,\forall j\in[1,4]$, then the contact point is in the fingertip workspace. The set of allowable contact locations for each contact is:

[TABLE]

and the full set of feasible contact locations is:

[TABLE]

Constraint-admissible States

Let $\mathscr{H}$, defined by:

[TABLE]

denote the set of grasp constraint admissible states. In the set $\mathscr{H}$, the hand configuration is non-singular, and, by Assumption 1, $J_{h}$ is full rank with rank $3n$. Furthermore, the contact points do not slip in $\mathscr{H}$ and so the grasp relation (26) holds. These properties of $\mathscr{H}$ are attractive, and exploited in related literature where the states are assumed to remain in $\mathscr{H}$ without any guarantee of such a claim [27].

The problem to be addressed is formally stated as:

Problem 1**.**

Suppose the hand-object system satisfies Assumptions 1-4, and consider the set of constraint-admissible states $\mathscr{H}$ defined by (41). Determine a control law that ensures forward invariance of $\mathscr{H}$.

III-D Proposed Grasp Constraint Satisfying Control

In the following section, the control input $\bm{u}_{k}$ is proposed to ensure no slip, no over-extension, and no excessive rolling are satisfied. Satisfaction of no over-extension and no excessive rolling constraints is addressed using the zeroing control barrier functions from Section II, while satisfaction of the no slip condition is developed as an extension of [3]. The input constraints are then combined to define the proposed controller to address Problem 1.

III-D1 Grasp Constraint Conditions

In practice, the robotic hand may have limited information about the object and/or may only have access to embedded sensors including joint angle senors and tactile sensors to gather information of the grasp. Here model uncertainties are taken into account to develop a controller that ensures forward invariance of the grasp constraint-admissible set $\mathscr{H}$. For notation, an approximation is denoted by $\hat{(\cdot)}$ and the error by $\Delta(\cdot)$. For example, the object inertia matrix is defined as $M_{o}=\hat{M}_{o}+\Delta(M_{o})$.

To develop a robust grasp constraint satisfying controller, the approach taken here is to exploit the robustness properties of zeroing control barrier functions presented in Section II. To do so, first the contact force, $\bm{f}_{c}$, must be analyzed with respect to model uncertainties as it defines the interaction between the hand and object. Differentiation of (26), and substitution of (21) and (23) provides an expression for the contact forces:

[TABLE]

where $B_{ho}=(J_{h}M_{h}^{-1}J_{h}^{T}+G^{T}M_{o}^{-1}G)$. Note that by Assumptions 1 and 2, $B_{ho}^{-1}$ is well defined for all states in $\mathscr{H}$. Furthermore, as with previous notation, $\hat{\bm{f}}_{c}$ will denote the approximate contact force, and $\Delta(\bm{f}_{c})$ denotes the error between the nominal and exact contact force.

From (42), it is clear that the contact force is affected by model uncertainties, which must be appropriately taken into consideration. In addressing robustness, note that complete lack of information of the system is extremely limiting, and a hard problem to address. For practical consideration, several simplifying assumptions are made, which are listed as follows:

Assumption 5**.**

The approximate friction coefficient satisfies: $\hat{\mu}\leq\mu(T)\leq\mu$, where $\mu(T)$ is the conservative friction coefficient to address inter-sampling behavior as discussed in [3].

Assumption 6**.**

All approximation errors, including $\Delta(\bm{f}_{c})$, are bounded.

Remark 4**.**

From Assumption 5, $\hat{\mu}$ defines the lower bound on the allowable contact friction. In practice, the designer should choose $\hat{\mu}$ for the specified task where a small $\hat{\mu}$ could be chosen to grasp a larger range of objects, including slippery objects such as ice, however more control effort will be required to do so. Assumption 6 is valid as contact forces never tend to infinity in practice.

In the following sections, constraints are defined on $\bm{u}_{k}$ to ensure $\mathscr{H}$ is forward invariant despite perturbations that result from model uncertainties.

Contact Force Constraint

Starting with the no slip condition, first note a property of the friction cone in relation to $\mu$ in (34). The use of the more conservative friction coefficient, $\hat{\mu}$, that satisfies Assumption 5 in (34) implies that (34) holds for the true friction coefficient, $\mu$. Thus $\hat{\mu}$ can be directly substituted for $\mu$ in (34) to ensure no slip.

Next, the model uncertainties are explicitly investigated to develop a robust means of ensuring no slip. To do so, let the approximate contact force be explicitly defined as:

[TABLE]

where $\hat{B}_{ho}=\hat{J}_{h}\hat{M}_{h}^{-1}\hat{J}_{h}^{T}+\hat{G}^{T}\hat{M}_{o}^{-1}\hat{G}$.

By substituting $\bm{f}_{c}$ with $\hat{\bm{f}}_{c}+\Delta(\bm{f}_{c})$, where $\hat{\bm{f}}_{c}$ is defined as in (43), and $\hat{\mu}$ for $\mu$ in (34), the following relation must hold to ensure no slip:

[TABLE]

From Assumption (6) it follows that the term $\Lambda(\hat{\mu})R_{cp}\Delta(\bm{f}_{c})$ is bounded. Thus to ensure satisfaction of (44), and thus (34), a tuning parameter $\varepsilon\in\mathbb{R}_{\geq 0}$ is chosen to be larger than the bound on $\Lambda(\hat{\mu})R_{cp}\Delta(\bm{f}_{c})$, which is incorporated in the following no slip constraint:

[TABLE]

The satisfaction of (45) by $\bm{u}$ thus ensures that the no slip condition (34) is satisfied for an appropriately chosen $\varepsilon$. Let the set of admissible control torques for $\mathscr{C}_{f}$ be $\mathscr{S}_{u_{f}}=\{\bm{u}\in\mathbb{R}^{m}\mathrel{\mathop{\ordinarycolon}}\eqref{eq:no slip constraint}\ \text{holds}\}$. This result is summarized in the following lemma:

Lemma 1**.**

Under Assumptions 1-6, there exists a $\varepsilon^{*}\in\mathbb{R}_{\geq 0}$ such that if $\bm{u}_{k}$ satisfies (45) for $\varepsilon>\varepsilon^{*}$, then \underaccent{\bar}{\mathscr{C}}_{f} is forward invariant.

Remark 5**.**

As discussed in [3], slip can occur if perturbations sufficiently increase the ratio of tangential to normal forces with respect to the contact surface. The use of $\varepsilon$ in (45) enforces a lower bound of $\varepsilon/\hat{\mu}$ on the normal force. Essentially this ensures the hand is squeezing the object hard enough to resist such disturbances. Also, the use of $\varepsilon$ also ensures robustness to sampling time effects [3].

Joint Angle Constraint

The zeroing control barrier functions from Section II are used here to guarantee that the hand joints remain inside a desired joint space to prevent over-extension. First, robustness margins are incorporated into the functions $h_{q}$, and a conservative \underaccent{\bar}{\mathscr{C}}_{q} is defined:

[TABLE]

[TABLE]

where $\delta_{q\text{max}_{j}},\delta_{q\text{max}_{j}}\in\mathbb{R}_{\geq 0}$ are the robustness margins.

Similarly, the following zeroing control barrier functions are defined with robustness margins to prevent over-extension:

[TABLE]

where $\alpha_{1}(h)$ is a twice continuously differentiable, extended class-$\mathcal{K}$ function, and $\beta_{q\text{max}_{j}},\beta_{q\text{min}_{j}}\in\mathbb{R}_{\geq 0}$ define the robustness margins. Let \hat{\underaccent{\bar}{\mathscr{B}}}_{q} be defined by:

[TABLE]

The zeroing control barrier functions are applied to the dynamics of $\bm{q}$ where the control input appears, namely $\ddot{\bm{q}}$. However (21) does not fully represent the effect of $\bm{u}$ as the contact forces, $\bm{f}_{c}$, are also dependent on $\bm{u}_{k}$ as shown in (42). Substitution of (42) in (21) define the proper system dynamics for the constraint set \underaccent{\bar}{\mathscr{C}}_{q}, which is omitted here for brevity.

Following Theorem 2, the following constraint is defined to ensure forward invariance of \underaccent{\bar}{\mathscr{C}}_{q}:

[TABLE]

where $\hat{A}_{q}\in\mathbb{R}^{2m\times m}$ and $\hat{\bm{b}}_{q}\in\mathbb{R}^{2m}$ are defined in the Appendix.

Let the set of admissible control torques for \hat{\underaccent{\bar}{\mathscr{C}}}_{q} be \hat{\underaccent{\bar}{\mathscr{S}}}_{u_{q}}=\{\bm{u}_{k}\in\mathbb{R}^{m}\mathrel{\mathop{\ordinarycolon}}k\in[1,N]\mathrel{\mathop{\ordinarycolon}}\hat{A}_{q}\bm{u}_{k}\geq\hat{\bm{b}}_{q}+\hat{\nu}_{q}\bm{1}\}, for the sampling time margin $\hat{\nu}_{q}\in\mathbb{R}_{\geq 0}$.

Contact Location Constraint

The zeroing control barrier functions are also used to ensure the contact points remain in the fingertip workspace. Robustness margins are incorporated into $h_{r_{l}}$ and used to define the conservative $\mathscr{C}_{r}$:

[TABLE]

[TABLE]

where $\delta_{r_{l}}\in\mathbb{R}_{\geq 0}$ define the robustness margins for $l\in[1,4]$.

Let the robust zeroing control barrier functions to prevent excessive rolling be defined by:

[TABLE]

where $\beta_{r_{l}}\in\mathbb{R}_{\geq 0}$ defines the robustness margins for $l\in[1,4]$. Let \hat{\underaccent{\bar}{\mathscr{B}}_{r}} be defined by:

[TABLE]

The zeroing control barrier functions are applied to the dynamics of $\bm{\xi}_{cf_{i}}$ where the control input appears, namely $\ddot{\bm{\xi}}_{cf_{i}}$. The derivation of $\ddot{\bm{\xi}}_{cf_{i}}$ is quite involved and is broken down into the following steps. For ease of notation let $H_{i}\mathrel{\mathop{\ordinarycolon}}=H(\bm{\xi}_{cf_{i}},\bm{\xi}_{co_{i}},\psi_{i})$ be defined by:

[TABLE]

First, $J_{s_{i}}E_{i}\dot{\bm{q}}$ is substituted for $\bm{\omega}_{f_{i}}$ in (30), which is then differentiated, resulting in :

[TABLE]

Further substitution of (21) and (23) into (56) results in:

[TABLE]

Finally, the contact force (42) is then substituted into (57), which is omitted here for brevity. Following the approach from Theorem 2, the following condition must be satisfied to ensure forward invariance of \underaccent{\bar}{\mathscr{C}}_{r}:

[TABLE]

where $\hat{A}_{r}\in\mathbb{R}^{4n\times m}$, $\hat{\bm{b}}_{r}\in\mathbb{R}^{4n}$ are defined in the Appendix. Let the set of admissible control torques for ensuring the contacts remain inside the fingertip workspace be \hat{\underaccent{\bar}{\mathscr{S}}}_{u_{r}}=\{\bm{u}_{k}\in\mathbb{R}^{m}\mathrel{\mathop{\ordinarycolon}}k\in[0,N]\mathrel{\mathop{\ordinarycolon}}\hat{A}_{r}\bm{u}_{k}\geq\hat{\bm{b}}_{r}+\hat{\nu}_{r}\bm{1}\}, for the sampling time margin $\hat{\nu}_{r}\in\mathbb{R}_{\geq 0}$.

Actuator Constraints

Finally, actuators are limited to a finite actuation range in real-life applications. To ensure the proposed controller is implementable on real systems the following actuator constraint is defined:

[TABLE]

where $\bm{u}_{\text{min}},\bm{u}_{\text{max}}\in\mathbb{R}^{m}$ denote the minimum and maximum allowable torque values, respectively. Let the set of bounded control torques be \underaccent{\bar}{\mathscr{S}}_{u_{\tau}}=\{\bm{u}_{k}\in\mathbb{R}^{m}\mathrel{\mathop{\ordinarycolon}}k\in[0,N]\mathrel{\mathop{\ordinarycolon}}\bm{u}_{\text{min}}\leq\bm{u}_{k}\leq\bm{u}_{\text{max}}\}.

Proposed Control

Thus far, the constraints (45), (50), and (58) have been presented to prevent slip, joint over-extension, and excessive rolling, respectively. The proposed control admits a nominal manipulation controller, $\bm{u}_{\text{nom}}\in\mathbb{R}^{m}$, and outputs a control torque that minimizes $||\bm{u}-\bm{u}_{\text{nom}}||^{2}$, while adhering to the grasp constraints (45), (50), (58), and (59). The proposed control is:

[TABLE]

where $\hat{A}_{k}$ and $\hat{\bm{b}}_{k}$ are the concatenations of the constraints (45), (50), and (58) evaluated at the sampling time $t=kT$, and $\hat{\nu}_{h}=\text{max}\{\hat{\nu}_{q},\hat{\nu}_{r}\}$

Let the set of grasp constraint admissible control torques be:

[TABLE]

The following assumption is made to ensure that a solution to (61) exists as per Theorem 3:

Assumption 7**.**

The set of grasp constraint admissible control torques \hat{\underaccent{\bar}{\mathscr{S}}}_{u} is non-empty.

The following theorem guarantees forward invariance of $\mathscr{H}$ despite model uncertainties. For ease of notation, let $\bm{c}$ be the concatenation of the robustness margin terms $\delta_{q\text{min}_{j}}$,$\delta_{q\text{max}_{j}}$,$\delta_{r_{l}}$, $\beta_{q\text{min}_{j}}$,$\beta_{q\text{max}_{j}}$,$\beta_{r_{l}}$ for all $j\in[1,m],l\in[1,4]$.

Theorem 4**.**

Suppose Assumptions 1-7 hold for the controllable system (21). For twice continuously differentiable, extended class-$\mathcal{K}$ function $\alpha_{1}$, and locally Lipschitz extended class-$\mathcal{K}$ function $\alpha_{2}$, suppose that \underaccent{\bar}{\mathscr{C}}_{f}, \hat{\underaccent{\bar}{\mathscr{B}}}_{q}\bigcap\hat{\underaccent{\bar}{\mathscr{C}}}_{q}, \hat{\underaccent{\bar}{\mathscr{B}}}_{r}\bigcap\hat{\underaccent{\bar}{\mathscr{C}}}_{r} are non-empty, and \bm{f}_{c}(0)\in\underaccent{\bar}{\mathscr{C}}_{f}, (\bm{q}(0),\dot{\bm{q}}(0))\in\hat{\underaccent{\bar}{\mathscr{B}}}_{q}\bigcap\hat{\underaccent{\bar}{\mathscr{C}}}_{q}, $\bm{\xi}_{f}(0)$, \dot{\bm{\xi}}_{f}(0))\in\hat{\underaccent{\bar}{\mathscr{B}}}_{r}\bigcap\hat{\underaccent{\bar}{\mathscr{C}}}_{r}. Then there exists $\hat{\nu}_{h}$, $\varepsilon$, $\bm{c}$ such that $\bm{u}_{k}^{*}$ from (60) applied to (21) ensures $(\bm{f}_{c},\bm{q},\bm{\xi}_{cf})$ remains in $\mathscr{H}$ for $t\in[0,NT)$.

Proof.

By Assumptions 1-4, and for $(\bm{f}_{c},\bm{q},\bm{\xi}_{f})\in\mathscr{H}$, the constraints (45), (50), and (58) are well defined. By Lemma 1, there exists a $\varepsilon$ such that $\bm{f}_{c}$ remains in \underaccent{\bar}{\mathscr{C}}_{f} for all $t\in[0,NT)$. By Assumption 3 and construction of $h_{q_{j}}$, $h_{r_{j}}$, all terms in (45), (50), (58), (59) are locally Lipschitz continuous between sampling periods. Furthermore, by Assumption 7, the constraint set is feasible for all $t\geq 0$, and the sets \underaccent{\bar}{\mathscr{B}}_{q}, \underaccent{\bar}{\mathscr{C}}_{q}, \underaccent{\bar}{\mathscr{B}}_{r}, and \underaccent{\bar}{\mathscr{C}}_{r} are compact by construction. Thus the conditions of Theorem 3 are satisfied such that there exist $\hat{\nu}_{h}$, $\bm{c}$, such that $\bm{q}$ remains in \underaccent{\bar}{\mathscr{C}}_{q}, and $\bm{\xi}_{cf}$ remains in \underaccent{\bar}{\mathscr{C}}_{r} for all $t\in[0,NT)$, and the proof is complete. ∎

IV Results

The proposed zeroing control barrier functions have been presented with guarantees of forward invariance of the constraint set in the presence of sampling and external perturbations. The zeroing control barrier functions were then applied to address the challenging problem of robotic grasping to ensure that the object does not slip, the hand does not exceed joint limits, and the contact locations do not exceed the workspace of the fingertips. In this section, the proposed control is applied to robotic grasping and is implemented in simulation and hardware to demonstrate robust constraint satisfaction.

Robust constraint satisfaction in this context refers to satisfaction of constraints with uncertain model parameters. This is to demonstrate the capability of the proposed method to a wide range of grasping scenarios in which intimate knowledge of the hand-object properties are unavailable. Here, tactile-based blind grasping is addressed in which the controller only has access to the contact location, $\bm{p}_{fc}$, and joint angle positions/velocities, $\bm{q}$, and $\dot{\bm{q}}$. The remaining object parameters are approximated as follows. The approximate object center of mass, $\hat{\bm{p}}_{o}$ and orientation $\hat{\bm{\gamma}}_{o}$ are defined by the virtual frame [3, 19, 49]. The approximations $\hat{\bm{p}}_{o}$ and $\hat{\bm{\gamma}}_{o}$ are used to compute the approximations $\hat{G}$, $\hat{M}_{o}$, and $\hat{C}_{h}$ terms. Nominal object parameters for object mass, $m_{o}$ and inertia $I_{o}$ are then arbitrarily defined, as will be discussed in the proceeding section.

The contact model is addressed by conservatively approximating the local object surface as flat. Note this approximation only holds if the fingertip surfaces are locally curved about the contact point. By approximating the object locally as flat, the contact kinematic parameters simplify to $\hat{K}_{co_{i}}=0_{2\times 2},\hat{M}_{co_{i}}=I_{2\times 2},\hat{T}_{co_{i}}=0_{1\times 2}$. This not only simplifies the computation of $\hat{A}_{r}$, $\hat{\bm{b}}_{r}$, but also introduces robustness because flat object surfaces result in the largest contact point displacement for the same given rolling angular velocity.

IV-A Simulation Results

The purpose of this simulation is to demonstrate constraint satisfaction via the proposed constraint satisfying controller. These results will demonstrate that the proposed control (60) can ensure grasp constraint satisfaction for a given nominal control despite model uncertainties.

The hand used in the simulations is a nine degree of freedom, fully-actuated hand with three identical fingers and hemispherical fingertips. The maximum control torque is $3.5$ Nm. The object being grasped is a cube, and the model parameters of the hand-object are found in Table I with the initial hand-object configuration shown in Figure 3.

The nominal manipulation control used here is the manipulation control presented from [3] with the internal force control [50] which is re-defined here as:

[TABLE]

[TABLE]

[TABLE]

Note the saturation function is used to anticipate integrator wind-up should the proposed control $\bm{u}_{k}^{*}$ from (60) diverge from $\bm{u}_{nom}$.

Implementation of the contact location constraints requires appropriate parameterizations of the fingertip surface to satisfy Assumption 4. The parameterization used here is $\bm{c}_{f_{i}}=[-R\cos(a_{f_{i}})\cos(b_{f_{i}}),R\sin(a_{f_{i}}),-R\cos(a_{f_{i}})\sin(b_{f_{i}})]^{T}$. The associated box constraints to define the fingertip workspace are: $-\pi/2<a_{f_{i}}<\pi/2$, $-\pi<b_{f_{i}}<0$. The joint angle limits for each finger are $\bm{q}_{\text{max}_{i}}=(3\pi/4,\pi/3,3\pi/4)$, $\bm{q}_{\text{min}_{i}}=(0,-\pi/3,0)$, for $i\in[1,3]$.

The control gains used in the simulation are $K_{p}=0.26I_{6\times 6}$, $K_{i}=0.1I_{6\times 6}$, $K_{d}=0.125I_{6\times 6}$, and $k_{f}=1.0$. The set-point object reference command is $\bm{r}=\bm{x}(0)+(0,0,0,0,0,\pi/2)$, where $\bm{x}\in\mathbb{R}^{6}$ is the task state defined by the virtual frame [3]. The proposed constraint satisfying control (60) is implemented with the following robustness margins: $\varepsilon=0.03$, $\delta_{r_{l}}=0.1$ rad, $\beta_{r_{l}}=1.0$ rad/s, $\delta_{q_{i}}=0.1$ rad, $\beta_{q_{i}}=0.05$ rad/s. The extended class-$\mathcal{K}$ functions used in this simulation were $\alpha_{1}(h)=2h$ and $\alpha_{2}(h)=h^{3}$. The approximate object model parameters are purposefully offset from the true mass parameters. The object mass error was set to $\Delta(m)_{o}=0.1$ kg, inertia error set to $\Delta(I)_{o}=0.001I_{3\times 3}\ \text{kg m}^{2}$. The sampling time margin was set to $\hat{\nu}_{h}=0.0001$. A four-sided pyramid was used to approximate the friction cone with an associated friction coefficient of $\hat{\mu}=0.64$. The simulations were implemented in Matlab 2018 with a sampling time of $T=0.003$ s.

Figure 4 shows the results of the nominal control as it attempts to track the set-point reference command by twisting the object about the $Z$-axis. The plots show multiple constraint violations including slip, joint over-extension, and excessive rolling, which result in a failed grasp. At $t=0.928$ s, the contact location $b_{cf_{2}}$ exceeded the fingertip surface and the simulation stopped. Figures 4(e) and 4(f) show the reference error for the $Z$ component of the orientation error and final grasp configuration. It is important to emphasize that the final orientation of $1.394$ rad shown in Figure 4(e) is not feasible due to slip (see Figure 4(d)), singular configurations (see Figure 4(c) at $t=0.461$ s) and joint limits that were exceeded (see Figure 4(b) at $t=0.498$ s) during the manipulation motion, prior to the excessive rolling that stopped the simulation. These results demonstrate that the conventional assumption that the grasp conditions hold are not valid and may result in grasp failure despite using a stable manipulation controller.

Figures 5 shows the nominal control implemented with the proposed control (60). The resulting plots show that the proposed control is able to ensure the grasp states remain in $\mathscr{H}$ despite the model uncertainty from the tactile-based blind grasping implementation. Figure 5(e) shows the resulting tracking error of the proposed control. To prevent grasp failure, the proposed control prevents the hand-object system from reaching the infeasible reference command.

Remark 6**.**

One important note is that the proposed control is effectively a disturbance on the nominal control input when the proposed control must intervene to satisfy the grasp constraints. Although the proposed control ensures grasp constraint satisfaction, there is yet no analysis of how the nominal control will behave. For implementation, it may be advantageous for the nominal controller to be passive to avoid undesired manipulation motion. Fortunately, many tactile-based blind grasping controllers are passivity-based [27]. Further analysis is required to address passivity of the proposed control (60).

IV-B Hardware Results

The purpose of the hardware implementation is to demonstrate the effectiveness of the proposed control in practice. This is done by performing three demonstrations in which the nominal manipulation control used in Section IV-A is implemented along with the proposed control (60). In the first demonstration, a feasible reference is provided to the proposed control to show that when no constraint violation occurs, the proposed control admits the original nominal control law. In the second demonstration, a compromising reference is provided to the nominal control to show that when the grasp constraints are not formally addressed, instabilities may occur in the hand-object system that result in grasp failure. In the final demonstration, the same compromising reference is provided to the proposed control to show how the proposed method ensures constraint satisfaction.

The Allegro Hand is used to implement the controllers and is depicted in Figure 6. The Allegro Hand is a fully-actuated hand with 16 degrees of freedom, and with encoders at each joint to provide measurements of $\bm{q}$. The maximum torque output of each motor is $0.65$ Nm. Tactile sensors are emulated to provide approximate contact measurements of $\bm{\xi}_{cf}$ to the proposed controller. Again, the implementation of the proposed control is as a tactile-based blind grasping scheme in which only measurements of $\bm{q}$, $\bm{\xi}_{cf}$ are available. Thus robustness in this context refers to uncertainty in the object model and state. The experimental setup includes a NI USB-8473s High-Speed CAN with a fixed sampling time of $T_{s}=0.003s$. The model parameters used for the Allegro Hand are shown in Table II.

The nominal object used to implement the proposed control is a cube of side lengths $0.04$ m and mass of $\hat{m}_{o}=0.05$ kg. The true object used in the experiments is a sphere of radius $0.0375$ m and mass of $0.09$ kg. Note the difference in mass and shape also correspond to discrepancies in the object inertia.

The nominal control gains used in the experiment are $K_{p}$ = diag(500, 500, 500, 0.8, 0.8, 0.8), $K_{i}=$ diag(50, 50, 50, 0.6, 0.6, 0.6), $K_{d}=$ diag(0.008, 0.008, 0.008, 0.16, 0.16, 0.16), and $k_{f}=60$. The set-point object reference command is $\bm{r}=\bm{x}(0)+(0,0,0,0,0,r_{\psi})$, where $r_{\psi}\in\mathbb{R}$ and the same virtual frame from [3] is used to define $\bm{x}$. The proposed constraint satisfying control (60) is implemented with the following robustness margins: $\hat{\nu}_{h}=0.01$, $\varepsilon=0.15$, $\delta_{r_{j}}=0.10$ rad, $\beta_{r_{j}}=0.10$ rad/s, $\delta_{q_{j}}=0.05$ rad, $\beta_{q_{j}}=0.10$ rad/s, $j\in[1,l]$. The extended class-$\mathcal{K}$ functions used were $\alpha_{1}(h)=3.3h$ and $\alpha_{2}(h)=10h^{3}$. A four-sided pyramid was used to approximate the friction cone with associated friction coefficient of $\hat{\mu}=1.06$. The same contact parameterizations from Section IV-A were used here for the hemispherical fingertips. Note, in the following figures, the gray regions depict the area outside of the constraint admissible set.

Figure 7 shows the results of the first demonstration in which the proposed control (60) is implemented with the nominal control for the feasible reference, $r_{\psi}=0.3\pm 0.06$ rad. Figure 7(c) shows the $\psi$ component of the state $\bm{x}$ reach within the reference tolerance for a successful manipulation, and the final configuration is depicted in Figure 7(b). The proposed control torque (see Figure 7(e) closely matches the nominal control torque (see Figure 7(d)). This demonstration illustrates how the proposed control admits the nominal controller to achieve the desired manipulation motion with limited interference, and successful manipulation. The following demonstration investigates the use of the nominal control alone to reach a compromising reference command.

In the second demonstration, shown in Figure 8, the nominal control alone is implemented for the compromising reference $r_{\psi}=0.7\pm 0.06$ rad. The plots clearly depict the unstable behavior of the system as the nominal control attempts to reach the reference. Figure 8(b) shows the unstable configuration of the hand that results in loss of contact and grasp failure. Note, Figure 8(d) shows the nominal control exceeding the actuation capabilities of the hand. This demonstration shows that when no proposed control is implemented, the nominal control is subject to instabilities and ultimately grasp failure for a compromising reference. The final demonstration will investigate how the proposed control compensates for this compromising reference.

Figure 9 shows the results of the final demonstration in which the proposed control (60) is implemented for the same compromising reference of $r_{\psi}=0.7\pm 0.06$ rad. Figure 9(c) shows the $\psi$ component of $\bm{x}$ reaching a steady-state value outside of the reference tolerance, with the final configuration shown in Figure 9(b). The reason for this steady-state offset is that the proposed control prioritizes constraint violation over implementation of the nominal control. Figures 9(e) and 9(f) show the deviation between nominal and proposed control torque as the proposed control intervenes to ensure constraint satisfaction. Figure 9(d) shows the trajectory of $\bm{a}_{cf_{i}}$, an element of $\bm{\xi}_{cf}$, as $\bm{a}_{cf_{1}}$ approaches the constraint boundary of \underaccent{\bar}{\mathscr{C}}_{r}. The plots show that the proposed control prevents $\bm{\xi}_{cf}$ from exceeding the constraint set \underaccent{\bar}{\mathscr{C}}_{r} to enforce grasp constraint satisfaction in the presence of model uncertainty and sampling time effects.

V Conclusion

In this paper, a novel control barrier function formulation was developed to ensure constraint satisfaction for mechanical systems. The proposed method is robust to external perturbations and sampling time effects, and provides a systematic method of bounding the system’s velocity near the constraint boundary. The proposed method was then applied to the challenging problem of robotic grasping. A novel controller was proposed to ensure no slip, no over-extension (e.g. singular configurations), and no excessive rolling occurs while admitting an existing controller from the literature. The proposed control was implemented in simulation and hardware to demonstrate the efficacy of the proposed approach.

VI Appendix

The joint constraint-related terms $A_{q}$ and $b_{q}$ are:

[TABLE]

[TABLE]

where

[TABLE]

[TABLE]

The contact location constraint-related terms $A_{r}$ and $b_{r}$ are:

[TABLE]

where

[TABLE]

[TABLE]

[TABLE]