# Optimal Grasping Manipulation for Multifingered Robots Using Semismooth Newton Method

Authors: Chun-Hsu Ko; Jein-Shan Chen
Journal: Mathematical Problems in Engineering
 (2013)

---

## Abstract

Multifingered robots play an important role in manipulation applications. They can grasp various shaped objects to perform point-to-point movement. It is important to plan the motion path of the object and appropriately control the grasping forces for multifingered robot manipulation. In this paper, we perform the optimal grasping control to find both optimal motion path of the object and minimum grasping forces in the manipulation. The rigid body dynamics of the object and the grasping forces subjected to the second-order cone (SOC) constraints are considered in optimal control problem. The minimum principle is applied to obtain the system equalities and the SOC complementarity problems. The SOC complementarity problems are further recast as the equations with the Fischer-Burmeister (FB) function. Since the FB function is semismooth, the semismooth Newton method with the generalized Jacobian of FB function is used to solve the nonlinear equations. The 2D and 3D simulations of grasping manipulation are performed to demonstrate the effectiveness of the proposed approach.


---

## Body

## 1. Introduction

Multifingered robots have attracted much attention in robotics manipulation applications. They can grasp various shaped objects and dexterously perform point-to-point manipulations. Many researches [1–6] have been proposed for grasping and manipulating objects with multifingered robots. Miller and Allen [2] proposed a user interface with grasp quality evaluation for the robot hand design. Yokokohji et al. [6] proposed a measure of dynamic manipulability of multifingered grasping for the systems consisting of a multifingered hand and a grasped object. Xu and Li [5] proposed a modeling method for the manipulation involving finger gaits. Kawamura et al. [1] used soft finger tips for stable grasping. Takahashi et al. [4] proposed robust force and position control with the information of tactile sensor. It is important to appropriately control the grasping forces for multifingered robot manipulation.Since the grasping manipulation utilizes the contact and friction forces to hold and move an object, the grasping forces should satisfy the point-contact friction constraint and be equal to the dynamic wrench of the grasped object. It is required to find the minimum forces for moving the grasped object in the manipulation. Boyd and Wegbreit [7] used the semidefinite programming and second-order cone programming to efficiently find the grasping forces. Helmke et al. [8] proposed quadratically convergent algorithms for optimal dexterous grasping. Han et al. [9] used the convex optimization involving linear matrix inequalities for grasping forces computation. Liu et al. [10] presented a unified geometric framework for efficient grasping force optimization. Zheng et al. [11] developed an algorithm to determine the minimum required friction coefficient and the corresponding reliable minimum contact forces in practice. Ko et al. [12] proposed a neural network to calculate the optimal grasping forces. Because the external wrench of the object varies with the manipulation path and orientation of the object, it is important to plan a manipulation trajectory [13] for achieving the minimum grasping forces.In this paper, we perform the optimal grasping control to find both optimal manipulation path of the object and minimum grasping forces. The rigid body dynamics of the object and the grasping forces subjected to the second-order cone (SOC) constraints are considered in the grasping control problem. The minimum principle [14] is applied to obtain the system equalities and the SOC complementarity problems. The SOC complementarity problems can be recast as the equations with the Fischer-Burmeister (FB) function. The semismooth Newton method with the generalized Jacobian of FB function is then used to solve the equations. Finally, simulations of optimal grasping manipulation are performed to demonstrate the effectiveness of the proposed approach.The remainder of this paper is organized as follows: Section2 describes the optimal grasping control problem. In Section 3, the semismooth Newton method with the generalized Jacobian of Fischer-Burmeister function is addressed. Section 4 presents the simulation results of 2D and 3D grasping manipulations. Finally, concluding remarks are given in Section 5.


## 2. Optimal Grasping Control

