Optimal Grasping Manipulation for Multifingered Robots Using Semismooth Newton Method

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.

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: Section 2 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

Figure 1 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𝑦=𝑣,𝑣=𝑚𝑅𝐺₁𝑢+00−𝑔^𝑇,̇̇𝑞=𝑄𝜔,𝜔=𝐼⁻¹𝑅𝐺₂𝑢−𝜔×(𝐼𝜔), (1) where 𝑦 is the position, 𝑣 is the velocity, 𝑞=𝑞₁𝑞₂𝑞₃^𝑇 is the quaternion, 𝜔 is the angular velocity, 𝑚 is the object mass, 𝐼 is the matrix of moment of inertia, 𝑔 is the gravity constant, 𝑢 means the grasping forces which is represented by a matrix, [𝐺₁𝐺₂] is the contact matrix, 𝑅 is the rotation matrix of the object, and 𝑄 can be expressed as 1𝑄=2𝑞₀𝑞₃−𝑞₂−𝑞₃𝑞₀𝑞₁𝑞₂−𝑞₁𝑞₀with𝑞₀=𝑞²₁+𝑞²₂+𝑞²₃. (2) Moreover, the grasping forces are subject to the contact friction constraint, expressed as 𝑢_𝑖2,𝑢_𝑖3≤𝜇𝑢_𝑖1, (3) where 𝑢_𝑖1 is the normal force of the ith finger, 𝑢_𝑖2 and 𝑢_𝑖3 are the friction forces of the 𝑖th 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 min^𝑇₀̇(),𝑥(0)𝐿𝑑𝑡,s.t.𝑥=𝑓𝑥,𝑢=𝑥₀,𝑥(𝑇)=𝑥_𝑇,𝐷𝑢∈𝒦^𝑑×𝒦^𝑑×⋯×𝒦^𝑑, (4) where 𝑢𝐿=^𝑇𝑢2𝑦𝑣𝑞𝜔,𝑥=. (5) In addition, 𝑓 represents the right hand side of system (1), 𝑇 is the control duration, 𝑥₀ and 𝑥_𝑇 are the initial and final states, respectively, 𝐷 is the diagonal matrix with the friction coefficient, and 𝒦 denotes the second-order cone which is given by 𝒦^𝑑𝑧∶=¹𝑧₂∈ℝ×ℝ^𝑑−1∣𝑧₂≤𝑧₁. (6)

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: 𝑥̇−𝐻_𝜆=̇()̇𝑥−𝑓𝑥,𝑢=0,𝜆+𝐻_𝑥=𝜆̇+𝜆^𝑇𝑓_𝑥𝐻=0,_𝑢=𝐿_𝑢+𝜆^𝑇𝑓_𝑢+𝜂^𝑇𝜙(𝑥(0)(𝑇𝜆(0)𝐷=0,,𝑥))=0,+𝜙^𝑇_𝑥(0)𝜆(𝑇)𝜎=0,+𝜙^𝑇_𝑥(𝑇)𝜎=0, (7) where 𝜆, 𝜂, and 𝜎 are the Lagrange multipliers, and 𝜙(𝑥(0),𝑥(𝑇))=^{𝑥(0)−𝑥0}_{𝑥(𝑇)−𝑥𝑇}. The second part forms a second-order cone complementarity problem (SOCCP) as follows: −𝜂∈𝒦,𝐷𝑢∈𝒦,𝜂^𝑇𝐷𝑢=0, (8) where 𝒦=𝒦^𝑑×𝒦^𝑑×⋯×𝒦^𝑑.

