# Grasping Torque Optimization for a Dexterous Robotic Hand Using the Linearization of Constraints

Authors: Peng Jia; Lei Wu; Gang Wang; Wei Na Geng; Feihong Yun; Ning Zhang
Journal: Mathematical Problems in Engineering
 (2019)

---

## Abstract

A new algorithm is proposed for the optimization of grasping torques. Previous work revealed that the optimization of the contact force can be simplified as a linear programming problem by replacing the nonlinear friction cone with a polyhedral cone. On this basis, further simplification and linearization is completed for various grasp contact constraints of the grasping system, and an optimization model is established with the minimum sum-of-squares of joint torques as an objective function. A timely and effective algorithm based on key constraint sets is then established. This methodology optimizes the joint torque directly and facilitates the control of the dexterous hand. A numerical example and experimental results show that the optimization algorithm has high precision and good real-time performance. The joint torque output obtained using the proposed algorithm was 27.6% lower than the result of the previous algorithm. The optimization algorithm can thus be used for optimal grasping control of a dexterous hand.


---

## Body

## 1. Introduction

In the action of grasping, the mechanical dexterous hand needs to control the joint torque according to the condition of the external wrench so that the contact forces counterbalance the external wrench and meet the friction constraint at each place of contact. However, subject to the same external wrench, a contact force that conforms to the stable grasp and friction constraints is not unique, and the torque at each joint is also not unique. Owing to the limitations of the structure and driving ability of the dexterous hand joint, the maximum driving torque output by each joint is limited. To make the best use of the driving torque of each joint, the torque of each joint needs to be reasonably assigned, and this problem comes down to the optimization of the grasping torque in the joint torque space of the dexterous hand. There are presently two methods of obtaining the optimal joint torque.(1)
The contact force is optimized according to a certain contact force index in the contact force space, and the contact force is then mapped to the joint torque space to obtain the optimal joint torqueKerr and Roth [1] proposed an early method of optimizing the grasping force, which solved the optimal force using a polyhedron to linearize the friction cone and involved linear programming. Cheng et al. [2, 3] converted the friction constraint into a set of linear inequalities by replacing the nonlinear friction cone with a polyhedral cone and simplified the contact force optimization as a linear programming problem. Afterward, Klein et al. [4, 5] put forward the simplex algorithm and the interior-point algorithm based on linear programming to solve the optimization problem. Zhang and Gruver [6] decomposed the contact force space into subspaces and developed a method of determining the connectivity dimension of the subspace relative to the object that was being grasped, thus simplifying the analysis of the contact force distribution. Zhen et al. [7] recently developed an optimization algorithm to calculate a set of initial and minimum forces more quickly than was possible in earlier calculations. In addition, Zhu et al. [8–13] proposed a variety of linear optimization methods for the contact force to reduce the amount of calculation; these methods were studied in terms of simplification of the point-contact friction cone, contact force optimization index, linear programming method, and real-time performance. While linear programming methods may be advantageous in terms of the computational efficiency, they may provide grasping forces that are too conservative or violate nonlinear friction constraints. The accuracy of the linear programming method also depends in part on how many segments are used in the linear approximation of the friction constraint. Adding more segments can improve the solution optimality but at the cost of computational efficiency, and the calculation time becomes great even for a moderate number of segments.From the perspective of nonlinear programming, Buss et al. [14] verified that adopting the friction cone constraint is equivalent to testing the positive definiteness of symmetric matrices, and the grasping force optimization is expressed as a convex optimization problem on a Riemannian manifold with linear constraints. On this basis, algorithms have been further explored and developed. Buss et al. [15] proposed a general-class Dikin-type algorithm for strictly convex quadratic continuously cost differentiable functions, guaranteeing a unique solution to the semidefinite programming problem related to the optimization of a smart grasping force. Helmke et al. [16] put forward a new compact representation of the friction cone constraint, reducing the complexity of the optimization problem. Remond et al. [17, 18] proposed a method that is based on the minimization cost function and gave an analytical solution; however, the solution does not ensure that friction constraints are met. Liu et al. [19] reported how to simplify the structure of the constraints and select appropriate initial conditions, step size, and stopping criteria. Gazeau et al. [20] turned the problem into a minimum distance problem and solved it using a gradient projection method. Cornella et al. [21] gave a similar expression but solved the dual form rather than the primal form. Lippiello et al. [22, 23] proposed a solution to lower the computational complexity of the problem by dynamically reducing the number of effective torque constraints, and they imported the previous one-handed operation into the two-handed operating system. The aforementioned various algorithms do not linearize the friction cone; the force optimization is thus more accurate, and the results obtained using the nonlinear method are more reasonable.However, such nonlinear gradient algorithms also have the disadvantage of requiring a valid initial value of the grasping force before starting the iteration. To address this disadvantage, Han et al. [24] proposed a MAX-DET algorithm that uses linear matrix inequalities for initial grasping; however, the algorithm encounters singularity problems in some cases. Boyd and Wegbreit [25] constructed a simple single-valued optimization problem to get the initial value. Employing a Lagrangian multiplier, Wang et al. [26] proposed an iterative method of obtaining the initial values that meet the friction constraint by adjusting the weighting factor of the normal force. Chen et al. [27] gave a linear combination for the initial value of the grasping force. Wang et al. [28] recently proposed an iterative method for the initial value of the grasping force and safe edge detection, ensuring that the initial safe value meets the friction cone constraint in the optimization process of the gradient algorithm.To reduce the computational complexity, Zheng et al. [29, 30] proposed a now-popular method that divides the algorithm into online and offline phases. By solving as many offline problems as possible, suboptimal but still acceptable solutions can be found with reduced online computing time. Using sparse matrix techniques and reducing matrix dimensions, Li [31] and Ji and Peng [32] divided the calculations of a solution into online and offline components. In addition, nonlinear methods were studied for this type of method because they have higher precision and higher computational speed than linear programming methods, which benefits online application. Wang et al. [33–35] proposed a primal-dual interior-point algorithm based on the kernel function to solve this convex quadratic programming over circular cones. Chen et al. [36] proposed a method implemented by using the fmincon function based on sequential quadratic programming in MATLAB optimization toolbox.As more joints are used in modeling a system, the procedures required to derive the relevant motion equations become more laborious and complicated. Hollerbach [37] developed Lagrangian formulation of manipulator dynamics and verified that recursive formulations based on the Lagrangian or Newton–Euler dynamics offer the best method of dynamics calculation. Korayem et al. [38, 39] presented new methods to derive the dynamic equations of manipulators using the recursive Gibbs–Appell formulation while using hierarchical optimal control for path planning of mobile manipulator. Agarwal et al. [40] investigated the dynamic behaviour of serial chains with degrees of freedom and used a Recursive Dynamic Simulator which is based on the Newton–Euler formulation and the decoupled natural orthogonal complement matrices to simulate the dynamics of these systems. Recently, Korayem et al. [41–43] proposed a path planning method for mobile manipulator, which decouples the whole system into a group of subsystems and performs hierarchical optimal control, and they directly put forward additional constraints in the task space of mobile manipulators to resolve extra degrees of freedom introduced by the base mobility.The most appropriate method for real-time grasping and control is to use neural networks. In recent years, deep learning has shown promising results. Xia et al. [44] developed a neural network algorithm that converges to the optimal solution at any initial point. Other methods have been developed for general problems similar to the grasping force optimization problem [45–47]. Levine et al. [48] described a learning-based approach to hand-eye coordination for robotic grasping from monocular images, a large convolutional neural network was trained to predict the probability that task space motion of the gripper will result in successful grasps, and then used this network to servo the gripper in real time to achieve successful grasps. Qin et al. [49] used convolution neural network to find the optimal configuration of grasping stability prediction by tactile data, which improves the success rate of grasping. Chao et al. [50] proposed a deep learning-based object detection method for a five-fingered industrial robot hand model, which improves the accuracy of robotic grasp in some uncertain environments. To optimize the grasping force of the grasping manipulation, Ko et al. [51, 52] proposed a semismooth Newton gripping force optimization method, which converts friction constraints into Cartesian products of second-order cones.(2)
Optimize the joint torque in the joint torque space according to a certain joint torque optimization index.Li et al. [53] used the maximization of the carrying capacity of the dexterous hand joint as the objective function and used an iterative algorithm to optimize the nonlinear joint torque. However, the nonlinearity of the constraints and the complexity of the objective function introduce difficulties to the optimization process in terms of convergence and real-time processing. And, little work on this type of method has been carried out.Both linear programming and nonlinear programming are carried out in the contact force space. The dimension of the optimization problem increases with the number of contact points and the complexity of the contact constraint, which will increase the amount of calculation. In addition, the joint torque mapped from the optimal contact force may not be the best in the joint space. At the same time, the nonlinearity of constraints brings difficulties to the convergence and real time of the optimization process in the joint torque space.The present paper proposes a new efficient algorithm for obtaining the optimal grasping torque of a joint of the dexterous hand. On the basis of previous studies, this paper further simplifies and linearizes various grasping contact constraints of the grasping system and establishes an optimization model with the minimum sum-of-squares of joint torques as an objective function. Then, a timely and effective algorithm based on key constraint sets is established. Unlike the majority of algorithms that first obtain the optimal contact force and then convert the force into the joint torque to realize control of the dexterous hand, this algorithm obtains the optimal grasping torque directly, simplifying the control process and reducing the control time. Compared with the result obtained using the algorithm proposed by Li et al. [53], the joint torque obtained by our algorithm is weaker and the optimization result is more reasonable.Grasping experiments were carried out on a variety of different shapes for each grasping system. Comparing the optimized joint torque output with the original output, the effectiveness of the grasping torque optimization method based on the key constraint set is verified.The remainder of the paper is structured as follows. Section2 is devoted to the linearization of the contact constraint, Section 3 presents the simplification of grasping constraints, Section 4 develops the mathematical optimization model and algorithm, Section 5 presents numerical results, Section 6 presents experiments, and Section 7 presents conclusions and proposes future work.


