Manipulation of 2D Object with Arbitrary Shape by Robot Finger under

ICROS-SICE International Joint Conference 2009 August 18-21, 2009, Fukuoka International Congress Center, Japan

Manipulation of 2D Object with Arbitrary Shape by Robot Finger under Rolling Constraint Morio Yoshida1 , Suguru Arimoto1,2 and Kenji Tahara3,1 1

RIKEN-TRI Collaboration Center for Human-Interactive Robot Research, RIKEN, Nagoya, Japan (Tel: +81-52-736-5866; E-mail: [email protected]) 2 Department of Robotics, Ritsumeikan University, Shiga, Japan (E-mail: [email protected]) 3 Organization for the Promotion of Advanced Research, Kyushu University, Fukuoka, Japan (E-mail: [email protected])

Abstract: Contact of two contours of a pinched object and a robot finger tip, with arbitrary shape, is expressed in terms of differential geometry. The overall finger-object dynamics with rolling and contact constraints is derived as EulerLagrange’ equation of motion and the first differential equation with curvatures of the contours is derived for updating the length parameters. Despite the complicated mathematical structure, a control input signal is proposed, which can be constructed without using object geometrical information or external sensing, and it is shown that it is effective to stabilize rotation of the object. Consequently, numerical simulations are carried out in order to demonstrate the practicality of the proposed model and control signal. Keywords: Robot Finger, Rolling Constraint, Arbitrary Shape, Numerical Simulation

1. INTRODUCTION

ferential geometry and in a computational manner (see Fig.1). Despite the complicated mathematical structure, a simple control input, which does not need to use object information or external sensing, is proposed to realize stable grasping. Finally numerical simulations are carried out to testify the validity of the derived model and proposed control signal.

Object manipulation with various shapes such as a coffee cup, pencil, and plate has close ties with our daily lives. This is easy for humans by virtue of multiple joints, sensory feedback, and accumulation of learned data. But it is difficult for robots since the scientific exploration of such interactive tasks of manipulation is not yet advanced. The understanding of physical interaction between an object and robot needs to be advanced in order to realize a human-friendly robot. This has led many robotics researchers to examine robotic hands [1]. Most of the explorations in robot hands have focused on kinematics or motion planning for the realization of force-torque closure to accomplish stable grasp. Rolling geometry between two surfaces of objects, an important phenomenon expressing a physical interaction of rigid bodies, was investigated [2]. In the field of multibody dynamics, a lot of useful models expressing contact situations are presented [3]. However, they missed an aspect of the physical interaction between two bodies under rolling constraints in a dynamical sense, despite the demand for unveiling the mechanism of such mutual action among bodies to realize interactive robots with human movements. Arimoto et al. formulated the 2-D and 3-D dynamic pinching models by a pair of robot fingers with hemispherical ends under rolling constraints [4]. This was restricted to the flat surfaces. The group extended the dynamics of pinching to the case of treating objects with arbitrary shape in a horizontal plane [5]. Very recently, this agument is extended to cover the case of 2-D pinching under the arbitrary geometry [6]. In this paper, a 2-D dynamic pinching model with the two contours on arbitrary shapes of a pinched object and a robot fingertip is rederived from the standpoint of dif-

x

O

q1 y

θ n1

q2 O

X Om

b1 Y

Fig. 1 Robot finger pinching an object pivoted at a fixed point Om

2. DYNAMICS In order to understand a rolling phenomenon between two contours with arbitrary shape, the simplest mechanical setup is considered (see Fig.1). The robot finger with a finger end of arbitrary shape has two degrees of freedom (2.D.O.Fs). The object pivots around the fixed point Om = (x, y). It is assumed that motion of the overall finger-object system is restricted to a horizontal plane and that the effect of gravity can be ignored. q1 and q2 express

- 695 -

PR0002/09/0000-0695 ¥400 © 2009 SICE

the joint angles of the robot finger, and θ the angle of the object. In the coordinate system, numerical values of all angles are positive in counterclockwise direction. Local coordinate Om -XY is attached to the pinched object, and local coordinate O01 -X1 Y1 is attached to the finger tip of the robot. O-xy expresses the inertial frame coordinates. The contour of the left side of the object is expressed by a curve attached to the local coordinates (X(s), Y (s)) by using arclength parameter s. Similarly the contour of the finger tip is expressed by a curve attached to the local coordinates (X0 (s), Y0 (s)). P1A and P1B are the contact

l n1 ln0

P1 P1

O01 l b0

X0 x n0

P1A

P1B

P0 b0

lb1

n1 Om b1

b1