Figure1 shows the multifingered robot grasping manipulation. The multifingered robot grasps and moves the object from the initial position to the final position. The dynamic equation of the object can be expressed with Newton-Euler equations [15, 16] as
(1)y˙=v,v˙=1mRG1u+[00-g]T,q˙=Qω,ω˙=I-1(RG2u-ω×(Iω)),
where y is the position, v is the velocity, q=[q1q2q3]T is the quaternion, ω is the angular velocity, m is the object mass, I is the matrix of moment of inertia, g is the gravity constant, u means the grasping forces which is represented by a matrix, [G1G2] is the contact matrix, R is the rotation matrix of the object, and Q can be expressed as
(2)Q=12[q0q3-q2-q3q0q1q2-q1q0]withq0=q12+q22+q32.
Moreover, the grasping forces are subject to the contact friction constraint, expressed as
(3)∥(ui2,ui3)∥≤μui1,
where ui1 is the normal force of the ith finger, ui2 and ui3 are the friction forces of the ith finger, ∥·∥ is the 2-norm, and μ is the friction coefficient.Figure 1
Multifingered robot manipulation.To find the path that can be achieved with the minimum grasping forces, the optimal control problem can be recast as(4)min∫0T‍Ldt,0s.t.x˙=f(x,u),x(0)=x0,x(T)=xT,Du∈𝒦d×𝒦d×⋯×𝒦d,
where
(5)L=uTu2,x=[yvqω].
In addition, f represents the right hand side of system (1), T is the control duration, x0 and xT are the initial and final states, respectively, D is the diagonal matrix with the friction coefficient, and 𝒦 denotes the second-order cone which is given by
(6)𝒦d:={[z1z2]∈ℝ×ℝd-1∣∥z2∥≤z1}.The optimal control problem (4) can be solved by using the Pontryagin’s minimum principle, see [14, 17, 18]. In optimization language, it is to write out the KKT conditions for problem (4) which consist of two parts. The first part involves a few equalities about Lagrange multipliers, while the other part is related to complementarity conditions. More specifically, with the Hamiltonian function, the first part can be reformulated as follows:
(7)x˙-Hλ=x˙-f(x,u)=0,λ˙+Hx=λ˙+λTfx=0,Hu=Lu+λTfu+ηTD=0,ϕ(x(0),x(T))=0,λ(0)+ϕx(0)Tσ=0,λ(T)+ϕx(T)Tσ=0,
where λ, η, and σ are the Lagrange multipliers, and ϕ(x(0),x(T))=[x(0)-x0x(T)-xT]. The second part forms a second-order cone complementarity problem (SOCCP) as follows:
(8)-η∈𝒦,Du∈𝒦,ηTDu=0,
where 𝒦=𝒦d×𝒦d×⋯×𝒦d.From [19, 20], we see that the previous SOCCP (8) can be further recast as a system of equations:
(9)ϕFB(Du,-η)=0
by employing the so-called complementarity function ϕFB which is a vector-valued function defined as
(10)ϕFB(a,b):=(a2+b2)1/2-(a+b)
for a=[a1a2]∈ℝ×ℝd-1,b=[b1b2]∈ℝ×ℝd-1. We point out that the square term and square-root term in (10) are calculated via Jordan product
(11)a∘b=[aTba1b2+b1a2].
In particular, the expressions for a2 and a1/2 are given by
(12)a2=[∥a∥22a1a2],(13)a1/2=[sa22s]withs=12(a1+a12-∥a2∥2),
respectively.In summary, the optimal grasping forces can be obtained by solving (7) and (9). Then, the semismooth Newton method is used to solve these equations, which will be described in next section.


## 3. Semismooth Newton Method with Generalized Jacobian of FB Function