## 2. Linearization of the Contact Constrain

This section presents basic knowledge on grasp contact constraints. Various grasp contact constraints are linearized by replacing the nonlinear single friction cone, paired friction cones, and friction ellipsoid with the linear single polyhedral cone, paired polyhedral cones, and ellipsoidal polyhedron, respectively.


### 2.1. Contact Constraint

The dexterous hand grasps and manipulates an object through contact. There are three types of contact between the finger and object, as depicted in Figure1: (a) frictionless point contact (FPC), where the contact force comprises only the normal force fz; (b) point contact with friction (PCwF), where the contact force comprises the normal force component fz and tangential force components fx and fy; and (c) soft-finger contact (SFC), where the contact force comprises the normal force component fz, tangential force components fx and fy, and normal torque mz. SFC can be divided into two types: SFC with a linear approximation (SFCl) and SFC with an elliptical approximation (SFCe), with the difference between SFCl and SFCe being that the contact force needs to meet different contact constraints [7, 54–58].Figure 1
Three common contact types. (a) FPC. (b) PCwF. (c) SFC.
(a)
(b)
(c)To avoid slippage and separation at the point of contact, for the above three contact types, the contact force must satisfy the following contact constraints:(1)FPC:fiz>0,(2)PCwF:fix2+fiy2≤μ1fiz;fiz>0,(3)SFCl:fix2+fiy2u1+mizu2≤fiz;fiz>0,(4)SFCe:fix2+fiy2u12+miz2u2′2≤fiz;fiz>0.Here,fiz is the normal force component of the contact, fix and fiy are the tangential force components, miz is the normal torque, u1 is the tangential friction coefficient, and u2 and u2′ are, respectively, the torsional friction coefficients for SFCl and SFCe.


### 2.2. Linearization of the PCwF Constraint

If the contact between the dexterous hand and object being grasped can be simplified as PCwF, the normal force componentfiz and tangential force components fix and fiy of each point of contact need to meet the nonlinear constraints determined by equation (2) during the grasping. The following discusses how to linearize the nonlinear constraint and simplify the contact constraint.The contact forcefi=fixfiyfizT inside the friction cone meets the condition of the PCwF constraint, as shown in Figure 2. Making an internal polyhedral cone that linearly simulates the friction cone, the contact force fi located in the polyhedral cone will meet the condition of the PCwF constraint.Figure 2
Linearization of the PCwF constraint.The faces of the polyhedral cone satisfy a linear equation, and the points inside the polyhedral cone meet a linear inequality equation. The linear polyhedral cone can then replace the friction cone and realize the linearization of the PCwF constraint. The linear inequality equation of the contact force that satisfies the polyhedral cone constraint is derived as follows.Assuming that the internal polygon hasN sides, the vectors of the polygon cone sides can be expressed as(5)Sj=Sj1Sj2Sj3T=μ1cos2πNjμ1cos2πNj1T,where j=0,1,2,…,N−1,N.The plane of the contact force formed by sidesSj and Sj+1 can be expressed as(6)ajbjcjfxfyfz=0,where aj=Sj2Sj+13−Sj3Sj+12,bj=Sj3Sj+11−Sj+13Sj1, andcj=Sj1Sj+12−Sj+11Sj2,j=0,1,2,…,N−1,N.The contact forcefi inside the polyhedral cone can, therefore, be expressed using a linear inequality:(7)a0b0c0a1b1c1⋮⋮⋮aN−1bN−1cN−1001N+1×3fixfiyfiz≥0.Because the polyhedral cone is inscribed in the friction cone, the contact force satisfying equation (7) must satisfy the condition of the PCwF constraint determined by equation (2). Meanwhile, the friction constraint is simplified to the linear inequality equation. It is noted that the contact force inside the interlayer part between the polyhedral cone and friction cone also satisfies the condition of the PCwF constraint. However, this part of the feasible contact force is eliminated in the linearization process, and the contact force fi determined by the linearization constraint is, therefore, also relatively conservative.The above analysis reveals that increasing the inscribed polyhedron side numberN results in higher accuracy of the linearization, and a less conservative contact force provides more linearized inequalities, leading to a larger computation and worse real-time performance. The situation is just the opposite when reducing the number of sides N of the inscribed polyhedron. The number of inscribed polyhedron sides should thus be reasonably selected according to the accuracy and real-time processing requirements in the linearization process.


### 2.3. Linearization of the SFCl Constraint

To simplify the SFCl constraint, the nonlinear inequality constraint given as equation (3) can be written equivalently as(8)fix2+fiy2≤u1fiz−mizu1u2;fiz>0.In the Cartesian coordinate frameo−fxfymz, as shown in Figure 3, the contact force fi=fixfiyfizmizT satisfying the above constraint is distributed in the double cone. The position of the vertices and the radius of the round base of the double cone are related to the normal force fiz at the point of contact, the coordinates of the upper and lower vertices are, respectively, 00μ2fiz and 00−μ2fiz, and the radius of the round base is μ1fiz.Figure 3
Linearization of the SFCl constraint.An internal bipyramid is taken to simulate the double cone, as shown in Figure3. Because the faces of the bipyramid satisfy a linear equation, a bipyramid is used in place of the friction double cone to linearize the SFCl constraint. The linear inequality of the contact force that satisfies the bipyramid constraints is derived as follows.Assuming that the inscribed polygon in the round base of the double cone hasN sides, the coordinates of the inscribed polygon points in the coordinate frame o−fxfymz can be expressed as(9)fj=fxjfyjfzj=μ1fizcos2πNjμ1fizsin2πNj0,where j=0,1,2,…,N−1,N.The upper and lower vertices of the bipyramid and vertices of theN-gon in the round base form 2N planes. From the coordinates of the bipyramid vertices and equation (9), the linear inequality equation that satisfies the contact constraint of the bipyramid is obtained as(10)μ2sj−sj+1fix+μ2cj+1−cjfiy+μ1μ2kfiz−μ1kmiz≥0,μ2sj−sj+1fix+μ2cj+1−cjfiy+μ1μ2kfiz+μ1kmiz≥0,where sj=sin2πj/N,cj=cos2πj/N,k=sin2πj/N,andj=0,1,2,…,N−1.Because the normal contact forcefiz>0, the linear inequality constraint of the SFCl can be expressed as(11)a0b0l1l2a1b1l1l2⋮⋮⋮⋮aN−1bN−1l1l2a0b0l1−l2⋮⋮⋮⋮aN−1bN−1l1−l200102N+1×4fixfiyfizmiz≥0,where aj=μ2sj−sj+1,bj=μ2cj+1−cj,l1=μ1μ2k,l2=−μ1k,sj=sin2πj/N,cj=cos2πj/N,k=sin2πj/N,andj=0,1,2,…,N−1.


### 2.4. Linearization of the SFCe Constraint

