Energy based Control Barrier Functions for Robotic Systems

Control barrier function (CBF) based Quadratic Programs (QPs) were introduced in early 2014 as a means to guarantee safety in affine control systems in conjunction with stability/tracking. However, due to the presence of modelbased terms, they fail to provide guarantees under model perturbations. Therefore, in this paper, we propose a new class of CBFs for robotic systems that augment kinetic energy with the traditional forms. We show that with torque limits permitting, and with the kinematic models accurately known, forward invariance of safe sets generated by kinematic constraints (position and velocity) can be guaranteed. The proposed methodology is motivated by the control Lyapunov function (CLF) based QPs that use the kinetic energy function. By the property of CBF-QPs, we show that the pointwise min-norm control laws obtained are feasible and Lipschitz continuous, and can be derived analytically via the KKT conditions. In order to include stability with safety, we also augment CLF based constraints in the CBF-QPs to realize a unified control law that allows tracking with safety irrespective of the inertial parameters of the robot. We will demonstrate the robustness of this class of CBF-QPs in two robotic platforms: a 1-DOF and a 2-DOF manipulator, by scaling the masses by up to 100, and then simulating the resulting dynamics.


I. INTRODUCTION
Due to the ever increasing requirement for safety, there is a push for realizing real-time control laws that guarantee safety limits in robotic systems. Some of the examples include position limits, angle and velocity limits of joints of manipulators, hip velocity of bipeds, velocity limits of autonomous vehicles and other platforms. Enforcing these limits in the form of constraints in quadratic programs (QPs) is one of the popular choices used currently [1], [22]. They are realized via the control barrier functions (CBFs) [2], and the constraints obtained from these functions are necessary and sufficient for set-invariance. However, they are not robust to modeling uncertainties. In other words, the safety constraints are violated even for a small change in the model parameters.
In order to improve robustness to modeling and sensing uncertainties, different types of QP based control laws have been proposed over the last few years: using adaptive techniques [18], learning techniques [19], and also using the notion of input-to-state safety [12]. With a view toward providing robustness in robotic systems, [16] demonstrated a novel approach in QPs that can handle both stability and safety in the presence of high levels of model uncertainty. In *This work is supported by DST INSPIRE Fellowship IFA17-ENG212, and the Robert Bosch Centre for Cyber Physical Systems. 1 S. Kolathaya is an Assistant Professor of the Robert Bosch Centre for Cyber Physical Systems (RBCCPS), and the Department of Computer Science and Automation (CSA), Indian Institute of Science, Bengaluru, India shishirk at iisc.ac.in particular, under the assumption of bounded uncertainty, the proposed controller uses two constraints to strictly guarantee safety. This was demonstrated in dynamic walking of a bipedal robot while carrying an unknown load, subject to precise foot-step location constraints. However, as this uncertainty is state-dependent, its upper bound can dynamically change with the state values. While this limitation can be overcome by choosing a compact statespace, our goal is to generalize this formulation to obtain a robust QP that has no state dependent bounds.
The approach used in this paper is mainly based on the fact that robotic systems have specific properties that can be utilized. Unlike a general nonlinear dynamical system, we know that robotic systems have kinetic energy. With this property, we construct a new class of control barrier functions (CBFs) that enable us to construct robust QPs for ensuring safety for a broad range of model parameter perturbations. Additional properties are also utilized, and are described in the paper (see Properties 1 and 2 further ahead). We will study two types of constraint functions a) that depend on the configuration q, called the position based constraints, and b) that depend on both configuration and velocity q,q, called the velocity based constraints. Examples for position based constraints are space limits, angle limits, and task space position constraints. Examples for velocity based constraints are velocity limits and task space velocity constraints. These constraints are incorporated in the reciprocal form of CBFs described in [2]. The resulting min-norm QP with the CBF based constraint has a closed form solution that is feasible and Lipschitz continuous w.r.t. the state. In addition, we augment CLF based constraints with this class of QPs to yield a multi-objective control law that combines tracking and safety.
It is worth noting that there is a specific class of Lyapunov functions widely used for establishing stability results for PD controlled robotic systems [3], [10]. Specifically, these Lyapunov functions have kinetic energy, which allow for a model-free QP formulation for a broad range of tracking applications [13]. It was shown in [13] that ultimate boundedness with desirable bounds can be guaranteed even if the inertial parameters are scaled by 200%. This was demonstrated for a broad class of robotic systems including the cart-pole, manipulators, and bipeds. Motivated by this observation, in this paper, we explore this idea of augmenting kinetic energy with control barrier functions (CBFs) to realize robust CBF based QPs. The traditional form of CBFs, called the reciprocal control barrier functions shown in [2] are augmented with the kinetic energy terms to realize a new class of CBFs for robotic systems.
The paper is structured as follows. We will start with a brief description of the robot model along with its properties in Section II. In the same section, we will also provide a brief preliminary on control barrier functions (CBFs) used for nonlinear dynamical systems. Specifically, reciprocal control barrier functions (RCBFs) introduced in [1] will be studied in detail. Having described the RCBFs, we will make extensions for robotic systems by including the kinetic energy or its variant in Section III and Section IV. Specifically, we define this new class of CBFs for position (relative degree two) constraints in Section III, and for velocity (relative degree one) constraints in Section IV. Augmentation of the CLF based terms with CBF based terms to yield robust QPs for ensuring both stability and safety will be discussed in Section V. Finally, in Section VI, these QPs will be validated on two robot models in simulation: a 1-DOF and a 2-DOF manipulator.
Notation. Let R denote the set of real numbers and R n denote the Euclidean space of dimension n. An open Euclidean ball of radius r > 0 centered at x ∈ R n is denoted by B r (x).
For any x ∈ R n , the Euclidean norm is denoted by |x|, and for any matrix A ∈ R n×m , the matrix norm is represented by A . Given a symmetric matrix A ∈ R m×m , we denote its minimum and maximum eigenvalues as λ min (A), λ max (A) respectively. A continuous function α : R ≥0 → R ≥0 is said to belong to class K if it is strictly increasing and α(0) = 0. We say that a function h : R n → R m is locally Lipschitz at x ∈ R n if there exist constants L > 0 and r > 0 such that h(y) − h(x) ≤ L y − x for all y ∈ B r (x).

