Kiss, Lévine, and Lantos / On Motion Planning for Robotic Manipulation 445. Table 1. Classification of HOSs. Number of. Hand Structure. Model. Fingers.
Bálint Kiss Jean Lévine Centre Automatique et Systèmes Ecole des Mines de Paris Fontainebleau, France, kiss@cas.ensmp.fr levine@cas.ensmp.fr
Béla Lantos
On Motion Planning for Robotic Manipulation with Permanent Rolling Contacts
Department of Control Engineering and Information Technology Budapest University of Technology and Economics Budapest, Hungary lantos@iit.bme.hu
Abstract We address the motion planning problem (open-loop trajectory design) for manipulating rigid bodies with permanent rolling contacts without slipping. This problem is related in particular to dextrous manipulation with robotic hands, consisting in changing the position and orientation of the manipulated object together with its grasp. We prove the flatness property for planar structures allowing to solve the motion planning problem by simple interpolation, without need to integrate the system differential equations and without quasi-static approximations. Though this property fails to be valid for general three-dimensional hand structures, similar results can be obtained for special structures thanks to the notion of Liouvillian systems. The links between flat or Liouvillian hand-object structures and non-holonomy of the contacts between fingers and object are also discussed. Several examples are studied in detail.
KEY WORDS—dextrous manipulation, nonholonomic mechanical systems, motion planning, flat systems, Liouvillian systems
1. Introduction We study the motion planning problem (MPP) or, roughly speaking, the open-loop trajectory design of robotic hands manipulating objects via one-point rolling contacts without slipping. In such systems all the fingers’ degrees of freedom are actuated as opposed to the object’s ones. The control objective is to change the orientation, position and grasp of the manipulated object. The International Journal of Robotics Research Vol. 21, No. 5-6, May-June 2002, pp. 443-461, ©2002 Sage Publications
The modeling of hand-object structures (HOS) and the kinematics of contact in the context of dextrous manipulation is treated in detail in the robotics literature (see, e.g., Cole, Hsu, and Sastry (1992), Mason and Salisbury (1985), Montana (1988), Murray, Li, and Sastry (1994), Li, Canny, and Sastry (1989), and Marigo and Bicchi (2000), including controllability aspects). An exhaustive review of static grasping methods is presented in Shimoga (1997) and a recent survey paper (Bicchi 2000) gives the state-of-the-art of the field of dextrous manipulation. Motion planning algorithms which can be applied to a large class of dextrous manipulation problems are reported in Cherif and Gupta (1997) and Vass, Payandeh, and Lantos (1999) using quasi-static approximations. These algorithms involve optimization in order to find contact forces for a given constant velocity, based on Peshkin’s minimum power principle (Peshkin and Sanderson 1989). Due to the constraints of rolling without slipping, threedimensional HOSs are nonholonomic. The underlying motion planning problem can be solved without quasi-static approximations by methods reported in Lafferriere and Sussmann (1991) and Murray and Sastry (1993) using piecewise constant or sinusoidal inputs. An explicit solution in this framework is provided for the case of two parallel planar fingers with a symmetric object including non-pivoting constraints by Marigo and Bicchi (2000). An extension of the method of Lafferriere and Sussmann (1991) when the fingers are allowed to break contact with the object (i.e., for stratified configuration spaces) can be found in Goodwine (1998). Our approach uses the notions of (differential) flatness (Fliess et al. 1992, 1995, 1997, 1999; Martin and Rouchon 1994) and Liouvillian systems. The latter notion was defined
443
444
THE INTERNATIONAL JOURNAL OF ROBOTICS RESEARCH / May-June 2002
in an algebraic context in Chelouah (1997) and already used for motion planning purposes in Sira-Ramírez (1999). Note that, though flatness can be also exploited for feedback design purposes, the feedback design and tracking aspects are not dealt with in this paper. For the sake of completeness, let us mention interesting contributions concerning feedback control of object manipulation: Cole, Hsu, and Sastry (1992) for the rigid body case, Arimoto et al. (2000) and Cheah et al. (1998) with soft fingers and with uncertainties on the Jacobian matrices, the learning control scheme proposed in Naniwa, Arimoto, and Wada (1999) with unknown inertia parameters of the object, and the input-output linearization technique proposed in Sarkar, Yun, and Kumar (1997) for the manipulation of large objects with two manipulators having six joints each. A state-space representation of the entire nonholonomic system is also given in Sarkar, Yun, and Kumar (1997). Although we exclusively address the case with rolling contacts, let us also mention the references Cole, Hauser, and Sastry (1989) and Zheng, Nakashima, and Yoshikawa (2000) for the sliding contact case. Coming back to motion planning, the flatness property allows an easy parameterization of all feasible trajectories of the system which does not require integration of differential equations, and does not restrict to quasi-static approximations. More precisely, the definition of flatness directly implies the existence of a so-called flat output, of the same dimension as the input, such that there is a one-to-one mapping between sufficiently smooth trajectories of the flat output and feasible trajectories of the system, including the inputs, making the integration of the system dynamics useless. Thus, the motion planning can be done in the flat output space using elementary interpolation methods. The Liouvillian property is slightly weaker. Roughly speaking, a system is Liouvillian if there exists an output of the same dimension as the input such that all the system variables can be expressed as functions of this output, a finite number of its derivatives, and a finite number of functions of its integrals. The output in this case is called a partially flat output, and the additional functions of the output integrals are referred to as integral variables. Thus, the flatness based approach to motion planning can be extended up to the computation of a finite number of integrals of functions of time, which is still simpler than integrating the system differential equations. We prove that the dynamic model of planar HOSs is flat as a consequence of the constraints holonomy. Here the inputs are the joint torques of the fingers and a flat output can be chosen as the position and orientation of the object and combinations of the contact forces. In the three-dimensional case, the rolling without slipping condition gives rise to nonholonomic constraints. As is, non-holonomy does not exclude flatness (see for instance the examples of the car with n-trailers or of the rolling hoop, or penny, in Martin and Rouchon (1994)), but in our case the kinematic equations form a Liouvillian system.
According to the dimension, the number of fingers, and the model type (i.e., dynamic or kinematic) we show that HOSs can be classified as summarized in Table 1. We consider both kinematic and dynamic models of the HOS. Note that, for the kinematic models, the problem of determining the contact forces is not addressed. The paper is organized as follows. The next section gives the definitions of flat and Liouvillian systems. Section 3 is devoted to the modeling of three-dimensional and planar HOSs and gives the proof of the holonomy in the planar case. The MPP is solved in Section 4, following the classification of Table 1 and exploiting the flatness and Liouvillian properties. A special class of HOSs having mechanical symmetries is discussed in Section 5. The paper ends with a short conclusion and, for the reader’s convenience, an Appendix summarizes some notions used extensively throughout the text.
2. Flatness and Liouvillian Properties DEFINITION 1.
(flatness) The system 0 = F (x, x, ˙ x, ¨ . . . , x (q) , u)
(1)
with q finite integer, x ∈ Rn , u ∈ Rm , and F : Rn(q+1)+m → Rn is differentially flat if one can find a set of variables, called flat output, ˙ u, ¨ . . . , u(r) ), y = h(x, x, ˙ . . . , x (q−1) , u, u,
y ∈ Rm (2)
with r finite integer, such that x = α(y, y, ˙ y, ¨ . . . , y (a) )
u = β(y, y, ˙ y, ¨ . . . , y (b) )
(3)
with a and b finite integers, and such that the system equations dα (y, y, ˙ y, ¨ . . . , y (a+1) ), 0 = F α(y, y, ˙ y, ¨ . . . , y (a) ), dt . . . , β(y, y, ˙ y, ¨ . . . , y (b) ) are identically satisfied. The weaker notion of Liouvillian systems is defined in Chelouah (1997) using differential algebra. We adopt here a slightly different definition. DEFINITION 2. (Liouvillian system) The system (1) is said to be Liouvillian if there exists a set of variables defined as (2), and a finite number of variables ξ1 , . . . , ξp given by ˙ . . . , y (a+1) ) ξi(µi ) = γi (y, y,
i = 1, . . . , p
(4)
such that x = α(y, y, ˙ y, ¨ . . . , y (a) , ξ¯1µ1 −1 , . . . u = β(y, y, ˙ y, ¨ . . . , y (b) , ξ¯1µ1 −1 , . . .
, ξ¯pµp −1 ) , ξ¯pµp −1 ),
(5)
Kiss, Lévine, and Lantos / On Motion Planning for Robotic Manipulation
445
Table 1. Classification of HOSs Hand Structure
Model
Number of Fingers
Holonomy
Flatness
Liouvillian
2D 3D 3D 3D + symmetry
dynamics kinematics kinematics dynamics
>1 1 >1 >2
yes no no no
yes yes (3 inputs) ? ?
– yes (2 inputs) yes yes
with ξ¯iµi −1 = (ξi , ξ˙i , . . . , ξi(µi −1) ), and such that α and β identically satisfy the system equations. We refer to y as a partially flat output and to ξ1 , . . . , ξp as integral variables. The flat (resp. partially flat) output is not unique in the sense that if y is a flat (resp. partially flat) output then y = (y) is also a flat (resp. partially flat) output, assuming that is a diffeomorphism. Note also that a flat (resp. partially flat) output does not have an a priori physical interpretation. Nevertheless, in our case, it turns out that y has almost always a clear physical meaning. Recall that the notion of Liouvillian systems is closely related to integrability by quadratures and thus to strictly triangular forms, also appearing in Marigo and Bicchi (2000). Let us also insist on the fact that the differential equations (4), satisfied by the integral variables ξi , only depend on the partially flat output y. Therefore, once we choose a smooth trajectory t → y(t), the RHS of (4) becomes a function of t only which justifies the fact that ξi are called integral variables. As it has been reported in Fliess et al. (1995, 1997) and SiraRamírez (1999), the definitions imply that both the flatness and the Liouvillian properties are useful to solve the MPP. This motivates the present study in the case of HOSs.
3. Modeling and Holonomy Consider the system setup illustrated by Figure 1. It comprises an object to be manipulated, and, without loss of generality, three fingers, enumerated from 1 to 3. Each finger is considered as a fully actuated manipulator with three joints. We assume that each segment of every finger and the manipulated object are rigid bodies. Contacts between the fingertips (last segments of the fingers) and the object are one point rolling contacts without slipping and are permanently maintained. We also assume that the object surface is smooth and convex and the finger surfaces are smooth and strictly convex. The convexity assumptions imply that the contact point between any of the fingers and the object is globally unique. The HOS is described by a set of differential and algebraic equations and inequality constraints ensuring that the above assumptions remain valid. They are presented in the next two subsections. A more detailed presentation of the modeling may be found
K1 O1
K
C1
Ko Oo
finger 1
C3 O3
Kb
2
C2
O2
object
K3
finger 2
finger 3
Fig. 1. Robot hand with the manipulated object.
in Cole, Hsu, and Sastry (1992), Kerr (1984), Kiss (2001), Montana (1988), and Murray (1994). 3.1. Geometric and Kinematic Conditions of Contact Denote the inertial base frame by K b . Let K i (i = 1, 2, 3) and K o be frames fixed to the center of gravity (c-o-g) of the ith fingertip and of the object with origins Oi and Oo , respectively. For any position vector, the subscript represents the point the vector is directed to and the superscript represents the coordinate frame. Hence the position of the c-o-g of the object (resp. finger i) with respect to the origin of the frame K b is denoted by pOb o = (xOb o , yOb o , zOb o ) (resp. pOb i = (xOb i , yOb i , zOb i )). Similar notation is used for the orientations of the frames: φob (resp. φib ) denotes the vector of the RP Y orientation angles of the frame K o (resp. K i , i = 1, 2, 3) with respect to the inertial base frame. The corresponding orientation matrices #(φob ) (resp. #(φib )) are obtained using eq. (B2) of Appendix B. Hence the pair i pOo , #(φoi ) = #(φib )T (pOb o − pOb i ), #(φib )T #(φob ) , (6) gives the relative position pOi o and the relative orientation #(φoi ) of the object with respect to the finger i. Denoting by Ci the contact point between the object and the finger i, the permanent contact assumption reads pCi i − pOi o − #(φoi )pCo i = 0.
(7)
446
THE INTERNATIONAL JOURNAL OF ROBOTICS RESEARCH / May-June 2002
Let the smooth function1 ci : R3 → R (resp. co : R3 → R) characterize the surface of fingertip i (resp. object) such that any surface point P of the ith fingertip (resp. object) with coordinates pPi (resp. pPo ) satisfies ci (pPi ) = 0 (resp. co (pPo ) = 0). Moreover, for any interior point P of the corresponding rigid body we have ci (pPi ) < 0 (resp. co (pPo ) < 0). Since Ci is a point of both surfaces in contact, one has ci (pCi i )
= 0 (8) co (pCo i ) = 0. (9) ∂c ∂c ∂c Denote by Dc = ∂x , ∂y , ∂z the derivative of a surface function c. Since the tangent planes of the surfaces at Ci must coincide, the unit normal vectors must be colinear with opposite directions: Dci (pCi i ) Dci (p ) i Ci
=−
Dco (pCo i )#(φoi )T Dco (pCo i )
.
(10)
Denote by qi the vector of joint coordinates of the finger i and by di the direct geometry function such that di : U ⊂ R3 → R3 × SO(3),
di (qi ) = (pOb i , #(φib )), (11)
where U is the set of admissible joint configurations. Constraints (7)–(11) are geometric. However, the rolling without slipping assumption imposes additional constraints on the relative velocities at the contact points. Recall that the two-dimensional vector space ker Dci (pCi i ) = {w ∈ R3 |Dci (pCi i )w = 0} is identified to the tangent plane of the i i surface of the finger at the point Ci . Denote by {w1,C , w2,C } i i i i i a basis of ker Dc (pCi ) and by v˜Ci the instantaneous relative velocity of the bodies in contact at the point Ci . Using eq. (B3) of Appendix B we have v˜Ci i = p˙ Oi o + [ωoi ×]#(φoi )pCo i . According to the rolling without slipping constraint: i (wj,C )T v˜Ci i = 0, i
j = 1, 2.
(12)
Constraints (8)–(11) and (12) must hold true at every contact point, namely for i = 1, 2, 3. 3.2. Dynamic Equations Denote by fib the contact force applied to the object by the ith fingertip. The Newton-Euler equations of the object read: mp¨Ob o
=
fgb +
3
fib
(13)
i=1
2ω˙ ob
=
3
#(φob )pCo i × fib − ωob × 2ωob ,
where 2 is the inertia matrix of the object, m is its mass and fgb is the gravitational force. The dynamic equation of the ith finger is Hi (qi )q¨i + hi (qi , q˙i ) = τi − τi,ext ,
(15)
where τi is the vector of joint torques, τi,ext is the vector of external joint torques, Hi is the inertia matrix of the ith finger and hi contains the quadratic (with respect to the joint velocities) and gravitational terms. The relation between the forces fib and torques τi,ext is given by the transformation: τi,ext = JiT GTi · fib
(16)
where Ji is the Jacobian matrix of the ith finger (i.e., the derivative of its direct geometry function) and GTi is the linear mapping transforming the contact force fib to an equivalent wrench (force and torque taken together) at Oi . 3.3. Inequality Constraints For the sake of completeness, let us state the inequality constraints implied by the rolling without slipping contact assumptions. The contact forces applied by the fingers must be oriented inwards the object Dco (pCo i ) · #(φob )T fib < 0
(17)
and because of the rolling contacts they cannot leave the friction cone described by the inequality fitb