To linearize the SFCe constraint, the nonlinear inequality constraint equation (4) can be written equivalently as(12)fix2u12+fiy2u12+miz2u2′2≤fiz2;fiz>0.In the Cartesian coordinate frameo−fxfymz, as shown in Figure 4, the contact force fi=fixfiyfizmizT satisfying the above constraint is distributed in an ellipsoid. The radius of the circle formed by the ellipsoid and fxfy coordinate plane and the vertexes of the ellipsoid are related to the normal force fiz at the point of contact. The radius of the circle is μ1fiz while the coordinates of the upper and lower vertices are, respectively, 00u2′fiz and 00−u2′fiz.Figure 4
Linearization of the SFCe constraint.The above analysis reveals that the contact force in the ellipsoid satisfies the condition of the SFCe constraint so that the linearization of the SFCe constraint becomes the linearization of the ellipsoid. As shown in Figure4, the internal ellipsoidal polyhedron is used to linearly simulate the ellipsoid and linearize the contact constraint. The linear inequality equation of the contact force that satisfies the constraints of the ellipsoidal polyhedron is derived as follows.The entire ellipsoid surface is marked with lines of latitude and longitude. The ellipsoid surface is uniformly distributed withN longitude lines, and the longitude line that intersects the positive axis fix of the ellipsoid coordinate frame is referred to as the zero-longitude line, which coincides with the Nth longitude line. The circle formed by the surface of the ellipsoid and the coordinate plane fixfiy of the ellipsoid coordinate frame is referred to as the zero-latitude line. The surfaces of the north and south semiellipsoids are equally divided by K latitude lines, and the Kth latitude lines of the north and south semiellipsoids become the north and south vertices.The upper semiellipsoid is selected as the research object; the coordinates of the north vertex are00u2′fiz. The number of intersections of the latitude and longitude on the surface of the upper semiellipsoid is n=K×N, and the coordinates of each point of intersection can be expressed as(13)fxi,jfyi,jmzi=fzlicjfzlisjfzliki,where li=μ1μ2′/μ12ki2+μ2′2,ki=tanπi/2K,sj=sin2πj/N, sj=sin2πj/N, cj=cos2πj/N, j=0,1,2,…,N−1, and i=0,1,2,…,K−1.TheseK×N+1 points are combined into an ellipsoidal polyhedron with N×K planes. The surface equation of the upper semiellipsoidal polyhedron can be described as(14)liki−li+1ki+1sj−sj+1fix+liki−li+1ki+1cj+1−cjfiy−lili+1kki+1−kifiz+kli−li+1miz=0,where j=0,1,2,…,N−1,i=0,1,2,…,K−1,andk=sin2π/N.The surface equation of the lower semiellipsoidal polyhedron can be obtained from symmetry as(15)liki−li+1ki+1sj−sj+1fix+liki−li+1ki+1cj+1−cjfiy−lili+1kki+1−kifiz−kli−li+1miz=0,where j=0,1,2,…,N−1,i=0,1,2,…,K−1,andk=sin2π/N.Combining the above2N×K surface linear equations, the linear equation of the contact force that satisfies the constraint of the ellipsoidal polyhedron is obtained as(16)liki−li+1ki+1sj+1−sjfix+liki−li+1ki+1cj−cj+1fiy−lili+1kki−ki+1fiz±kli+1−limiz≥0,where j=0,1,2,…,N−1,i=0,1,2,…,K−1,andk=sin2π/N.Because the normal contact forcefiz>0, the linear inequality constraint of the SFCe can be expressed as(17)a00b00c0d0⋮⋮⋮⋮aN−10bN−10c0d0⋮⋮⋮⋮a0K−1b0K−1cK−1dK−1⋮⋮⋮⋮aN−1K−1bN−1K−1cK−1dK−1a00b00c0−d0⋮⋮⋮⋮aN−10bN−10c0−d0⋮⋮⋮⋮a0K−1b0K−1cK−1−dK−1⋮⋮⋮⋮aN−1K−1bN−1K−1cK−1−dK−100102NK+1×4fixfiyfizmiz≥0,where aji=liki−li+1ki+1sj+1−sj,bji=liki−li+1ki+1cj−cj+1,ci=−lili+1kki−ki+1,di=kli+1−li,j=0,1,2,…,N−1,i=0,1,2,…,K−1, k=sin2π/N,li=μ1μ2′/μ12ki2+μ2′2,ki=tanπi/2K,sj=sin2πj/N,andcj=cos2πj/N.


## 2.1. Contact Constraint

The dexterous hand grasps and manipulates an object through contact. There are three types of contact between the finger and object, as depicted in Figure1: (a) frictionless point contact (FPC), where the contact force comprises only the normal force fz; (b) point contact with friction (PCwF), where the contact force comprises the normal force component fz and tangential force components fx and fy; and (c) soft-finger contact (SFC), where the contact force comprises the normal force component fz, tangential force components fx and fy, and normal torque mz. SFC can be divided into two types: SFC with a linear approximation (SFCl) and SFC with an elliptical approximation (SFCe), with the difference between SFCl and SFCe being that the contact force needs to meet different contact constraints [7, 54–58].Figure 1
Three common contact types. (a) FPC. (b) PCwF. (c) SFC.
(a)
(b)
(c)To avoid slippage and separation at the point of contact, for the above three contact types, the contact force must satisfy the following contact constraints:(1)FPC:fiz>0,(2)PCwF:fix2+fiy2≤μ1fiz;fiz>0,(3)SFCl:fix2+fiy2u1+mizu2≤fiz;fiz>0,(4)SFCe:fix2+fiy2u12+miz2u2′2≤fiz;fiz>0.Here,fiz is the normal force component of the contact, fix and fiy are the tangential force components, miz is the normal torque, u1 is the tangential friction coefficient, and u2 and u2′ are, respectively, the torsional friction coefficients for SFCl and SFCe.


## 2.2. Linearization of the PCwF Constraint

If the contact between the dexterous hand and object being grasped can be simplified as PCwF, the normal force componentfiz and tangential force components fix and fiy of each point of contact need to meet the nonlinear constraints determined by equation (2) during the grasping. The following discusses how to linearize the nonlinear constraint and simplify the contact constraint.The contact forcefi=fixfiyfizT inside the friction cone meets the condition of the PCwF constraint, as shown in Figure 2. Making an internal polyhedral cone that linearly simulates the friction cone, the contact force fi located in the polyhedral cone will meet the condition of the PCwF constraint.Figure 2
Linearization of the PCwF constraint.The faces of the polyhedral cone satisfy a linear equation, and the points inside the polyhedral cone meet a linear inequality equation. The linear polyhedral cone can then replace the friction cone and realize the linearization of the PCwF constraint. The linear inequality equation of the contact force that satisfies the polyhedral cone constraint is derived as follows.Assuming that the internal polygon hasN sides, the vectors of the polygon cone sides can be expressed as(5)Sj=Sj1Sj2Sj3T=μ1cos2πNjμ1cos2πNj1T,where j=0,1,2,…,N−1,N.The plane of the contact force formed by sidesSj and Sj+1 can be expressed as(6)ajbjcjfxfyfz=0,where aj=Sj2Sj+13−Sj3Sj+12,bj=Sj3Sj+11−Sj+13Sj1, andcj=Sj1Sj+12−Sj+11Sj2,j=0,1,2,…,N−1,N.The contact forcefi inside the polyhedral cone can, therefore, be expressed using a linear inequality:(7)a0b0c0a1b1c1⋮⋮⋮aN−1bN−1cN−1001N+1×3fixfiyfiz≥0.Because the polyhedral cone is inscribed in the friction cone, the contact force satisfying equation (7) must satisfy the condition of the PCwF constraint determined by equation (2). Meanwhile, the friction constraint is simplified to the linear inequality equation. It is noted that the contact force inside the interlayer part between the polyhedral cone and friction cone also satisfies the condition of the PCwF constraint. However, this part of the feasible contact force is eliminated in the linearization process, and the contact force fi determined by the linearization constraint is, therefore, also relatively conservative.The above analysis reveals that increasing the inscribed polyhedron side numberN results in higher accuracy of the linearization, and a less conservative contact force provides more linearized inequalities, leading to a larger computation and worse real-time performance. The situation is just the opposite when reducing the number of sides N of the inscribed polyhedron. The number of inscribed polyhedron sides should thus be reasonably selected according to the accuracy and real-time processing requirements in the linearization process.


## 2.3. Linearization of the SFCl Constraint

To simplify the SFCl constraint, the nonlinear inequality constraint given as equation (3) can be written equivalently as(8)fix2+fiy2≤u1fiz−mizu1u2;fiz>0.In the Cartesian coordinate frameo−fxfymz, as shown in Figure 3, the contact force fi=fixfiyfizmizT satisfying the above constraint is distributed in the double cone. The position of the vertices and the radius of the round base of the double cone are related to the normal force fiz at the point of contact, the coordinates of the upper and lower vertices are, respectively, 00μ2fiz and 00−μ2fiz, and the radius of the round base is μ1fiz.Figure 3
Linearization of the SFCl constraint.An internal bipyramid is taken to simulate the double cone, as shown in Figure3. Because the faces of the bipyramid satisfy a linear equation, a bipyramid is used in place of the friction double cone to linearize the SFCl constraint. The linear inequality of the contact force that satisfies the bipyramid constraints is derived as follows.Assuming that the inscribed polygon in the round base of the double cone hasN sides, the coordinates of the inscribed polygon points in the coordinate frame o−fxfymz can be expressed as(9)fj=fxjfyjfzj=μ1fizcos2πNjμ1fizsin2πNj0,where j=0,1,2,…,N−1,N.The upper and lower vertices of the bipyramid and vertices of theN-gon in the round base form 2N planes. From the coordinates of the bipyramid vertices and equation (9), the linear inequality equation that satisfies the contact constraint of the bipyramid is obtained as(10)μ2sj−sj+1fix+μ2cj+1−cjfiy+μ1μ2kfiz−μ1kmiz≥0,μ2sj−sj+1fix+μ2cj+1−cjfiy+μ1μ2kfiz+μ1kmiz≥0,where sj=sin2πj/N,cj=cos2πj/N,k=sin2πj/N,andj=0,1,2,…,N−1.Because the normal contact forcefiz>0, the linear inequality constraint of the SFCl can be expressed as(11)a0b0l1l2a1b1l1l2⋮⋮⋮⋮aN−1bN−1l1l2a0b0l1−l2⋮⋮⋮⋮aN−1bN−1l1−l200102N+1×4fixfiyfizmiz≥0,where aj=μ2sj−sj+1,bj=μ2cj+1−cj,l1=μ1μ2k,l2=−μ1k,sj=sin2πj/N,cj=cos2πj/N,k=sin2πj/N,andj=0,1,2,…,N−1.


## 2.4. Linearization of the SFCe Constraint