II. ROBOT MODEL AND SAFETY CRITICAL CONTROL
The main goal of this section is to study the generic formulations of control barrier functions for robotic systems, and the associated quadratic programming (QP) formulation that ensures forward invariance of a safe set. We will start with the modeling of a robotic system.

A. Robot model
Consider an n-DOF fully actuated rigid robotic system with the configuration manifold Q. Let the configuration of the robot be denoted by q ∈ Q, and letq ∈ T q Q be the rateof-change of the configuration q. Further, let the state of the system be denoted by x := (q,q) ∈ T Q, where T Q is the tangent bundle of Q. Then, the Euler-Lagrangian dynamics of the robotic system is given by where D(q) ∈ R n×n is the positive definite inertia matrix, C(q,q) ∈ R n×n is the Coriolis-centrifugal matrix, G(q) ∈ R n is the vector of gravity terms, u ∈ U = R n is the vector of control inputs. The dynamics (1) can alternatively be expressed in the state-space form aṡ where f , g are appropriately obtained. We assume that the choice of q is such that the mapping of the control inputs to the coordinates is one-to-one, i.e., the inputs are noninteracting. We have the following properties for D,C, G [5]: Property 1: D is symmetric positive definite, andḊ − 2C is skew-symmetric. In addition, there exist positive constants c l , c u , such that for any (q,q) ∈ T Q, The class of robots that satisfy this property is described in [5], [6]. For example, this is true for serial manipulators with all of their prismatic joints preceding the revolute joints. Even for the prismatic joints, like in spring deflections, we know that these deflections are usually restricted by hardstops. This allows us to include a larger class of mechanical systems. Proof of this property can be found in [5] (for D), [7] (for C), and in [6] (for G). Given the robotic system described by (1) along with the associated properties, we know that the kinetic energy is The main focus in this paper is to use this energy function for construction of robust safety-critical control laws. To this end, we will now describe the notion of safety.