Y0 b0 s=0

θ1

X0

s=0

P1

O01

x n0

P0 q1 +q2

ψ0

ϕ1

P1A

P1B

b0 Y0 b0

n1

θ

θ1 y

θ

The length P0� P1� is expressed in the inertial coordinates as follows:

x

Om b1

Fig. 3 Geometrical relationship of ln0 , ln1 , lb0 , and lb1

X

P1

P0� P1� = (x − x01 ) cos(θ + θ1 ) −(y − y01 sin(θ + θ1 ))

Y b1

This length is equal to ln0 + ln1 and then we define Q1 as follows:

Fig. 2 Geometrical relationship between local coordinates Om -XY and O01 -X0 Y0

−Q1

point between the robot fingertip and the object surface from the robot and object views respectively. Similarly n0 and n1 are unit normal vectors orthogonal to the tangent plane at the contact from the robot and object views respectively. The angle between the vector n1 and X axis is expressed by θ1 as follows: θ1 (s) =

arctan {X � (s)/Y � (s)}

X0� (s)

arctan {X0� (s)/Y0� (s)}

dX0� /ds

ϕ1 = π − (q1 + q2 ) + (θ + θ1 (s)) ϕ1 − ψ0 = π/2

(1)

(2)

Y0� (s)/ds.

X0 (s) sin ψ0 + Y0 (s) cos ψ0 −X(s) sin θ1 − Y (s) cos θ1 X0 (s) cos ψ0 − Y0 (s) sin ψ0 −X(s) cos θ1 + Y (s) sin θ1

(12)

(13)

We define the contact constraint as a holonomic constraint in the form: � π� + (ln0 + ln1 ) Q = Q + lb0 ψ0 + 2 � π� = Q1 + (ln0 + ln1 ) − lb0 ϕ1 − ψ0 − 2 = 0 (14) where Q = Q1 − lb0 {π + θ + θ1 (s) − q1 − q2 }

(15)

d Q=0 dt

(16)

and

and also we derive the equation:

It is noted that n0 and b0 must be equal to −n1 and b1 respectively in order to satisfy the rolling constraint. We define ln0 , ln1 , lb0 , and lb1 (see Fig.3) as follows: = = = =

(x − x01 ) cos(θ + θ1 ) −(y − y01 sin(θ + θ1 )) ln0 (s) + ln1 (s)

We define the relative angle ϕ1 by the definitions of θ1 and ψ0 as follows:

where = and The unit normal vectors n0 and n1 and the unit tangent vectors b0 and b1 are expressed as follows: � � sin(θ + θ1 ) (3) b1 = cos(θ + θ1 ) � � cos(θ + θ1 ) n1 = (4) − sin(θ + θ1 ) � � − cos(q1 + q2 + ψ0 ) (5) b0 = sin(q1 + q2 + ψ0 ) � � sin(q1 + q2 + ψ0 ) n0 = − (6) cos(q1 + q2 + ψ0 )

lb0 (s) lb1 (s) ln0 (s) ln1 (s)

= =

where X � (s) = dX(s)/ds and Y � (s) = dY (s)/ds. Similarly the angle between the vector n0 and X0 is expressed by ψ0 as follows: ψ0 (s) =

(11)

�

(7) (8) (9) (10)

∂Q/∂θ, ∂Q/∂q1 , ∂Q/∂q2

which is reduced to �

- 696 -

�

⎞ dθ ⎝ dq1 ⎠ = 0 dq2 ⎛

lb1 (s), nT 1 (s)J01 (q) + lb0 (s)e

� T

(17)

⎞ dθ ⎝ dq1 ⎠ = 0 (18) dq2 ⎛

where J01 (q) = ∂(x01 , y01 )/∂(q1 , q2 ). The length Om P1� + P0� O01 (see Fig.3) is expreesed in the inertial frame as follows: Om P1� + P0� O01

=

where κ0 (s) expresses the curvature of the fingertip’s contour and κ1 (s) the curvature of the object’s contour. κ0 (s) and κ1 (s) are defined as follows (see [6]):

−(x − x01 ) sin(θ + θ1 ) −(y − y01 ) cos(θ + θ1 ) (19)

κ0 κ1

This length is equal to lb0 (s) + lb1 (s) and we define R1 as follows: R1

= −(x − x01 ) sin(θ + θ1 ) −(y − y01 ) cos(θ + θ1 ) = lb0 (s) + lb1 (s)

�

Though the contours of the finger tip and object are of complicated geometry, we propose a simple control input which does not need to use the information of the object and the data of external sensing, in order to stabilize the object. This is described as � � fd T x01 − x u = −cq˙ − J01 (q) (32) y01 − y r