To linearize the SFCe constraint, the nonlinear inequality constraint equation (4) can be written equivalently as(12)fix2u12+fiy2u12+miz2u2′2≤fiz2;fiz>0.In the Cartesian coordinate frameo−fxfymz, as shown in Figure 4, the contact force fi=fixfiyfizmizT satisfying the above constraint is distributed in an ellipsoid. The radius of the circle formed by the ellipsoid and fxfy coordinate plane and the vertexes of the ellipsoid are related to the normal force fiz at the point of contact. The radius of the circle is μ1fiz while the coordinates of the upper and lower vertices are, respectively, 00u2′fiz and 00−u2′fiz.Figure 4
Linearization of the SFCe constraint.The above analysis reveals that the contact force in the ellipsoid satisfies the condition of the SFCe constraint so that the linearization of the SFCe constraint becomes the linearization of the ellipsoid. As shown in Figure4, the internal ellipsoidal polyhedron is used to linearly simulate the ellipsoid and linearize the contact constraint. The linear inequality equation of the contact force that satisfies the constraints of the ellipsoidal polyhedron is derived as follows.The entire ellipsoid surface is marked with lines of latitude and longitude. The ellipsoid surface is uniformly distributed withN longitude lines, and the longitude line that intersects the positive axis fix of the ellipsoid coordinate frame is referred to as the zero-longitude line, which coincides with the Nth longitude line. The circle formed by the surface of the ellipsoid and the coordinate plane fixfiy of the ellipsoid coordinate frame is referred to as the zero-latitude line. The surfaces of the north and south semiellipsoids are equally divided by K latitude lines, and the Kth latitude lines of the north and south semiellipsoids become the north and south vertices.The upper semiellipsoid is selected as the research object; the coordinates of the north vertex are00u2′fiz. The number of intersections of the latitude and longitude on the surface of the upper semiellipsoid is n=K×N, and the coordinates of each point of intersection can be expressed as(13)fxi,jfyi,jmzi=fzlicjfzlisjfzliki,where li=μ1μ2′/μ12ki2+μ2′2,ki=tanπi/2K,sj=sin2πj/N, sj=sin2πj/N, cj=cos2πj/N, j=0,1,2,…,N−1, and i=0,1,2,…,K−1.TheseK×N+1 points are combined into an ellipsoidal polyhedron with N×K planes. The surface equation of the upper semiellipsoidal polyhedron can be described as(14)liki−li+1ki+1sj−sj+1fix+liki−li+1ki+1cj+1−cjfiy−lili+1kki+1−kifiz+kli−li+1miz=0,where j=0,1,2,…,N−1,i=0,1,2,…,K−1,andk=sin2π/N.The surface equation of the lower semiellipsoidal polyhedron can be obtained from symmetry as(15)liki−li+1ki+1sj−sj+1fix+liki−li+1ki+1cj+1−cjfiy−lili+1kki+1−kifiz−kli−li+1miz=0,where j=0,1,2,…,N−1,i=0,1,2,…,K−1,andk=sin2π/N.Combining the above2N×K surface linear equations, the linear equation of the contact force that satisfies the constraint of the ellipsoidal polyhedron is obtained as(16)liki−li+1ki+1sj+1−sjfix+liki−li+1ki+1cj−cj+1fiy−lili+1kki−ki+1fiz±kli+1−limiz≥0,where j=0,1,2,…,N−1,i=0,1,2,…,K−1,andk=sin2π/N.Because the normal contact forcefiz>0, the linear inequality constraint of the SFCe can be expressed as(17)a00b00c0d0⋮⋮⋮⋮aN−10bN−10c0d0⋮⋮⋮⋮a0K−1b0K−1cK−1dK−1⋮⋮⋮⋮aN−1K−1bN−1K−1cK−1dK−1a00b00c0−d0⋮⋮⋮⋮aN−10bN−10c0−d0⋮⋮⋮⋮a0K−1b0K−1cK−1−dK−1⋮⋮⋮⋮aN−1K−1bN−1K−1cK−1−dK−100102NK+1×4fixfiyfizmiz≥0,where aji=liki−li+1ki+1sj+1−sj,bji=liki−li+1ki+1cj−cj+1,ci=−lili+1kki−ki+1,di=kli+1−li,j=0,1,2,…,N−1,i=0,1,2,…,K−1, k=sin2π/N,li=μ1μ2′/μ12ki2+μ2′2,ki=tanπi/2K,sj=sin2πj/N,andcj=cos2πj/N.


## 3. Simplification of Constraints

This section expresses the contact force using the joint torque and external wrench. The equilibrium constraint and friction constraint that need to be satisfied in grasping are simplified as a linear constraint with the joint torque as the variable, while an optimization model is established with linear constraints as optimization constraints and with the minimum sum-of-squares of joint torques as the objective function. The solution of the optimization model then comes down to a quadratic function optimization problem under linear constraints, and a timely and effective algorithm for the optimal grasping torque based on key constraint sets is proposed.


### 3.1. Decomposition of the Contact Force

In grasping, a multifinger dexterous hand must meet the equilibrium conditions(18)Gf=w,(19)JTf=τ.Here,G∈R6×n is the grasping matrix, f∈Rn is the contact force applied at points of contact on the fingers, w∈R6 is the external wrench applied to the object, J∈Rn×m is the Jacobian matrix of the dexterous hand, and τ∈Rm is the joint torque of a finger of the dexterous hand. Detailed static and kinematic equations that need to be satisfied in the grasping process and general solutions of the equations are given in the literature [59, 60].The contact forcef is an N-dimensional vector space, which can be decomposed into four mutually orthogonal subspaces:(20)Γh2=fh2Gfh2=0,JTfh2=0,Γh1=fh1Gfh1=0,JTfh1=τ,τ≠0,Γp2=fp2Gfp2=w,JTfp2=0,w≠0,Γp1=fp2Gfp2=w,JTfp1=τ,w≠0,τ≠0,where fh2 can be called a passive internal force because it cannot be provided by joint torque τ and is unable to equilibrate the external wrench w. There is no passive internal force fh2 for the rigid grasp, where both objects and fingers are considered as rigid bodies, but the passive internal force fh2 acts as a preload for the elastic grasp where both objects and fingers are considered elastomers. Mainly used to adjust and control the contact friction, fh1 can be called an active internal force because it can be provided by joint torque τ, but it is unable to equilibrate the external wrench w. Equilibrating the external wrench w through the mechanical structure and friction passively, fp2, which cannot be provided by joint torque τ, can be called a passive external force. fp1 can be called an active external force because it can be provided by joint torque τ and equilibrate the external wrench w.The contact forcef can therefore be decomposed as(21)f=fh2+fh1+fp2+fp1,where fh2 is the passive internal force, fh1 is the active internal force, fp2 is the passive external force, and fp1 is the active external force.The mapping relationship of the external wrenchw, contact force f, and joint torque τ is shown in Figure 5, where RJT is the range for JT and RG is the range for G.Figure 5
Decomposition of the contact force space.The representation of the contact forcef is directly related to the research problem. In order to simplify the contact force, the method of undetermined coefficient is used to express the contact force f.BecauseGfh2=0 and JTfh2=0, fh2 belonging to the null space of matrix A=GJT can be expressed as(22)fh2=NAxA,xA∈Rn−rankA.BecauseGfh1=0 and NATfh1=0, fh1 belonging to the null space of matrix B=GNAT can be expressed as(23)fh1=NBxB,xB∈Rn−rankB.BecauseJTfp2=0 and NATfp2=0, fp2 belonging to the null space of matrix C=JTNAT can be expressed as(24)fp2=NCxC,xC∈Rn−rankC.BecauseNCTfp1=0, NBTfp1=0, and NATfp1=0, fp1 belonging to the null space of matrix D=NCTNBTNAT can be expressed as(25)fp1=NDxD,xD∈Rn−rankD.The contact forcef of the dexterous hand can be obtained using the method of the undetermined coefficient as(26)f=NAxA+NBxB+NCxC+NDxD.To further simplify the constraint of the grasping force on the basis of the linearization of the contact constraint, the contact forcef is decomposed into the form of a joint torque τ and external wrench w.Equations (18) and (19) can be combined to give(27)Bf=d,where d=wτ and B=GJT.Substituting equation (26), Gfh2=0, and JTfh2=0 into equation (27), we have(28)Qx=d,where Q=BNDBNCBNB and x=xDxCxBT.The solution obtained by solving the above matrix equation is(29)x=QTQ−1QTd.Substituting the above solution into equation (26), we have(30)f=NAxA+Z1w+Z2τ.The dexterous-hand grasping system is generally rigid, allowing the effect of the first item to be ignored. Equation (30) can then be simplified as(31)f=Z1w+Z2τ,where Z1 is the matrix of external wrench equivalent coefficients and Z2 is the matrix of joint torque equivalent coefficients; both are obtained from G and J and reflect the shape of the grasp and object being grasped.


### 3.2. Conversion of Variables