B. Safety as forward invariance of a set
For a Lipschitz continuous control law k : T Q → R n , we obtain the closed loop dynamics of (2) aṡ Given an initial condition x 0 := x(t 0 ) ∈ T Q, there exists a maximum time interval I(x 0 ) = [t 0 ,t max ) such that x(t) is the unique solution to (4) on I(x 0 ); in the case when (4) is forward complete, t max = ∞. A set S ⊂ R n is forward invariant w.r.t. (4) if for every x 0 ∈ S, x(t) ∈ S for all t ∈ I(x 0 ). If S is forward invariant, then we call the set S safe. Given a closed set C ⊂ T Q, we determine conditions such that it is forward invariant. C is defined as where h : T Q → R is a continuously differentiable function. It is assumed that Int(C) is non-empty and C has no isolated points, i.e., Int(C) = / 0, and Int(C) = C. When the set C is forward invariant under the natural dynamics of the system,ẋ = f (x), we are interested in the controller, k : T Q → R n , that can be specified that will ensure invariance of C. We call this controller a safeguarding controller w.r.t. the set C. We can obtain a suitable safeguarding controller via control barrier functions (CBFs), which is explained next.

C. Control barrier functions
For the set C, we have the following definition of a reciprocal control barrier function (RCBF) [2].
Definition 1: Given a set C ⊂ T Q defined by (5)-(7) for a continuously differentiable function h : T Q → R, the function B : Int(C) → R is called a reciprocal control barrier function (RCBF) defined for the set C, α 1 , α 2 , α 3 ∈ K [0,a) such that for all x ∈ Int(C), Here L f B, L g B are the Lie derivatives of B w.r.t. f , g. This definition is obtained from [9] and it is assumed that there are no restrictions on u. Note that, in the original definition of RCBF in [2], it was sufficient to satisfy the following inequality: instead of the condition (9). The strict inequality sign in (9) is essential to show that the universal formulas [4, 4.19] based on such an RCBF are continuous, and this is independent of the non-strict constraint inequalities typically used in the QP formulation. More details on the relationship between the QPs and the continuity requirements are provided in [9]. Given the RCBF of the form, we know by [4, 4.19] that the following QP: not only yields a Lipschitz continuous control law, but also renders the set Int(C) forward invariant [2, Corollary 1]. This control law is, in fact, known as the pointwise min-norm control law, and can be obtained in closed form as: It is worth noting that there is one more class of functions defined in [2] called the zeroing control barrier functions (ZCBFs), which will not be studied in this paper. We are studying only RCBFs in the paper, which will henceforth be called simply as a control barrier function (CBF) B.