(20)

T lb1 (s), bT 1 (s)J01 (q) − ln0 (s)e

(21)

�

⎞ dθ ⎝ dq1 ⎠ = 0 (22) dq2 ⎛

Equation (22) can be integrated in the sense of Frobenius. We define the rolling constraint as follows:

(23)

R = ln0 (s) {π + θ + θ1 (s) − q1 − q2 } + R1

(24)

where

=

where c and r are positive constants. The first term of the right hand side of eq.(32) works as damping. The second term is introduced to cease the rotational motion of the object. By applying the proposed control input (eq.(32)) to the overall finger-object dynamics (eqs (27) and (28)), we obtain the closed-loop dynamics as follows: � � 1 ˙ G(q)¨ q+ G + S + cq˙ 2

∂R fd ∂Q + Δλ − N1 e = 0 ∂q ∂q r ∂R fd ∂Q + Δλ + N1 = 0 I θ¨ + Δf ∂θ ∂θ r where fd fd Δf = f + Q1 , Δλ = λ + R1 r r N1 = lb0 Q1 − ln0 R1 = −lb0 ln1 + ln0 lb1 +Δf

R + {lb0 (s) + lb1 (s)} � π� =0 −ln0 (s) ψ0 (s) + 2

R

(30) (31)

3. CONTROL SIGNAL & CLOSED DYNAMICS

Rolling constraint expresses the condition of rolling on the surface without slipping. The tangent velocity of the object on the tangent plane of the contact point P1 must be equal to the tangent velocity of the finger tip on the same tangent plane, at instant t, as follows: ∂ ∂ ln0 ϕ1 + R1 = 0 ∂t ∂t wihch is reduced to

= −X0�� (s)Y0 (s) + X0� (s)Y0�� (s) = X �� (s)Y � (s) − X � (s)Y �� (s)

and also d R=0 (25) dt The total kinetic energy of the finger robot and object is obtained as follows: 1 1 (26) K = q˙T G(q)q˙ + I θ˙2 2 2 where G(q) stands for the inertia matrix of the robot finger and I for the inertia moment of the object around Om . By defining the Lagrangian L = K and installing tangent vectors (∂θ, ∂q 1 ,∂q 2 ) satisfying eqs (18) and (22), the dynamic equations of motion of the overall finger-object system are derived:

(33) (34)

(35) (36)

4. NUMERICAL SIMULATION

I θ¨ + f lb1 − λln1 = 0 (27) � � � � 1 ˙ T (q)n1 + lb0 e G(q)¨ q+ G + S q˙ + f J01 2 � T � λ J01 (q)b1 − ln0 e = u (28)

where q = (q1 , q2 )T . From the difinition of directional derivatives of b0 and b1 (see[6]), the arclength parameter s should be updated as follows � ds � = q˙1 + q˙2 − θ˙ {κ0 (s) + κ1 (s)} (29) dt

We carry out numerical simulations in order to verify the effectiveness of our proposed model and control input. The physical parameters of the overall system shown in Table 1 and the parameters of control input shown in Table 2 are used in the simulations. As an example of an object with arbitrary shape, the curve c0 (s) = (X0 (s), Y0 (s)) of the fingerend’s contour, attached to the local coordinate O01 -X0 Y0 , is used as follows (see Fig.4-a): √ 1 + 4 × 202 × s2 X0 (s) = 0.035 − (37) 2 × 20 Asinh(2 × 20 × s) (38) Y0 (s) = 2 × 20

Similarly the curve c(s) = (X(s), Y (s)) of the object’s contour, attached to the local coordinate Om -XY , is used as follows (see Fig.4-b):

X(s) = −0.03 � 1 + 4 × 502 × (s − 3.363 × 10−3 )2 + (39) 2 × 50

- 697 -

Table 1 Physical parameters of the fingers and object.

-0.05

-0.05

0.500 [N] 0.006 [Nms] 0.010 [m] 1500 3000 225.0 × 104 900.0 × 104

-0.1

-0.1

Δf The value of zero

-0.15 0

1

2

time[s]

0

3

0

0

f r N The value of zero -0.003

Y [m]

Y0[m]

0

Om

0

1

time[s]

2

-0.005

3

0

1

fd r N1

Fig. 8

0

0.01

X0[m]

0.02

0.03

-0.03

. -0.02

-0.01

X [m]

0

0.01

1

.

(a) The curve of (b) The curve of the fingertip’s contour the object’s contour Fig. 4 The local coordinates of the fingertip and the object