From [19, 20], we see that the previous SOCCP (8) can be further recast as a system of equations: 𝜙_FB𝐷𝑢,−𝜂=0 (9) by employing the so-called complementarity function 𝜙_FB which is a vector-valued function defined as 𝜙_FB(𝐚,𝐛)∶=𝐚²+𝐛^21/2−(𝐚+𝐛) (10) for 𝐚=^𝑎1_𝑎2∈ℝ×ℝ^𝑑−1,𝐛=^𝑏1_𝑏2∈ℝ×ℝ^𝑑−1. We point out that the square term and square-root term in (10) are calculated via Jordan product 𝐚𝐚∘𝐛=^𝐓𝐛𝑎₁𝑏₂+𝑏₁𝑎₂. (11) In particular, the expressions for 𝐚² and 𝐚^1/2 are given by 𝐚²=‖𝐚‖²2𝑎₁𝑎₂𝐚_1/2,=𝑠𝑎₂2𝑠with𝐬=12𝐚₁+𝐚²₁−𝐚₂², (12)(13) 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: Δ𝑥̇Δ𝜆̇−𝑓_𝑥^(𝑘)0−𝐻^(𝑘)_𝑥𝑥−𝑓_𝑥^(𝑘)𝑇−Δ𝑥𝑓Δ𝜆^𝑢(𝑘)0−𝐻^(𝑘)_𝑥𝑢0𝑥̇Δ𝑢Δ𝜂=−^(𝑘)−𝑓^(𝑘)𝜆̇^(𝑘)+𝐻_𝑥^(𝑘),𝜙_𝑥(0)^(𝑘)𝜙000𝐼_𝑥(0)^(𝑘)𝑇00−𝜙_𝑥(0)^(𝑘)𝑇(0)(0)(0)+Δ𝑥𝜙Δ𝜆Δ𝜎^{𝑥(𝑇)(𝑘)}(𝑇)(𝑇)(𝑇)𝜙000000𝐼0Δ𝑥Δ𝜆Δ𝜎=−𝜆^(𝑘)(0)+𝜙^(𝑘)_𝑥(0)^(𝑘)𝑇𝜎^(𝑘)𝜆^(𝑘)(𝑇)+𝜙_𝑥(𝑇)^(𝑘)𝑇𝜎^(𝑘),𝐼𝐷^𝑇𝑉_𝐚^(𝑘)𝐷−𝑉_𝐛^(𝑘)+Δ𝑢𝜆Δ𝜂^𝑇𝑓^(𝑘)_𝑥𝑢𝑓_𝑢^(𝑘)𝐻00Δ𝑥Δ𝜆=−^𝑢(𝑘)𝜙_FB𝐷𝑢^(𝑘),−𝜂^(𝑘). (14)(15)(16) Most information in linear equations (14)–(16) is known except the generalized Jacobian 𝑉_𝐚, 𝑉_𝐛 in (16). What do they represent? We provide a brief introduction here. First, we recall the concept of the 𝐵-subdifferential. Given a mapping Ψ∶ℝ^𝑛→ℝ^𝑚, if Ψ is locally Lipschitz continuous, then the set 𝜕_𝐵𝐻(𝑧)∶=𝑉∈ℝ^𝑚×𝑛∣∃𝑧^𝑘⊆𝐷_Ψ∶𝑧^𝑘⟶𝑧,Ψ^′𝑧^𝑘⟶𝑉 (17) is nonempty and is called the 𝐵-subdifferential of Ψ at 𝑧, where 𝐷_Ψ⊆ℝ^𝑛 denotes the set of points at which Ψ is differentiable. The convex hull 𝜕Ψ(𝑧)∶=conv𝜕_𝐵Ψ(𝑧) (18) is the generalized Jacobian of Clarke [23]. From this definition, we see that the generalized Jacobian of 𝜙_FB can be obtained by computing 𝜕_𝐵𝜙_FB. From [24, Proposition 3.1], the 𝐵-subdifferential of 𝜙_FB in (16) is exactly expressed as 𝜕_𝐵𝜙_FB()=𝑉𝐚,𝐛^𝑇𝐚𝑉^𝑇_𝐛. (19) Moreover, by denoting 𝐜∶=(𝐚²+𝐛²)^1/2, we have(a) If 𝐚²+𝐛²∈int(𝒦^𝐝), then 𝑉_𝐚=𝐿_𝐜⁻¹𝐿_𝐚 and 𝑉_𝐛=𝐿_𝐜⁻¹𝐿_𝐛.(b) If 𝐚²+𝐛²∈bd(𝒦^𝐝) and (𝐚,𝐛)≠(𝟎,𝟎), then 𝑉_𝐚∈122𝑤₁1𝑤^𝑇₂𝑤₂4𝐼−3𝑤₂𝑤^𝑇₂𝐿_𝐚+121−𝑤₂𝛼^𝑇,𝑉_𝐛∈122𝑤₁1𝑤^𝑇₂𝑤₂4𝐼−3𝑤₂𝑤^𝑇₂𝐿_𝐛+121−𝑤₂𝛽^𝑇 (20) for some 𝛼=^𝛼1_𝛼2∈ℝ×ℝ^𝑑−1𝛽=^𝛽1_𝛽2∈ℝ×ℝ^𝑑−1 satisfying |𝛼₁|≤‖𝛼₂‖≤1 and |𝛽₁|≤‖𝛽₂‖≤1, where 𝑤₂=𝑤₂/‖𝑤₂‖.(c) If (𝐚,𝐛)=(𝟎,𝟎), then 𝑉_𝐚∈{𝐿𝐚}, 𝑉_𝐛𝐛∈{𝐿} for some 𝐚, 𝐛 with ‖𝐚‖²+‖𝐛‖²=1, or 𝑉_𝐚∈121𝑤₂𝜉^𝑇+121−𝑤₂𝛼^𝑇+200𝐼−𝑤₂𝑤^𝑇₂𝛿₂𝐼−𝑤₂𝑤^𝑇₂𝛿₁,𝑉_𝐛∈121𝑤₂𝜏^𝑇+121−𝑤₂𝛽^𝑇+200𝐼−𝑤₂𝑤^𝑇₂𝛾₂𝐼−𝑤₂𝑤^𝑇₂𝛾₁ (21)for some 𝛼=^𝛼1_𝛼2,𝛽=^𝛽1_𝛽2,𝜉=^𝜉1_𝜉2,𝜏=^𝜏1_𝜏2∈ℝ×ℝ^𝑙−1 such that 𝛼₁≤𝛼₂≤1,𝛽₁≤𝛽₂𝜉≤1,₁≤𝜉₂≤1,𝜏₁≤𝜏₂≤1, (22)𝑤₂∈ℝ^𝑙−1 satisfying ‖𝑤₂‖=1, and 𝛿=^𝛿1_𝛿2,𝛾=^𝛾1_𝛾2∈ℝ×ℝ^𝑑−1 satisfying ‖𝛿‖²+‖𝛾‖²≤1/2.