III. SAFETY OF POSITION BASED CONSTRAINTS
In this section, we will study position based barrier functions, which are obtained from relative degree two constraint functions h with ∂ h(x) ∂q ≡ 0 1 . As a prelude to the main result, we will first establish the following.
Lemma 1: Given a set C ⊂ T Q defined by (5)-(7) for a continuously differentiable function h : T Q → R with relative degree two, i.e., h is two times differentiable and L g h(x) ≡ 0, 1 Some of the velocity based constraints are relative degree two e.g., nonholonomic systems. This type of constraints will be studied elsewhere. ∀ x ∈ Int(C), then for a constant H max > 0, and a continuously differentiable function H : the function B p : Int(C) → R defined by is an RCBF defined for the set C.
Lemma 1 is obtained by slightly modifying [2, Proposition 4], which was mainly used for high relative degree constraint functions. Our modification is specific to the class of robotic systems, where the configuration velocityq is used. The proof is provided below.
Proof: Proof is similar to [2, Proof of Proposition 4]. For all x ∈ Int(C), we have that and thus 1 where α 1 (ξ ) := ξ and are both class K functions. Thus condition (8) is satisfied. We also have that for all x ∈ Int(C). This completes the proof.
As mentioned previously, a suitable candidate for H is obtained by using the kinetic energy function E(x) (3): and it can be verified that both the conditions (12), (13) are satisfied. Therefore the function B p (14) with H defined by (19) is a valid RCBF. More importantly, the derivative of B p simplifies to the following: and sinceḂ p (q,q, u) satisfies (9), we know that the resulting min-norm control law (QP) renders the set Int(C) safe (i.e., forward invariant). It is worth noting that (20) does not require the knowledge of the Coriolis-centrifugal matrix C. Therefore, with the knowledge of the inertia and gravity terms, safety guarantees can be provided. Since our ultimate goal is realize a model-free QP, we can replace E(x), G(q) by their extreme values: (21) which are obtained by using Property 1. Based on the sign oḟ q T u, appropriate bounds (lower or upper) must be substituted in (20). This results in two constraints similar to (QP) instead of one. This formulation is, in fact, similar to [17, eqns. (37), (38)], where a single constraint function with unknown terms is replaced with two constraints with their extreme values. We have the following two inequalities: where Accordingly, we define the point (state) to set mapping with these two constraints as We will now show that any control k(x) belonging to the set K pcbf (x) guarantees safety of Int(C).
Theorem 1: Given a set C ⊂ T Q defined by (5)-(7) for a continuously differentiable function h : R n → R with relative degree two, then for all classes of Lipschitz continuous control laws k : T Q → R ≥0 belonging to the set K pcbf given by (25), the set Int(C) is forward invariant.
Proof: Given the control law k(x) ∈ K pcbf (x), the goal is to show thatḂ Ifq = 0, then the inequality (26) is satisfied. Whenq = 0, then we have the following two cases:q T k(x) > 0 anḋ q T k(x) ≤ 0. For the former case, we have thaṫ and similar procedure follows for the latter case. This competes the proof.
Theorem 1 shows that the set K pcbf yields the class of controllers that that guarantee forward invariance of Int(C). However, this theorem does not yield a Lipschitz continuous control law k(x) explicitly. To this end, we have the following result.
Corollary 1: The QP of the form is feasible and yields a Lipschitz continuous control law. In addition, the control u * (x) obtained from (PQP) renders the set Int(C) forward invariant.
Note that only one constraint is used in the QP above. Hence, Corollary 1 shows that one only model-free constraint is sufficient to obtain a Lipschitz continuous control law that belongs to the set K pcbf . The proof is given below.
To establish the last part, we first note that ψ 01 , ψ 11 only differ by the scaling. In addition, based on (24), we know that ψ 01 u * > ψ 11 u * > 0. This implies that if (PC) is active then the first constraint in the set K pcbf is satisfied when ψ 00 > 0. Hence, both the constraints in the set K pcbf are satisfied. This completes the proof.