1

.

0

0

-1

-1 0

1

time[s]

2

3

0

1

Fig. 10 q˙1 2

2

3

0 -1

1

R [m]

θ [rad/s]

time[s]

Fig. 11 q˙2 (×10-10)

.

3

q2 The value of zero

2

q2[rad/s]

-0.01

q1[rad/s]

-0.02

2

2

.

q1 The value of zero

-0.01

time[s]

Fig. 9 s

-0.01 -0.02

3

0.005

-0.001

0.01

0

2

time[s]

0.01

-0.002

O01

1

Fig. 7 Δλ

Fig. 6 Δf

0.02 0.01

Δλ The value of zero

-0.15

f N[Nm] r

internal force damping coefficient constant value CSM gain CSM gain CSM gain CSM gain

fd c r γf 1 γλ1 ωf 1 ωλ1

0

Δ f[N]

Table 2 Parameters of control signals & CSM gains

0

Δλ[N]

0.065 [m] 0.065 [m] 0.045 [kg] 0.040 [kg] 0.040 [kg]

s[m]

length length weight weight object weight

l11 l12 m11 m12 M

maintains the rolling and contact constraints during the simulation. Consequently it is confirmed through the numerical simulations that our proposed model, control input, and simulation method are effective.

-2 -3

0

. 0

1

time[s]

2

R The value of zero

-4

θ The value of zero

-1

-5

3

0

1

Fig. 12 θ˙ (×10-9)

(a) Initial pose (b) After 3 seconds Fig. 5 Motion of pinching a 2-D object with arbitrary shape

time[s]

2

3

Fig. 13 R

6

Q The value of zero

Q[m]

4

Asinh(2 × 50 × (s − 3.363 × 10−3 )) (40) 2 × 50 It is arclength parameter, � is noted that, because s � X0� (s)2 + Y0� (s)2 = 1 and X � (s)2 + Y � (s)2 = 1. If a solution to the closed-loop dynamics (eqs. (33) and (34)) converges to an equilibrium point, it should satisfy q¨ → 0,θ¨ → 0,q˙ → 0,θ˙ → 0, Δf → 0, Δλ → 0, and (fd /r)N1 → 0. It is confirmed, from Figs.6 ∼ 12, that key variables of the closed dynamics converge to the equilibrium point. These graphs and the pictures in the middle of the simulation (see Fig.5) show that motion of the robot finger achieves stable grasping by applying our proposed control input. It is also shown, from Figs.13 and 14, that the “Constraint Stabilization Method” (CSM) [7]

2

0

Y (s) =

0

1

time[s]

2

3

Fig. 14 Q

5. CONCLUSION A 2-D dynamic pinching model with two contours of a fingertip and object with arbitrary geometry is proposed. It is important that our numerical simulation method becomes easy because rolling and contact constraints treating the contours of arbitrary shapes are fortunately expressed as holonomic constraints in a 2-D case. Our proposed control input is simple and practical though the fin-

- 698 -

gertip and object models have complicated shapes. Finally it is confirmed that our proposed model and control input are effective, in terms of the graphs and visualization of the results of numerical simulation.

6. ACKNOWLEDGMENTS This work was partially supported by Japan Society for the Promotion of Science (JSPS), Grant-in-Aid for Scientific Research (B) (20360117).

REFERENCES [1] R. M. Murray, Z. Li, and S. S. Sastry, A Mathematical Introduction to Robotic Manipulation, CRC Press, Boca Raton, 1994. [2] D.J. Montana. The kinematics of contact and grasp, Int. J.of Robotics Research, Vol.7, No.3, pp.17-32, 1988. [3] A.A. Shabana, Computational Dynamics, Second Edition, Wiley-Interscience, New York, USA, 2001. [4] S. Arimoto, Control Theory of Multi-fingered Hands, Springer-Verlag, London, 2008. [5] S. Arimoto, M. Yoshida, M. Sekimoto, and K. Tahara. A Reimannian-Geometry Approach for Control of Robotic Systems under Constraints, SICE J. of Control, Measurement , and System Integration, Vol.2, No.2, pp.107-116, 2009. [6] S. Arimoto, M. Yoshida, M. Sekimoto, and K. Tahara. Modeling and Control of 2-D grasping of an object with arbitrary shape under rolling contact, accepted for publication in SICE JCMSI, Vol.2, 2009. [7] J. Baumgarte. Stabilization of constraint and integrals of motion in dynamical systems, Comput. Methods in Appl. Mech. and Eng., Vol.1, No.1, pp.1-16, 1972.

- 699 -