In order to apply the semismooth Newton method [21, 22] to (7) and (9), we need the following three linear equations:
(14)[Δx˙Δλ˙]-[fx(k)0-Hxx(k)-(fx(k))T][ΔxΔλ]-[fu(k)0-Hxu(k)0][ΔuΔη]=-[x˙(k)-f(k)λ˙(k)+Hx(k)],(15)[ϕx(0)(k)000I(ϕx(0)(k))T00-(ϕx(0)(k))T][Δx(0)Δλ(0)Δσ(0)]+[ϕx(T)(k)000000I0][Δx(T)Δλ(T)Δσ(T)]=-[ϕ(k)λ(k)(0)+(ϕx(0)(k))Tσ(k)λ(k)(T)+(ϕx(T)(k))Tσ(k)],(16)[IDTVa(k)D-Vb(k)][ΔuΔη]+[λTfxu(k)fu(k)00][ΔxΔλ]=-[Hu(k)ϕFB(Du(k),-η(k))].
Most information in linear equations (14)–(16) is known except the generalized Jacobian Va, Vb in (16). What do they represent? We provide a brief introduction here. First, we recall the concept of the B-subdifferential. Given a mapping Ψ:ℝn→ℝm, if Ψ is locally Lipschitz continuous, then the set
(17)∂BH(z)∶={V∈ℝm×n∣∃{zk}⊆DΨ:zk⟶z,Ψ′(zk)⟶V}
is nonempty and is called the B-subdifferential of Ψ at z, where DΨ⊆ℝn denotes the set of points at which Ψ is differentiable. The convex hull
(18)∂Ψ(z)∶=conv∂BΨ(z)
is the generalized Jacobian of Clarke [23]. From this definition, we see that the generalized Jacobian of ϕFB can be obtained by computing ∂BϕFB. From [24, Proposition 3.1], the B-subdifferential of ϕFB in (16) is exactly expressed as
(19)∂BϕFB(a,b)=[VaTVbT].
Moreover, by denoting c:=(a2+b2)1/2, we have(a)
Ifa2+b2∈int(𝒦d), then Va=Lc-1La and Vb=Lc-1Lb.(b)
Ifa2+b2∈bd(𝒦d) and (a,b)≠(0,0), then
(20)Va∈{122w1(1w-2Tw-24I-3w-2w-2T)La+12(1-w-2)αT},Vb∈{122w1(1w-2Tw-24I-3w-2w-2T)Lb+12(1-w-2)βT}for someα=[α1α2]∈ℝ×ℝd-1β=[β1β2]∈ℝ×ℝd-1 satisfying |α1|≤∥α2∥≤1 and |β1|≤∥β2∥≤1, where w-2=w2/∥w2∥.(c)
If(a,b)=(0,0), then Va∈{La^}, Vb∈{Lb^} for some a^, b^ with ∥a^∥2+∥b^∥2=1, or
(21)Va∈{12(1w-2)ξT+12(1-w-2)αT+2(00(I-w-2w-2T)δ2(I-w-2w-2T)δ1)},Vb∈{12(1w-2)τT+12(1-w-2)βT+2(00(I-w-2w-2T)γ2(I-w-2w-2T)γ1)}for some α=[α1α2],β=[β1β2],ξ=[ξ1ξ2],τ=[τ1τ2]∈ℝ×ℝl-1 such that
(22)|α1|≤∥α2∥≤1,|β1|≤∥β2∥≤1,|ξ1|≤∥ξ2∥≤1,|τ1|≤∥τ2∥≤1,w-2∈ℝl-1 satisfying ∥w-2∥=1, and δ=[δ1δ2],γ=[γ1γ2]∈ℝ×ℝd-1 satisfying ∥δ∥2+∥γ∥2≤1/2.Note that the calculations ofLa and La-1 are given by
(23)La=[a1a2Ta2a1I],La-1=1a12-∥a2∥2[a1-a2T-a2a12-∥a2∥2a1I+1a1a2a2T].
For more details, please refer to [24]. Now, we write down the iterative scheme of semismooth Newton method for solving the optimal grasping control problem.AlgorithmStep 1.
Choosex0, u0, λ0, η0, σ0 and set k=0.Step 2.
If convergence criterion is satisfied, stop.Step 3.
Compute the directionΔxk, Δuk, Δλk, Δηk, Δσk from the linear equations (14)–(16).Step 4.
Set(24)[xk+1uk+1λk+1ηk+1σk+1]=[xkukλkηkσk]+[ΔxkΔukΔλkΔηkΔσk],k=k+1
and go to Step 2.A few words about the implementations. From (14) and (16), the parameters [ΔuΔη] can be eliminated and the differential equations regarding [ΔxΔλ] are obtained. With the boundary conditions (15), the solutions of [ΔxΔλ] can be achieved. Finally, the solutions of [ΔuΔη] can be obtained by (16). Once all linear equations are solved by the above procedures, the iterative scheme for the calculation of optimal grasping force is kept going.


## 4. Simulations