IV. SAFETY OF RELATIVE DEGREE ONE CONSTRAINTS
Having established a model-free QP for position constraints (PQP), we will now focus our attention on velocity constraints, i.e., relative degree one constraints. The key methodology is to obtain appropriate energy type functions for velocity constraints. For example, if a velocity constraint is imposed on one of the joints: q 1 (say), we then have the constraint function as where v d is the maximum limit on the velocity. It is worth noting that generalized versions of velocity constraints like where J v : Q → R is the linear map fromq to desirable velocity functions, can also be included. Here, it is assumed that the constraint is linear w.r.t.q. This is a fair assumption for kinematic velocity constraints. For example, the endeffector velocity can be expressed as the Jacobian J(q) times the configuration velocityq. By appropriate coordinate changes, we can reformulate (30) to the form (29). The resulting dynamics after the coordinate change will still be of the form (1) satisfying Property 1 locally. However, this coordinate change must exist, and must be a diffeomporphism (to avoid singular configurations). We will demonstrate this transformation for the 2-DOF manipulator in Section VI. The interested reader may see [15,Chapter 4,Section 5.4] for details on the coordinate change and the corresponding properties. For the rest of this section, we will stick with the velocity constraint of the form (29).
Having obtained the constraint (29), the next steps involve separating the robotic dynamics (1) into two parts: where the terms corresponding to D,C, G, u are apparent from the setup. q 1 , q 2 form the configuration q = (q 1 , q 2 ) i.e., if q 1 is the first element in the tuple q, then the remaining elements are combined to form the tuple q 2 .q 2 can be eliminated from (31) to obtain where D s is nothing but the Schur complement form, and it is known to be symmetric positive definite [14,Proposition 1]. This matrix also has the following additional properties.
Property 2: There exist positive constants c l , c u such that Note that we have used the same constants c l , c u for ease of notations. Proof of Proposition 2 is in [14,Appendix A].
With the reduced dynamics (32), we define a new constraint as and the corresponding CBF can be defined as which is the boundary of the safe set C. Note that h v (x) ≥ 0 for all x, which is akin to a kinetic energy form with D s being the inertia matrix. However, with this new CBF, the goal remains the same, i.e., the set Int(C) must be forward invariant. Hence, even if h v is non-negative, the sign of h will not change unless the solution hits the boundary of C. Therefore, we can continue to useḂ v for the formulations similar to (QP). Accordingly, we have the dynamics of B v (x) aṡ and by using (32), we havė Similar to (QP), we can realize a QP formulation for the velocity constraint B v . However, a model-free version of this QP requires the elimination of D 12 D −1 22 . Hence, if we choose u 2 = 0, we can define a new state to set mapping K vcbf : T Q ⇒ R m similar to (25). By using the same notations as in (22), (23), ψ 00 , ψ 01 , ψ 10 , ψ 11 can now be redefined as Having defined this set, we have the following result similar to Theorem 1.
Theorem 2: Given a set C ⊂ T Q defined by (5)-(7) for a continuously differentiable function h : R n → R with relative degree two, then for all classes of Lipschitz continuous control laws k : R 2n → R ≥0 belonging to the set K vcbf , the set Int(C) is forward invariant.
Proof of this theorem is not necessary, as it is similar to Proof of Theorem 1. If u 2 is included, then the resulting set K vcbf will consist of model based terms D 12 and D 22 . To realize a model-free constraint, D 12 D −1 22 can be replaced by its rough estimate. Let this estimate be denoted byD 12D −1 22 . Inclusion of these terms in ψ 01 , ψ 11 in (36) yields the new state to set mapping. However, due to the difference between the real and estimated inertia terms D,D, safety guarantees cannot be ensured with the constraints obtained via (36). In the following result, we show a min-norm QP formulation with the modified constraint, which includes the terms fromD, and guarantees safety irresepective of the model parameters of the robot: is feasible, and yields a Lipschitz continuous control law. In addition, there is a small enough γ such that the resulting control u * (x) obtained from (VQP) renders the set Int(C) forward invariant.
Proof: Since h = 0 in the set Int(C), it can be verified that ψ 11 = 0. Hence, the QP of the form (VQP) is feasible. Therefore, we have a min-norm control law obtained similar to (11). In particular, if ψ 10 > 0, then we have that which is Lipschitz. The goal now is to establish forward invariance of Int(C). In other words, we need to establish that the input (37) ensures that the derivative of B v (given by (35)) satisfies where Above equation is satisfied when ψ 10 ≤ 0. When ψ 10 > 0, we can substitute (37) to obtain This can be simplified to obtain We know that h v is quadratic in h, and ψ v , ψ v are always bounded. Therefore, by choosing a small enough 0 < γ < 1, we can ensure that the above expression is ≤ 0. This completes the proof.