To optimize the grasping force of the dexterous hand in the joint torque space, the constraint of the contact force must be converted into the constraint of the joint torque. The contact force has previously been broken down into the joint torque and external wrench. Substituting the contact force into the linearized contact constraint, the linear inequality constraint with the joint torque as the variable can be obtained.The unified form of the linearization formula of each contact constraint mentioned above has not been obtained.For the PCwF constraint, equation (7) can be transformed as(32)Sifi≥0,Si=a0b0c0a1b1c1⋮⋮⋮aN−1bN−1cN−1001N+1×3,fi=fixfiyfiz.Assuming that the dexterous hand hasn points of contact with the object and all contacts are of type PCwF, the total contact force f can be expressed as f=f1Tf2T⋯fnTT, where fi=fixfiyfizT.LetS=−BlockdiagSi. The contact constraint inequalities for the n points of contact of the dexterous hand can then be expressed as(33)SnN+n×3nf3n×1≥0.For the SFCl constraint, equation (11) can be rewritten as(34)Sifi≥0,Si=a0b0l1l2a1b1l1l2⋮⋮⋮⋮aNbNl1l2a0b0l1−l2⋮⋮⋮⋮aNbNl1−l200102N+1×4,fi=fixfiyfizmiz.Assuming that the dexterous hand hasn points of contact with the object and all contacts are of type SFCl, the total contact force f can be expressed as f=f1Tf2T⋯fnTT, where fi=fixfiyfizmizT.LetS=BlockdiagSi. The contact constraint inequalities for the n points of contact of the dexterous hand can then be expressed as(35)S2Nn+n×4nf4n×1≥0.For the SFCe constraint, equation (17) can be rewritten as(36)Sifi≥0,Si=a00b00c0d0⋮⋮⋮⋮aN−10bN−10c0d0⋮⋮⋮⋮a0K−1b0K−1cK−1dK−1⋮⋮⋮⋮aN−1K−1bN−1K−1cK−1dK−1a00b00c0−d0⋮⋮⋮⋮aN−10bN−10c0−d0⋮⋮⋮⋮a0K−1b0K−1cK−1−dK−1⋮⋮⋮⋮aN−1K−1bN−1K−1cK−1−dK−100102NK+1×4,fixfiyfizmiz≥0.Assuming that the dexterous hand hasn points of contact with the object and all contacts are of type SFCe, the total contact force f can be expressed as f=f1Tf2T⋯fnTT, where fi=fixfiyfizmizT.LetS=BlockdiagSi. The contact constraint inequalities for the n points of contact of the dexterous hand can then be expressed as(37)S2Nkn+n×4nf4n×1≥0.The unified form of equations (33), (35), and (37) is(38)Sf≥0.Substituting equation (31) into equation (38), the linear inequality constraint with joint torque as the variable that applies to different friction constraints can be expressed as(39)SZ2τ+SZ1w≥0.Here, the matrix of external wrench equivalent coefficientsZ1 and the matrix of joint torque equivalent coefficients Z2 are related to the situation of grasping and the shape of the object, and the matrix S is related to the contact type and the linearization accuracy of the frictional contact. When the dexterous hand grasps the object stably in a certain grasping situation, to satisfy the force equilibrium and the contact force constraint, the joint torque and external wrench must satisfy the linear constraint of equation (39).


## 3.1. Decomposition of the Contact Force

In grasping, a multifinger dexterous hand must meet the equilibrium conditions(18)Gf=w,(19)JTf=τ.Here,G∈R6×n is the grasping matrix, f∈Rn is the contact force applied at points of contact on the fingers, w∈R6 is the external wrench applied to the object, J∈Rn×m is the Jacobian matrix of the dexterous hand, and τ∈Rm is the joint torque of a finger of the dexterous hand. Detailed static and kinematic equations that need to be satisfied in the grasping process and general solutions of the equations are given in the literature [59, 60].The contact forcef is an N-dimensional vector space, which can be decomposed into four mutually orthogonal subspaces:(20)Γh2=fh2Gfh2=0,JTfh2=0,Γh1=fh1Gfh1=0,JTfh1=τ,τ≠0,Γp2=fp2Gfp2=w,JTfp2=0,w≠0,Γp1=fp2Gfp2=w,JTfp1=τ,w≠0,τ≠0,where fh2 can be called a passive internal force because it cannot be provided by joint torque τ and is unable to equilibrate the external wrench w. There is no passive internal force fh2 for the rigid grasp, where both objects and fingers are considered as rigid bodies, but the passive internal force fh2 acts as a preload for the elastic grasp where both objects and fingers are considered elastomers. Mainly used to adjust and control the contact friction, fh1 can be called an active internal force because it can be provided by joint torque τ, but it is unable to equilibrate the external wrench w. Equilibrating the external wrench w through the mechanical structure and friction passively, fp2, which cannot be provided by joint torque τ, can be called a passive external force. fp1 can be called an active external force because it can be provided by joint torque τ and equilibrate the external wrench w.The contact forcef can therefore be decomposed as(21)f=fh2+fh1+fp2+fp1,where fh2 is the passive internal force, fh1 is the active internal force, fp2 is the passive external force, and fp1 is the active external force.The mapping relationship of the external wrenchw, contact force f, and joint torque τ is shown in Figure 5, where RJT is the range for JT and RG is the range for G.Figure 5
Decomposition of the contact force space.The representation of the contact forcef is directly related to the research problem. In order to simplify the contact force, the method of undetermined coefficient is used to express the contact force f.BecauseGfh2=0 and JTfh2=0, fh2 belonging to the null space of matrix A=GJT can be expressed as(22)fh2=NAxA,xA∈Rn−rankA.BecauseGfh1=0 and NATfh1=0, fh1 belonging to the null space of matrix B=GNAT can be expressed as(23)fh1=NBxB,xB∈Rn−rankB.BecauseJTfp2=0 and NATfp2=0, fp2 belonging to the null space of matrix C=JTNAT can be expressed as(24)fp2=NCxC,xC∈Rn−rankC.BecauseNCTfp1=0, NBTfp1=0, and NATfp1=0, fp1 belonging to the null space of matrix D=NCTNBTNAT can be expressed as(25)fp1=NDxD,xD∈Rn−rankD.The contact forcef of the dexterous hand can be obtained using the method of the undetermined coefficient as(26)f=NAxA+NBxB+NCxC+NDxD.To further simplify the constraint of the grasping force on the basis of the linearization of the contact constraint, the contact forcef is decomposed into the form of a joint torque τ and external wrench w.Equations (18) and (19) can be combined to give(27)Bf=d,where d=wτ and B=GJT.Substituting equation (26), Gfh2=0, and JTfh2=0 into equation (27), we have(28)Qx=d,where Q=BNDBNCBNB and x=xDxCxBT.The solution obtained by solving the above matrix equation is(29)x=QTQ−1QTd.Substituting the above solution into equation (26), we have(30)f=NAxA+Z1w+Z2τ.The dexterous-hand grasping system is generally rigid, allowing the effect of the first item to be ignored. Equation (30) can then be simplified as(31)f=Z1w+Z2τ,where Z1 is the matrix of external wrench equivalent coefficients and Z2 is the matrix of joint torque equivalent coefficients; both are obtained from G and J and reflect the shape of the grasp and object being grasped.


## 3.2. Conversion of Variables

To optimize the grasping force of the dexterous hand in the joint torque space, the constraint of the contact force must be converted into the constraint of the joint torque. The contact force has previously been broken down into the joint torque and external wrench. Substituting the contact force into the linearized contact constraint, the linear inequality constraint with the joint torque as the variable can be obtained.The unified form of the linearization formula of each contact constraint mentioned above has not been obtained.For the PCwF constraint, equation (7) can be transformed as(32)Sifi≥0,Si=a0b0c0a1b1c1⋮⋮⋮aN−1bN−1cN−1001N+1×3,fi=fixfiyfiz.Assuming that the dexterous hand hasn points of contact with the object and all contacts are of type PCwF, the total contact force f can be expressed as f=f1Tf2T⋯fnTT, where fi=fixfiyfizT.LetS=−BlockdiagSi. The contact constraint inequalities for the n points of contact of the dexterous hand can then be expressed as(33)SnN+n×3nf3n×1≥0.For the SFCl constraint, equation (11) can be rewritten as(34)Sifi≥0,Si=a0b0l1l2a1b1l1l2⋮⋮⋮⋮aNbNl1l2a0b0l1−l2⋮⋮⋮⋮aNbNl1−l200102N+1×4,fi=fixfiyfizmiz.Assuming that the dexterous hand hasn points of contact with the object and all contacts are of type SFCl, the total contact force f can be expressed as f=f1Tf2T⋯fnTT, where fi=fixfiyfizmizT.LetS=BlockdiagSi. The contact constraint inequalities for the n points of contact of the dexterous hand can then be expressed as(35)S2Nn+n×4nf4n×1≥0.For the SFCe constraint, equation (17) can be rewritten as(36)Sifi≥0,Si=a00b00c0d0⋮⋮⋮⋮aN−10bN−10c0d0⋮⋮⋮⋮a0K−1b0K−1cK−1dK−1⋮⋮⋮⋮aN−1K−1bN−1K−1cK−1dK−1a00b00c0−d0⋮⋮⋮⋮aN−10bN−10c0−d0⋮⋮⋮⋮a0K−1b0K−1cK−1−dK−1⋮⋮⋮⋮aN−1K−1bN−1K−1cK−1−dK−100102NK+1×4,fixfiyfizmiz≥0.Assuming that the dexterous hand hasn points of contact with the object and all contacts are of type SFCe, the total contact force f can be expressed as f=f1Tf2T⋯fnTT, where fi=fixfiyfizmizT.LetS=BlockdiagSi. The contact constraint inequalities for the n points of contact of the dexterous hand can then be expressed as(37)S2Nkn+n×4nf4n×1≥0.The unified form of equations (33), (35), and (37) is(38)Sf≥0.Substituting equation (31) into equation (38), the linear inequality constraint with joint torque as the variable that applies to different friction constraints can be expressed as(39)SZ2τ+SZ1w≥0.Here, the matrix of external wrench equivalent coefficientsZ1 and the matrix of joint torque equivalent coefficients Z2 are related to the situation of grasping and the shape of the object, and the matrix S is related to the contact type and the linearization accuracy of the frictional contact. When the dexterous hand grasps the object stably in a certain grasping situation, to satisfy the force equilibrium and the contact force constraint, the joint torque and external wrench must satisfy the linear constraint of equation (39).