Note that the calculations of 𝐿_𝐚 and 𝐿_𝐚⁻¹ are given by 𝐿_𝐚=𝑎₁𝑎^𝑇₂𝑎₂𝑎₁𝐼,𝐿_𝐚⁻¹=1𝑎²₁−𝑎₂²𝑎₁−𝑎^𝑇₂−𝑎₂𝑎²₁−‖𝑎₂‖²𝑎₁1𝐼+𝑎₁𝑎₂𝑎^𝑇₂. (23) 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.

Algorithm

Step 1. Choose 𝑥⁰, 𝑢⁰, 𝜆⁰, 𝜂⁰, 𝜎⁰ and set 𝑘=0.

Step 2. If convergence criterion is satisfied, stop.

Step 3. Compute the direction Δ𝑥^𝑘, Δ𝑢^𝑘, Δ𝜆^𝑘, Δ𝜂^𝑘, Δ𝜎^𝑘 from the linear equations (14)–(16).

Step 4. Set 𝑥^𝑘+1𝑢^𝑘+1𝜆^𝑘+1𝜂^𝑘+1𝜎^𝑘+1=𝑥^𝑘𝑢^𝑘𝜆^𝑘𝜂^𝑘𝜎^𝑘+Δ𝑥^𝑘Δ𝑢^𝑘Δ𝜆^𝑘Δ𝜂^𝑘Δ𝜎^𝑘,𝑘=𝑘+1 (24) and go to Step 2.

A few words about the implementations. From (14) and (16), the parameters Δ𝑢Δ𝜂 can be eliminated and the differential equations regarding Δ𝑥Δ𝜆 are obtained. With the boundary conditions (15), the solutions of Δ𝑥Δ𝜆 can be achieved. Finally, the solutions of Δ𝑢Δ𝜂 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 are 𝑚=1kg, 𝐼=0.01kgm², 𝜇=0.6, and the grasping matrices are 𝐺₁=,𝐺010−10−1−101010₂=0−0.03−0.030.030.030.03. (25)

The first 2D simulation is the manipulation of 90 degrees rotation of the object. The start and end points (𝑥,𝑦,𝜃) 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 𝑇=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 𝑥, 𝑦, 𝜃, 𝑣_𝑥, 𝑣_𝑦, 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 𝐽 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 of 90 degrees rotation with 𝑇=1s and 𝜇=0.3.

Figure 3 

The trajectories of the variables 𝑥,𝑦,𝜃,𝑣𝑥,𝑣𝑦, and 𝜔 in 90 degrees rotation simulation.

Figure 4 

The trajectories of the grasping forces in 90 degrees rotation simulation.

Figure 5 

The manipulation path of 90 degrees rotation with 𝑇=1s and 𝜇=0.1.

The second 2D simulation is the manipulation of 180 degrees rotation of the object. The start and end points (𝑥,𝑦,𝜃) 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 𝑇=1s and 𝑇=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 𝑇 increases.

Figure 6 

The manipulation path of 180 degrees rotation with 𝑇=1s and 𝜇=0.3.

Figure 7 

The manipulation path of 180 degrees rotation with 𝑇=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 𝑚=1kg,𝐼₁₁=8.33𝑒−3kgm²,𝐼₂₂=4.17𝑒−3kgm²,𝐼₃₃=1.08𝑒−2kgm². (26) The two fingers of the robot grasp the top of the object, while the other three grasp the bottom of the object. The matrices 𝐺₁ and 𝐺₂ are 𝐺₁=,𝐺01001001001001000−100−1001001001−100−100100100100₂=0.01⋅√005005605−305−305350−3500−50−33−503√√3−5000−30030−60033303−5√3. (27)The start and end points (𝑥,𝑦,𝑧,𝑞₀,𝑞₁,𝑞₂,𝑞₃) 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 𝑇=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 𝑥, 𝑦, 𝑧, 𝑞₀, 𝑞₁, 𝑞₂, 𝑞₃ and 𝑣_𝑥, 𝑣_𝑦, 𝑣_𝑧, 𝜔_𝑥, 𝜔_𝑦, 𝜔_𝑧, 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 variables 𝑥,𝑦,𝑧,𝑞0,𝑞1,𝑞2,𝑞3 in five-fingered robot simulation.

Figure 10 

The trajectories of the variables 𝑣𝑥,𝑣𝑦,𝑣𝑧,𝜔𝑥,𝜔𝑦,𝜔𝑧 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.

Acknowledgments

This work was supported in part by the National Science Council of Taiwan, ROC under the Grant NSC 100-2221-E-214-016. The author's work is supported by National Science Council of Taiwan.