V. ALTERNATIVE FORMULATIONS WITH STABILITY CONSIDERATIONS
The goal of this section is to incorporate tracking constraints along with the CBF based constraints, which results in multi-objective QP formulations. We will be specifically using control Lyapunov function (CLF) based constraints for realizing tracking of desired trajectories.
Choose a desired position q d ∈ R n . The error is defined as e(q) = q − q d . With this error, we have the change of variables for the state coordinates η(x) := (e(x),ė(x)). The CLF is formally defined as follows.
Similar to the CBF constraints used in (QP), a CLF based constraint can be obtained from (43) in the following manner: Note that since η is a function of x, we have used x instead of η(x) as arguments for V . This can be included as a constraint in the QP to yield a stabilizing control law. However, since the goal here is to realize a model-free QP, PD based QPs [13] i.e., CLF-QPs inspired by PD control laws will be used. This CLF is defined below: V e (e,ė, q) = V 0 (e,ė, q) +V c (e,ė, q) (45) K p is a constant matrix to be defined later (see (52)). It can be verified that V e is positive definite. The addition of the cross terms V c does not affect the positive definiteness as long as k 0 is sufficiently small. For example, we can pick k 0 that satisfies Therefore, we can choose It is worth noting that this is the template Lyapunov function used to establish stability of PD controlled robotic systems [21], [14], [11]. The PD control law is of the form where the proportional and derivative gains K p , K d ∈ R n×n are tuned to track the desired trajectory. It was established that ultimate boundedness of the tracking error can be guaranteed with large enough PD gains [21]. Hence, the PD-QPs are based on this result. By using the derivative of V e , we obtain the following constraint that guarantees boundedness of e (see [13,Theorem 1] for more details): (α(e)e T +ė T ) With this constraint, we obtain the following QP formulation that combines stability (via CLF) and safety (via CBFs) to yield a unified control law that guarantees either both stability and safety or prioritize safety over stability in robotic systems: Here p > 0 is the penality. Depending on the constraints being position or velocity based ψ XX,cb f , ψ XX,cb f in (54), (55) can be one of the following columns:  Note that, unlike in Corollaries 1 and 2, we have incorporated two safety constraints instead of one. This is necessary, as the CLF constraint (53) influences the solution for (PC). Hence, the solution u * (x) may satisfy (54) but not (55), and vice versa. With this new form of QP (MFQP), we have the following result. Proof: We will not consider the case when all the constraints are inactive (for which the solution is zero). In addition, we know that (54), (55) are simultaneously active only if u * (x) = 0. Otherwise, if (54) is active, then (55) is inactive, and vice versa. Hence, we will study only the following three cases: (a) (53) is active, (54), (55) are inactive, (b) (53) is inactive, and one of (54), (55) is active, and (c) (53) and one of (54), (55) are active. We will study the three cases below: Fig. 1: Left figure is showing the simple pendulum, where the goal is to ensure that it is above the dashed line. Right figure shows a two-link manipulator arm, where the goal is to ensure that its end-point (end-effector) is inside the square dashed box. We will also demonstrate safety of velocity based constraints on these two models. It is assumed that the kinematic model is accurately known, but the masses are not accurately known.
(a) When (53) is active, we have the solution for u * (x) and δ * (x) from the pointwise min-norm controller: (b) When either (54) or (55) is active, we have the solution for u * (x) given by (37), and δ * (x) = 0. (c) When both the CLF, CBF constraints are active, we have the solutions as Here A, b are given by To establish the continuity of the resulting control law, we pick a compact subset of Int(C) and note that all of the conditions of [8, Theorem 3.1] are satisfied. A detailed analysis of the steps for a similar CLF-CBF-QP formulation are provided in [9, Proof of Theorem 1].
In Theorem V, we establish Lipschitz continuity for compact subsets of Int(C). While extending this result for the entire Int(C) is possible for position based constraints (by using Theorem 1), the velocity based constraints depend on the terms used in CLF based constraints. This will be a subject of future work.

VI. ILLUSTRATIVE EXAMPLES
The goal in this section is to analyze specific examples that allow us to build an intuition for the robustness exhibited by the various QPs proposed in this paper. We will study two types of robot models a) a 1-DOF manipulator (pendulum), and b) a 2-DOF manipulator (double pendulum). Both of these models are depicted in Fig. 1.