## 4. Mathematical Model and Algorithm

This section establishes the optimization model with linear constraints as optimization constraints and with the minimum sum-of-squares of joint torques as the objective function. A real-time and effective algorithm of the active constraint method based on key constraint sets is then established.


### 4.1. Mathematical Optimization Model

When the grasping situation and contact are determined, to grasp the object stably, the dexterous hand must meet the following constraints:(1)
The contact force equilibrates the external wrench(2)
The contact force is inside the friction cone(3)
The joint torque equilibrates the contact forceThe contact force is passively generated when the finger is in contact with the object, and the joint torque is directly controllable. Subject to the same external wrench, there are various joint torque distribution schemes that satisfy the constraint. To optimize the grasping force in the joint torque space and obtain the optimal joint torque, it is necessary to construct a reasonable joint torque objective function. Considering the limitation of the power output and structural size of the dexterous hand driver, the maximum output torque of the joint is limited. It is always expected that the greatest external wrench be equilibrated with the weakest possible driving torque, and the objective function of joint torque optimization can thus be expressed as(40)σ=∑i=1mτi2=τTτ,where τi is the torque of the dexterous hand joint and m is the number of dexterous hand joints.Taking the minimum sum-of-squares of joint torques as the objective function, the joint torque as the optimization variable, and the equilibrium constraint that satisfies the joint torque and the friction constraint of the contact force as the optimization constraints, the mathematical model of the optimization of the grasping torque for the dexterous hand in the joint torque space can be described as(41)Minσ=∑i=1mτi2=τTτ,subject to2,3,4,18,and19.Because the optimization constraint is nonlinear with the joint torque as the variable, the optimization problem comes down to a nonlinear programming problem with equality and inequality constraints. Although there are corresponding calculation methods in mathematics, they usually require many iterative operations and do not meet the requirement of real-time processing. It is thus necessary to simplify the optimization constraints appropriately to meet the needs of the real-time control of dexterous hands. The linear constraint with the joint torque as the variable is then used instead of the nonlinear constraint, and the mathematical optimization model is simplified as(42)σ=∑i=1mτi2=τTτ,subject toSZ2τ+SZ1w≥0.All constraints of the simplified mathematical optimization model become linear inequality constraints, and the joint torques satisfying the constraints constitute a feasible convex set in the joint torque space. When using two joints for grasping, the physical meaning of equation (42) can be visually described as shown in Figure 6. The shadow of the polygon represents the feasible set of joint torque τ that satisfies the constraints of equation (42), and the polygon is a convex polygon because all constraints are linear. Several circles represent the distribution of joint torques τ that obey the functional equation σ=τ12+τ22.Figure 6
Optimal joint torque of grasping with two joints.In Figure6, circle 1 does not intersect with the feasible convex set and it is thus not possible to obtain the joint torque τ on the circle. Circle 3 has multiple intersections with the feasible convex set; the joint torque τ at the intersections satisfies all the constraints but does not provide the smallest σ, and an optimal solution is thus not obtained. Circle 2 has a single tangent point A with the feasible convex set; this is the optimum point satisfying all constraints and providing the smallest σ, and joint torques τ1 and τ2 corresponding to point A are thus optimal.When equation (42) is generalized to multiple joint torque spaces, the torque τ satisfies all linear constraints is a convex set in the joint torque space, the objective function becomes a generalized sphere with a variable diameter, and the tangent point of the generalized sphere and convex set is the optimal solution. Because all the constraints are linear and the objective function is quadratic, the optimal solution problem becomes a quadratic programming problem. The algorithm for solving this quadratic programming is discussed as follows.


### 4.2. Algorithm

Equation (42) can be written equivalently as(43)Minσ=∑i=1mτi2=τTτ,subject toHτ≥c,where τ=τ1τ2⋯τmT∈Rm, H=SZ2, and c=−SZ1w.To solve the above problem, we select the initial feasible point of joint torqueτ1 that satisfies the constraint, take the point of the joint torque as the initial point, remove the constraint set that is useless at the point of the joint torque in the force equilibrium and the linear friction constraint, and seek the key constraint set at the point of the joint torque. Under the simplified equality constraint, the Lagrange multiplier method is used to obtain the minimum value of the objective function, which is the optimal feasible point of the joint torque.Then, with the better feasible point of the joint torque as the new initial point, the above process can be repeated for a limited time, and the optimal joint torque satisfying the above constraint can be obtained.Assuming that the feasible point of joint torqueτk is obtained in the kth iteration and that the key constraint set that takes effect at the point of the joint torque is Ik, the problem is simplified as the quadratic function minimization problem under an equality constraint:(44)Minσ=τTτ,subject tohiTτ=ci,i∈Ik,where hi is an m-dimensional vector, comprising elements of the ith column of the constraint matrix HT.The Lagrange multiplier method is used to solve the quadratic function minimization problem under the above equality constraints. The optimal joint torque is denoted asτk′, and it is assumed that dk=τk′−τk. The following steps are carried out depending on the situation:(1)
Ifτk+dk still satisfies the constraints while dk≠0, then in the next iteration, the point of joint torque τk+1=τk+dk is taken as the initial point.(2)
Ifτk+dk no longer satisfies the constraints, then in the next iteration, the initial joint torque is τk+1=τk+λkdk. The following question is how to choose a suitable λk such that τk+1 for any i, where i∉Ik, meets the constraint(45)hiTτk+λkdk≥ci.WhenhiTdk≥0 and if the step size λk≥0, equation (45) can always be established. When hiTdk<0, to establish equation (45), the step size λk needs to meet the condition(46)λk≤minci−hiTτkhiTdki∉Ik,hiTdk<0.To get a better point of torque, takeλk=min1,λk′, where λk′ is the rightmost term of equation (46). If there is an index p that is not in the key constraint set but satisfies(47)λk=cp−hpTτkhpTdk<1,thenhpTd≥cp is also a key constraint at the feasible point of torque τk+1. The constraint corresponding to the index p is added to the key constraint set Ik+1. The process returns to the beginning of the question, and a Lagrange multiplier is used to obtain the optimal solution of equation (44) again.(3)
Ifdk=0, then the optimal solution of equation (44) is τk, but the point of torque τk may not be the optimal solution of equation (43). The Lagrange multiplier vik is further calculated under all key constraints to determine whether the point of torque τk is the optimal solution of equation (43).If∀i∈Ik satisfy vik≥0, then the point of torque τk is the optimal solution of equation (45), and the whole problem is solved.If∃q∈Ik for which vqk<0, then the point of torque τk is not the optimal solution and can thus be optimized in some direction. Therefore, let vrk=minvrki∈Ik and the rth constraint equation is removed from the key constraint set Ik, the process returns to the beginning of the question, and a Lagrangian multiplier is used to obtain the optimal solution of equation (44) again.The specific algorithm is given as follows:Step 1: Select the initial data. Give the initial feasible point of joint torqueτ1. The corresponding key constraint set is I1. Let k=1.Step 2: Solve the equality constraint problem. Solve equation (44) and set the optimal solution as τk′. Assume that dk=τk′−τk.Ifdk=0, then go to Step 4.Otherwise, letλk=min1,λk′, τk+1=τk+λkdk, and go to Step 3.Step 3: Modify the key constraint set.Ifλk<1, then let Ik+1=Ik∪p,k=k+1, and return to Step 2.Ifλk=1, then let k=k+1, and go to Step 4.Step 4: Determine whether the point of the torque is the optimal solution and calculate the Lagrangian multipliervik corresponding to all active constraints, where i∈Ik, and mark it as vrk=minviki∈Ik.Ifvrk≥0, the iteration is terminated and the optimal solution is τk.Otherwise, delete indexr from Ik and return to Step 2.The flow of the above process is shown in Figure7.Figure 7
Optimization flow of the joint torque.


## 4.1. Mathematical Optimization Model