To evaluate the performance of the proposed approach, we do simulations for 2D and 3D multifingered robots for grasping manipulations. The 2D grasping simulations are performed with a plane three-fingered robot. The parameter values of the object arem=1kg, I=0.01kgm2, μ=0.6, and the grasping matrices are
(25)G1=[010-10-1-101010],G2=[0-0.03-0.030.030.030.03].The first 2D simulation is the manipulation of90 degrees rotation of the object. The start and end points (x,y,θ) are set to be (0m,0m,0rad) and (1m,1m,-(π/2)rad), respectively. Figure 2 shows the manipulation path of 90 degrees rotation with the time T=1 s and the friction coefficient μ=0.3. The simulation result indicates that the proposed scheme grasps the object to the end point smoothly and accurately. Figure 3 depicts the trajectories of the variables x, y, θ, vx, vy, and ω. We observe that the rotation angle θ varies around 0 which results in a small grasping force. Moreover, the translation speeds are kept within 1.8m/s and the turning speed within 5.5rad/s. The trajectories of the grasping forces are shown in Figure 4. The simulation results show that the normal forces are all nonnegative and the tangent forces satisfy the friction constraint. To evaluate the effect of the friction, simulation with a different value of friction coefficient is also conducted for 90 degrees rotation simulation. Figure 5 shows the manipulation path with the friction coefficient μ=0.1. We observe that the manipulation path length becomes longer as the friction coefficient decreases. Meanwhile, the value of the objective function J is computed to be 3.07 when μ was 0.3 and it becomes 3.39 when μ reduces to 0.1, leading to the increase in grasping force.Figure 2
The manipulation path of90 degrees rotation with T=1s and μ=0.3.Figure 3
The trajectories of the variablesx,y,θ,vx,vy, and ω in 90 degrees rotation simulation.Figure 4
The trajectories of the grasping forces in90 degrees rotation simulation.Figure 5
The manipulation path of90 degrees rotation with T=1s and μ=0.1.The second 2D simulation is the manipulation of180 degrees rotation of the object. The start and end points (x,y,θ) are set to be (0m,0m,-(π/2)rad) and (1m,1m,(π/2)rad), respectively. The friction coefficient is set as μ=0.3. Figures 6-7 depict the manipulation paths of 180 degrees rotation with the time T=1s and T=2s, respectively. We observe that the object moves down initially and reaches to the end point accurately. Moreover, the mean of rotation angle decreases as T increases.Figure 6
The manipulation path of180 degrees rotation with T=1s and μ=0.3.Figure 7
The manipulation path of180 degrees rotation with T=2s and μ=0.3.The 3D grasping simulation is performed with a five-fingered robot which has not been implemented in the literature. The object is considered as a block and its parameter values were set as(26)m=1kg,I11=8.33e-3kgm2,I22=4.17e-3kgm2,I33=1.08e-2kgm2.
The two fingers of the robot grasp the top of the object, while the other three grasp the bottom of the object. The matrices G1 and G2 are (27)G1=[01001001001001000-100-1001001001-100-100100100100],G2=0.01·[005005605-305-305350-3500-50-33-5033-5000-30030-60033303-53].The start and end points (x,y,z,q0,q1,q2,q3) are set to be (0m,0m,0m,1,0,0,0) and (2m,2m,2m,0.5,-0.5,0.5,-0.5), respectively. Figure 8 shows the 3D manipulation path with the time T=2s and the friction coefficient μ=0.3. As we can see, the object moves and rotates to the end point smoothly and accurately. Figures 9-10 depict the trajectories of the variables x, y, z, q0, q1, q2, q3 and vx, vy, vz, ωx, ωy, ωz, respectively. We observe that the trajectories are smooth. The translation speeds are kept within 1.67m/s and the turning speed within 3.77rad/s. The trajectories of the grasping forces are shown in Figure 11. The simulation results indicate that the grasping forces satisfy the friction constraint. The 3D grasping simulations are also performed with a four-fingered robot and a six-fingered robot, respectively. The four-fingered and six-fingered robots place one finger and three fingers on the top center of the object, respectively, while their other three fingers grasping the bottom. Figures 12-13 show the 3D manipulation paths with the four-fingered and six-fingered robots, respectively. From Figures 8, 12, and 13, we observe that increasing the number of the robot fingers can reduce manipulation path length and enhance the maneuverability. Consequently, the simulation results show that the proposed scheme can achieve the accurate manipulation for multifingered robots.Figure 8
The 3D manipulation path of the five-fingered robot.Figure 9
The trajectories of the variablesx,y,z,q0,q1,q2,q3 in five-fingered robot simulation.Figure 10
The trajectories of the variablesvx,vy,vz,ωx,ωy,ωz in five-fingered robot simulation.Figure 11
The trajectories of the grasping forces in five-fingered robot simulation.Figure 12
The 3D manipulation path of the four-fingered robot.Figure 13
The 3D manipulation path of the six-fingered robot.


## 5. Conclusion

In this paper, we have proposed an effective method for multifingered robot path planning and grasping forces computation. The optimal grasping control problem was formulated with the rigid body dynamics of the object and the second-order cone constraints of grasping forces. The SOC complementarity problem was recast as the equations with the Fischer-Burmeister (FB) function, and the semismooth Newton method with the generalized Jacobian of FB function was used to solve the system equations. The simulation results show that the optimal grasping forces can accurately move the object to a goal, demonstrating the effectiveness of the proposed method.



---

*Source: 681710-2013-10-30.xml*