A. Pendulum
In Fig. 1, the angle w.r.t. the vertical θ forms the configuration of the pendulum. The state is given by x = (θ ,θ ). Suppose that the goal is to constrain the pendulum above the horizontal i.e., |θ | ≤ π/2. In other words, we choose the dashed lines in Fig. 1 to be horizontal. Two types of QP formulations will be considered: one with a position constraint, and another with a velocity constraint.
The dynamics of a simple pendulum of length L is: where g is acceleration due to gravity, m is the mass, and u is the control input. Given the constraint function: h(θ ) = (π/2) 2 − θ 2 , we have the control barrier function defined as Accordingly, we have the following dynamics for B(θ ,θ ): the goal is to ensure that the following is satisfied for some c > 0:Ḃ If the upper bound on the mass m is known, then we need to ensure thaṫ where c u is the upper bound obtained from Property 1. For the QP shown as in (PQP), we have the following min-norm control law: Note that if we wish to add more constraints, we can use composition [20], and use the same energy formulation. In the interest of space, we will demonstrate this constraint formulation for the 2-DOF manipulator. Suppose that the goal now is to constraint the velocity h(x) = v d −θ to be ≥ 0 for all time. We choose the barrier function as We need to ensure that its derivative satisfies the following inequality: which ensures that h(x(t)) > 0 for all t. The model parameters can be replaced with suitable upper and lower bounds to yield the following QP based control: Table II shows the list of different parameter values used, and results are shown in Fig. 2. Note that if we wish to add a velocity constraint on the negative side, i.e., h(x) =θ − v d ≥   [20]. The simulation results for both the position and velocity constraints are shown in Fig. 2. It can be verified that the mass was increased from 0.1 kg to 10 kg, and the constraints were still satisfied. Note that increasing the mass any further requires updating of the parameter c u accordingly.
B. 2-DOF manipulator arm (double pendulum) Fig. 1 shows a 2-DOF manipulator arm (double pendulum), where the end-effector is required to be constrained in a square box of length r. The lengths of the links (denoted by l) are equal. The configuration is given by q = (q 1 , q 2 ). We first compute the x, y position of the end-effector: x e (x) := l sin(q 2 ) + l sin(q 1 − q 2 ) y e (x) := l cos(q 2 ) − l cos(q 1 − q 2 ).
Let the center of the square be at (x c , y c ). Accordingly, we have two constraint functions: By following the methods presented in [20], we use composition to combine the constraints: h(x) := h 1 (x)h 2 (x). We accordingly have the barrier function given by (14) with the kinetic energy augmented. With this new function, we obtain the new QP formulation given by (PQP) accordingly. For the 2-DOF manipulator, we will demonstrate QP formulations with stability considerations, i.e., we include a CLF based constraint for combining tracking and safety. Since we would like to achieve tracking for the endeffector position, we apply a change of coordinates: Φ(q) := [x e (q), y e (q)]. Accordingly, we obtain the derivative as where J(q) is the Jacobian matrix. It must be noted that the Jacobian J must be non-singular for this transformation to be valid. We re-write the equations of motion of the 2-DOF manipulator as where are the new terms that define the dynamics in the end-effector space. Since J consists of only sin, cos terms, Property 1 is still valid for a subset of Q (decided by the singularity of J). Given the destination point (x d , y d ) ∈ R 2 , we denote the error by e := (x e − x d , y e − y d ). Accordingly, we obtain the CLF based constraint as (α(e)e T +ė T )J −T u ≤ −(α(e)e T +ė T )(K p e + K dė ), where α is given by (48), and K p , K d are the tunable gains. Compared to the original constraint (52), the above constraint includes the Jacobian matrix J(q) that maps the control u to the newly derived dynamics (69). We will choose four different destination points around the square boundary, and track them one by one. Accordingly, we have the QP formulation given by (MFQP) with the appropriate CLF based constraint for each desired position. Details of the parameters for the constraints used are given in Table III. In particular, we scale the mass of the double pendulum from 0.1 kg to 10 kg, while using the same parameters in the constraints. The resulting QP based control is simulated for two seconds before switching the desired end-effector position. The simulation results for one of the desired positions is shown in Fig. 3. The video submission shows the simulation results for all the four desired positions.
Suppose the goal is to include a velocity based constraint, for which we use the following:   where we impose an upper limit on the derivative of x e , which is the horizontal position of the end-effector. Given this constraint, we have the barrier function as where D s is the Schur complement form obtained from eliminatingÿ e in (69) similar to (32). Note that the resulting constraint obtained is different than (VC): 1 where the Jacobian J(q) is included similar to (71), and ψ 10 , ψ v are appropriately reformulated. Remaining steps use the model-free QP formulation (MFQP) with the newly obtained CLF, CBF based constraints. The CLF constraints were constructed to track/reach two points in a sequence i.e., left-to-right and right-to-left (more details are in the video submission). Since the constraint was for the forward motion of the end-effector, velocity for only the left-to-right direction was restricted. The hyperparameters used are shown in Table  III, and the corresponding simulation results are shown in Fig. 4. Video link: https://youtu.be/iD_OH0iuXeI

VII. CONCLUSIONS
In this paper, we introduced a new class of control barrier functions for robotic systems. We use the kinetic energy term to realize robust QPs for guaranteed safety of sets generated by kinematic constraints. Specifically, the kinematic constraints are position and velocity functions of the robot manipulator. This approach is motivated by the success of PD based QPs [11], in which the kinetic energy was used to construct CLF based constraints in QPs. It is worth mentioning that the velocity forms, despite not having the kinetic energy in the strict sense, have properties that are well associated with the same. We also demonstrated the unification of tracking and safety, i.e., unification of CLF and CBF based constraints to yield a robust QP. We demonstrated the effectiveness of this class of QPs in two of the well known robotic system models in simulation. Future work will involve establishing experimental results, and extending this work for the class of underactuated robotic systems.