When the grasping situation and contact are determined, to grasp the object stably, the dexterous hand must meet the following constraints:(1)
The contact force equilibrates the external wrench(2)
The contact force is inside the friction cone(3)
The joint torque equilibrates the contact forceThe contact force is passively generated when the finger is in contact with the object, and the joint torque is directly controllable. Subject to the same external wrench, there are various joint torque distribution schemes that satisfy the constraint. To optimize the grasping force in the joint torque space and obtain the optimal joint torque, it is necessary to construct a reasonable joint torque objective function. Considering the limitation of the power output and structural size of the dexterous hand driver, the maximum output torque of the joint is limited. It is always expected that the greatest external wrench be equilibrated with the weakest possible driving torque, and the objective function of joint torque optimization can thus be expressed as(40)σ=∑i=1mτi2=τTτ,where τi is the torque of the dexterous hand joint and m is the number of dexterous hand joints.Taking the minimum sum-of-squares of joint torques as the objective function, the joint torque as the optimization variable, and the equilibrium constraint that satisfies the joint torque and the friction constraint of the contact force as the optimization constraints, the mathematical model of the optimization of the grasping torque for the dexterous hand in the joint torque space can be described as(41)Minσ=∑i=1mτi2=τTτ,subject to2,3,4,18,and19.Because the optimization constraint is nonlinear with the joint torque as the variable, the optimization problem comes down to a nonlinear programming problem with equality and inequality constraints. Although there are corresponding calculation methods in mathematics, they usually require many iterative operations and do not meet the requirement of real-time processing. It is thus necessary to simplify the optimization constraints appropriately to meet the needs of the real-time control of dexterous hands. The linear constraint with the joint torque as the variable is then used instead of the nonlinear constraint, and the mathematical optimization model is simplified as(42)σ=∑i=1mτi2=τTτ,subject toSZ2τ+SZ1w≥0.All constraints of the simplified mathematical optimization model become linear inequality constraints, and the joint torques satisfying the constraints constitute a feasible convex set in the joint torque space. When using two joints for grasping, the physical meaning of equation (42) can be visually described as shown in Figure 6. The shadow of the polygon represents the feasible set of joint torque τ that satisfies the constraints of equation (42), and the polygon is a convex polygon because all constraints are linear. Several circles represent the distribution of joint torques τ that obey the functional equation σ=τ12+τ22.Figure 6
Optimal joint torque of grasping with two joints.In Figure6, circle 1 does not intersect with the feasible convex set and it is thus not possible to obtain the joint torque τ on the circle. Circle 3 has multiple intersections with the feasible convex set; the joint torque τ at the intersections satisfies all the constraints but does not provide the smallest σ, and an optimal solution is thus not obtained. Circle 2 has a single tangent point A with the feasible convex set; this is the optimum point satisfying all constraints and providing the smallest σ, and joint torques τ1 and τ2 corresponding to point A are thus optimal.When equation (42) is generalized to multiple joint torque spaces, the torque τ satisfies all linear constraints is a convex set in the joint torque space, the objective function becomes a generalized sphere with a variable diameter, and the tangent point of the generalized sphere and convex set is the optimal solution. Because all the constraints are linear and the objective function is quadratic, the optimal solution problem becomes a quadratic programming problem. The algorithm for solving this quadratic programming is discussed as follows.


## 4.2. Algorithm

Equation (42) can be written equivalently as(43)Minσ=∑i=1mτi2=τTτ,subject toHτ≥c,where τ=τ1τ2⋯τmT∈Rm, H=SZ2, and c=−SZ1w.To solve the above problem, we select the initial feasible point of joint torqueτ1 that satisfies the constraint, take the point of the joint torque as the initial point, remove the constraint set that is useless at the point of the joint torque in the force equilibrium and the linear friction constraint, and seek the key constraint set at the point of the joint torque. Under the simplified equality constraint, the Lagrange multiplier method is used to obtain the minimum value of the objective function, which is the optimal feasible point of the joint torque.Then, with the better feasible point of the joint torque as the new initial point, the above process can be repeated for a limited time, and the optimal joint torque satisfying the above constraint can be obtained.Assuming that the feasible point of joint torqueτk is obtained in the kth iteration and that the key constraint set that takes effect at the point of the joint torque is Ik, the problem is simplified as the quadratic function minimization problem under an equality constraint:(44)Minσ=τTτ,subject tohiTτ=ci,i∈Ik,where hi is an m-dimensional vector, comprising elements of the ith column of the constraint matrix HT.The Lagrange multiplier method is used to solve the quadratic function minimization problem under the above equality constraints. The optimal joint torque is denoted asτk′, and it is assumed that dk=τk′−τk. The following steps are carried out depending on the situation:(1)
Ifτk+dk still satisfies the constraints while dk≠0, then in the next iteration, the point of joint torque τk+1=τk+dk is taken as the initial point.(2)
Ifτk+dk no longer satisfies the constraints, then in the next iteration, the initial joint torque is τk+1=τk+λkdk. The following question is how to choose a suitable λk such that τk+1 for any i, where i∉Ik, meets the constraint(45)hiTτk+λkdk≥ci.WhenhiTdk≥0 and if the step size λk≥0, equation (45) can always be established. When hiTdk<0, to establish equation (45), the step size λk needs to meet the condition(46)λk≤minci−hiTτkhiTdki∉Ik,hiTdk<0.To get a better point of torque, takeλk=min1,λk′, where λk′ is the rightmost term of equation (46). If there is an index p that is not in the key constraint set but satisfies(47)λk=cp−hpTτkhpTdk<1,thenhpTd≥cp is also a key constraint at the feasible point of torque τk+1. The constraint corresponding to the index p is added to the key constraint set Ik+1. The process returns to the beginning of the question, and a Lagrange multiplier is used to obtain the optimal solution of equation (44) again.(3)
Ifdk=0, then the optimal solution of equation (44) is τk, but the point of torque τk may not be the optimal solution of equation (43). The Lagrange multiplier vik is further calculated under all key constraints to determine whether the point of torque τk is the optimal solution of equation (43).If∀i∈Ik satisfy vik≥0, then the point of torque τk is the optimal solution of equation (45), and the whole problem is solved.If∃q∈Ik for which vqk<0, then the point of torque τk is not the optimal solution and can thus be optimized in some direction. Therefore, let vrk=minvrki∈Ik and the rth constraint equation is removed from the key constraint set Ik, the process returns to the beginning of the question, and a Lagrangian multiplier is used to obtain the optimal solution of equation (44) again.The specific algorithm is given as follows:Step 1: Select the initial data. Give the initial feasible point of joint torqueτ1. The corresponding key constraint set is I1. Let k=1.Step 2: Solve the equality constraint problem. Solve equation (44) and set the optimal solution as τk′. Assume that dk=τk′−τk.Ifdk=0, then go to Step 4.Otherwise, letλk=min1,λk′, τk+1=τk+λkdk, and go to Step 3.Step 3: Modify the key constraint set.Ifλk<1, then let Ik+1=Ik∪p,k=k+1, and return to Step 2.Ifλk=1, then let k=k+1, and go to Step 4.Step 4: Determine whether the point of the torque is the optimal solution and calculate the Lagrangian multipliervik corresponding to all active constraints, where i∈Ik, and mark it as vrk=minviki∈Ik.Ifvrk≥0, the iteration is terminated and the optimal solution is τk.Otherwise, delete indexr from Ik and return to Step 2.The flow of the above process is shown in Figure7.Figure 7
Optimization flow of the joint torque.


## 5. Numerical Examples

This section provides a specific example of the grasping torque simulation, verifies the accuracy and real-time performance of the grasping optimization method, and analyzes the effect of linearization accuracy on the optimization results.In the grasping system shown in Figure8, the contact model is SFCl, the rod lengths are l1=l3=2 and l2=l4=1, and the object is a flat disc with radius R=1. O−xyz is the object coordinate frame established at the center of the disc, and ci−xiyizii=1,2 denotes a contact coordinate frame established at a point of contact. Assume that the friction coefficients of the points of contact are μ1=0.5 and μ2=0.2. The contact force is expressed as f=fx1fy1fz1mz1fx2fy2fz2mz2T, the external wrench is expressed as w=FxFyFzMxMyMzT=−1−10001/2T, and the joint torque is expressed as τ=τ1τ2τ3τ4T. The analysis of the grasping torque optimization results under different linearization precision is as follows.Figure 8
Optimization example of the joint torque.The grasping matrixG and Jacobian transposed matrix JT are(48)G=1000−10000−1000−100001000−1001000−100100010000001000−1,JT=0−12000000−1000000000001−2000000100.The external wrench equivalent coefficient matrixZ1 and joint torque equivalent coefficient matrix Z2 are(49)Z1=0.50000.500−0.33330.10.3000−0.16670.20.10000000.50−0.50000.500−0.3333−0.1−0.3000−0.1667−02−0.10000000−0.5,Z2=0000−0.05−0.3167−0.050.01670.4−0.1333−0.10.0333000000000.05−0.01670.050.31670.1−0.0333−0.40.13330000.The contact is SFCl. To simplify the linearization of the contact constraint,N is set to 4, which means that the friction double cone is simplified into a four-sided double cone, and the linear inequality can be expressed as (50)BlockdiagS1f≥0,S1=−0.2−0.20.1−0.50.2−0.20.1−0.50.20.20.1−0.5−0.20.20.1−0.5−0.2−0.20.10.50.2−0.20.10.50.20.20.10.5−0.20.20.10.50010.Substituting the above results into equation (35), the optimization of constraints is obtained as (51)Hτ≥c,where(52)H=−0.050.050.030.030.050.050.030.030.4000.020.02000.020.020.10.050.05−0.076−0.0760.050.05−0.076−0.076−0.1300−0.006−0.00600−0.006−0.006−0.0300−0.020−0.02000−0.020−0.020−0.1−0.05−0.05−0.03−0.03−0.05−0.05−0.03−0.03−0.4000.0060.006000.0060.0060.033−0.05−0.050.0070.007−0.05−0.050.0070.0070.13,c=−0.27−0.27−0.140.050.17−0.0250.10.30.1−0.0250.170.30.1−0.27−0.070.05−0.140.16T.Initial valuesτ1=7.5−2−7.52T are selected and I1=2,14. The joint torque is optimized using the optimization algorithm presented in this paper. The optimization result is τopt=4.50070.9993−4.5007−0.9993T after eight iterations, and the optimal solution of the objective function is σ=42.5105.The optimization was conducted on a computer with a Pentium IV 2.8-GHz CPU and 1 GB RAM. The calculation time was less than 0.2 s, and the real-time performance was thus good. The joint torque optimized using a method in the literature [61] was τopt=6.351.2−6.35−1.29T. A comparison of the optimized results reveals that the sum of the absolute value of joint torque obtained using the optimization method presented in the present paper is 27.6% smaller than that of the joint torque obtained using the method from the literature [61]. In addition, the joint torque obtained using the optimization method presented in the present paper is more reasonable.The following analysis investigates the effect of linearization accuracy on the optimization results. Figure9 shows the trends of the optimization result σ and number of iterations n as N increases continuously, which means that the linearization error decreases, for optimization performed in the same way. It is seen that the optimization result σ improves as N increases, but the improvement in the linearization accuracy inevitably increases the linear inequality and the number of iterations in the optimization process, worsening the real-time performance.Figure 9
Trends of the optimization resultσ and number of iterations n with a change in N.


## 6. Experiment

This section introduces the experimental system of the machine dexterous hand and carries out a grasping experiment on a variety of differently shaped objects.


### 6.1. Composition and Working Principle

Figure10 shows that the experiment system of the dexterous hand mainly comprises the host computer control system, joint motion control system, dexterous hand mechanical body, fingertip force sensor processing system, and power source. As the host controller, the host computer mainly completes the grasping planning, the processing of fingertip force/torque information and human-computer interaction. The joint motion control system as the lower layer controller mainly performs the control of the force and positioning of the dexterous hand joint. The dexterous hand mechanical body acts as an actuator for grasping and manipulating the object. The fingertip force sensor processing system is primarily responsible for the measurement of the fingertip force/torque.Figure 10
Experimental system of the dexterous hand. (a) Host computer control system. (b) Dexterous hand mechanical body. (c) Fingertip force sensor process. (d) Power. (e) Joint motion control system.


### 6.2. Optimization Experiment

Grasping experiments were carried out on a variety of object shapes, as shown in Figure11. The algorithm module was first removed from the control program, and grasping experiments were then carried out for the differently shaped objects. After each hold stabilized, the host personal computer communicated with the joint servo control unit through the CAN bus, read and stored the torque information measured by each joint torque sensor, and obtained the output value of each joint torque of the dexterous hand when the optimization was not performed.Figure 11
Optimization examination of grasping forces. (a) Grasping an aluminum block (cuboid). (b) Grasping a screwdriver (hexagonal prism). (c) Grasping a plastic bottle (cylinder). (d) Grasping a carbon pen (slender rod). (e) Grasping an iron piece (flat plate). (f) Grasping a cup vigorously (cylinder).
(a)
(b)
(c)
(d)
(e)
(f)The algorithm module based on the key constraint set was then added to the control program and the above grasping experiment re-executed. Considering that the type of contact between the mechanical body of the dexterous hand and grasped object is PCwF, to balance the precision and real-time processing of the grasping torque optimization, the contact constraint model was linearized into a linear polyhedral model with a face numberN=8. The joint torque for a stable grasp before optimization was taken as the initial joint torque of the optimization algorithm. After several iterations, the joint torque was stably output and the host computer obtained the torque of each joint through the joint servo control unit again.Outputs of each joint torque of the dexterous hand before and after optimization are given in Table1.Table 1
Absolute values of joint torque outputs before and after optimization (N·m).
Thumb joint
Forefinger joint
Middle finger joint
Bottom joint
Middle joint
Top joint
Bottom joint
Middle joint
Top joint
Bottom joint
Middle joint
Top joint
Aluminum block
Before
0.062
0.642
0.240
0.120
0.593
0.206
0.116
0.607
0.215
After
0.048
0.509
0.190
0.106
0.587
0.166
0.110
0.590
0.171
Screwdriver
Before
0.023
0.156
0.079
0.019
0.155
0.076
0.020
0.153
0.077
After
0.021
0.106
0.065
0.020
0.096
0.065
0.019
0.098
0.065
Plastic bottle
Before
0.052
0.342
0.157
0.155
0.453
0.160
0.161
0.463
0.161
After
0.037
0.229
0.143
0.125
0.358
0.147
0.124
0.360
0.145
Carbon pen
Before
0.230
0.102
0.104
0.212
0.102
0.097
0.008
0.030
0.006
After
0.182
0.085
0.080
0.193
0.085
0.095
0.008
0.030
0.006
Iron piece
Before
0.112
0.423
0.182
0.202
0.090
0.035
0.007
0.050
0.005
After
0.105
0.328
0.150
0.175
0.085
0.033
0.007
0.050
0.004
Cup
Before
0.162
0.524
0.140
0.150
0.413
0.136
0.146
0.397
0.135
After
0.085
0.385
0.108
0.097
0.305
0.118
0.111
0.297
0.109A comparison of the joint torque output of optimization with the original output shows that when the dexterous hand grasps the six differently shaped objects, the optimized joint torques are lower by varying degrees, especially in the case of the output torque of the middle finger joint.


## 6.1. Composition and Working Principle

Figure10 shows that the experiment system of the dexterous hand mainly comprises the host computer control system, joint motion control system, dexterous hand mechanical body, fingertip force sensor processing system, and power source. As the host controller, the host computer mainly completes the grasping planning, the processing of fingertip force/torque information and human-computer interaction. The joint motion control system as the lower layer controller mainly performs the control of the force and positioning of the dexterous hand joint. The dexterous hand mechanical body acts as an actuator for grasping and manipulating the object. The fingertip force sensor processing system is primarily responsible for the measurement of the fingertip force/torque.Figure 10
Experimental system of the dexterous hand. (a) Host computer control system. (b) Dexterous hand mechanical body. (c) Fingertip force sensor process. (d) Power. (e) Joint motion control system.


## 6.2. Optimization Experiment

Grasping experiments were carried out on a variety of object shapes, as shown in Figure11. The algorithm module was first removed from the control program, and grasping experiments were then carried out for the differently shaped objects. After each hold stabilized, the host personal computer communicated with the joint servo control unit through the CAN bus, read and stored the torque information measured by each joint torque sensor, and obtained the output value of each joint torque of the dexterous hand when the optimization was not performed.Figure 11
Optimization examination of grasping forces. (a) Grasping an aluminum block (cuboid). (b) Grasping a screwdriver (hexagonal prism). (c) Grasping a plastic bottle (cylinder). (d) Grasping a carbon pen (slender rod). (e) Grasping an iron piece (flat plate). (f) Grasping a cup vigorously (cylinder).
(a)
(b)
(c)
(d)
(e)
(f)The algorithm module based on the key constraint set was then added to the control program and the above grasping experiment re-executed. Considering that the type of contact between the mechanical body of the dexterous hand and grasped object is PCwF, to balance the precision and real-time processing of the grasping torque optimization, the contact constraint model was linearized into a linear polyhedral model with a face numberN=8. The joint torque for a stable grasp before optimization was taken as the initial joint torque of the optimization algorithm. After several iterations, the joint torque was stably output and the host computer obtained the torque of each joint through the joint servo control unit again.Outputs of each joint torque of the dexterous hand before and after optimization are given in Table1.Table 1
Absolute values of joint torque outputs before and after optimization (N·m).
Thumb joint
Forefinger joint
Middle finger joint
Bottom joint
Middle joint
Top joint
Bottom joint
Middle joint
Top joint
Bottom joint
Middle joint
Top joint
Aluminum block
Before
0.062
0.642
0.240
0.120
0.593
0.206
0.116
0.607
0.215
After
0.048
0.509
0.190
0.106
0.587
0.166
0.110
0.590
0.171
Screwdriver
Before
0.023
0.156
0.079
0.019
0.155
0.076
0.020
0.153
0.077
After
0.021
0.106
0.065
0.020
0.096
0.065
0.019
0.098
0.065
Plastic bottle
Before
0.052
0.342
0.157
0.155
0.453
0.160
0.161
0.463
0.161
After
0.037
0.229
0.143
0.125
0.358
0.147
0.124
0.360
0.145
Carbon pen
Before
0.230
0.102
0.104
0.212
0.102
0.097
0.008
0.030
0.006
After
0.182
0.085
0.080
0.193
0.085
0.095
0.008
0.030
0.006
Iron piece
Before
0.112
0.423
0.182
0.202
0.090
0.035
0.007
0.050
0.005
After
0.105
0.328
0.150
0.175
0.085
0.033
0.007
0.050
0.004
Cup
Before
0.162
0.524
0.140
0.150
0.413
0.136
0.146
0.397
0.135
After
0.085
0.385
0.108
0.097
0.305
0.118
0.111
0.297
0.109A comparison of the joint torque output of optimization with the original output shows that when the dexterous hand grasps the six differently shaped objects, the optimized joint torques are lower by varying degrees, especially in the case of the output torque of the middle finger joint.


## 7. Conclusion and Future Work

This paper presented an algorithm for obtaining the optimal grasping torque of a dexterous hand joint. Addressing the disadvantage of a previous algorithm that cannot achieve the driving torque of the joint directly, the optimization method presented in this paper optimizes the joint torque in the joint torque space. The proposed optimization model with linear optimization constraints replacing nonlinear optimization constraints reduces the large number of iterative computations. In addition, the optimization model is simplified using the minimum sum-of-squares of joint torques as the objective function. Experiments show that the algorithm proposed in this paper can reduce the output torque of the dexterous hand joints, lighten the burden on the joint driver, improve the carrying capacity of the dexterous hand, and be used in the control system of a dexterous hand.Despite the progress made in this paper and dexterous robotic hands shifting from prototypes to practical implementations, there remain many problems in grasping theory that need further solutions and in-depth exploration. Future work may include better indicators and more efficient optimization algorithms for grasping planning.



---

*Source: 5235109-2019-11-23.xml*